How to use

getControllerDT

method

in

us.ihmc.avatar.drcRobot.DRCRobotModel

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);