if (!jointsToIgnore.contains(legJoint.getName())) OneDoFJointCheckUpDiagnosticTask checkUp = new OneDoFJointCheckUpDiagnosticTask(legJoint, toolbox); if (enableLogging) checkUp.setupForLogging(); checkUps.add(checkUp); if (!jointsToIgnore.contains(armJoint.getName())) OneDoFJointCheckUpDiagnosticTask checkUp = new OneDoFJointCheckUpDiagnosticTask(armJoint, toolbox); if (enableLogging) checkUp.setupForLogging(); checkUps.add(checkUp); if (!jointsToIgnore.contains(spineJoint.getName())) OneDoFJointCheckUpDiagnosticTask checkUp = new OneDoFJointCheckUpDiagnosticTask(spineJoint, toolbox); if (enableLogging) checkUp.setupForLogging(); diagnosticController.submitDiagnostic(checkUp);
@Override public boolean isDone() { if (validityChecker.sensorsCannotBeTrusted()) { disableEstimators(); if (logger != null) logger.severe(joint.getName() + " Joint sensors can't be trusted, skipping to the next diagnostic task."); return true; } if (dataReporter != null && !dataReporter.isDoneExportingData()) return false; return getTimeInCurrentTask() >= checkUpDuration.getDoubleValue() + 2.0 * rampDuration.getDoubleValue(); }
@Override public void doAction() if (getTimeInCurrentTask() < rampDuration.getDoubleValue()) ramp.set(getTimeInCurrentTask() / rampDuration.getDoubleValue()); else if (getTimeInCurrentTask() > checkUpDuration.getDoubleValue() + rampDuration.getDoubleValue()) reportCheckUpResults(); disableEstimators(); disableEstimators = false; ramp.set(1.0 - (getTimeInCurrentTask() - checkUpDuration.getDoubleValue() - rampDuration.getDoubleValue()) / rampDuration.getDoubleValue()); enableEstimators(); enableEstimators = false; double positionOffset = ramp.getDoubleValue() * functionGenerator.getValue(getTimeInCurrentTask()); double velocityOffset = ramp.getDoubleValue() * functionGenerator.getValueDot();
@Override public void doAction() if (getTimeInCurrentTask() < rampDuration.getDoubleValue()) ramp.set(getTimeInCurrentTask() / rampDuration.getDoubleValue()); else if (getTimeInCurrentTask() > checkUpDuration.getDoubleValue() + rampDuration.getDoubleValue()) reportCheckUpResults(); disableEstimators(); disableEstimators = false; ramp.set(1.0 - (getTimeInCurrentTask() - checkUpDuration.getDoubleValue() - rampDuration.getDoubleValue()) / rampDuration.getDoubleValue()); enableEstimators(); enableEstimators = false; double positionOffset = ramp.getDoubleValue() * functionGenerator.getValue(getTimeInCurrentTask()); double velocityOffset = ramp.getDoubleValue() * functionGenerator.getValueDot();
if (!jointsToIgnore.contains(legJoint.getName())) OneDoFJointCheckUpDiagnosticTask checkUp = new OneDoFJointCheckUpDiagnosticTask(legJoint, toolbox); if (enableLogging) checkUp.setupForLogging(); checkUps.add(checkUp); if (!jointsToIgnore.contains(armJoint.getName())) OneDoFJointCheckUpDiagnosticTask checkUp = new OneDoFJointCheckUpDiagnosticTask(armJoint, toolbox); if (enableLogging) checkUp.setupForLogging(); checkUps.add(checkUp); if (!jointsToIgnore.contains(spineJoint.getName())) OneDoFJointCheckUpDiagnosticTask checkUp = new OneDoFJointCheckUpDiagnosticTask(spineJoint, toolbox); if (enableLogging) checkUp.setupForLogging(); diagnosticController.submitDiagnostic(checkUp);
@Override public boolean isDone() { if (validityChecker.sensorsCannotBeTrusted()) { disableEstimators(); if (logger != null) logger.severe(joint.getName() + " Joint sensors can't be trusted, skipping to the next diagnostic task."); return true; } if (dataReporter != null && !dataReporter.isDoneExportingData()) return false; return getTimeInCurrentTask() >= checkUpDuration.getDoubleValue() + 2.0 * rampDuration.getDoubleValue(); }
if (!jointsToIgnore.contains(legJoint.getName())) OneDoFJointCheckUpDiagnosticTask checkUp = new OneDoFJointCheckUpDiagnosticTask(legJoint, toolbox); if (enableLogging) checkUp.setupForLogging(); checkUps.add(checkUp); if (!jointsToIgnore.contains(armJoint.getName())) OneDoFJointCheckUpDiagnosticTask checkUp = new OneDoFJointCheckUpDiagnosticTask(armJoint, toolbox); if (enableLogging) checkUp.setupForLogging(); checkUps.add(checkUp); if (!jointsToIgnore.contains(spineJoint.getName())) OneDoFJointCheckUpDiagnosticTask checkUp = new OneDoFJointCheckUpDiagnosticTask(spineJoint, toolbox); if (enableLogging) checkUp.setupForLogging(); diagnosticController.submitDiagnostic(checkUp);