How to use

DiagnosticControllerToolbox

in

us.ihmc.wholeBodyController.diagnostics

public AutomatedDiagnosticConfiguration(DiagnosticControllerToolbox toolbox, AutomatedDiagnosticAnalysisController diagnosticController)
{
 this.toolbox = toolbox;
 this.diagnosticController = diagnosticController;
 enableLogging = toolbox.getDiagnosticParameters().enableLogging();
}

private static IMUDefinition findIMUDefinition(String imuName, DiagnosticControllerToolbox toolbox)
{
 IMUDefinition imuDefinition = null;
 IMUDefinition[] imuDefinitions = toolbox.getFullRobotModel().getIMUDefinitions();
 for (int i = 0; i < imuDefinitions.length; i++)
 {
   if (imuDefinitions[i].getName().equals(imuName))
    imuDefinition = imuDefinitions[i];
 }
 if (imuDefinition == null)
   throw new RuntimeException("Could not find the IMUDefinition for the imu: " + imuName);
 return imuDefinition;
}

public PelvisIMUCheckUpDiagnosticTask(IMUDefinition imuToCheck, DiagnosticControllerToolbox toolbox)
 FullHumanoidRobotModel fullRobotModel = toolbox.getFullRobotModel();
 RigidBodyBasics pelvis = fullRobotModel.getPelvis();
 if (imuToCheck.getRigidBody() != pelvis)
 String nameSuffix = "CheckUp";
 imuSensor = toolbox.getIMUSensorReadOnly(imuName);
 diagnosticParameters = toolbox.getDiagnosticParameters();
 validityChecker = toolbox.getIMUSensorValidityChecker(imuName);
 orientationVelocityConsistency = toolbox.getIMUOrientationAngularVelocityConsistencyChecker(imuName);
 delayEstimator = new DelayEstimatorBetweenTwoSignals(imuName + nameSuffix + "DelayAgainstJointSensors", toolbox.getDT(), registry);
 delayEstimator.setAlphaFilterBreakFrequency(diagnosticParameters.getDelayEstimatorFilterBreakFrequency());
 double maxAbsoluteLead = diagnosticParameters.getDelayEstimatorMaximumLead();
 velocityIMUVsJointDelayStandardDeviation = new YoDouble(imuName + nameSuffix + "VelocityIMUVsJointDelayStandardDeviation", registry);
 YoDouble yoTime = toolbox.getYoTime();
 functionGenerator = new YoFunctionGenerator(imuName + nameSuffix, yoTime, registry);
 functionGenerator.setAmplitude(diagnosticParameters.getCheckUpOscillationPositionAmplitude());
   for (OneDoFJointBasics joint : jointsToWiggle.get(axis))
    jointValidityCheckerList.add(toolbox.getJointSensorValidityChecker(joint));

public AutomatedDiagnosticAnalysisController(DiagnosticControllerToolbox toolbox, InputStream gainStream, InputStream setpointStream,
   YoVariableRegistry parentRegistry)
 this.yoTime = toolbox.getYoTime();
 this.controlDT = toolbox.getDT();
 FullHumanoidRobotModel fullRobotModel = toolbox.getFullRobotModel();
 DiagnosticParameters diagnosticParameters = toolbox.getDiagnosticParameters();
 for (String jointToIgnore : toolbox.getWalkingControllerParameters().getJointsToIgnoreInController())

String nameSuffix = "CheckUp";
registry = new YoVariableRegistry(jointName + nameSuffix);
diagnosticParameters = toolbox.getDiagnosticParameters();
ramp = new YoDouble(jointName + nameSuffix + "SignalRamp", registry);
validityChecker = toolbox.getJointSensorValidityChecker(joint);
positionVelocityConsistency = toolbox.getJointPositionVelocityConsistencyChecker(joint);
forceTrackingDelay = toolbox.getJointForceTrackingDelayEstimator(joint);
fourierAnalysis = toolbox.getJointFourierAnalysis(joint);
forceTrackingDelayStandardDeviation = new YoDouble(jointName + nameSuffix + "ForceTrackingDelayStandardDeviation", registry);
YoDouble yoTime = toolbox.getYoTime();
functionGenerator = new YoFunctionGenerator(jointName + nameSuffix, yoTime, registry);
functionGenerator.setAmplitude(diagnosticParameters.getCheckUpOscillationPositionAmplitude());

public void addJointCheckUpDiagnostic()
 FullHumanoidRobotModel fullRobotModel = toolbox.getFullRobotModel();
 RobotSpecificJointNames robotSpecificJointNames = fullRobotModel.getRobotSpecificJointNames();
 List<String> jointsToIgnore = toolbox.getDiagnosticParameters().getJointsToIgnoreDuringDiagnostic();

DiagnosticControllerToolbox toolbox = new DiagnosticControllerToolbox(fullRobotModel, estimatorDesiredJointDataHolder, sensorOutputMap,
                                   diagnosticParameters, walkingControllerParameters, diagnosticControllerTime,
                                   diagnosticControllerDT, diagnosticSensorProcessingConfiguration, registry);

