public Map<String, OrientationAngularVelocityConsistencyChecker> addIMUOrientationAngularVelocityConsistencyCheckers() { LinkedHashMap<String, OrientationAngularVelocityConsistencyChecker> consistencyCheckerMap = new LinkedHashMap<>(); for (int i = 0; i < imuSensorDefinitions.size(); i++) { IMUDefinition imuToCheck = imuSensorDefinitions.get(i); ReferenceFrame referenceFrame = imuToCheck.getRigidBody().getBodyFixedFrame(); YoFrameQuaternion orientation = intermediateOrientations.get(imuToCheck); YoFrameVector angularVelocity = intermediateAngularVelocities.get(imuToCheck); OrientationAngularVelocityConsistencyChecker consistencyChecker = new OrientationAngularVelocityConsistencyChecker(imuToCheck.getName(), orientation, angularVelocity, referenceFrame, updateDT, registry); consistencyCheckerMap.put(imuToCheck.getName(), consistencyChecker); diagnosticModules.add(consistencyChecker); } return consistencyCheckerMap; }
private void enableEstimators(Axis currentAxis) { if (currentAxis != null) { List<OneDoFJointSensorValidityChecker> jointValidityCheckerList = jointValidityCheckers.get(currentAxis); for (int i = 0; i < jointValidityCheckerList.size(); i++) jointValidityCheckerList.get(i).enable(); } else { for (Axis axis : Axis.values) { List<OneDoFJointSensorValidityChecker> jointValidityCheckerList = jointValidityCheckers.get(axis); for (int i = 0; i < jointValidityCheckerList.size(); i++) jointValidityCheckerList.get(i).enable(); } } validityChecker.enable(); orientationVelocityConsistency.enable(); delayEstimator.enable(); }
return; if (orientationVelocityConsistency.isEstimatingDelay(currentDirection)) velocityToOrientationQualityMeanCalculator.increment(orientationVelocityConsistency.getCorrelation(currentDirection)); velocityToOrientationQualityMean.set(velocityToOrientationQualityMeanCalculator.getResult()); velocityToOrientationQualityStandardDeviationCalculator.increment(orientationVelocityConsistency.getCorrelation(currentDirection)); velocityToOrientationQualityStandardDeviation.set(velocityToOrientationQualityStandardDeviationCalculator.getResult()); velocityToOrientationDelayMeanCalculator.increment(orientationVelocityConsistency.getEstimatedDelay(currentDirection)); velocityToOrientationDelayMean.set(velocityToOrientationDelayMeanCalculator.getResult()); velocityToOrientationDelayStandardDeviationCalculator.increment(orientationVelocityConsistency.getEstimatedDelay(currentDirection)); velocityToOrientationDelayStandardDeviation.set(velocityToOrientationDelayStandardDeviationCalculator.getResult());
private void disableEstimators(Axis currentAxis) { if (currentAxis != null) { List<OneDoFJointSensorValidityChecker> jointValidityCheckerList = jointValidityCheckers.get(currentAxis); for (int i = 0; i < jointValidityCheckerList.size(); i++) jointValidityCheckerList.get(i).disable(); } else { for (Axis axis : Axis.values) { List<OneDoFJointSensorValidityChecker> jointValidityCheckerList = jointValidityCheckers.get(axis); for (int i = 0; i < jointValidityCheckerList.size(); i++) jointValidityCheckerList.get(i).disable(); } } validityChecker.disable(); orientationVelocityConsistency.disable(); delayEstimator.disable(); velocityToOrientationQualityMeanCalculator.clear(); velocityToOrientationQualityStandardDeviationCalculator.clear(); velocityToOrientationDelayMeanCalculator.clear(); velocityToOrientationDelayStandardDeviationCalculator.clear(); velocityIMUVsJointQualityMeanCalculator.clear(); velocityIMUVsJointQualityStandardDeviationCalculator.clear(); velocityIMUVsJointDelayMeanCalculator.clear(); velocityIMUVsJointDelayStandardDeviationCalculator.clear(); }
return; if (orientationVelocityConsistency.isEstimatingDelay(currentAxis)) velocityToOrientationQualityMeanCalculator.increment(orientationVelocityConsistency.getCorrelation(currentAxis)); velocityToOrientationQualityMean.set(velocityToOrientationQualityMeanCalculator.getResult()); velocityToOrientationQualityStandardDeviationCalculator.increment(orientationVelocityConsistency.getCorrelation(currentAxis)); velocityToOrientationQualityStandardDeviation.set(velocityToOrientationQualityStandardDeviationCalculator.getResult()); velocityToOrientationDelayMeanCalculator.increment(orientationVelocityConsistency.getEstimatedDelay(currentAxis)); velocityToOrientationDelayMean.set(velocityToOrientationDelayMeanCalculator.getResult()); velocityToOrientationDelayStandardDeviationCalculator.increment(orientationVelocityConsistency.getEstimatedDelay(currentAxis)); velocityToOrientationDelayStandardDeviation.set(velocityToOrientationDelayStandardDeviationCalculator.getResult());
private void disableEstimators(Direction currentDirection) { if (currentDirection != null) { List<OneDoFJointSensorValidityChecker> jointValidityCheckerList = jointValidityCheckers.get(currentDirection); for (int i = 0; i < jointValidityCheckerList.size(); i++) jointValidityCheckerList.get(i).disable(); } else { for (Direction direction : Direction.values) { List<OneDoFJointSensorValidityChecker> jointValidityCheckerList = jointValidityCheckers.get(direction); for (int i = 0; i < jointValidityCheckerList.size(); i++) jointValidityCheckerList.get(i).disable(); } } validityChecker.disable(); orientationVelocityConsistency.disable(); delayEstimator.disable(); velocityToOrientationQualityMeanCalculator.clear(); velocityToOrientationQualityStandardDeviationCalculator.clear(); velocityToOrientationDelayMeanCalculator.clear(); velocityToOrientationDelayStandardDeviationCalculator.clear(); velocityIMUVsJointQualityMeanCalculator.clear(); velocityIMUVsJointQualityStandardDeviationCalculator.clear(); velocityIMUVsJointDelayMeanCalculator.clear(); velocityIMUVsJointDelayStandardDeviationCalculator.clear(); }
public void addIMUConsistencyCheckers(String q_prefix, String q_suffix, String qd_prefix, String qd_suffix, boolean performAnalysisInBodyFrame) { IMUDefinition[] imuDefinitions = fullRobotModel.getIMUDefinitions(); for (IMUDefinition imuDefinition : imuDefinitions) { String imuName = imuDefinition.getName(); ReferenceFrame bodyFrame = imuDefinition.getRigidBody().getBodyFixedFrame(); ReferenceFrame imuFrame = imuDefinition.getIMUFrame(); YoFrameQuaternion q = logDataProcessorHelper.findYoFrameQuaternion(q_prefix, imuName + q_suffix, ReferenceFrame.getWorldFrame()); YoFrameVector qd = logDataProcessorHelper.findYoFrameVector(qd_prefix, imuName + qd_suffix, imuFrame); if (q == null) { System.err.println("Could not find the find the position variable for the IMU: " + imuName); continue; } if (qd == null) { System.err.println("Could not find the find the velocity variable for the IMU: " + imuName); continue; } ReferenceFrame referenceFrameUsedForComparison = performAnalysisInBodyFrame ? bodyFrame : imuFrame; diagnosticUpdatables.add(new OrientationAngularVelocityConsistencyChecker(imuName, q, qd, referenceFrameUsedForComparison, dt, registry)); } }
private void enableEstimators(Direction currentDirection) { if (currentDirection != null) { List<OneDoFJointSensorValidityChecker> jointValidityCheckerList = jointValidityCheckers.get(currentDirection); for (int i = 0; i < jointValidityCheckerList.size(); i++) jointValidityCheckerList.get(i).enable(); } else { for (Direction direction : Direction.values) { List<OneDoFJointSensorValidityChecker> jointValidityCheckerList = jointValidityCheckers.get(direction); for (int i = 0; i < jointValidityCheckerList.size(); i++) jointValidityCheckerList.get(i).enable(); } } validityChecker.enable(); orientationVelocityConsistency.enable(); delayEstimator.enable(); }
public void addIMUConsistencyCheckers(String q_prefix, String q_suffix, String qd_prefix, String qd_suffix, boolean performAnalysisInBodyFrame) { IMUDefinition[] imuDefinitions = fullRobotModel.getIMUDefinitions(); for (IMUDefinition imuDefinition : imuDefinitions) { String imuName = imuDefinition.getName(); ReferenceFrame bodyFrame = imuDefinition.getRigidBody().getBodyFixedFrame(); ReferenceFrame imuFrame = imuDefinition.getIMUFrame(); YoFrameQuaternion q = logDataProcessorHelper.findYoFrameQuaternion(q_prefix, imuName + q_suffix, ReferenceFrame.getWorldFrame()); YoFrameVector3D qd = logDataProcessorHelper.findYoFrameVector(qd_prefix, imuName + qd_suffix, imuFrame); if (q == null) { System.err.println("Could not find the find the position variable for the IMU: " + imuName); continue; } if (qd == null) { System.err.println("Could not find the find the velocity variable for the IMU: " + imuName); continue; } ReferenceFrame referenceFrameUsedForComparison = performAnalysisInBodyFrame ? bodyFrame : imuFrame; diagnosticUpdatables.add(new OrientationAngularVelocityConsistencyChecker(imuName, q, qd, referenceFrameUsedForComparison, dt, registry)); } }
public void addIMUConsistencyCheckers(String q_prefix, String q_suffix, String qd_prefix, String qd_suffix, boolean performAnalysisInBodyFrame) { IMUDefinition[] imuDefinitions = fullRobotModel.getIMUDefinitions(); for (IMUDefinition imuDefinition : imuDefinitions) { String imuName = imuDefinition.getName(); ReferenceFrame bodyFrame = imuDefinition.getRigidBody().getBodyFixedFrame(); ReferenceFrame imuFrame = imuDefinition.getIMUFrame(); YoFrameQuaternion q = logDataProcessorHelper.findYoFrameQuaternion(q_prefix, imuName + q_suffix, ReferenceFrame.getWorldFrame()); YoFrameVector qd = logDataProcessorHelper.findYoFrameVector(qd_prefix, imuName + qd_suffix, imuFrame); if (q == null) { System.err.println("Could not find the find the position variable for the IMU: " + imuName); continue; } if (qd == null) { System.err.println("Could not find the find the velocity variable for the IMU: " + imuName); continue; } ReferenceFrame referenceFrameUsedForComparison = performAnalysisInBodyFrame ? bodyFrame : imuFrame; diagnosticUpdatables.add(new OrientationAngularVelocityConsistencyChecker(imuName, q, qd, referenceFrameUsedForComparison, dt, registry)); } }