Support closed kinematics

[ahoarau]

Hi all,

This is just a feature request. I think it would be useful to have closed loop kinematics support such as :

It may start by have a "passive" chain that would follow the other one, the "active". That would mean solve an IK problem in forward kinematics. what do you think ?

[jcarpent]

I don't know what is the current status of using Pinocchio inside iDyntree, but soon, Pinocchio will support such kind of mechanisms both at the kinematic and dynamic level. So, if relying Pinocchio remains valid, it will be normally straightforward.

[traversaro]

Hi @ahoarau ! In theory iDynTree::Model is already designed to support closed kinematics, the problem is that most of the algorithms does not support it, starting from computeFullTreeTraversal that in the case of closed kinematics model should provide a suitable spanning tree of the closed kinematics model. In general any improvements to better support closed kinematics in iDynTree is welcome, but at the moment I don't think anyone is working in that direction.

[traversaro]

I don't what is the current status of using Pinocchio inside iDyntree, but soon, Pinocchio will support such kind of mechanisms both at the kinematic and dynamic level. So, if relying Pinocchio remains valid, it will be normally straighforward.

Hi @jcarpenter, the work on building a shim on the top of Pinocchio that loads models from iDynTree::Model and offer a functionality similar to an API similar to iDynTree::KinDynComputations is currently tracked in the separate repository (https://github.com/dic-iit/idynfor) and at the moment we don't have an ETA for when the work on it will start. However, I imagined that even in that case to support closed kinematics chain use cases the APIs of iDynTree::KinDynComputations would need to be revised accordingly.

[traversaro]

fyi @FabioBergonti , given that you are working on closed kinematics (even if you are not using iDynTree at the moment).

[traversaro]

Discussing about this in a meeting with @CarlottaSartore @xela-95 @rob-mau , I think I realized some aspect for which it would make sense to support closed-loop structures in iDynTree.

I am not sure it would make sense to try to support closed-loop structures in Forward/Inverse Kinematics and Dynamics (at least in iDynTree, higher-level libraries such as IK/TSID implemented in https://ami-iit.github.io/bipedal-locomotion-framework/namespaceBipedalLocomotion_1_1IK.html effectively already solve open mechanical structure under constraints.

However, I think it would made sense to do the following:

• [ ] Make sure that you can generate a iDynTree::Traversal (that is effectively a data structure representing a spanning tree) out of iDynTree::Model (that is effectively a graph) that contains a loop. In this case the function that computes the iDynTree::Traversal would need to either take in input or compute via an heuristics where to "cut" the loops in the graph to generate the spanning tree (see for example the SimTK::MultibodyGraphMaker of simbody that implements something similar.
• [ ] Make sure that we can support loading a iDynTree::Model with closed-loop structures from a file. The most generic solution probably involves https://github.com/robotology/idyntree/issues/481, but as a first solution we can also look into supporting some unofficial extensions of URDF to support closed structures, for example the one described in https://classic.gazebosim.org/tutorials?tut=kinematic_loop&cat=#Split4-barlinkageinURDFwithanSDFormatfixedjoint .
• [ ] Make sure that we can support exporting a iDynTree::Model with closed-loop structures to a file. The most generic solution probably involves https://github.com/robotology/idyntree/issues/835, but as a first solution we can also look into supporting some unofficial extensions of URDF to support closed structures, for example the one described in https://classic.gazebosim.org/tutorials?tut=kinematic_loop&cat=#Split4-barlinkageinURDFwithanSDFormatfixedjoint .