Copy/assignment bug for Inertia information of the root segment

[annaborn]

Hi, by copying or assigning the KDL::Tree the inertia information of the root segment is not copied and is restored to zero.

The inertia information:

segment.getInertia().getMass()
segment.getInertia().getCOG()
segment.getInertia().getRotationalInertia()

Hier is the code:

    printOutTree(this->kdlTree);
    KDL::Tree treeTest(kdlTree);
    printOutTree(treeTest);

The original kdlTree:

---body
Mass: 2
COG: 0.035 0 0.07825
RotationalInertia: 0.0281261 0 -0.0054775 0 0.0641161 0 -0.0054775 0 0.05125
.......
---wheel_front_right_motor
Mass: 0.1
COG: -0.00031 -0.00016 0.00047
RotationalInertia: 0.00176002 -4.96e-09 1.457e-08 -4.96e-09 0.00346003 7.52e-09 1.457e-08 7.52e-09 0.00176001

Its copy treeTest:

---body
Mass: 0
COG: 0 0 0
RotationalInertia: 0 0 0 0 0 0 0 0 0
........
---wheel_front_right_motor
Mass: 0.1
COG: -0.00031 -0.00016 0.00047
RotationalInertia: 0.00176002 -4.96e-09 1.457e-08 -4.96e-09 0.00346003 7.52e-09 1.457e-08 7.52e-09 0.00176001

[smits]

How did you create the original kdlTree AFAIK you cannot assign inertia information to the root segment.

[annaborn]

@smits To create a kldTree, we use the library that creates kdlTree from URDF file. (https://github.com/rock-control/control-kdl_parser/blob/master/src/kdl_parser.cpp) In the rows 203-207 an inertial information is added to the root segment. Since root segment is of the type kdl::Segment (like other segments in the tree), technically it is possible to add inertial information to the root. Maybe I am wrong at that point, please correct me.

[mintar]

ROS prints a warning if the root link has an inertia, and recommends that the user adds an extra dummy link to their URDF:

https://github.com/ros/kdl_parser/blob/f98550669b5821c6fd99ffdbe79f3463f152e348/kdl_parser/src/kdl_parser.cpp#L198-L200

[doudou]

While KDL-as-an-algorithm-library does not support it, why doesn't KDL-as-a-data-structure ?

People here are using KDL with their own algorithm, which does support having inertia in the root segment ...

[smits]

I'm not saying KDL should not support it ;) , I was just looking at the KDL code and wondered how they created such a root element. I do not remember having a good reason why the root segment should not have inertia properties, so I'm not against adding/fixing that. I hope the dynamic algorithms (only a few) will properly cope with that.

[annaborn]

@smits: if there is no reason not to support inertia information in the root segment, I could introduce the fix for copy operator. Are there test units for the dynamic algorithms, which I should take care about? Which ones?

[smits]

@annaborn A PR is definitely welcome, there are some tests in orocos_kdl/tests/solvertest.cpp , but the do not seem to exercise the algorithmic parts of the dynamic solvers :(