How to use

getSimulateDT

method

in

us.ihmc.avatar.drcRobot.DRCRobotModel

private void setupSimulatedRobotTimeProvider()
{
 simulatedRobotTimeProvider = new SimulatedDRCRobotTimeProvider(robotModel.get().getSimulateDT());
 humanoidFloatingRootJointRobot.setController(simulatedRobotTimeProvider);
}

private void setupCOMVisualization()
{
 SimulatedRobotCenterOfMassVisualizer simulatedRobotCenterOfMassVisualizer = new SimulatedRobotCenterOfMassVisualizer(humanoidFloatingRootJointRobot,
                                                            robotModel.get().getSimulateDT());
 humanoidFloatingRootJointRobot.setController(simulatedRobotCenterOfMassVisualizer);
}

private void setupCOMVisualization()
{
 SimulatedRobotCenterOfMassVisualizer simulatedRobotCenterOfMassVisualizer = new SimulatedRobotCenterOfMassVisualizer(humanoidFloatingRootJointRobot,
                                                            robotModel.get().getSimulateDT());
 humanoidFloatingRootJointRobot.setController(simulatedRobotCenterOfMassVisualizer);
}

private void setupSimulatedRobotTimeProvider()
{
 simulatedRobotTimeProvider = new SimulatedDRCRobotTimeProvider(robotModel.get().getSimulateDT());
 humanoidFloatingRootJointRobot.setController(simulatedRobotTimeProvider);
}

public void startSimulation()
{
 SimulationConstructionSetParameters simulationParameters = new SimulationConstructionSetParameters(true, 16000);
 SimulationConstructionSet scs = new SimulationConstructionSet(simulatedRobot, simulationParameters);
 scs.setDT(robotModel.getSimulateDT(), 10);
 scs.setCameraPosition(scsCameraPosition.x, scsCameraPosition.y, scsCameraPosition.z);
 scs.setCameraFix(scsCameraFix.x, scsCameraFix.y, scsCameraFix.z);
 scs.startOnAThread();
}

private DRCFlatGroundWalkingTrack setupSimulationTrack(WalkingControllerParameters drcControlParameters, GroundProfile3D groundProfile,
   GroundProfile3D groundProfile3D, boolean drawGroundProfile, boolean useVelocityAndHeadingScript, boolean cheatWithGroundHeightAtForFootstep,
   DRCRobotInitialSetup<HumanoidFloatingRootJointRobot> robotInitialSetup)
{
 DRCGuiInitialSetup guiInitialSetup = createGUIInitialSetup();
 DRCSCSInitialSetup scsInitialSetup;
 if (groundProfile != null)
 {
   scsInitialSetup = new DRCSCSInitialSetup(groundProfile, robotModel.getSimulateDT());
 }
 else
 {
   scsInitialSetup = new DRCSCSInitialSetup(groundProfile3D, robotModel.getSimulateDT());
 }
 scsInitialSetup.setDrawGroundProfile(drawGroundProfile);
 if (cheatWithGroundHeightAtForFootstep)
   scsInitialSetup.setInitializeEstimatorToActual(true);
 DRCFlatGroundWalkingTrack drcFlatGroundWalkingTrack = new DRCFlatGroundWalkingTrack(robotInitialSetup, guiInitialSetup,
                              scsInitialSetup, useVelocityAndHeadingScript, cheatWithGroundHeightAtForFootstep, robotModel);
 SimulationConstructionSet scs = drcFlatGroundWalkingTrack.getSimulationConstructionSet();
 scs.setGroundVisible(false);
 setupCameraForUnitTest(scs);
 return drcFlatGroundWalkingTrack;
}

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 setupTrack(boolean runMultiThreaded, DRCRobotModel robotModel)
{
 DRCGuiInitialSetup guiInitialSetup = new DRCGuiInitialSetup(true, false, simulationTestingParameters);
 GroundProfile3D groundProfile = new FlatGroundProfile();
 DRCSCSInitialSetup scsInitialSetup = new DRCSCSInitialSetup(groundProfile, robotModel.getSimulateDT());
 // scsInitialSetup.setInitializeEstimatorToActual(true);
 scsInitialSetup.setDrawGroundProfile(true);
 scsInitialSetup.setRunMultiThreaded(runMultiThreaded);
 DRCRobotInitialSetup<HumanoidFloatingRootJointRobot> robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0.0, 0.0);
 drcFlatGroundWalkingTrack = new DRCFlatGroundWalkingTrack(robotInitialSetup, guiInitialSetup, scsInitialSetup, true, false, robotModel);
 drcFlatGroundWalkingTrack.getAvatarSimulation();
}

