@Override
public void read()
{
twistCalculator.compute();
spatialAccelerationCalculator.reset();
twistCalculator.getTwistOfBody(rigidBody, twist);
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);
}