[wholeBodyInterface] Link and frames concepts

[traversaro]

The link concept is necessary in the wholeBodyInterface in a few user cases:

• We want to calculate a Floating Base Jacobian of a link.
• When getting the ESTIMATE_EXTERNAL_FORCE_TORQUE we want to get the estimate of the external force torque acting on a link.

In this two user cases there is an hidden assumption: that a link is associated to a unique frame.

Regardless of the orientation in which the jacobian and the external force torque is expressed (in wbi it is the world one) the origin of this frame is used as the reference point for the linear velocity of the jacobian and for the external torque.

This is ok as long as every link is associated to a unique frame.

Unfortunately the URDF format constraint the link frames to have their origin placed in the axis of the parent joint, which is quite inconvenient for control (for example if you want the foot frame origin to be placed on the sole of the foot, as actually recommended by REP-120 .

The usual workaround, used also in the iCub URDF, is to add a fake link connected to a real link with a fixed joint, and to place the frame of the fake link in the desired arbitrary location.

Clearly external forces can be applied only to a real link, but in the URDF there is no way to semantically discriminate between a real link and fake link. The workaround usually used to distinguish between real and fake links, for example in gazebo, is to consider any link with a fixed parent joint to be a fake link. Any fake link is then "merged" from the physics point of view with its closest "real" ancestor.

This workaround fails when two real links are actually connected by a fixed joint, if the two bodies are connected by a 6-axis Force Torque sensor.

Very long term proper solution would be to clearly distinguish between frame and link concepts in the file describing the robot structure.

A clean short term workaround to continue use URDF is to use an additional tag to clearly mark "fake/frame" links. In this case, the semantics of get(ESTIMATE_EXTERNAL_FORCE_TORQUE, fake_link) would be "get the force/torque applied on the real_link corresponding to the fake_link, but expressing the torque at the origin of the frame of the fake_link).

[traversaro]

cc @luca-fiorio

[traversaro]

Preliminary support for this added in iDynTree in robotology/idyntree@b6cbe7d .

[traversaro]

Full support also in the URDF parsing added as part of robotology/idyntree#103 .
While it is necessary to solve robotology/idyntree#93 before this is actually used in the yarp-wholeBodyInterface, I thinks that once that PR is merged in iDynTree master we can consider this issue as closed.

[traversaro]

robotology/idyntree#93 was solved, and the distinction between link and frames is fully documented in iDynTree (see https://github.com/robotology/idyntree/blob/2470a71cee49ee3a611195e36b8dc447eca9f154/src/model/include/iDynTree/Model/Model.h#L34). Furthermore, an attempt to raise the awareness on this issue in the wider robotics community has been attempted in https://discourse.ros.org/t/urdf-ng-link-and-frame-concepts/56 .