Remove matrix inverse operations and use pinocchio routines for forward and impulse dynamics

1. Currently we are doing inv(M) in calcdiff. This should be removed.
2. start using pinocchio routines for forward/impulse dynamics
3. I created a PR to extract the K_inv matrix from the pinocchio routines for forward/impulse dynamics. https://github.com/stack-of-tasks/pinocchio/pull/665. After it is merged, we should use this instead of doing inv(K) in calc.