public PelvisIMUCheckUpDiagnosticTask(IMUDefinition imuToCheck, DiagnosticControllerToolbox toolbox)
 FullHumanoidRobotModel fullRobotModel = toolbox.getFullRobotModel();
 RigidBody pelvis = fullRobotModel.getPelvis();
 if (imuToCheck.getRigidBody() != pelvis)
 String nameSuffix = "CheckUp";
 imuSensor = toolbox.getIMUSensorReadOnly(imuName);
 diagnosticParameters = toolbox.getDiagnosticParameters();
 validityChecker = toolbox.getIMUSensorValidityChecker(imuName);
 orientationVelocityConsistency = toolbox.getIMUOrientationAngularVelocityConsistencyChecker(imuName);
 delayEstimator = new DelayEstimatorBetweenTwoSignals(imuName + nameSuffix + "DelayAgainstJointSensors", toolbox.getDT(), registry);
 delayEstimator.setAlphaFilterBreakFrequency(diagnosticParameters.getDelayEstimatorFilterBreakFrequency());
 double maxAbsoluteLead = diagnosticParameters.getDelayEstimatorMaximumLead();
 velocityIMUVsJointDelayStandardDeviation = new DoubleYoVariable(imuName + nameSuffix + "VelocityIMUVsJointDelayStandardDeviation", registry);
 DoubleYoVariable yoTime = toolbox.getYoTime();
 functionGenerator = new YoFunctionGenerator(imuName + nameSuffix, yoTime, registry);
 functionGenerator.setAmplitude(diagnosticParameters.getCheckUpOscillationPositionAmplitude());
   for (OneDoFJoint joint : jointsToWiggle.get(direction))
    jointValidityCheckerList.add(toolbox.getJointSensorValidityChecker(joint));

public AutomatedDiagnosticAnalysisController(DiagnosticControllerToolbox toolbox, InputStream gainStream, InputStream setpointStream,
   YoVariableRegistry parentRegistry)
 this.yoTime = toolbox.getYoTime();
 this.controlDT = toolbox.getDT();
 FullHumanoidRobotModel fullRobotModel = toolbox.getFullRobotModel();
 JointDesiredOutputList lowLevelOutput = toolbox.getLowLevelOutput();
 DiagnosticParameters diagnosticParameters = toolbox.getDiagnosticParameters();
 for (String jointToIgnore : toolbox.getWalkingControllerParameters().getJointsToIgnoreInController())

String nameSuffix = "CheckUp";
registry = new YoVariableRegistry(jointName + nameSuffix);
diagnosticParameters = toolbox.getDiagnosticParameters();
ramp = new DoubleYoVariable(jointName + nameSuffix + "SignalRamp", registry);
validityChecker = toolbox.getJointSensorValidityChecker(joint);
positionVelocityConsistency = toolbox.getJointPositionVelocityConsistencyChecker(joint);
forceTrackingDelay = toolbox.getJointForceTrackingDelayEstimator(joint);
fourierAnalysis = toolbox.getJointFourierAnalysis(joint);
forceTrackingDelayStandardDeviation = new DoubleYoVariable(jointName + nameSuffix + "ForceTrackingDelayStandardDeviation", registry);
DoubleYoVariable yoTime = toolbox.getYoTime();
functionGenerator = new YoFunctionGenerator(jointName + nameSuffix, yoTime, registry);
functionGenerator.setAmplitude(diagnosticParameters.getCheckUpOscillationPositionAmplitude());

public void addJointCheckUpDiagnostic()
 FullHumanoidRobotModel fullRobotModel = toolbox.getFullRobotModel();
 RobotSpecificJointNames robotSpecificJointNames = fullRobotModel.getRobotSpecificJointNames();
 List<String> jointsToIgnore = toolbox.getDiagnosticParameters().getJointsToIgnoreDuringDiagnostic();

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 void addJointCheckUpDiagnostic()
 FullHumanoidRobotModel fullRobotModel = toolbox.getFullRobotModel();
 RobotSpecificJointNames robotSpecificJointNames = fullRobotModel.getRobotSpecificJointNames();
 List<String> jointsToIgnore = toolbox.getDiagnosticParameters().getJointsToIgnoreDuringDiagnostic();

public AutomatedDiagnosticConfiguration(DiagnosticControllerToolbox toolbox, AutomatedDiagnosticAnalysisController diagnosticController)
{
 this.toolbox = toolbox;
 this.diagnosticController = diagnosticController;
 enableLogging = toolbox.getDiagnosticParameters().enableLogging();
}

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

private static IMUDefinition findIMUDefinition(String imuName, DiagnosticControllerToolbox toolbox)
{
 IMUDefinition imuDefinition = null;
 IMUDefinition[] imuDefinitions = toolbox.getFullRobotModel().getIMUDefinitions();
 for (int i = 0; i < imuDefinitions.length; i++)
 {
   if (imuDefinitions[i].getName().equals(imuName))
    imuDefinition = imuDefinitions[i];
 }
 if (imuDefinition == null)
   throw new RuntimeException("Could not find the IMUDefinition for the imu: " + imuName);
 return imuDefinition;
}

public AutomatedDiagnosticConfiguration(DiagnosticControllerToolbox toolbox, AutomatedDiagnosticAnalysisController diagnosticController)
{
 this.toolbox = toolbox;
 this.diagnosticController = diagnosticController;
 enableLogging = toolbox.getDiagnosticParameters().enableLogging();
}

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

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