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