public void update(RigidBodyTransform transformBodyToWorld, FrameVector3D linearVelocity, FrameVector3D angularVelocity, FrameVector3D linearAcceleration,
FrameVector3D angularAcceleration)
{
rootJoint.setJointConfiguration(transformBodyToWorld);
updateFrames();
Twist bodyTwist = new Twist(bodyFrame, elevatorFrame, bodyFrame, angularVelocity, linearVelocity);
rootJoint.setJointTwist(bodyTwist);
SpatialAcceleration accelerationOfChestWithRespectToWorld = new SpatialAcceleration(bodyFrame, worldFrame, bodyFrame,
angularAcceleration,
linearAcceleration);
accelerationOfChestWithRespectToWorld.setBaseFrame(getElevatorFrame());
rootJoint.setJointAcceleration(accelerationOfChestWithRespectToWorld);
updateFrames();
}