Quick implementation of forwardJacDot. However, forwardJacDot should disappear in my opinion.
Reimplemented bodyKin using forwardKin. P computation is still missing.