Pinocchio: Add support of SDFormat

[jcarpent]

SDFormat is becoming a standard in Robotics to define kinematics chain + their environment (mainly for simulation purposes).
I do think that this format should be supported by Pinocchio.

I'm starting the discussion about this support via this thread. Feel free to raise some comments and suggestions about this extended feature.

[proyan]

The SDF parser provides a directional graph, with each node corresponding to a joint, and each joint specifying a parent and a child.
In order to properly parse a closed loop, the parser needs to understand how to choose the "loop closing joint". For example, in the typical closed loop chain, we might have 4 revolute joints and one ball joint. Logically, it would be preferable to keep revolute joint in the kinematic tree, and save the ball joint as the loop closure joint.

However, it is not always the last joint in the chain. So, the steps forward for sdf parser that I see is :
1) Be able to parse the sdf file.
2) Find a logic for choosing the loop closing joint.
3) If the kinematic chain that is parsed violated the logic in (2), re-order the chain. This would mean being able to invert the parent-child pairs in the tree.

The preview of Pinocchio 3 that we are working on would contain (1). If we go in the direction that I mention above, (2) and (3) would need to be developed.

The views and opinions of the other pinocchio developers would be welcome in this thread

[jmirabel]

What ever is implemented, I think we should keep the possibility to explicitly say which is the loop closure joint.

That being said, I think you can have (2) and (3) outside of the parser, as a function that a user can call after having parsed a SDF file.

[proyan]

Yes, we discussed internally about keeping (3) as a separate algorithm and discarding (2).

Along with (3), we would need another algo like you suggest. It would mean something like "changeConstraintJoint" would call "invertKinematicGraph" or similar

[traversaro]

For the logic of choosing the loop closure joint, a relevant class that could be useful as inspiration (perhaps some of you already know it, but for everyone else reading the issue they could be useful) is Simbody's SimTK::MultibodyGraphMaker, see https://github.com/simbody/simbody/blob/a8f49c84e98ccf3b7e6f05db55a29520e5f9c176/SimTKmath/include/simmath/MultibodyGraphMaker.h#L46 and https://simtk.org/api_docs/simbody/3.5/classSimTK_1_1MultibodyGraphMaker.html#details . https://github.com/RobotLocomotion/drake/pull/10068 is a interesting discussion that tried to port that class to Drake.

[jcarpent]

Thanks @traversaro for the very informative presentation.
In Pinocchio, we plan to represent a ClosedLoop Mechanism with a full span tree containing all the joints depicted in the model and using 6d constraints for closing the loops. Of course, this means that will work with the maximum number of constraints, but the algorithmic behind is very robust to singular and redundant cases, as depicted here: https://hal.inria.fr/hal-03271811/
We are not yet planning to hide joints, as proposed for instance in SimBody.

@traversaro @jmirabel any feedback is more than welcome.

[traversaro]

In Pinocchio, we plan to represent a ClosedLoop Mechanism with a full span tree containing all the joints depicted in the model and using 6d constraints for closing the loops. Of course, this means that will work with the maximum number of constraints, but the algorithmic behind is very robust to singular and redundant cases, as depicted here: https://hal.inria.fr/hal-03271811/

Thanks for the reference! In a recent meeting we were discussing exactly about that (splitting along joint or splitting links) and we all agree that splitting links was easier from the implementation complexity side, it is nice to know that it also have other useful properties. FYI @gabrielenava @VenusPasandi @CarlottaSartore .

We are not yet planning to hide joints, as proposed for instance in SimBody.

From what I understand, SimBody and its SimTK::MultibodyGraphMake supports both (see https://github.com/RobotLocomotion/drake/issues/1867#issuecomment-197004468) but indeed I did not look into the details so I am not 100% sure.

[jcarpent]

I indeed share the same opinion than Russ in https://github.com/RobotLocomotion/drake/issues/1867#issue-141083377:

RussTedrake adds: That would indeed be cleaner. The current implementation is limited in that we cannot easily add an actuator directly to the “loop joint”, nor friction, etc. And we’re already adding 6 constraints for a revolute joint anyhow (should be 5, but was cleaner to implement as 6). It would be a definite improvement!

[proyan]

Initial development completed with https://github.com/stack-of-tasks/pinocchio/pull/1478