private void callSCSMethodSimulateOneTimeStep(SimulationConstructionSet scs) { try { scs.simulateOneTimeStep(); } catch (UnreasonableAccelerationException e) { e.printStackTrace(); } }
private WholeBodyTrajectoryToolboxOutputStatus runToolboxController(int maxNumberOfIterations) throws UnreasonableAccelerationException { AtomicReference<WholeBodyTrajectoryToolboxOutputStatus> status = new AtomicReference<>(null); statusOutputManager.attachStatusMessageListener(WholeBodyTrajectoryToolboxOutputStatus.class, status::set); initializationSucceeded.set(false); this.numberOfIterations.set(0); if (visualize) { for (int i = 0; !toolboxController.isDone() && i < maxNumberOfIterations; i++) scs.simulateOneTimeStep(); } else { for (int i = 0; !toolboxController.isDone() && i < maxNumberOfIterations; i++) toolboxUpdater.doControl(); } return status.getAndSet(null); }
private void runKinematicsPlanningToolboxController(int numberOfIterations) throws SimulationExceededMaximumTimeException, UnreasonableAccelerationException { initializationSucceeded.set(false); this.numberOfIterations.set(0); if (visualize) { for (int i = 0; !toolboxController.isDone() && i < numberOfIterations; i++) if (visualize) scs.simulateOneTimeStep(); } else { for (int i = 0; !toolboxController.isDone() && i < numberOfIterations; i++) { toolboxUpdater.doControl(); } } trackSolution(); }
private void visualizeSolution(WholeBodyTrajectoryToolboxOutputStatus solution, double timeResolution) throws UnreasonableAccelerationException { hideRobot(); robot.getControllers().clear(); FullHumanoidRobotModel robotForViz = getRobotModel().createFullRobotModel(); FloatingJointBasics rootJoint = robotForViz.getRootJoint(); OneDoFJointBasics[] joints = FullRobotModelUtils.getAllJointsExcludingHands(robotForViz); double trajectoryTime = solution.getTrajectoryTimes().get(solution.getTrajectoryTimes().size() - 1); double t = 0.0; while (t <= trajectoryTime) { t += timeResolution; KinematicsToolboxOutputStatus frame = findFrameFromTime(solution, t); MessageTools.unpackDesiredJointState(frame, rootJoint, joints); robotForViz.updateFrames(); snapGhostToFullRobotModel(robotForViz); scs.simulateOneTimeStep(); } }