private void setupTrack(boolean runMultiThreaded, DRCRobotModel robotModel)
{
 DRCGuiInitialSetup guiInitialSetup = new DRCGuiInitialSetup(true, false, simulationTestingParameters);
 GroundProfile3D groundProfile = new FlatGroundProfile();
 DRCSCSInitialSetup scsInitialSetup = new DRCSCSInitialSetup(groundProfile, robotModel.getSimulateDT());
 scsInitialSetup.setInitializeEstimatorToActual(true);
 scsInitialSetup.setDrawGroundProfile(true);
 scsInitialSetup.setRunMultiThreaded(runMultiThreaded);
 DRCRobotInitialSetup<HumanoidFloatingRootJointRobot> robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0.0, 0.0);
 drcFlatGroundWalkingTrack = new DRCFlatGroundWalkingTrack(robotInitialSetup, guiInitialSetup, scsInitialSetup, true, false,
    robotModel);
}

private void setupSimulationConstructionSet()
{
 SimulationConstructionSetParameters simulationConstructionSetParameters = guiInitialSetup.get().getSimulationConstructionSetParameters();
 simulationConstructionSetParameters.setDataBufferSize(scsInitialSetup.get().getSimulationDataBufferSize());
 List<Robot> allSimulatedRobotList = new ArrayList<Robot>();
 allSimulatedRobotList.add(humanoidFloatingRootJointRobot);
 if (commonAvatarEnvironment.get() != null && commonAvatarEnvironment.get().getEnvironmentRobots() != null)
 {
   allSimulatedRobotList.addAll(commonAvatarEnvironment.get().getEnvironmentRobots());
   commonAvatarEnvironment.get().addContactPoints(humanoidFloatingRootJointRobot.getAllGroundContactPoints());
   commonAvatarEnvironment.get().createAndSetContactControllerToARobot();
 }
 simulationConstructionSet = new SimulationConstructionSet(allSimulatedRobotList.toArray(new Robot[0]), guiInitialSetup.get().getGraphics3DAdapter(),
                              simulationConstructionSetParameters);
 simulationConstructionSet.setDT(robotModel.get().getSimulateDT(), 1);
}

private void setupSimulationConstructionSet()
{
 SimulationConstructionSetParameters simulationConstructionSetParameters = guiInitialSetup.get().getSimulationConstructionSetParameters();
 simulationConstructionSetParameters.setDataBufferSize(scsInitialSetup.get().getSimulationDataBufferSize());
 List<Robot> allSimulatedRobotList = new ArrayList<Robot>();
 allSimulatedRobotList.add(humanoidFloatingRootJointRobot);
 if (commonAvatarEnvironment.get() != null && commonAvatarEnvironment.get().getEnvironmentRobots() != null)
 {
   allSimulatedRobotList.addAll(commonAvatarEnvironment.get().getEnvironmentRobots());
   commonAvatarEnvironment.get().addContactPoints(humanoidFloatingRootJointRobot.getAllGroundContactPoints());
   commonAvatarEnvironment.get().createAndSetContactControllerToARobot();
 }
 simulationConstructionSet = new SimulationConstructionSet(allSimulatedRobotList.toArray(new Robot[0]), guiInitialSetup.get().getGraphics3DAdapter(),
                              simulationConstructionSetParameters);
 simulationConstructionSet.setDT(robotModel.get().getSimulateDT(), 1);
}

private void setupLidarController()
{
 DRCRobotLidarParameters lidarParameters = robotModel.get().getSensorInformation().getLidarParameters(0);
 if (lidarParameters != null && lidarParameters.getLidarSpindleJointName() != null)
 {
   PIDLidarTorqueController pidLidarTorqueController = new PIDLidarTorqueController(humanoidFloatingRootJointRobot,
                                           lidarParameters.getLidarSpindleJointName(),
                                           lidarParameters.getLidarSpindleVelocity(),
                                           robotModel.get().getSimulateDT());
   humanoidFloatingRootJointRobot.setController(pidLidarTorqueController);
 }
}

private void setupLidarController()
{
 DRCRobotLidarParameters lidarParameters = robotModel.get().getSensorInformation().getLidarParameters(0);
 if (lidarParameters != null && lidarParameters.getLidarSpindleJointName() != null)
 {
   PIDLidarTorqueController pidLidarTorqueController = new PIDLidarTorqueController(humanoidFloatingRootJointRobot,
                                           lidarParameters.getLidarSpindleJointName(),
                                           lidarParameters.getLidarSpindleVelocity(),
                                           robotModel.get().getSimulateDT());
   humanoidFloatingRootJointRobot.setController(pidLidarTorqueController);
 }
}

private DRCFlatGroundWalkingTrack setupFlatGroundSimulationTrackForSameWayTwiceVerifier(DRCRobotModel robotModel)
{
 DRCGuiInitialSetup guiInitialSetup = createGUIInitialSetup();
 GroundProfile3D groundProfile = new FlatGroundProfile();
 DRCSCSInitialSetup scsInitialSetup = new DRCSCSInitialSetup(groundProfile, robotModel.getSimulateDT());
 scsInitialSetup.setDrawGroundProfile(drawGroundProfile);
 if (cheatWithGroundHeightAtForFootstep)
   scsInitialSetup.setInitializeEstimatorToActual(true);
 DRCRobotInitialSetup<HumanoidFloatingRootJointRobot> robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0.0, 0.0);
 DRCFlatGroundWalkingTrack drcFlatGroundWalkingTrack = new DRCFlatGroundWalkingTrack(robotInitialSetup, guiInitialSetup,
    scsInitialSetup, useVelocityAndHeadingScript, cheatWithGroundHeightAtForFootstep, robotModel);
 SimulationConstructionSet scs = drcFlatGroundWalkingTrack.getSimulationConstructionSet();
 setupCameraForUnitTest(scs);
 return drcFlatGroundWalkingTrack;
}

