Idyntree: Enforce meaningful initial joint ordering for chain models

[traversaro]

Using the new parser introduced in https://github.com/robotology/idyntree/pull/460, it is possible that joints of a chain model are loaded out of order.

Why the precise mapping between joint indexes and joint names cannot be fixed, at least the simple invariant that a chain robot with joints joint_0, joint_1, joint_2, ... should be have index 0, 1, 2, etc etc should be respected. This is especially required by industrial robots.

[traversaro]

I think this can be implemented as a pass on the model after the model has been loaded, similarly to the process we use to remove "fake" link and transform them in additional frames.

[traversaro]

Note that the problem was actually there also with the old parser, the new parser just highlighted the issue.

[ahoarau]

Hello again ! We are now 5 people working on this and we tried to load a human urdf.
The order of joint in the chains is not respected. Here's the files to debug the issue :

human_joint_wrong_order_as_given_by_iDynTree.txt
human_joint_names_ordered.txt
human_model_fixed_base.urdf.txt

Thanks again for your help !

[ahoarau]

And icub is strangely missing 1 dof with this PR.

[traversaro]

The order of joint in the chains is not respected.

I am not sure how you define the "correct" ordering for a tree kinematic structure. The order with which the joint elements appear in the URDF file? The problem is this criteria can be conflicting with the "normalized" order as introduced in https://github.com/robotology/idyntree/pull/492 .

[ahoarau]

You're right it's not trivial. In what order does gazebo load the joints ?

[traversaro]

In what order does gazebo load the joints ?

Not really sure, but to be honest the Gazebo API does not really use joint/dof serialization a lot (for example there are no methods to get/set joint positions/velocity/accelerations as a vector .

[traversaro]

And icub is strangely missing 1 dof with this PR.

This is strange. Which one?

[ahoarau]

False alarm on my tests.. mister @rlober ?

[ahoarau]

@traversaro False alarm, 32 joints loaded with and without the PR. Good to go sir !

[ahoarau]

I am not sure how you define the "correct" ordering for a tree kinematic structure. The order with which the joint elements appear in the URDF file? The problem is this criteria can be conflicting with the "normalized" order as introduced in #492 .

Very true. If nothing is ordered, then maybe having an ordered map could at least help visibility.
Example on the icub : The torso joints indexes are very far from each other.

     Joint 0: 'torso_roll'
     Joint 1: 'torso_pitch'    
     # Many more joints ....
    Joint 20: 'torso_yaw'

What do you think ?

[traversaro]

"Having an ordered map" in which sense? The Model::getJointIndex(const std::string &jointName) method should basically implement that. Do you think a separate method to obtain a map of some kind would be more useful?

[ahoarau]

I meant order them alphabetically as a rule of thumb, just to improve readability.
I know it's a weak ordering, but usually jonts that are close to one another (like torso_yaw, _pitch, _roll) maybe expected to be found next to each other in the final joint ordering.
Having :

     Joint 0: 'torso_roll'
     Joint 1: 'torso_pitch'    
     # Many more joints ....
    Joint 20: 'torso_yaw'

is very strange. You'd expect them to be 0-1-2 (or 18-19-20).

[traversaro]

Mhh, the problem is that pure alphabetic ordering will fail to work for chains (unless the name of the chain are joint_1, joint_2, etc. I need to think a bit about it.

[ahoarau]

Yes this remark is only valid for non-chains.

[ahoarau]

any news on this issue ?

[traversaro]

I try to get https://github.com/robotology/idyntree/pull/492 merged before the end of September, for release 0.12, that I need to use for an IIT-internal project.

[ahoarau]

Any news on this and 0.12 release ?

[traversaro]

Any news on this and 0.12 release ?

Thanks for the ping, we'll try to do a release by the end of November.

[traversaro]

Fixed by https://github.com/robotology/idyntree/pull/492 .