Fix bug in importing com if inertia frame orientation and link frame orientation are different

[traversaro]

The URDF specification ( http://wiki.ros.org/urdf/XML/link ) specify that the center of mass is expressed in the link reference frame (as xyz of the origin element) while the inertia matrix is expressed in the "inertia reference frame" given by the origin element.

The current version of the parser is flawed because it rotates both the com and the inertia matrix, while only the inertia matrix should be rotated. I guess this went unnoticed because the large majority or URDF models uses an identity (rpy="0 0 0") for the rotational part of the "inertial" origin element.

[isucan]

@hsu ?

[isucan]

This is a pretty core change, I'm happy to merge it, but can we add a test?

[traversaro]

For sure. I am quite busy at a moment, but I guess I can provide a test in the next month.
A consistency check of the gravity torques of two equivalent arms, that fails without this patch would be sufficient?

[jacquelinekay]

pinging @traversaro: any interest in revisiting this? I can spend a bit of time writing a test case if you're busy, so that we can fast-track this to getting merged.

[traversaro]

@jacquelinekay if you could write a test it would be great, given that I don't have access to a ROS installation and so it would take me a bit of time to set up one.

The test I was thinking about was something like this:

• Load this two equivalent (according to URDF specs) models:

<robot name="testInertialRPYmodel1">
   <link name="base_link" />
   <joint name="base_fixed_joint" type="fixed">
        <origin xyz="0 0 0" rpy="0 -0 0" />
        <axis xyz="0 0 0" />
        <parent link="base_link" />
        <child link="link1" />
    </joint>
    <link name="link1">
        <inertial>
            <mass value="1" />
            <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
        </inertial>
    </link>
    <joint name="joint_1_2" type="continuous">
        <origin xyz="0 0 0" rpy="0 -0 0" />
        <axis xyz="0 0 1" />
        <parent link="link1" />
        <child link="link2" />
    </joint>
    <link name="link2">
        <inertial>
            <mass value="100" />
            <origin xyz="1 2 3" rpy="0 -0 0" />
            <inertia ixx="5" ixy="0.0" ixz="0.0" iyy="15" iyz="0.0" izz="25" />
        </inertial>
    </link>
</robot>

and

<robot name="testInertialRPYmodel2">
   <link name="base_link" />
   <joint name="base_fixed_joint" type="fixed">
        <origin xyz="0 0 0" rpy="0 -0 0" />
        <axis xyz="0 0 0" />
        <parent link="base_link" />
        <child link="link1" />
    </joint>
    <link name="link1">
        <inertial>
            <mass value="1" />
            <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
        </inertial>
    </link>
    <joint name="joint_1_2" type="continuous">
        <origin xyz="0 0 0" rpy="0 -0 0" />
        <axis xyz="0 0 1" />
        <parent link="link1" />
        <child link="link2" />
    </joint>
    <link name="link2">
        <inertial>
            <mass value="100" />
            <origin xyz="1 2 3" rpy="3.141592653589793 0 0" />
            <inertia ixx="5" ixy="0.0" ixz="0.0" iyy="15" iyz="0.0" izz="25" />
        </inertial>
    </link>
</robot>

The only difference between the two models is the rpy of the link2 inertia, that does not change anything in the model because it just change the signs of axis y and z, and the inertia matrix is not changed by this orientation change.

However due to the bug that this PR solves, if you follow this steps:

• load the urdf in a KDL::Tree using kdl_parser
• extract a KDL::Chain from the KDL::Tree (see http://people.mech.kuleuven.be/~rsmits/kdl/api/html/classKDL_1_1Tree.html#a45e93dab5e0ef6b390820904edf82bd5 )
• load the KDL::Chain in the ChainIdSolver_RNE
• get the gravity torques (set gravity in the ChainIdSolver__RNE and then set q,q_dot,q_dotdot vectors to have all zeros element)
  You will get different torques between the two equivalent models. With this PR, the two gravity torques vector are equal.

[jacquelinekay]

I'm working on the test, but I'm running into an unrelated issue where I can't call treeFromFile on two different trees in the same process without segfaulting. I'm going to try again with the latest version of urdfdom (right now I'm using the current Trusty debs). Posting here in case anyone has experience with this bug...

[traversaro]

I never had this problem, but to be honest I forked this code 2 years ago (to avoid the catkin/ROS/everything else dependency) and apparently I modified the treeFromFile function to change the file loading logic, I don't know if this could solve the issue: robotology/idyntree@d1ce021#diff-061ea33473e57342c14e190b975aee83L133 .

[jacquelinekay]

Got it. I have a workaround right now (allocate the trees on the heap and call delete manually) that works without using the current urdfdom from source. But, maybe we can gain insight from your fork to improve this copy of kdl_parser in the future.

The test is written:

https://github.com/ros/robot_model/tree/fix-com-import/kdl_parser

but I still can't run it because kdl_parser is quite broken, it thinks that KDL tree generated from that URDF has 0 joints and 0 segments.

[traversaro]

I am sorry. : (
Just tested on my fork and those URDF are loaded normally.

[jacquelinekay]

I figured out the issue with kdl_parser and I've pull requested this commit and my test case in #117. Thanks again.

[jacquelinekay]

The fix and test are merged from my other branch. Thanks for contributing, @traversaro!