- Add the Codota plugin to your IDE and get smart completions
private void myMethod () {StringBuilder s =
new StringBuilder()
- Smart code suggestions by Codota
}
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); } }