-
Notifications
You must be signed in to change notification settings - Fork 374
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Fix URDF inertia parser #60
Conversation
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. |
If I understand correctly, the mass matrix becomes semi definite only if the URDF model contains a pair:
AFAIK there are no such models, as inertial-less links are added for semantic purposes. |
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. |
I don't understand, do you propose to initialize the mass and inertia data in the URDF or in the parser? |
Sorry for being unclear. The idea is to set to NAN the default inertia in the parser, not in the URDF model. |
Thanks for the clarification. |
I do agree with your last comments. 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. |
OK I see your point.
|
For the first case, it just means that no fusion is needed. |
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. |
Yes, just keep information in the model as a fixed body. |
* Inertial-less links under fixed joints exist for semantic purposes, i.e. to provide referential frames. * Keep storing these links as fixed bodies. * The case of pairs with non-fixed joints and inertial-less links still needs to be properly addresses, as the current behavior (identity inertia) is clearly bad.
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. |
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.