Add support of closed kinematic chains

[jcarpent]

Closed kinematic chains offer interesting features at the mechanical level for building robust and efficient robots (compared to open kinematic structure).
Definitively, it will a plus to support them in Pinocchio.

I open this thread for open discussions on this topic. Feel free to add your personal and scientific vision on this feature.

[zapplelove]

Closed kinematic chains offer interesting features at the mechanical level for building robust and efficient robots (compared to open kinematic structure).
Definitively, it will a plus to support them in Pinocchio.

I open this thread for open discussions on this topic. Feel free to add your personal and scientific vision on this feature.

and I want the support of sdf parser.

[ahoarau]

+1 for closed kinematics as well as SDF support.
This could also be done with the URDF format. That would just need a backward-compatible update on your parser.

[jcarpent]

As far as I know, URDF format does provide the support of closed kinematic chain,. Do you see an alternative @ahoarau?

[ahoarau]

Officially it is not supported. But it can be written in XML. It's the official parser (and especialy the Urdf::Model class) that would need an update.

In XML it could be written like this :

<link name="link_0"/>
<link name="link_1a"/>
<link name="link_1b"/>
<link name="link_2"/>

<!-- First branch -->
<joint name="joint-link_0-1a">
<parent name="link_0"/>
<child name="link_1a"/>
</joint>

<joint name="joint-link_1a-2">
<parent name="link_1a"/>
<child name="link_2"/>
</joint>

<!-- Second branch -->
<joint name="joint-link_0-1b">
<parent name="link_0"/>
<child name="link_1b"/>
</joint>

<joint name="joint-link_1b-2">
<parent name="link_1b"/>
<child name="link_2"/>
</joint>

Then the parser must support building a tree with closed loop.
https://github.com/ros/urdfdom_headers/blob/master/urdf_model/include/urdf_model/joint.h#L190:L194
Right now it is only one parent + one child per joint, fine.
But there is only one parent joint in Links : https://github.com/ros/urdfdom_headers/blob/master/urdf_model/include/urdf_model/link.h#L213
That need to be patched.

[traversaro]

I am not sure about the semantics of the extension to URDF proposed by @ahoarau . URDF has the (quite cumbersome in some cases) constraint that the link frame origin must be placed on the axis of the joint connecting the link to its parent, what happens if the link has two parents? How do you place the link frame?

[ahoarau]

I guess the FK solver would probably throw a big exception if it cannot joint the link_2_tmp1 and link_2_tmp2 together to place the final link_2 in 3d space.
Having new types of joints like "passive-prismatic" and "passive-revolute" might indicate that "this branch is passive, so let's solve the first branch, then compute the ik for the passive branch". That would be the case for this guy :

Tricky part is that the passive branch, as it is computed by an IK, might have multiple solutions. That's the price to pay.

[olivier-stasse]

To specify how to close the loop, I would use a passive joint from the SRDF format. This would keep the compatibility and avoid having a standard specific to pinocchio.

[jcarpent]

I do agree with @olivier-stasse and @traversaro. We don't want to extend the URDF format which is for sure limited. In Pinocchio, we rely on urdfdom in such a way to not have to reimplement the parsing, which is boring and error-prone.
For closed kinematic chains, the solution will be the SDF format, by only parsing the robot Model.

[ahoarau]

SDFormat support would indeed be appreciated !

[olivier-stasse]

Note that i mentionned SRDF (an already present format in ROS less known than URDF) and not SDF which is Gazebo related.

[jcarpent]

Yes, you're right. But the main issue with SRDF, is that there is no real standard. Unfortunately :(

[zapplelove]

Tosupport lose the loop, I would use a passive joint from the SRDF format. This would keep the compatibility and avoid having a standard specific to pinocchio.

So, the srdf format support the closed loop and can offer the tf in the ROS?

[olivier-stasse]

No actually I am wrong after double checking the SRDF format, the passive joint is a tag on an already existing joint...

[NmBoyd]

Has there been any update on this by chance? I would love to use Pinocchio for some work I am doing with Cassie.

[jcarpent]

@NmBoyd Thanks for your interest in Pinocchio. The feature will be made available very soon, hopefully, mid-April.
Do you have a full SDF model of Cassie to test on it?

[NmBoyd]

@jcarpent That's great to hear.
I use the Cassie SDF model provided by Agility. You can find it here.

[proyan]

Hi NmBoyd,

We're using a slightly modified version of the cassie available here: https://github.com/agilityrobotics/cassie-gazebo-sim/blob/master/cassie/cassie.sdf. The modified version is to automate the discovery of constraints from the sdf, and parsing the sdf directly within the pinocchio.
These would be released soon with the closed kinematic support as @jcarpent mentioned.

[NmBoyd]

@proyan
Good to know. This is just about the same file as the one I shared, just with 2 minor name changes. Let me know if there is anything I can do to help this along. It would be fantastic if I could test the closed kinematics on hardware by June!

[jcarpent]

@duburcqa The way you handle constraints is not the smartest and fastest way to do it.
In addition, it would be much more beneficial to everybody to include within Pinocchio rather than having a full stack of additional dependencies. More to come.

[alme96]

Hi all,

Just to be sure: the closed chain kinematics support will be available for a future version of Pinocchio 3.x and is not currently available for the version 2.6.4 that can be downloaded from GitHub?

I need this information as I am doing a project on control that involves a delta arm and I would like to make use of Pinocchio and OCS2

Thank you for clarifying.

[proyan]

@alme96 The beta version is released in the pinocchio3-preview branch at https://github.com/stack-of-tasks/pinocchio/tree/pinocchio3-preview. The branch, however is not fully supported, and the full support would be available from Pinocchio3 release.

[jcarpent]

Done in pinocchio-3x which should be released soon.