Fix problem related to floating joint in parser.

[fjandrad]

When using the floating type of joint in the URDF model iDyntree gives the following errors

[ERROR] JointElement :: setAttributes : Joint world_to_robot has type floating that is not currently supported by iDynTree.
[ERROR] RobotElement :: exitElementScope : Multiple (2) root links: (world, base_trans) found in URDF string 
[ERROR] XMLParser :: parserCallbackEndDocument : Document final callback failed processing
[ERROR] Model :: computeFullTreeTraversal : requested traversalBase is out of bounds
[ERROR] Model :: getJointIndex : jointName #jointName not found in the model.
[ERROR] SubModelDecomposition::splitModelAlongJoints error :  requested to split the model along joint #jointName but no joint with that is in the model.  
[ERROR] createReducedModel error :  error in splitting models across joints. 

from a previous issue :

That is one of the several blurry aspects of the URDF "specification" (if you can call that the wiki page URDF http://wiki.ros.org/urdf a "specification"). Basically, a "floating" joint is mentioned in the URDF specification (in http://wiki.ros.org/urdf/XML/joint), however the main tools that parse URDF files in ROS world, such as Gazebo or the KDL parser ignore floating joints. If you check humanoid or in any case "moving base" models released by OSRF in the past (such at Atlas one: https://bitbucket.org/osrf/drcsim/src/default/atlas_description/urdf/atlas.urdf or even the PR2 one: https://github.com/PR2/pr2_common/blob/6f96993793230c81737478729c4e324c3574424a/pr2_description/robots/pr2.urdf.xacro), none of those have a floating joint at all, but no world "link" is explicitly added, and so the base link of the URDF tree is considered to be a "floating base" that is implicitly connected with an inertial frame with a "floating" joint. This is handled even more naturally in iDynTree, were every iDynTree::Model is always considered to be "floating base", without the need to add an explicit "floating" joint. Note that the fatal error in the iDynTree's URDF parser is not the floating joint, that is simply ignored: the fatal error is that the parser finds that there are two root , in this case world and base_trans. To handle in a clean way this case in iDynTree, probably it could make sense if a floating joint is parser, to also ignore its parent link, as it is probably some kind of world frame that has no place in a iDynTree::Model, that only consider as links the actual bodies of the floating base model. This could be considered as additional hack in the iDynTree's URDF parser, but given the fuzzy nature of URDF format definition, having a "clean" URDF parser is basically an ill-posed problem.

cc @traversaro @GiulioRomualdi @lrapetti @DanielePucci

[fjandrad]

cc @CarlottaSartore

[traversaro]

The code to modify is in:

• https://github.com/robotology/idyntree/blob/fb68adf8d730969a072bd32f9f175cedc22fc397/src/model_io/urdf/src/JointElement.cpp#L142
• https://github.com/robotology/idyntree/blob/fb68adf8d730969a072bd32f9f175cedc22fc397/src/model_io/urdf/src/JointElement.cpp#L50
• https://github.com/robotology/idyntree/blob/fb68adf8d730969a072bd32f9f175cedc22fc397/src/model_io/urdf/src/URDFDocument.cpp#L178

It would make a lot of sense to ad an example of a model with a floating joint in https://github.com/robotology/idyntree/blob/fb68adf8d730969a072bd32f9f175cedc22fc397/src/tests/data/CMakeLists.txt#L16 , so that is automatically tested, for example in https://github.com/robotology/idyntree/blob/fb68adf8d730969a072bd32f9f175cedc22fc397/src/model_io/urdf/tests/URDFModelImportUnitTest.cpp#L227 and https://github.com/robotology/idyntree/blob/c8bf721b771fa4b1e7c3a940632e121060719a19/src/tests/integration/DenavitHartenbergIntegrationTest.cpp#L209 .