[dartsim] Fix joint construction errors due to link name duplication or BodyNodes moving to other skeletons

[azeey]

🦟 Bug fix

Summary

This started out as a fix for link name collision error when two links in different models have the same name. b54cd35. However, I also found that when creating joints between links in different models, DART actually moves the BodyNode associated with the child link of the joint from its skeleton to the skeleton of the parent BodyNode. After the BodyNode has been moved, SDFFeatures::FindBodyNode was unable to find the child link because it looked for it in the original skeleton.

For example,

<?xml version="1.0" ?>
<sdf version="1.8">
  <world name="nested_model_world">
    <model name="parent_model">

      <model name="nested_model1">
        <link name="nested_link1"/>
      </model>
      <model name="nested_model2">
        <link name="nested_link2"/>
      </model>
      <model name="nested_model3">
        <link name="nested_link3"/>
      </model>

      <joint name="joint1" type="revolute">
        <parent>nested_model1::nested_link1</parent>
        <child>nested_model2::nested_link2</child>
      </joint>
      <joint name="joint2" type="revolute">
        <parent>nested_model2::nested_link2</parent>
        <child>nested_model3::nested_link3</child>
      </joint>

    </model>
  </world>
</sdf>

When joint1 is created, the link parent_model::nested_model2::nested_link2 is moved from it's original skeleton, parent_model::nested_model2 to parent_model::nested_model1. When it's time to create joint2, it will look for it's parent link nested_link2 in the skeleton parent_model::nested_model2, but it will not be found.

This PR adds a new map that tracks BodyNodes by their fully qualified name. I encountered the errors when testing with ign-gazebo, so I have added an alternative LoadWorld function in SDFFeatures_TEST.cc that loads an SDFormat world in a piecemeal manner like ign-gazebo.

PS: A simpler test would be to load the example world above and check if all the joints have been created. However, since the joints themselves move to a new skeleton, we need more changes in the code for parentModel->GetJointCount() to return the correct number. I'll create a separate issue for that.

Checklist

• Signed all commits for DCO
• Added tests
• Updated documentation (as needed)
• Updated migration guide (as needed)
• codecheck passed (See contributing)
• All tests passed (See test coverage)
• While waiting for a review on your PR, please help review another open pull request to support the maintainers

Note to maintainers: Remember to use Squash-Merge

[azeey]

Thanks for the review!