Inverting the mass matrix $M \in \mathbb{R}^{(6+n)\times(6+n)}$ is a operation commonly used in model-based control. Also for simulation purpose, it can be used to compute the forward dynamics without relying on ABA, like we do in:

If we decompose the free-floating as follows:

we can exploit the known topology of the kinematic tree defined by the parent array $\lambda(i)$ to speed up the inversion of $M_{ss} \in \mathbb{R}^{n \times n}$.

Enhancing the performance of this inversion could enable downstream users to implement alternative forward dynamics beyond our ABA and CRB implementations, for example including second-order dynamics like advanced motor dynamics (e.g. #62) or musculoskeletal models. If performance are not too far from ABA, it could be a great alternative of include these effects in ABA since it might be a daunting task.

Some references:

• Slide 10 of CRBA.pdf from https://royfeatherstone.org/teaching/CompuRobDyn2022.zip
• https://royfeatherstone.org/spatial/v2/index.html#LTL
• https://royfeatherstone.org/spatial/v2/sourceText/LTL.txt

Note that the code from Featherstone only inverts $M_{ss}$. We can exploit the following property to extend the result to the free-floating mass matrix:

https://www.wikiwand.com/en/Block_matrix#Block_matrix_inversion