Updated RigidBody using new Jacobian computation