How to use

getEstimatorDT

method

in

us.ihmc.avatar.drcRobot.DRCRobotModel

@Override
public double getEstimatorDT()
{
 return robotModel.getEstimatorDT();
}

public DiagnosticAnalysisProcessor(LogDataProcessorHelper logDataProcessorHelper, DRCRobotModel drcRobotModel)
{
 this.logDataProcessorHelper = logDataProcessorHelper;
 diagnosticParameters = new DiagnosticParameters(DiagnosticEnvironment.OFFLINE_LOG, false);
 fullRobotModel = logDataProcessorHelper.getFullRobotModel();
 dt = drcRobotModel.getEstimatorDT();
 logYoVariableHolder = logDataProcessorHelper.getLogYoVariableHolder();
}

public DiagnosticAnalysisProcessor(LogDataProcessorHelper logDataProcessorHelper, DRCRobotModel drcRobotModel)
{
 this.logDataProcessorHelper = logDataProcessorHelper;
 diagnosticParameters = new DiagnosticParameters(DiagnosticEnvironment.OFFLINE_LOG, false);
 fullRobotModel = logDataProcessorHelper.getFullRobotModel();
 dt = drcRobotModel.getEstimatorDT();
 logYoVariableHolder = logDataProcessorHelper.getLogYoVariableHolder();
}

int estimatorTicksPerSimulationTick = (int) Math.round(robotModel.getEstimatorDT() / robotModel.getEstimatorDT());
int controllerTicksPerSimulationTick = (int) Math.round(robotModel.getControllerDT() / robotModel.getEstimatorDT());
                                           robotModel.getEstimatorDT());

private void setupYoVariableServer()
{
 if (createYoVariableServer.get())
 {
   yoVariableServer = new YoVariableServer(getClass(), new PeriodicNonRealtimeThreadSchedulerFactory(), robotModel.get().getLogModelProvider(),
                       robotModel.get().getLogSettings(), robotModel.get().getEstimatorDT());
 }
}

private void setupControllerThread()
{
 controllerThread = new DRCControllerThread(robotModel.get(), robotModel.get().getSensorInformation(), momentumBasedControllerFactory.get(),
                       threadDataSynchronizer, simulationOutputWriter, humanoidGlobalDataProducer.get(), yoVariableServer,
                       gravity.get(), robotModel.get().getEstimatorDT());
}

this.estimatorDTInNS = TimeTools.secondsToNanoSeconds(robotModel.getEstimatorDT());
sendFingerJointGains.set(true);

private void setupYoVariableServer()
{
 if (robotModel.get().getLogSettings().isLog())
 {
   yoVariableServer = new YoVariableServer(getClass(), new PeriodicNonRealtimeThreadScheduler("DRCSimulationYoVariableServer"),
                       robotModel.get().getLogModelProvider(), robotModel.get().getLogSettings(), robotModel.get().getEstimatorDT());
 }
 else
 {
   yoVariableServer = null;
 }
}

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

this.estimatorDTInNS = Conversions.secondsToNanoseconds(robotModel.getEstimatorDT());
sendFingerJointGains.set(true);

success &= drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(getRobotModel().getEstimatorDT() * 10);
targets[i].getTranslation(targetTranslation);
targets[i].getRotation(targetQuat);
StampedPosePacket posePacket = HumanoidMessageTools.createStampedPosePacket("/pelvis", timeStampedTransform, 1.0);
success &= drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(getRobotModel().getEstimatorDT() * 3);
externalPelvisPosePublisher.setNewestPose(posePacket);
 success &= drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(getRobotModel().getEstimatorDT());
 success &= drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(getRobotModel().getEstimatorDT() * 100);

private void setupControllerThread()
{
 String robotName = robotModel.get().getSimpleRobotName();
 controllerThread = new DRCControllerThread(robotName, robotModel.get(), robotModel.get().getSensorInformation(), highLevelHumanoidControllerFactory.get(),
                       threadDataSynchronizer, simulationOutputProcessor, realtimeRos2Node.get(), yoVariableServer, gravity.get(),
                       robotModel.get().getEstimatorDT());
}

success &= drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(getRobotModel().getEstimatorDT() * 10);
targets[i].getTranslation(targetTranslation);
targets[i].getRotation(targetQuat);
StampedPosePacket posePacket = HumanoidMessageTools.createStampedPosePacket("/pelvis", timeStampedTransform, 1.0);
success &= drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(getRobotModel().getEstimatorDT() * 3);
externalPelvisPosePublisher.setNewestPose(posePacket);
 success &= drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(getRobotModel().getEstimatorDT());
 success &= drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(getRobotModel().getEstimatorDT() * 100);

