remove inertial filtering

[richardrl]

This filtering of links was causing a very gnarly bug as we were confused why the forward kinematics of the attached URDF was not as expected.

It seems there is no good reason to do this? It adds a lot of confusion to new users, as we expect the non-inertial links and frames to still exist.

For this URDF, for the hand we needed to add a fictitious wrist frame that coincides with the end effector tool frame.

Your code was recognizing palm_lower as the base link when I was really expecting wrist_axis to be the base link. leap_right.zip

[traversaro]

As a general comment, we typically separate links (that have an inertia and play a role in dynamics) from additional frames (that do not have an inertia and do not play a role in dynamics). The idea is that you to not want to have additional frames in dynamics-related loops for efficiency reason, but you still want to do kinematics (such as get the transform or the jacobian) for additional frames.

That is not a difference that is in URDF (unfortunately, see for example https://discourse.ros.org/t/urdf-ng-link-and-frame-concepts/56), so to distinguish between the two, it is necessary to have some kind of heuristics to distinguish between the two.

Having said that, indeed it is curious that we do not list massless links at least in the frames list. @Giulero do you have any insight on this?

[traversaro]

@richardrl fyi the tests seems to be failing with this change, can you provide a snippet of code that was not working before this change and is working after so we can find a solution that fixes your code while avoiding to fail the existing tests?

[traversaro]

As a general comment, we typically separate links (that have an inertia and play a role in dynamics) from additional frames (that do not have an inertia and do not play a role in dynamics). The idea is that you to not want to have additional frames in dynamics-related loops for efficiency reason, but you still want to do kinematics (such as get the transform or the jacobian) for additional frames.

To make you an example, these is how the provided URDF gets parsed by iDynTree, that is the C++ library from which adam took inspiration for the semantics of the KinDynComputations class:

D:\leap_right> conda create -n idyntree idyntree
D:\leap_right> conda activate idyntree
(idyntree) D:\leap_right>idyntree-model-info -m ./leap_right.urdf --print
Model:
  Links:
    [0] palm_lower
    [1] mcp_joint
    [2] mcp_joint_2
    [3] mcp_joint_3
    [4] pip_4
    [5] thumb_pip
    [6] thumb_dip
    [7] thumb_fingertip
    [8] pip_3
    [9] dip_3
    [10] fingertip_3
    [11] pip_2
    [12] dip_2
    [13] fingertip_2
    [14] pip
    [15] dip
    [16] fingertip
  Frames:
    [17] wrist_axis --> palm_lower
    [18] mcp_joint_root --> mcp_joint
    [19] mcp_joint_2_root --> mcp_joint_2
    [20] mcp_joint_3_root --> mcp_joint_3
    [21] thumb_fingertip_tip --> thumb_fingertip
    [22] fingertip_3_tip --> fingertip_3
    [23] fingertip_2_tip --> fingertip_2
    [24] fingertip_tip --> fingertip
  Joints:
    [0] 1 (dofs: 1) : palm_lower<-->mcp_joint
    [1] 5 (dofs: 1) : palm_lower<-->mcp_joint_2
    [2] 9 (dofs: 1) : palm_lower<-->mcp_joint_3
    [3] 12 (dofs: 1) : palm_lower<-->pip_4
    [4] 13 (dofs: 1) : pip_4<-->thumb_pip
    [5] 14 (dofs: 1) : thumb_pip<-->thumb_dip
    [6] 15 (dofs: 1) : thumb_dip<-->thumb_fingertip
    [7] 8 (dofs: 1) : mcp_joint_3<-->pip_3
    [8] 10 (dofs: 1) : pip_3<-->dip_3
    [9] 11 (dofs: 1) : dip_3<-->fingertip_3
    [10] 4 (dofs: 1) : mcp_joint_2<-->pip_2
    [11] 6 (dofs: 1) : pip_2<-->dip_2
    [12] 7 (dofs: 1) : dip_2<-->fingertip_2
    [13] 0 (dofs: 1) : mcp_joint<-->pip
    [14] 2 (dofs: 1) : pip<-->dip
    [15] 3 (dofs: 1) : dip<-->fingertip

Note that the base link in this case is palm_lower as wrist_axis is considered an "additional frame".

[richardrl]

As a general comment, we typically separate links (that have an inertia and play a role in dynamics) from additional frames (that do not have an inertia and do not play a role in dynamics). The idea is that you to not want to have additional frames in dynamics-related loops for efficiency reason, but you still want to do kinematics (such as get the transform or the jacobian) for additional frames.

That is not a difference that is in URDF (unfortunately, see for example https://discourse.ros.org/t/urdf-ng-link-and-frame-concepts/56), so to distinguish between the two, it is necessary to have some kind of heuristics to distinguish between the two.

Having said that, indeed it is curious that we do not list massless links at least in the frames list. @Giulero do you have any insight on this?

I agree with this statement but the problem is the forward kinematics is currently being done with this ‘palm_lower’ instead of ‘wrist_axis’. The forward kinematics is done with the list of inertial links right now, so thats why this change is necessary

[richardrl]

So we seem to be on the same page that wrist_axis should be the root link for FK which is what this commit does

[richardrl]

@richardrl fyi the tests seems to be failing with this change, can you provide a snippet of code that was not working before this change and is working after so we can find a solution that fixes your code while avoiding to fail the existing tests?

The only test that seems to be failing is the one enforcing that all links are inertial, which is the assumption this code was designed to remove. Perhaps we just remove that test?

[richardrl]

I didnt look too deeply into the dynamics code but maybe what you’re proposing is we use a separate urdffactory for the links used in FK vs dynamics algorithms

[Giulero]

Hi @richardrl ! I guess I get your problem.

As @traversaro said, we are not tacking into account links with no inertial properties (called then frames).

In your urdf the massless link/frame wrist_axis is connecting the lower with the upper part of the tree. Removing it, the tree breaks into two parts with two root links.

I tried to modify the filtering, excluding from the removal the links with inertia and the links that have children

    def get_links(self) -> List[StdLink]:
        """
        Returns:
            List[StdLink]: build the list of the links
        """
        return [
            self.build_link(l)
            for l in self.urdf_desc.links
            if (l.inertial is not None or l.name in self.urdf_desc.child_map.keys())
        ]

and then done other modification in order to initialize the null inertia with default zero values.

The output tree is

 base_link┐
          └shoulder_link┐
                        └upper_arm_link┐
                                       └forearm_link┐
                                                    └wrist_1_link┐
                                                                 └wrist_2_link┐
                                                                              └wrist_3_link┐
                                                                                           └flange┐
                                                                                                  └tool0┐
                                                                                                        └wrist_axis┐
                                                                                                                   │          ┌mcp_joint┐
                                                                                                                   │          │         └pip┐
                                                                                                                   │          │             └dip┐
                                                                                                                   │          │                 └fingertip
                                                                                                                   │          ├mcp_joint_2┐
                                                                                                                   │          │           └pip_2┐
                                                                                                                   │          │                 └dip_2┐
                                                                                                                   │          │                       └fingertip_2
                                                                                                                   └palm_lower┤
                                                                                                                              ├pip_4┐
                                                                                                                              │     └thumb_pip┐
                                                                                                                              │               └thumb_dip┐
                                                                                                                              │                         └thumb_fingertip
                                                                                                                              └mcp_joint_3┐
                                                                                                                                          └pip_3┐
                                                                                                                                                └dip_3┐
                                                                                                                                                      └fingertip_3

Is this right?

[richardrl]

Hi @richardrl ! I guess I get your problem.

As @traversaro said, we are not tacking into account links with no inertial properties (called then frames).

In your urdf the massless link/frame wrist_axis is connecting the lower with the upper part of the tree. Removing it, the tree breaks into two parts with two root links.

I tried to modify the filtering, excluding from the removal the links with inertia and the links that have children

    def get_links(self) -> List[StdLink]:
        """
        Returns:
            List[StdLink]: build the list of the links
        """
        return [
            self.build_link(l)
            for l in self.urdf_desc.links
            if (l.inertial is not None or l.name in self.urdf_desc.child_map.keys())
        ]

and then done other modification in order to initialize the null inertia with default zero values.

The output tree is

 base_link┐
          └shoulder_link┐
                        └upper_arm_link┐
                                       └forearm_link┐
                                                    └wrist_1_link┐
                                                                 └wrist_2_link┐
                                                                              └wrist_3_link┐
                                                                                           └flange┐
                                                                                                  └tool0┐
                                                                                                        └wrist_axis┐
                                                                                                                   │          ┌mcp_joint┐
                                                                                                                   │          │         └pip┐
                                                                                                                   │          │             └dip┐
                                                                                                                   │          │                 └fingertip
                                                                                                                   │          ├mcp_joint_2┐
                                                                                                                   │          │           └pip_2┐
                                                                                                                   │          │                 └dip_2┐
                                                                                                                   │          │                       └fingertip_2
                                                                                                                   └palm_lower┤
                                                                                                                              ├pip_4┐
                                                                                                                              │     └thumb_pip┐
                                                                                                                              │               └thumb_dip┐
                                                                                                                              │                         └thumb_fingertip
                                                                                                                              └mcp_joint_3┐
                                                                                                                                          └pip_3┐
                                                                                                                                                └dip_3┐
                                                                                                                                                      └fingertip_3

Is this right?

Hey @Giulero , the tree looks right. the only important thing is that the wrist_axis exists before the palm_lower.

Actually, my commit in this PR fixes the issue just by removing the inertial filtering

[traversaro]

if (l.inertial is not None or l.name in self.urdf_desc.child_map.keys())

For reference, the similar check in iDynTree is https://github.com/robotology/idyntree/blob/v12.3.0/src/model/src/ModelTransformers.cpp#L25-L54 . Note that in iDynTree there is also the condition that the link is connected to its neighbor via a fixed joint to consider it an additional frame.

[Giulero]

Hi @richardrl, just removing the condition on inertia is not enough. The tests are failing since we should add a fake inertia to massless links. To print the tree and check that the tests are not failing, I created a new branch and hence opened PR #85.

[Giulero]

PR #85 merged.