search

stack-of-tasks / pinocchio

A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
http://stack-of-tasks.github.io/pinocchio/
BSD 2-Clause "Simplified" License
1.84k stars 386 forks source link

Inertia matrix not symmetric #1298

Closed jakewelde closed 4 years ago

jakewelde commented 4 years ago

Hi, I'm wondering where I can read more to understand why the inertia matrix I calculate using pinocchio::crba(model,data,q) is not symmetric. I dug into the unit tests a little and I see that in the CRBA unit tests file the test code copies the upper triangle of entries over the lower triangles. The code snippet from the test is below.

    Eigen::MatrixXd M(model.nv,model.nv);
    Eigen::VectorXd q = Eigen::VectorXd::Ones(model.nq);
    q.segment <4> (3).normalize();
    Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
    Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
    data.M.fill(0);  crba(model,data,q);
    data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();

Is this required in general to get the correct full inertia matrix? The magnitude of error I get in some entries is significant so I doubt it is simply floating point error. Is this an implementation detail of the library, or a detail of the Composite Rigid Body Algorithm I should read about in Featherstone's work?

Apologies if I have missed the documentation for this, I have looked but didn't find any mention except for looking at the unit tests.

jcarpent commented 4 years ago

The Joint Space Inertia Matrix is only filled on the upper diagonal part on purpose, because most algorithms only require it. Symmetricaly, the lower diagonal part can be used too. If you want to apply M times something, you indeed need to fill the lower diagonal part. But in general, you will never do this operation because the complexity is O(N^3) where N is the number of joints, while RNEA, which does internally a similar operation, is O(N).

Here, you may find the requested information: https://gepettoweb.laas.fr/doc/stack-of-tasks/pinocchio/master/doxygen-html/namespacepinocchio.html#ad3da115c312299adc15978286aa82b10 If it is not clear enough, it would be nice you contribute by helping us improving this documentation.

jakewelde commented 4 years ago

Thank you for this answer! Sorry that I did not find see myself as I now see it should have been obvious.

At this point I was mostly just looking to check that I was using the library correctly by comparing the dynamics I am getting using Pinocchio with the results I got from my own naive symbolic implementation. But thank you for clarifying!