Enforce meaningful initial joint ordering for chain models

[traversaro]

Using the new parser introduced in #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 #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 #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 #492 .