Add calculation of dynamics quantities (M, M^{-1}) that uses cache data from Featherstone's forward dynamics algorithm.
Note that BodyNode::aggregateInvMassMatrix() still has some bugs.