Minv computation - bug or inconvient behaviour?

[cmastalli]

Hi guys,

I have a question regarding the computation of the inverse of the inertial matrix using Pinocchio Cholesky. First, why we cannot use the data.Minv in computeMinv, e.g.:

#include <pinocchio/algorithm/cholesky.hpp>
...
pinocchio::cholesky::decompose(model, data);
data.Minv.setZero(); // the code doesn't work with or without this line
pinocchio::cholesky::computeMinv(model, data, data.Minv);

From my perspective it would be nice to reuse this matrix, avoiding to create an extra matrix.

My second question is: why do I need to set to zero Minv?, e.g.

#include <pinocchio/algorithm/cholesky.hpp>
...
pinocchio::cholesky::decompose(model, data);
Eigen::Matrix Minv(model.nv, model.nv)
Minv.setZero();
pinocchio::cholesky::computeMinv(model, data, Minv); //this line works
pinocchio::cholesky::computeMinv(model, data, Minv); // this second time doesn't work

I would strongly suggest to avoid the setZero(), however if this is needed then we should include in the documentation.

Thanks, Carlos

[jcarpent]

You're not using the right computeMinv. You should use the computeMinv from the global pinocchio namespace. It is defined in <pinocchio/algorithm/aba.hpp>.

[jcarpent]

The computeMinv you were using is performed by using the Sparse Cholesky decomposition of the Joint Space Inertia Matrix, which needs a call to CRBA first, then to the decomposition and then to the computeMinv.

For the second point, I will have a look, but I think this is due to the InPlace way of performing the computations.

[jmirabel]

For the second point, I will have a look, but I think this is due to the InPlace way of performing the computations.

It is worth an assertion, isn't it ?

[cmastalli]

You're not using the right computeMinv. You should use the computeMinv from the global pinocchio namespace. It is defined in <pinocchio/algorithm/aba.hpp>.

In my case I need to add the armature in the inertial matrix, e.g.

#include <pinocchio/algorithm/cholesky.hpp>
...
pinocchio::computeAllTerms(model, data, q, v);
data.M.diagonal() += armature;
pinocchio::cholesky::decompose(model, data);
Eigen::Matrix Minv(model.nv, model.nv)
Minv.setZero();
pinocchio::cholesky::computeMinv(model, data, Minv);
Eigen::VectorXd x = Minv * (u - data.nle);

is this proper way of doing it?

[jcarpent]

For the second point, I will have a look, but I think this is due to the InPlace way of performing the computations. It is worth an assertion, isn't it ?

I will change the behavior of the algorithm to make it compliant.

[jcarpent]

You're not using the right computeMinv. You should use the computeMinv from the global pinocchio namespace. It is defined in <pinocchio/algorithm/aba.hpp>.

In my case I need to add the armature in the inertial matrix, e.g.

#include <pinocchio/algorithm/cholesky.hpp>
...
pinocchio::computeAllTerms(model, data, q, v);
data.M.diagonal() += armature;
pinocchio::cholesky::decompose(model, data);
Eigen::Matrix Minv(model.nv, model.nv)
Minv.setZero();
pinocchio::cholesky::computeMinv(model, data, Minv);
Eigen::VectorXd x = Minv * (u - data.nle);

is this proper way of doing it?

Yes, indeed. This is the correct way. I will change the behavior and fix the consistency of the second call.

[cmastalli]

Thank you guys for your very fast and efficient responses!

@jcarpent feel free to close this issue if you want to create another one for point 2.

[jcarpent]

Solved by #846.