bullet-featherstone: fix how links are flattened in ConstructSdfModel

[shameekganguly]

Fixes a segfault in ConstructSdfModel when loading a model with links defined out of order. Here's an example model that triggers the segfault:

  <model name="test_model">
      <link name="child_linkC" />
      <link name="parent_link" />
      <link name="child_linkA" />
      <link name="child_linkB" />
      <joint name="world_joint" type="fixed">
        <parent>world</parent>
        <child>parent_link</child>
      </joint>
      <joint name="joint1" type="prismatic">
        <parent>parent_link</parent>
        <child>child_linkA</child>
        <axis>
            <xyz>0 0 1</xyz>
        </axis>
      </joint>
      <joint name="joint2" type="prismatic">
        <parent>child_linkA</parent>
        <child>child_linkB</child>
        <axis>
            <xyz>0 0 1</xyz>
        </axis>
      </joint>
      <joint name="joint3" type="prismatic">
        <parent>child_linkB</parent>
        <child>child_linkC</child>
        <axis>
            <xyz>0 0 1</xyz>
        </axis>
      </joint>
    </model>

The current logic for sorting links to ensure that a parent link is placed in the flatLinks vector before its children links is inadequate for such cases. So the fix is to build the link tree and do a preorder traversal of it to create the flatLinks vector.

[codecov]

Codecov Report

Merging #562 (1419fe2) into gz-physics6 (3b7a7c8) will increase coverage by 1.07%.
The diff coverage is 76.19%.

❗ Current head 1419fe2 differs from pull request most recent head ef0ef82. Consider uploading reports for the commit ef0ef82 to get more accurate results

@@               Coverage Diff               @@
##           gz-physics6     #562      +/-   ##
===============================================
+ Coverage        75.95%   77.02%   +1.07%     
===============================================
  Files              143      143              
  Lines             7327     7321       -6     
===============================================
+ Hits              5565     5639      +74     
+ Misses            1762     1682      -80     

Files	Coverage Δ
bullet-featherstone/src/SDFFeatures.cc	74.15% <76.19%> (+0.09%)	⬆️

... and 2 files with indirect coverage changes

[scpeters]

we should add a test that uses the snippet you provided. I'll look into the easiest way to do that

[scpeters]

we should add a test that uses the snippet you provided. I'll look into the easiest way to do that

I added a test in 49de232 without this branch, and it fails CI. I will push it to this branch

[scpeters]

@azeey I added a small test for this in case you want to review

[scpeters]

LGTM, though I think the test I added deserves a second look from someone else if possible

[azeey]

LGTM! Just a few minor comments.

[shameekganguly]

Thanks Steve, Ian, Addisu for taking care of this!