Fix computation of M, InvM, AugM, InvAugM
- add additional intermediate variables in BodyNode to compute them
- add setter and getter function for joint rest position
- add unit test for these computation