Got rid of some repeated computation (forwardJacDot).
RigidBodyInertialMeasurementUnit updates. Not quite sure that this works properly, because there is no test for this.
Got rid of some repeated computation (forwardJacDot). Fixed some v/qd discrepancies.