public AutomatedDiagnosticConfiguration createDiagnosticController(boolean startWithRobotAlive)
{
 simulatedRobot = robotModel.createHumanoidFloatingRootJointRobot(false);
 DiagnosticLoggerConfiguration.setupLogging(simulatedRobot.getYoTime(), getClass(), robotModel.getSimpleRobotName());
 if (robotInitialSetup == null)
   robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0.0, 0.0);
 robotInitialSetup.initializeRobot(simulatedRobot, robotModel.getJointMap());
 FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel();
 WalkingControllerParameters walkingControllerParameters = robotModel.getWalkingControllerParameters();
 DoubleYoVariable yoTime = simulatedRobot.getYoTime();
 double dt = robotModel.getEstimatorDT();
 StateEstimatorParameters stateEstimatorParameters = robotModel.getStateEstimatorParameters();
 if (diagnosticParameters == null)
   diagnosticParameters = new DiagnosticParameters(DiagnosticEnvironment.RUNTIME_CONTROLLER, false);
 DiagnosticSensorProcessingConfiguration sensorProcessingConfiguration = new DiagnosticSensorProcessingConfiguration(diagnosticParameters, stateEstimatorParameters);
 SensorOutputMapReadOnly sensorOutputMap = createStateEstimator(fullRobotModel, stateEstimatorParameters, sensorProcessingConfiguration);
 DiagnosticControllerToolbox diagnosticControllerToolbox = new DiagnosticControllerToolbox(fullRobotModel, sensorOutputMap, diagnosticParameters, walkingControllerParameters, yoTime, dt,
    sensorProcessingConfiguration, simulationRegistry);
 automatedDiagnosticAnalysisController = new AutomatedDiagnosticAnalysisController(diagnosticControllerToolbox, gainStream, setpointStream,
    simulationRegistry);
 automatedDiagnosticAnalysisController.setRobotIsAlive(startWithRobotAlive);
 automatedDiagnosticConfiguration = new AutomatedDiagnosticConfiguration(diagnosticControllerToolbox, automatedDiagnosticAnalysisController);
 outputWriter = new DRCSimulationOutputWriter(simulatedRobot);
 outputWriter.setFullRobotModel(fullRobotModel, null);
 int simulationTicksPerControlTick = (int) (robotModel.getEstimatorDT() / robotModel.getSimulateDT());
 simulatedRobot.setController(this, simulationTicksPerControlTick);
 return automatedDiagnosticConfiguration;
}

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

public AutomatedDiagnosticConfiguration createDiagnosticController(boolean startWithRobotAlive)
{
 simulatedRobot = robotModel.createHumanoidFloatingRootJointRobot(false);
 DiagnosticLoggerConfiguration.setupLogging(simulatedRobot.getYoTime(), getClass(), robotModel.getSimpleRobotName());
 if (robotInitialSetup == null)
   robotInitialSetup = robotModel.getDefaultRobotInitialSetup(0.0, 0.0);
 robotInitialSetup.initializeRobot(simulatedRobot, robotModel.getJointMap());
 FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel();
 WalkingControllerParameters walkingControllerParameters = robotModel.getWalkingControllerParameters();
 YoDouble yoTime = simulatedRobot.getYoTime();
 double dt = robotModel.getEstimatorDT();
 StateEstimatorParameters stateEstimatorParameters = robotModel.getStateEstimatorParameters();
 if (diagnosticParameters == null)
   diagnosticParameters = new DiagnosticParameters(DiagnosticEnvironment.RUNTIME_CONTROLLER, false);
 JointDesiredOutputList lowLevelOutput = new JointDesiredOutputList(fullRobotModel.getOneDoFJoints());
 DiagnosticSensorProcessingConfiguration sensorProcessingConfiguration = new DiagnosticSensorProcessingConfiguration(diagnosticParameters, stateEstimatorParameters, lowLevelOutput);
 SensorOutputMapReadOnly sensorOutputMap = createStateEstimator(fullRobotModel, stateEstimatorParameters, sensorProcessingConfiguration);
 DiagnosticControllerToolbox diagnosticControllerToolbox = new DiagnosticControllerToolbox(fullRobotModel, lowLevelOutput, sensorOutputMap, diagnosticParameters, walkingControllerParameters, yoTime, dt,
    sensorProcessingConfiguration, simulationRegistry);
 automatedDiagnosticAnalysisController = new AutomatedDiagnosticAnalysisController(diagnosticControllerToolbox, gainStream, setpointStream,
    simulationRegistry);
 automatedDiagnosticAnalysisController.setRobotIsAlive(startWithRobotAlive);
 automatedDiagnosticConfiguration = new AutomatedDiagnosticConfiguration(diagnosticControllerToolbox, automatedDiagnosticAnalysisController);
 lowLevelOutputWriter = new SimulatedLowLevelOutputWriter(simulatedRobot, false);
 lowLevelOutputWriter.setJointDesiredOutputList(lowLevelOutput);
 int simulationTicksPerControlTick = (int) (robotModel.getEstimatorDT() / robotModel.getSimulateDT());
 simulatedRobot.setController(this, simulationTicksPerControlTick);
 return automatedDiagnosticConfiguration;
}

this.estimatorDTInNS = Conversions.secondsToNanoseconds(robotModel.getEstimatorDT());