@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(); }