Fix URDF inertia parser

[aelkhour]

If a URDF link has no inertia tag, the spatial inertia is currently set to se3::spatial::Inertia::Identity(); the link mass is thus set to 1 kg and the inertia to the identity 3x3 matrix.

This PR fixes this issue by setting the spatial inertia to se3::spatial::Zero() instead.

[jcarpent]

This can be lead to bugs when one needs to inverse the mass matrix of the robot. Indeed, in this way, the mass matrix becomes semi definite.

It seems that the best solution is to always define a minimal spatial inertia to the link so that the mass matrix is positive definite. The URDF file needs to be modified accordingly.

[aelkhour]

If I understand correctly, the mass matrix becomes semi definite only if the URDF model contains a pair:

• of a link with no inertial tag
• and a non-fixed joint.

AFAIK there are no such models, as inertial-less links are added for semantic purposes.
Have you encountered such models?
If not, I would suggest adding a spec for pinocchio, stating that such pairs are not supported. I think tt would make a cleaner solution than making a user hack a URDF that is often automatically generated from CAD models.

[jcarpent]

There is some models, like romeo, with no information for the fingers body. In that case, we chose to add a very small mass and inertia, to avoid the mass matrix to be semi-definite.

In order to avoid such problem of semi-define mass matrix, we propose to initialize by default the mass and inertia data with NAN in such a way to be able to detect them in the computation of the mass matrix.

[aelkhour]

I don't understand, do you propose to initialize the mass and inertia data in the URDF or in the parser?

[jcarpent]

Sorry for being unclear. The idea is to set to NAN the default inertia in the parser, not in the URDF model.

[aelkhour]

Thanks for the clarification.
Why not fix the Romeo URDF instead? To me the issue is clearly due to the model being incomplete (As you mentioned, you had to add small mass values to the fingers).
We should not expect pinocchio, or any dynamics library, to make miracles and try to fix bad models.

[jcarpent]

I do agree with your last comments.
So, we indeed added small masses in the model, not in the parser.

Thus, the idea of setting to NAN the default spatial inertia is to be able to detect such behavior in the algorithms. Indeed, we can just need Pinocchio to do only kinematics computations.

In the context of dynamics, we need a watchdog to detect bad behaviors, like the one mentioned in this discussion.

[aelkhour]

OK I see your point.
I would then suggest:

• setting the spatial inertia to 0 for inertial-less links which are paired with a fixed joint. This would have no impact on dynamics computation (correct me if I'm wrong).
• setting the spatial inertia to NaN for inertial-less links which are paired with a non-fixed joint.

[jcarpent]

For the first case, it just means that no fusion is needed.
For the rest, I do agree !

[aelkhour]

Indeed, no fusion is needed, as this link is simply there to introduce semantics. This is particularly helpful when computing a task for a frame which does not coincide with any joint frame (e.g. a humanoid robot sole). The way I see it, it is useful to store such information in the model, although algorithms do not take advantage from it so far.

[jcarpent]

Yes, just keep information in the model as a fixed body.
I want to mention that in the very near future, the notion of (referential) frame will be added for this purpose.

[aelkhour]

I pushed a new commit, I hope it fixes the case (fixed joint, link with no inertial). I'll let you handle the case (non-fixed joint, link with no inertial), as it needs a completely different treatment.