@Override public void doTransitionOutOfAction() { if (logger != null) logger.info("Done with check up for the IMU: " + imuDefinition.getName()); for (Direction direction : Direction.values) ramps.get(direction).set(0.0); }
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 GazeboSensorReader(StateEstimatorSensorDefinitions stateEstimatorSensorDefinitions, DRCRobotSensorInformation sensorInformation, StateEstimatorParameters stateEstimatorParameters, RawJointSensorDataHolderMap rawJointSensorDataHolderMap, YoVariableRegistry parentRegistry) { this.sensorProcessing = new SensorProcessing(stateEstimatorSensorDefinitions, stateEstimatorParameters, registry); this.jointList = new ArrayList<>(stateEstimatorSensorDefinitions.getJointSensorDefinitions()); Collections.sort(jointList, new Comparator<OneDoFJoint>() { @Override public int compare(OneDoFJoint o1, OneDoFJoint o2) { return o1.getName().compareTo(o2.getName()); } });; this.imu = stateEstimatorSensorDefinitions.getIMUSensorDefinitions().get(0); imuToParentLinkRotationOffset = new Quat4d(); imu.getIMUFrame().getTransformToDesiredFrame(imu.getRigidBody().getBodyFixedFrame()).getRotation(imuToParentLinkRotationOffset); jointDataLength = jointList.size() * 8 * 2; imuDataLength = 10 * 8; forceSensorDefinitions = stateEstimatorSensorDefinitions.getForceSensorDefinitions(); forceSensorDataLength = forceSensorDefinitions.size() * 6 * 8; data = ByteBuffer.allocate(16 + jointDataLength + imuDataLength + forceSensorDataLength); data.order(ByteOrder.nativeOrder()); parentRegistry.addChild(registry); }
String imuName = imuDefinition.getName(); allIMUSensorNames.add(imuName); ReferenceFrame sensorFrame = imuDefinition.getIMUFrame();
ReferenceFrame imuFrame = imuDefinitions[sensorNumber].getIMUFrame(); rosImuPublisher.publish(timeStamp, imuPacket, imuFrame.getName());
private LinkedHashMap<IMUMount, IMUDefinition> generateIMUDefinitions(ArrayList<IMUMount> imuMounts) { LinkedHashMap<IMUMount, IMUDefinition> imuDefinitions = new LinkedHashMap<IMUMount, IMUDefinition>(); for (IMUMount imuMount : imuMounts) { RigidBodyBasics rigidBody = scsToInverseDynamicsJointMap.getRigidBody(imuMount.getParentJoint()); RigidBodyTransform transformFromMountToJoint = new RigidBodyTransform(); imuMount.getTransformFromMountToJoint(transformFromMountToJoint); IMUDefinition imuDefinition = new IMUDefinition(imuMount.getName(), rigidBody, transformFromMountToJoint); imuDefinitions.put(imuMount, imuDefinition); } return imuDefinitions; }
public static ReferenceFrame createMeasurementFrame(String sensorName, String frameName, IMUDefinition imuDefinition, RigidBody measurementBody) { RigidBodyTransform transformFromIMUToJoint = new RigidBodyTransform(); imuDefinition.getTransformFromIMUToJoint(transformFromIMUToJoint); ReferenceFrame perfectFrameAfterJoint = measurementBody.getParentJoint().getFrameAfterJoint(); if (transformFromIMUToJoint.epsilonEquals(new RigidBodyTransform(), 1e-10)) { return perfectFrameAfterJoint; } ReferenceFrame perfectMeasurementFrame = ReferenceFrame.constructBodyFrameWithUnchangingTransformToParent(sensorName + frameName, perfectFrameAfterJoint, transformFromIMUToJoint); return perfectMeasurementFrame; } }
ReferenceFrame imuFrame = imuDefinitions[sensorNumber].getIMUFrame(); rosImuPublisher.publish(timeStamp, imuPacket, imuFrame.getName());
private LinkedHashMap<IMUMount, IMUDefinition> generateIMUDefinitions(ArrayList<IMUMount> imuMounts) { LinkedHashMap<IMUMount, IMUDefinition> imuDefinitions = new LinkedHashMap<IMUMount, IMUDefinition>(); for (IMUMount imuMount : imuMounts) { RigidBody rigidBody = scsToInverseDynamicsJointMap.getRigidBody(imuMount.getParentJoint()); RigidBodyTransform transformFromMountToJoint = new RigidBodyTransform(); imuMount.getTransformFromMountToJoint(transformFromMountToJoint); IMUDefinition imuDefinition = new IMUDefinition(imuMount.getName(), rigidBody, transformFromMountToJoint); imuDefinitions.put(imuMount, imuDefinition); } return imuDefinitions; }
@Override public void doTransitionOutOfAction() { if (logger != null) logger.info("Done with check up for the IMU: " + imuDefinition.getName()); for (Axis axis : Axis.values) ramps.get(axis).set(0.0); }
public ArrayList<OrientationSensorConfiguration> createOrientationSensorConfigurations(Map<IMUDefinition, ControlFlowOutputPort<Matrix3d>> orientationSensors) { ArrayList<OrientationSensorConfiguration> orientationSensorConfigurations = new ArrayList<OrientationSensorConfiguration>(); Set<IMUDefinition> imuDefinitions = orientationSensors.keySet(); for (IMUDefinition estimatedIMUDefinition : imuDefinitions) { String sensorName = estimatedIMUDefinition.getName() + "Orientation"; double orientationMeasurementStandardDeviation = sensorNoiseParameters.getOrientationMeasurementStandardDeviation(); DenseMatrix64F orientationNoiseCovariance = createDiagonalCovarianceMatrix(orientationMeasurementStandardDeviation, 3); RigidBody estimatedMeasurementBody = estimatedIMUDefinition.getRigidBody(); ReferenceFrame estimatedMeasurementFrame = createMeasurementFrame(sensorName, "EstimatedMeasurementFrame", estimatedIMUDefinition, estimatedMeasurementBody); ControlFlowOutputPort<Matrix3d> outputPort = orientationSensors.get(estimatedIMUDefinition); OrientationSensorConfiguration orientationSensorConfiguration = new OrientationSensorConfiguration(outputPort, sensorName, estimatedMeasurementFrame, orientationNoiseCovariance); orientationSensorConfigurations.add(orientationSensorConfiguration); } return orientationSensorConfigurations; }
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)); } }
ReferenceFrame imuFrame = imuDefinitions[sensorNumber].getIMUFrame(); rosImuPublisher.publish(timeStamp, imuPacket, imuFrame.getName());
IMUDefinition imuDefinition = new IMUDefinition(imuSensor.getName(), joint.getSuccessor(), imuSensor.getTransformToJoint()); imuDefinitions.add(imuDefinition);
@Override public void doTransitionIntoAction() { if (logger != null) logger.info("Starting check up for the IMU: " + imuDefinition.getName()); for (Direction direction : Direction.values) ramps.get(direction).set(0.0); }
public ArrayList<LinearAccelerationSensorConfiguration> createLinearAccelerationSensorConfigurations(Map<IMUDefinition, ControlFlowOutputPort<Vector3d>> linearAccelerationSensors) { ArrayList<LinearAccelerationSensorConfiguration> linearAccelerationSensorConfigurations = new ArrayList<LinearAccelerationSensorConfiguration>(); Set<IMUDefinition> imuDefinitions = linearAccelerationSensors.keySet(); for (IMUDefinition estimatedIMUDefinition : imuDefinitions) { String sensorName = estimatedIMUDefinition.getName() + "LinearAcceleration"; double linearAccelerationMeasurementStandardDeviation = sensorNoiseParameters.getLinearAccelerationMeasurementStandardDeviation(); DenseMatrix64F linearAccelerationNoiseCovariance = createDiagonalCovarianceMatrix(linearAccelerationMeasurementStandardDeviation, 3); double linearAccelerationBiasProcessNoiseStandardDeviation = sensorNoiseParameters.getLinearAccelerationBiasProcessNoiseStandardDeviation(); DenseMatrix64F linearAccelerationBiasProcessNoiseCovariance = createDiagonalCovarianceMatrix(linearAccelerationBiasProcessNoiseStandardDeviation, 3); RigidBody estimatedMeasurementBody = estimatedIMUDefinition.getRigidBody(); ReferenceFrame estimatedMeasurementFrame = createMeasurementFrame(sensorName, "EstimatedMeasurementFrame", estimatedIMUDefinition, estimatedMeasurementBody); ControlFlowOutputPort<Vector3d> outputPort = linearAccelerationSensors.get(estimatedIMUDefinition); LinearAccelerationSensorConfiguration linearAccelerationSensorConfiguration = new LinearAccelerationSensorConfiguration(outputPort, sensorName, estimatedMeasurementBody, estimatedMeasurementFrame, gravitationalAcceleration, linearAccelerationNoiseCovariance, linearAccelerationBiasProcessNoiseCovariance); linearAccelerationSensorConfigurations.add(linearAccelerationSensorConfiguration); } return linearAccelerationSensorConfigurations; }
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)); } }
@Override public void doTransitionIntoAction() { if (logger != null) logger.info("Starting check up for the IMU: " + imuDefinition.getName()); for (Axis axis : Axis.values) ramps.get(axis).set(0.0); }
public ArrayList<AngularVelocitySensorConfiguration> createAngularVelocitySensorConfigurations(Map<IMUDefinition, ControlFlowOutputPort<Vector3d>> angularVelocitySensors) { ArrayList<AngularVelocitySensorConfiguration> angularVelocitySensorConfigurations = new ArrayList<AngularVelocitySensorConfiguration>(); Set<IMUDefinition> imuDefinitions = angularVelocitySensors.keySet(); for (IMUDefinition estimatedIMUDefinition : imuDefinitions) { String sensorName = estimatedIMUDefinition.getName() + "AngularVelocity"; double angularVelocityMeasurementStandardDeviation = sensorNoiseParameters.getAngularVelocityMeasurementStandardDeviation(); DenseMatrix64F angularVelocityNoiseCovariance = createDiagonalCovarianceMatrix(angularVelocityMeasurementStandardDeviation, 3); double angularVelocityBiasProcessNoiseStandardDeviation = sensorNoiseParameters.getAngularVelocityBiasProcessNoiseStandardDeviation(); DenseMatrix64F angularVelocityBiasProcessNoiseCovariance = createDiagonalCovarianceMatrix(angularVelocityBiasProcessNoiseStandardDeviation, 3); RigidBody estimatedMeasurementBody = estimatedIMUDefinition.getRigidBody(); ReferenceFrame estimatedMeasurementFrame = createMeasurementFrame(sensorName, "EstimatedMeasurementFrame", estimatedIMUDefinition, estimatedMeasurementBody); ControlFlowOutputPort<Vector3d> outputPort = angularVelocitySensors.get(estimatedIMUDefinition); AngularVelocitySensorConfiguration angularVelocitySensorConfiguration = new AngularVelocitySensorConfiguration(outputPort, sensorName, estimatedMeasurementBody, estimatedMeasurementFrame, angularVelocityNoiseCovariance, angularVelocityBiasProcessNoiseCovariance); angularVelocitySensorConfigurations.add(angularVelocitySensorConfiguration); } return angularVelocitySensorConfigurations; }