private void processAngularVelocity() { // use orientationOffset^T instead of rotationMatrix, because the angular velocity is measured in IMU body! // omega^B = R^B_IMUBody * omega^IMUBody rawIMUSensors.getAngularVelocity(angularVelocity, imuIndex); orientationOffsetTranspose.transform(angularVelocity); processedSensors.setAngularVelocityInBody(angularVelocity, imuIndex); }
private void processOrientation() { rawIMUSensors.getOrientation(rotationMatrixBeforeOffset, imuIndex); rotationMatrix.mul(rotationMatrixBeforeOffset, orientationOffset); processedSensors.setRotation(rotationMatrix, imuIndex); }
@Override public void read() { twistCalculator.compute(); spatialAccelerationCalculator.reset(); twistCalculator.getTwistOfBody(rigidBody, twist); // Twist of bodyCoM and not IMU! spatialAcceleration.setIncludingFrame(spatialAccelerationCalculator.getAccelerationOfBody(rigidBody)); spatialAcceleration.changeFrame(worldFrame, twist, twist); updatePerfectOrientation(); updatePerfectAngularVelocity(); updatePerfectAcceleration(); updatePerfectCompass(); simulateIMU(); orientation.set(rotationMatrix.getMatrix3d()); acceleration.set(accelX.getDoubleValue(), accelY.getDoubleValue(), accelZ.getDoubleValue()); angularVelocity.set(gyroX.getDoubleValue(), gyroY.getDoubleValue(), gyroZ.getDoubleValue()); compass.set(compassX.getDoubleValue(), compassY.getDoubleValue(), compassZ.getDoubleValue()); rawSensors.setOrientation(orientation, imuIndex); rawSensors.setAcceleration(new Vector3D(acceleration), imuIndex); rawSensors.setAngularVelocity(angularVelocity, imuIndex); rawSensors.setCompass(compass, imuIndex); }
private void processAcceleration() { // Compute acceleration in world frame, subtracting off gravity: Vector3d acceleration = new Vector3d(); rawIMUSensors.getAcceleration(acceleration, imuIndex); acceleration.sub(accelerationOffset); // use rotationMatrixBeforeOffset instead of rotationMatrix, because the accelerations are measured in IMU body! // a(M') = R(M'S') * a(S') rotationMatrixBeforeOffset.transform(acceleration); acceleration.setZ(acceleration.getZ() - localGravityZ); FrameVector frameAcceleration = new FrameVector(ReferenceFrame.getWorldFrame(), acceleration); processedSensors.setAcceleration(frameAcceleration, imuIndex); }