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); }
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)); } }
String imuName = imuDefinition.getName(); allIMUSensorNames.add(imuName); ReferenceFrame sensorFrame = imuDefinition.getIMUFrame();
public IMUSensor(IMUDefinition imuDefinition, SensorNoiseParameters sensorNoiseParameters) { sensorName = imuDefinition.getName(); measurementFrame = imuDefinition.getIMUFrame(); measurementLink = imuDefinition.getRigidBody(); if (sensorNoiseParameters != null) { orientationNoiseCovariance = createDiagonalCovarianceMatrix(sensorNoiseParameters.getOrientationMeasurementStandardDeviation()); angularVelocityNoiseCovariance = createDiagonalCovarianceMatrix(sensorNoiseParameters.getAngularVelocityMeasurementStandardDeviation()); angularVelocityBiasProcessNoiseCovariance = createDiagonalCovarianceMatrix(sensorNoiseParameters.getAngularVelocityBiasProcessNoiseStandardDeviation()); linearAccelerationNoiseCovariance = createDiagonalCovarianceMatrix(sensorNoiseParameters.getLinearAccelerationMeasurementStandardDeviation()); linearAccelerationBiasProcessNoiseCovariance = createDiagonalCovarianceMatrix(sensorNoiseParameters.getLinearAccelerationBiasProcessNoiseStandardDeviation()); } else { orientationNoiseCovariance = createDiagonalCovarianceMatrix(0.0); angularVelocityNoiseCovariance = createDiagonalCovarianceMatrix(0.0); angularVelocityBiasProcessNoiseCovariance = createDiagonalCovarianceMatrix(0.0); linearAccelerationNoiseCovariance = createDiagonalCovarianceMatrix(0.0); linearAccelerationBiasProcessNoiseCovariance = createDiagonalCovarianceMatrix(0.0); } }
ReferenceFrame imuFrame = imuDefinitions[sensorNumber].getIMUFrame(); rosImuPublisher.publish(timeStamp, imuPacket, imuFrame.getName());
ReferenceFrame imuFrame = imuDefinitions[sensorNumber].getIMUFrame(); rosImuPublisher.publish(timeStamp, imuPacket, imuFrame.getName());
ReferenceFrame imuFrame = imuDefinitions[sensorNumber].getIMUFrame(); rosImuPublisher.publish(timeStamp, imuPacket, imuFrame.getName());