@Before
public void setUp() throws Exception
{
transformIMUToJoint.setRotation(jointToIMURotation);
transformIMUToJoint.setTranslation(jointToIMUOffset);
transformJointToIMU.setAndInvert(transformIMUToJoint);
imuFrame = fullRobotModel.createOffsetFrame(fullRobotModel.getBodyLink().getParentJoint(), transformIMUToJoint, "imuFrame");
Vector3D linearAcceleration = new Vector3D(0.0, 0.0, GRAVITY);
Vector3D angularAcceleration = new Vector3D();
ReferenceFrame rootBodyFrame = fullRobotModel.getElevatorFrame();
SpatialAcceleration rootAcceleration = new SpatialAcceleration(rootBodyFrame, ReferenceFrame.getWorldFrame(), rootBodyFrame,
angularAcceleration, linearAcceleration);
simulatedIMURawSensorReader = new PerfectSimulatedIMURawSensorReader(rawSensors, IMU_INDEX, rigidBody, imuFrame, fullRobotModel.getElevator(),
rootAcceleration);
simulatedIMURawSensorReader.initialize();
}