How to use

AutomatedDiagnosticAnalysisController

in

us.ihmc.wholeBodyController.diagnostics

public void addWait(double timeToWait)
{
 diagnosticController.submitDiagnostic(new WaitDiagnosticTask(timeToWait));
}

  @Override
  public String getDescription()
  {
   return getName();
  }
}

  diagnosticController.setRobotIsAlive(false);
  diagnosticController.setRobotIsAlive(true);
  diagnosticController.initialize();
  firstDiagnosticControlTick = false;
diagnosticController.doControl();
sensorReader.writeCommandsToRobot();

@Override
public void doControl()
{
 long startTime = System.nanoTime();
 sensorReader.read();
 humanoidReferenceFrames.updateFrames();
 if (firstControlTick)
 {
   stateEstimator.initialize();
   automatedDiagnosticAnalysisController.initialize();
   firstControlTick = false;
 }
 else
 {
   stateEstimator.doControl();
   automatedDiagnosticAnalysisController.doControl();
 }
 outputWriter.writeAfterController(0);
 long endTime = System.nanoTime();
 controllerTime.set(TimeTools.nanoSecondstoSeconds(endTime - startTime));
 averageControllerTime.update();
}

  initialize();
  return;
  initialize();
  if (doIdleControl.getBooleanValue())
   doIdleControl();
  return;
  currentTask.getDataReporterToRun(dataReportersToExecute);
handleDataReporters();

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

submitDiagnostic(new WaitDiagnosticTask(trajectoryTimeProvider.getValue()));
setupJointControllers(gainStream, setpointStream);
  setupForLogging();

diagnosticController = new AutomatedDiagnosticAnalysisController(toolbox, gainStream, setpointStream, registry);
AutomatedDiagnosticConfiguration automatedDiagnosticConfiguration = new AutomatedDiagnosticConfiguration(toolbox, diagnosticController);
automatedDiagnosticConfiguration.addWait(1.0);

@Override
public void doControl()
{
 long startTime = System.nanoTime();
 sensorReader.read();
 humanoidReferenceFrames.updateFrames();
 if (firstControlTick)
 {
   stateEstimator.initialize();
   automatedDiagnosticAnalysisController.initialize();
   firstControlTick = false;
 }
 else
 {
   stateEstimator.doControl();
   automatedDiagnosticAnalysisController.doControl();
 }
 outputWriter.writeAfterController(0);
 long endTime = System.nanoTime();
 controllerTime.set(TimeTools.nanoSecondstoSeconds(endTime - startTime));
 averageControllerTime.update();
}

  initialize();
  return;
  initialize();
  if (doIdleControl.getBooleanValue())
   doIdleControl();
  return;
  currentTask.getDataReporterToRun(dataReportersToExecute);
handleDataReporters();

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

submitDiagnostic(new WaitDiagnosticTask(trajectoryTimeProvider.getValue()));
setupJointControllers(gainStream, setpointStream);
  setupForLogging();

@Override
public void doControl()
{
 long startTime = System.nanoTime();
 lowLevelOutputWriter.writeBefore(startTime);
 sensorReader.read();
 humanoidReferenceFrames.updateFrames();
 if (firstControlTick)
 {
   stateEstimator.initialize();
   forceSensorStateUpdater.initialize();
   automatedDiagnosticAnalysisController.initialize();
   firstControlTick = false;
 }
 else
 {
   stateEstimator.doControl();
   forceSensorStateUpdater.updateForceSensorState();
   automatedDiagnosticAnalysisController.doControl();
 }
 lowLevelOutputWriter.writeAfter();
 long endTime = System.nanoTime();
 controllerTime.set(Conversions.nanosecondsToSeconds(endTime - startTime));
 averageControllerTime.update();
}

public void addWait(double timeToWait)
{
 diagnosticController.submitDiagnostic(new WaitDiagnosticTask(timeToWait));
}

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

  @Override
  public String getDescription()
  {
   return getName();
  }
}

public void addWait(double timeToWait)
{
 diagnosticController.submitDiagnostic(new WaitDiagnosticTask(timeToWait));
}

public void setupForLogging()
{
 logger = Logger.getLogger(getName());
}

  public void addPelvisIMUCheckUpDiagnostic()
  {
   DiagnosticParameters diagnosticParameters = toolbox.getDiagnosticParameters();
   String pelvisIMUName = diagnosticParameters.getPelvisIMUName();
   if (pelvisIMUName == null || pelvisIMUName.isEmpty())
   {
     System.err.println(getClass().getSimpleName() + ": Cannot create the pelvis IMU check up diagnostic without a pelvisIMUName to look for.");
     return;
   }

   PelvisIMUCheckUpDiagnosticTask checkUp = new PelvisIMUCheckUpDiagnosticTask(pelvisIMUName, toolbox);
   if (enableLogging)
     checkUp.setupForLogging();
   diagnosticController.submitDiagnostic(checkUp);
  }
}

public void setupForLogging()
{
 logger = Logger.getLogger(getName());
}