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