How to use

getStateEstimatorParameters

method

in

us.ihmc.avatar.drcRobot.DRCRobotModel

public StepprSensorReaderFactory(DRCRobotModel robotModel)
{
 sensorInformation = robotModel.getSensorInformation();
 stateEstimatorParameters = robotModel.getStateEstimatorParameters();
}

public WandererSensorReaderFactory(DRCRobotModel robotModel, CostOfTransportCalculator costOfTransportCalculator)
{
 sensorInformation = robotModel.getSensorInformation();
 stateEstimatorParameters = robotModel.getStateEstimatorParameters();
 this.costOfTransportCalculator = costOfTransportCalculator;
}

private void setupSensorReaderFactory()
{
 if (scsInitialSetup.get().usePerfectSensors())
 {
   sensorReaderFactory = new DRCPerfectSensorReaderFactory(humanoidFloatingRootJointRobot, threadDataSynchronizer.getEstimatorForceSensorDataHolder(),
                               robotModel.get().getStateEstimatorParameters().getEstimatorDT());
 }
 else
 {
   sensorReaderFactory = new SimulatedSensorHolderAndReaderFromRobotFactory(humanoidFloatingRootJointRobot,
                                       robotModel.get().getStateEstimatorParameters());
 }
}

private void setupSensorReaderFactory()
{
 if (scsInitialSetup.get().usePerfectSensors())
 {
   sensorReaderFactory = new DRCPerfectSensorReaderFactory(humanoidFloatingRootJointRobot, threadDataSynchronizer.getEstimatorForceSensorDataHolder(),
                               robotModel.get().getStateEstimatorParameters().getEstimatorDT());
 }
 else
 {
   sensorReaderFactory = new SimulatedSensorHolderAndReaderFromRobotFactory(humanoidFloatingRootJointRobot,
                                       robotModel.get().getStateEstimatorParameters());
 }
}

private DRCKinematicsBasedStateEstimator createStateEstimator(FullInverseDynamicsStructure inverseDynamicsStructure, DRCRobotModel robotModel,
   final LogDataProcessorHelper logDataProcessorHelper, SensorOutputMapReadOnly postProcessedSensors)
 StateEstimatorParameters stateEstimatorParameters = robotModel.getStateEstimatorParameters();
 String[] imuSensorsToUseInStateEstimator = robotModel.getSensorInformation().getIMUSensorsToUseInStateEstimator();
 double gravitationalAcceleration = -9.80;

robotModel.getContactPointParameters(), robotModel, robotModel.getStateEstimatorParameters(),
sensorReaderFactory, threadDataSynchronizer, realtimeRos2Node, null, null, robotVisualizer,
gravity);

RobotContactPointParameters<RobotSide> contactPointParameters = robotModel.getContactPointParameters();
DRCRobotSensorInformation sensorInformation = robotModel.getSensorInformation();
StateEstimatorParameters stateEstimatorParameters = robotModel.getStateEstimatorParameters();

private void setupStateEstimationThread()
{
 stateEstimationThread = new DRCEstimatorThread(robotModel.get().getSensorInformation(), robotModel.get().getContactPointParameters(),
                         robotModel.get().getStateEstimatorParameters(), sensorReaderFactory, threadDataSynchronizer,
                         new PeriodicNonRealtimeThreadScheduler("DRCSimGazeboYoVariableServer"), humanoidGlobalDataProducer.get(),
                         yoVariableServer, gravity.get());
 if (humanoidGlobalDataProducer.get() != null)
 {
   PelvisPoseCorrectionCommunicatorInterface pelvisPoseCorrectionCommunicator = new PelvisPoseCorrectionCommunicator(humanoidGlobalDataProducer.get());
   humanoidGlobalDataProducer.get().attachListener(StampedPosePacket.class, pelvisPoseCorrectionCommunicator);
   stateEstimationThread.setExternalPelvisCorrectorSubscriber(pelvisPoseCorrectionCommunicator);
 }
 else
 {
   stateEstimationThread.setExternalPelvisCorrectorSubscriber(null);
 }
}

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

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

private void setupStateEstimationThread()
{
 String robotName = robotModel.get().getSimpleRobotName();
 MessageTopicNameGenerator publisherTopicNameGenerator = ControllerAPIDefinition.getPublisherTopicNameGenerator(robotName);
 MessageTopicNameGenerator subscriberTopicNameGenerator = ControllerAPIDefinition.getSubscriberTopicNameGenerator(robotName);
 PelvisPoseCorrectionCommunicatorInterface pelvisPoseCorrectionCommunicator;
 if (externalPelvisCorrectorSubscriber.hasValue())
 {
   pelvisPoseCorrectionCommunicator = externalPelvisCorrectorSubscriber.get();
 }
 else
 {
   pelvisPoseCorrectionCommunicator = new PelvisPoseCorrectionCommunicator(realtimeRos2Node.get(), publisherTopicNameGenerator);
   ROS2Tools.createCallbackSubscription(realtimeRos2Node.get(), StampedPosePacket.class, subscriberTopicNameGenerator,
                     s -> pelvisPoseCorrectionCommunicator.receivedPacket(s.takeNextData()));
 }
 stateEstimationThread = new DRCEstimatorThread(robotName, robotModel.get().getSensorInformation(), robotModel.get().getContactPointParameters(),
                         robotModel.get(), robotModel.get().getStateEstimatorParameters(), sensorReaderFactory,
                         threadDataSynchronizer, realtimeRos2Node.get(), pelvisPoseCorrectionCommunicator, simulationOutputWriter,
                         yoVariableServer, gravity.get());
}