private SimulationConstructionSet setupScs()
{
 boolean useVelocityAndHeadingScript = true;
 boolean cheatWithGroundHeightAtForFootstep = false;
 GroundProfile3D groundProfile = new FlatGroundProfile();
 DRCRobotModel robotModel = getRobotModel();
 DRCGuiInitialSetup guiInitialSetup = createGUIInitialSetup();
 DRCRobotInitialSetup<HumanoidFloatingRootJointRobot> robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0, 0);
 DRCSCSInitialSetup scsInitialSetup = new DRCSCSInitialSetup(groundProfile, robotModel.getSimulateDT());
 scsInitialSetup.setRunMultiThreaded(false);
 DRCFlatGroundWalkingTrack drcFlatGroundWalkingTrack = new DRCFlatGroundWalkingTrack(robotInitialSetup, guiInitialSetup, scsInitialSetup,
                              useVelocityAndHeadingScript, cheatWithGroundHeightAtForFootstep, robotModel);
 SimulationConstructionSet scs = drcFlatGroundWalkingTrack.getSimulationConstructionSet();
 setupCameraForUnitTest(scs);
 return scs;
}

public void startSimulation()
{
 SimulationConstructionSetParameters simulationParameters = new SimulationConstructionSetParameters(true, 16000);
 SimulationConstructionSet scs = new SimulationConstructionSet(simulatedRobot, simulationParameters);
 scs.setDT(robotModel.getSimulateDT(), 10);
 scs.setCameraPosition(scsCameraPosition.getX(), scsCameraPosition.getY(), scsCameraPosition.getZ());
 scs.setCameraFix(scsCameraFix.getX(), scsCameraFix.getY(), scsCameraFix.getZ());
 scs.startOnAThread();
}

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

private void setupPositionControlledJointsForSimulation()
{
 String[] positionControlledJointNames = robotModel.get().getJointMap().getPositionControlledJointsForSimulation();
 if (positionControlledJointNames != null)
 {
   for (String positionControlledJointName : positionControlledJointNames)
   {
    OneDegreeOfFreedomJoint simulatedJoint = humanoidFloatingRootJointRobot.getOneDegreeOfFreedomJoint(positionControlledJointName);
    FullRobotModel controllerFullRobotModel = threadDataSynchronizer.getControllerFullRobotModel();
    OneDoFJoint controllerJoint = controllerFullRobotModel.getOneDoFJointByName(positionControlledJointName);
    JointRole jointRole = robotModel.get().getJointMap().getJointRole(positionControlledJointName);
    boolean isUpperBodyJoint = ((jointRole != JointRole.LEG) && (jointRole != JointRole.SPINE));
    boolean isBackJoint = jointRole == JointRole.SPINE;
    JointLowLevelPositionControlSimulator positionControlSimulator = new JointLowLevelPositionControlSimulator(simulatedJoint, controllerJoint,
                                                          isUpperBodyJoint, isBackJoint, false,
                                                          robotModel.get().getSimulateDT());
    humanoidFloatingRootJointRobot.setController(positionControlSimulator);
   }
 }
}

private DRCFlatGroundWalkingTrack setupWalkingSim()
{
 DRCRobotModel robotModel = getRobotModel();
 DRCGuiInitialSetup guiInitialSetup = new DRCGuiInitialSetup(true, false, simulationTestingParameters);
 GroundProfile3D groundProfile = new FlatGroundProfile();
 DRCSCSInitialSetup scsInitialSetup = new DRCSCSInitialSetup(groundProfile, robotModel.getSimulateDT());
 scsInitialSetup.setDrawGroundProfile(false);
 externalPelvisPosePublisher = new ExternalPelvisPoseCreator();
 DRCFlatGroundWalkingTrack drcFlatGroundWalkingTrack = new DRCFlatGroundWalkingTrack(robotModel.getDefaultRobotInitialSetup(0.0, 0.0), guiInitialSetup,
    scsInitialSetup, true, false, getRobotModel(), externalPelvisPosePublisher);
 simulationConstructionSet = drcFlatGroundWalkingTrack.getSimulationConstructionSet();
 robot = drcFlatGroundWalkingTrack.getAvatarSimulation().getHumanoidFloatingRootJointRobot();
 registry = robot.getRobotsYoVariableRegistry();
 YoBoolean walk = (YoBoolean) simulationConstructionSet.getVariable("walkCSG");
 walk.set(true);
 return drcFlatGroundWalkingTrack;
}