public HumanoidControllerWarmup(DRCRobotModel robotModel) { this.robotModel = robotModel; controlDT = robotModel.getControllerDT(); setupController(); controllerToolbox.initialize(); walkingController.initialize(); controllerCore.initialize(); }
public static void main(String[] args) throws IOException { DRCRobotModel robotModel = new ValkyrieRobotModel(RobotTarget.SCS, false); DRCRobotJointMap jointMap = robotModel.getJointMap(); HumanoidFloatingRootJointRobot sdfRobot = robotModel.createHumanoidFloatingRootJointRobot(false); FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel(); FullHumanoidRobotModel fullRobotModelForSlider = robotModel.createFullRobotModel(); new PosePlaybackSCSBridge(sdfRobot, fullRobotModel, fullRobotModelForSlider, robotModel.getControllerDT()); } }
SingleThreadedRobotController robotController = new SingleThreadedRobotController("testSingleThreadedRobotController", robot, null); int estimatorTicksPerSimulationTick = (int) Math.round(robotModel.getEstimatorDT() / robotModel.getEstimatorDT()); int controllerTicksPerSimulationTick = (int) Math.round(robotModel.getControllerDT() / robotModel.getEstimatorDT());
private void setupTest() throws SimulationExceededMaximumTimeException { BambooTools.reportTestStartedMessage(simulationTestingParameters.getShowWindows()); drcSimulationTestHelper = new DRCSimulationTestHelper(simulationTestingParameters, getRobotModel()); drcSimulationTestHelper.createSimulation(getClass().getSimpleName()); ThreadTools.sleep(1000); FullHumanoidRobotModel fullRobotModel = drcSimulationTestHelper.getControllerFullRobotModel(); pelvis = fullRobotModel.getPelvis(); chest = fullRobotModel.getChest(); spineJoints = MultiBodySystemTools.createOneDoFJointPath(pelvis, chest); numberOfJoints = spineJoints.length; assertTrue(drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(1.0)); SimulationConstructionSet scs = drcSimulationTestHelper.getSimulationConstructionSet(); controllerSpy = new ControllerSpy(spineJoints, scs, getRobotModel().getControllerDT()); drcSimulationTestHelper.addRobotControllerOnControllerThread(controllerSpy); }
private void doSingleTimeUpdate() { // (1) do control and compute desired accelerations controllerToolbox.update(); walkingController.doAction(); ControllerCoreCommand coreCommand = walkingController.getControllerCoreCommand(); controllerCore.submitControllerCoreCommand(coreCommand); controllerCore.compute(); // (2) integrate accelerations in full robot model integrate(); // update viz and advance time fullRobotModel.updateFrames(); referenceFrames.updateFrames(); yoTime.add(robotModel.getControllerDT()); }
private void setupSimulationOutputWriter() { simulationOutputWriter = new DRCSimulationOutputWriter(humanoidFloatingRootJointRobot); if (doSmoothJointTorquesAtControllerStateChanges.get()) { DRCOutputWriterWithStateChangeSmoother drcOutputWriterWithStateChangeSmoother = new DRCOutputWriterWithStateChangeSmoother(simulationOutputWriter); momentumBasedControllerFactory.get() .attachControllerStateChangedListener(drcOutputWriterWithStateChangeSmoother.createControllerStateChangedListener()); simulationOutputWriter = drcOutputWriterWithStateChangeSmoother; } if (doSlowIntegrationForTorqueOffset.get()) { DRCOutputWriterWithTorqueOffsets outputWriterWithTorqueOffsets = new DRCOutputWriterWithTorqueOffsets(simulationOutputWriter, robotModel.get().getControllerDT()); simulationOutputWriter = outputWriterWithTorqueOffsets; } }
private void setupSimulationOutputProcessor() { simulationOutputProcessor = robotModel.get().getCustomSimulationOutputProcessor(humanoidFloatingRootJointRobot); if (doSmoothJointTorquesAtControllerStateChanges.get()) { DRCOutputProcessorWithStateChangeSmoother drcOutputWriterWithStateChangeSmoother = new DRCOutputProcessorWithStateChangeSmoother(simulationOutputProcessor); highLevelHumanoidControllerFactory.get() .attachControllerStateChangedListener(drcOutputWriterWithStateChangeSmoother.createControllerStateChangedListener()); simulationOutputProcessor = drcOutputWriterWithStateChangeSmoother; } if (doSlowIntegrationForTorqueOffset.get()) { DRCOutputProcessorWithTorqueOffsets outputWriterWithTorqueOffsets = new DRCOutputProcessorWithTorqueOffsets(simulationOutputProcessor, robotModel.get().getControllerDT()); simulationOutputProcessor = outputWriterWithTorqueOffsets; } }
private void setupMultiThreadedRobotController() { if (scsInitialSetup.get().getRunMultiThreaded()) { threadedRobotController = new MultiThreadedRobotController("DRCSimulation", humanoidFloatingRootJointRobot, simulationConstructionSet); } else { PrintTools.warn(this, "Running simulation in single threaded mode", true); threadedRobotController = new SingleThreadedRobotController("DRCSimulation", humanoidFloatingRootJointRobot, simulationConstructionSet); } int estimatorTicksPerSimulationTick = (int) Math.round(robotModel.get().getEstimatorDT() / robotModel.get().getSimulateDT()); int controllerTicksPerSimulationTick = (int) Math.round(robotModel.get().getControllerDT() / robotModel.get().getSimulateDT()); int slowPublisherTicksPerSimulationTick = (int) Math.round(10 * robotModel.get().getEstimatorDT() / robotModel.get().getSimulateDT()); threadedRobotController.addController(stateEstimationThread, estimatorTicksPerSimulationTick, false); threadedRobotController.addController(controllerThread, controllerTicksPerSimulationTick, true); MultiThreadedRobotControlElement simulatedHandController = robotModel.get().createSimulatedHandController(humanoidFloatingRootJointRobot, threadDataSynchronizer, realtimeRos2Node.get(), closeableAndDisposableRegistry); if (simulatedHandController != null) { threadedRobotController.addController(simulatedHandController, controllerTicksPerSimulationTick, false); } }
private void executeMessage(SpineTrajectoryMessage message) throws SimulationExceededMaximumTimeException { double controllerDT = getRobotModel().getControllerDT(); drcSimulationTestHelper.publishToController(message); double trajectoryTime = 0.0; for (int jointIdx = 0; jointIdx < numberOfJoints; jointIdx++) { OneDoFJointTrajectoryMessage jointTrajectory = message.getJointspaceTrajectory().getJointTrajectoryMessages().get(jointIdx); double jointTrajectoryTime = jointTrajectory.getTrajectoryPoints().getLast().getTime(); if (jointTrajectoryTime > trajectoryTime) trajectoryTime = jointTrajectoryTime; } assertTrue(drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(trajectoryTime + 5.0 * controllerDT)); assertDesiredsMatchAfterExecution(message, spineJoints, drcSimulationTestHelper.getSimulationConstructionSet()); }
private DRCSimulationStarter(DRCRobotModel robotModel, CommonAvatarEnvironmentInterface environment, GroundProfile3D groundProfile3D) { this.robotModel = robotModel; this.environment = environment; this.guiInitialSetup = new DRCGuiInitialSetup(false, false); this.robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0, 0); this.createSCSSimulatedSensors = true; this.scsInitialSetup = new DRCSCSInitialSetup(groundProfile3D, robotModel.getSimulateDT()); this.scsInitialSetup.setDrawGroundProfile(environment == null); this.scsInitialSetup.setInitializeEstimatorToActual(false); this.scsInitialSetup.setTimePerRecordTick(robotModel.getControllerDT()); this.scsInitialSetup.setRunMultiThreaded(true); this.highLevelControllerParameters = robotModel.getHighLevelControllerParameters(); this.walkingControllerParameters = robotModel.getWalkingControllerParameters(); this.capturePointPlannerParameters = robotModel.getCapturePointPlannerParameters(); this.contactPointParameters = robotModel.getContactPointParameters(); }
public DRCSimulationStarter(DRCRobotModel robotModel, CommonAvatarEnvironmentInterface environment) { this.robotModel = robotModel; this.environment = environment; this.guiInitialSetup = new DRCGuiInitialSetup(false, false); this.robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0, 0); this.createSCSSimulatedSensors = true; scsInitialSetup = new DRCSCSInitialSetup(environment, robotModel.getSimulateDT()); scsInitialSetup.setInitializeEstimatorToActual(false); scsInitialSetup.setTimePerRecordTick(robotModel.getControllerDT()); scsInitialSetup.setRunMultiThreaded(true); this.walkingControllerParameters = robotModel.getWalkingControllerParameters(); this.armControllerParameters = robotModel.getArmControllerParameters(); this.capturePointPlannerParameters = robotModel.getCapturePointPlannerParameters(); this.icpOptimizationParameters = robotModel.getICPOptimizationParameters(); this.contactPointParameters = robotModel.getContactPointParameters(); }
@ContinuousIntegrationAnnotations.ContinuousIntegrationTest(estimatedDuration = 25.5) @Test(timeout = 130000) public void testSingleTrajectoryPoint() throws SimulationExceededMaximumTimeException { double epsilon = 1.0e-10; double yaw = Math.toRadians(5.0); double pitch = Math.toRadians(-6.0); double roll = Math.toRadians(-5.0); double trajectoryTime = 0.5; Quaternion orientation = new Quaternion(); orientation.appendYawRotation(yaw); orientation.appendPitchRotation(pitch); orientation.appendRollRotation(roll); ReferenceFrame midFootZUpGroundFrame = humanoidReferenceFrames.getMidFootZUpGroundFrame(); FrameQuaternion pelvisOrientation = new FrameQuaternion(midFootZUpGroundFrame, orientation); pelvisOrientation.changeFrame(worldFrame); PelvisOrientationTrajectoryMessage message = HumanoidMessageTools.createPelvisOrientationTrajectoryMessage(trajectoryTime, pelvisOrientation); SO3TrajectoryPointMessage waypoint = message.getSo3Trajectory().getTaskspaceTrajectoryPoints().get(0); drcSimulationTestHelper.publishToController(message); drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(4.0 * getRobotModel().getControllerDT()); String pelvisName = fullRobotModel.getPelvis().getName(); String postFix = "Orientation"; EndToEndTestTools.assertNumberOfPoints(pelvisName, postFix, 2, scs); EndToEndTestTools.assertWaypointInGeneratorMatches(pelvisName, 1, waypoint, scs, epsilon); drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(trajectoryTime); EndToEndTestTools.assertCurrentDesiredsMatchWaypoint(pelvisName, waypoint, scs, epsilon); drcSimulationTestHelper.createVideo(getSimpleRobotName(), 2); }
private void executeMessage(ChestTrajectoryMessage message, RigidBodyBasics chest) throws SimulationExceededMaximumTimeException { double controllerDT = getRobotModel().getControllerDT(); drcSimulationTestHelper.publishToController(message); double trajectoryTime = message.getSo3Trajectory().getTaskspaceTrajectoryPoints().getLast().getTime(); assertTrue(drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(trajectoryTime + 5.0 * controllerDT)); Quaternion desired = new Quaternion(message.getSo3Trajectory().getTaskspaceTrajectoryPoints().getLast().getOrientation()); assertChestDesired(drcSimulationTestHelper.getSimulationConstructionSet(), desired, chest); }
int controllerTicksPerSimulationTick = (int) Math.round(robotModel.get().getControllerDT() / robotModel.get().getSimulateDT()); int slowPublisherTicksPerSimulationTick = (int) Math.round(10 * robotModel.get().getEstimatorDT() / robotModel.get().getSimulateDT());
ChestTrajectoryMessage holdChestInWorldMessage = HumanoidMessageTools.createChestTrajectoryMessage(0.0, chestOrientation, worldFrame, worldFrame); drcSimulationTestHelper.publishToController(holdChestInWorldMessage); drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(2.0 * getRobotModel().getControllerDT()); drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(2.0 * getRobotModel().getControllerDT());
public VisualizePoseWorkspace(DRCRobotModel robotModel) throws IOException { this.controlDT = robotModel.getControllerDT(); DRCRobotJointMap jointMap = robotModel.getJointMap(); HumanoidFloatingRootJointRobot sdfRobot = robotModel.createHumanoidFloatingRootJointRobot(false); interpolator = new PlaybackPoseInterpolator(registry); SimulationConstructionSet scs = new SimulationConstructionSet(sdfRobot); scs.addYoVariableRegistry(registry); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); fullRobotModelForSlider = robotModel.createFullRobotModel(); DRCRobotMidiSliderBoardPositionManipulation sliderBoard = new DRCRobotMidiSliderBoardPositionManipulation(scs, sdfRobot, fullRobotModelForSlider, yoGraphicsListRegistry); posePlaybackRobotPoseSequence = new PlaybackPoseSequence(fullRobotModelForSlider); CaptureSnapshotListener captureSnapshotListener = new CaptureSnapshotListener(sdfRobot, scs); sliderBoard.addCaptureSnapshotListener(captureSnapshotListener); SaveSequenceListener saveSequenceListener = new SaveSequenceListener(); sliderBoard.addSaveSequenceRequestedListener(saveSequenceListener); LoadSequenceListener loadSequenceListener = new LoadSequenceListener(fullRobotModelForSlider, sdfRobot, scs); sliderBoard.addLoadSequenceRequestedListener(loadSequenceListener); scs.startOnAThread(); }
sendQueuedFootTrajectoryMessages(scs, robotSide, numberOfMessages, trajectoryPoints); success = drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(getRobotModel().getControllerDT()); assertTrue(success);
public VisualizePoseWorkspace(DRCRobotModel robotModel) throws IOException { this.controlDT = robotModel.getControllerDT(); DRCRobotJointMap jointMap = robotModel.getJointMap(); HumanoidFloatingRootJointRobot sdfRobot = robotModel.createHumanoidFloatingRootJointRobot(false); interpolator = new PlaybackPoseInterpolator(registry); SimulationConstructionSet scs = new SimulationConstructionSet(sdfRobot); scs.addYoVariableRegistry(registry); scs.addYoGraphicsListRegistry(yoGraphicsListRegistry); fullRobotModelForSlider = robotModel.createFullRobotModel(); DRCRobotMidiSliderBoardPositionManipulation sliderBoard = new DRCRobotMidiSliderBoardPositionManipulation(scs, sdfRobot, fullRobotModelForSlider, yoGraphicsListRegistry); posePlaybackRobotPoseSequence = new PlaybackPoseSequence(fullRobotModelForSlider); CaptureSnapshotListener captureSnapshotListener = new CaptureSnapshotListener(sdfRobot, scs); sliderBoard.addCaptureSnapshotListener(captureSnapshotListener); SaveSequenceListener saveSequenceListener = new SaveSequenceListener(); sliderBoard.addSaveSequenceRequestedListener(saveSequenceListener); LoadSequenceListener loadSequenceListener = new LoadSequenceListener(fullRobotModelForSlider, sdfRobot, scs); sliderBoard.addLoadSequenceRequestedListener(loadSequenceListener); scs.startOnAThread(); }
drcSimulationTestHelper.publishToController(chestTrajectoryMessage); assertTrue(drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(getRobotModel().getControllerDT())); humanoidReferenceFrames.updateFrames(); desiredRandomChestOrientation.changeFrame(humanoidReferenceFrames.getPelvisZUpFrame());
drcSimulationTestHelper.publishToController(headTrajectoryMessage); boolean success = drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(getRobotModel().getControllerDT()); // Trick to get frames synchronized with the controller. assertTrue(success);