Best way to compute forward dynamics and M inverse

[andreadelprete]

Hi,

it seems that pinocchio offers different ways to compute forward dynamics and M inverse. For the forward dynamics we have:

• aba: algorithm for forward dynamics
• cholesky::decompose + cholesky::solve

For computing M inverse instead we have:

• computeMinverse: it uses the Articulated Body formulation
• cholesky::computeMinv: it uses a custom Cholesky decomposition

• crba + standard inversion (e.g., with Eigen)

  I was wondering if there was a way to know which approach to choose depending on the situation, or if one should benchmark the different algorithms to see which one performs best in a specific application.

[jcarpent]

To quickly answer your question and according to Analytical Derivatives of Rigid Body Dynamics Algorithms, the direct computation of Minv is much faster than all the other approaches:

• using Eigen based inverse will not be able to exploit the sparsity induced by the kinematic tree, which is relevant for a legged robot (not really for a robotic arm).
• if you have already computed the mass matrix at some point, then use the Cholesky decomposition approach
• if you've already computed the forward dynamics (via ABA), then you should prefer the call to computeMinverse. About that, new functions will appear soon with reduced complexity and timings.

If you just want to perform the operation Minv times a vector, then you should privilege the Cholesky decomposition, because, you don't need the full expression of the Mass Matrix inverse. I would say, you just need the full expression when you do matrix-matrix operations, e.g. when computing derivatives.

[andreadelprete]

Thanks for the answer @jcarpent ! I didn't know that paper also dealt with the computation of M inverse, but it actually makes a lot of sense because that's the derivative of the dynamics w.r.t. the control!

In my case I need to compute the whole matrix J*Minv*J because then I have to compute a matrix exponential with that. The Jacobian J is the contact Jacobian with 3k rows, with k being the number of contacts, so typically 3k is (slightly) smaller than nv (size of M), such as 3k=12 and nv=18. According to you suggestions I should then use computeMinverse. Do you confirm?

[jcarpent]

For getting this matrix, I would suggest taking insparition from this code: https://github.com/stack-of-tasks/pinocchio/blob/ada5b11e5881e3fbc28e3bac717075ed8fa6855c/src/algorithm/contact-dynamics.hxx#L39

[andreadelprete]

Thanks @jcarpent , but before you wrote

if you've already computed the forward dynamics (via ABA), then you should prefer the call to computeMinverse

Instead, in the code of contact-dynamics.hxx you're using Cholesky. Since I have to call ABA, and I don't need M, I thought computeMinverse would be the best option.

[jcarpent]

I will close this issue. @andreadelprete Feel free to reopen it when needed.