Implement analytical jacobian for JointToMarker
Improve rigid body dynamics