bullet-featherstone: fix how links are flattened in ConstructSdfModel

bullet-featherstone/src/SDFFeatures.cc

@@ -135,9 +135,12 @@ std::optional<Structure> ValidateModel(const ::sdf::Model &_sdfModel)
   const ::sdf::Joint *rootJoint = nullptr;
   bool fixed = false;
   const std::string &rootModelName = _sdfModel.Name();
+  // a map of parent link to a vector of its child links
+  std::unordered_map<const ::sdf::Link*, std::vector<const ::sdf::Link*>>
+    linkTree;
 
   const auto checkModel =
-      [&rootLink, &rootJoint, &fixed, &parentOf, &rootModelName](
+      [&rootLink, &rootJoint, &fixed, &parentOf, &rootModelName, &linkTree](
       const ::sdf::Model &model) -> bool
     {
       for (std::size_t i = 0; i < model.JointCount(); ++i)
@@ -217,6 +220,17 @@ std::optional<Structure> ValidateModel(const ::sdf::Model &_sdfModel)
                 << rootModelName << "] has multiple parent joints. That is not "
                 << "supported by the gz-physics-bullet-featherstone plugin.\n";
         }
+        if (parent != nullptr)
+        {
+          if (linkTree.count(parent) == 0)
+          {
+            linkTree[parent] = {child};
+          }
+          else
+          {
+            linkTree[parent].push_back(child);
+          }
+        }
       }
 
       return true;
@@ -231,11 +245,10 @@ std::optional<Structure> ValidateModel(const ::sdf::Model &_sdfModel)
       return std::nullopt;
   }
 
-  std::vector<const ::sdf::Link*> flatLinks;
-  std::unordered_map<const ::sdf::Link*, std::size_t> linkIndex;
-  const auto flattenLinks =
-      [&rootLink, &parentOf, &rootModelName, &flatLinks, &linkIndex](
-      const ::sdf::Model &model) -> bool
+  // Find root link in model and verify that there is only one root link in
+  // the model. Returns false if more than one root link is found
+  const auto findRootLink =
+      [&rootLink, &parentOf, &rootModelName](const ::sdf::Model &model) -> bool
     {
       for (std::size_t i = 0; i < model.LinkCount(); ++i)
       {
@@ -254,23 +267,35 @@ std::optional<Structure> ValidateModel(const ::sdf::Model &_sdfModel)
           }
 
           rootLink = link;
-          continue;
         }
-
-        linkIndex[link] = linkIndex.size();
-        flatLinks.push_back(link);
       }
 
       return true;
     };
 
-  if (!flattenLinks(_sdfModel))
+  if (rootLink == nullptr && !findRootLink(_sdfModel))
+  {
+    // No root link was found in this model
     return std::nullopt;
+  }
 
+  // find root link in nested models if one was not already found
   for (std::size_t i = 0; i < _sdfModel.ModelCount(); ++i)
   {
-    if (!flattenLinks(*_sdfModel.ModelByIndex(i)))
+    if (rootLink != nullptr)
+    {
+      break;
+    }
+    if (!findRootLink(*_sdfModel.ModelByIndex(i)))
+    {
       return std::nullopt;
+    }
+  }
+
+  if (!rootLink)
+  {
+    gzerr << "No root link was found for model [" << _sdfModel.Name() << "\n";
+    return std::nullopt;
   }
 
   // The documentation for bullet does not mention whether parent links must
@@ -280,44 +305,23 @@ std::optional<Structure> ValidateModel(const ::sdf::Model &_sdfModel)
   // come before their children in the array, and do not work correctly if that
   // ordering is not held. Out of an abundance of caution we will assume that
   // ordering is necessary.
-  for (std::size_t i = 0; i < flatLinks.size(); ++i)
+  std::vector<const ::sdf::Link*> flatLinks;
+  std::function<void(const ::sdf::Link *)> flattenLinkTree =
+      [&](const ::sdf::Link *link)
   {
-    // Every element in flatLinks should have a parent if the earlier validation
-    // was done correctly.
-    if (parentOf.size() == 0)
-    {
-      break;
-    }
-    const auto *parentJoint = parentOf.at(flatLinks[i]).joint;
-
-    const auto *parentLink =
-      _sdfModel.LinkByName(parentJoint->ParentName());
-    const auto p_index_it = linkIndex.find(parentLink);
-    if (p_index_it == linkIndex.end())
+    if (link != rootLink)
     {
-      // If the parent index cannot be found, that must mean the parent is the
-      // root link, so this link can go anywhere in the list as long as it is
-      // before its own children.
-      assert(parentLink == rootLink);
-      continue;
+      flatLinks.push_back(link);
     }
-
-    auto &p_index = p_index_it->second;
-    if (i < p_index)
+    if (auto it = linkTree.find(link); it != linkTree.end())
     {
-      // The current link is in front of its parent link in the array. We must
-      // swap their places.
-      std::swap(flatLinks[i], flatLinks[p_index]);
-      p_index = i;
-      linkIndex[flatLinks[p_index]] = p_index;
+      for (const auto &child_link : it->second)
+      {
+        flattenLinkTree(child_link);
+      }
     }
-  }
-
-  if (!rootLink)
-  {
-    gzerr << "No root link was found for model [" << _sdfModel.Name() << "\n";
-    return std::nullopt;
-  }
+  };
+  flattenLinkTree(rootLink);
 
   const auto &M = rootLink->Inertial().MassMatrix();
   const auto mass = static_cast<btScalar>(M.Mass());

test/common_test/world_features.cc

@@ -77,13 +77,10 @@ using GravityFeatures = gz::physics::FeatureList<
   gz::physics::ForwardStep
 >;
 
-using GravityFeaturesTestTypes =
-  ::testing::Types<GravityFeatures>;
-TYPED_TEST_SUITE(WorldFeaturesTest,
-                 GravityFeatures,);
+using WorldFeaturesTestGravity = WorldFeaturesTest<GravityFeatures>;
 
 /////////////////////////////////////////////////
-TYPED_TEST(WorldFeaturesTest, GravityFeatures)
+TEST_F(WorldFeaturesTestGravity, GravityFeatures)
 {
   for (const std::string &name : this->pluginNames)
   {
@@ -182,19 +179,59 @@ TYPED_TEST(WorldFeaturesTest, GravityFeatures)
   }
 }
 
-struct WorldModelFeatureList
-    : gz::physics::FeatureList<GravityFeatures, gz::physics::WorldModelFeature,
-                               gz::physics::RemoveEntities,
-                               gz::physics::GetNestedModelFromModel,
-                               gz::physics::sdf::ConstructSdfLink,
-                               gz::physics::sdf::ConstructSdfJoint,
-                               gz::physics::sdf::ConstructSdfModel,
-                               gz::physics::sdf::ConstructSdfNestedModel,
-                               gz::physics::ConstructEmptyLinkFeature,
-                               gz::physics::ConstructEmptyNestedModelFeature
-                               >
+using ConstructModelFeatures = gz::physics::FeatureList<
+  gz::physics::GetEngineInfo,
+  gz::physics::sdf::ConstructSdfWorld,
+  gz::physics::sdf::ConstructSdfModel,
+  gz::physics::GetModelFromWorld,
+  gz::physics::ForwardStep
+>;
+
+using WorldFeaturesTestConstructModel =
+  WorldFeaturesTest<ConstructModelFeatures>;
+
+/////////////////////////////////////////////////
+TEST_F(WorldFeaturesTestConstructModel, ConstructModelUnsortedLinks)
 {
-};
+  for (const std::string &name : this->pluginNames)
+  {
+    std::cout << "Testing plugin: " << name << std::endl;
+    gz::plugin::PluginPtr plugin = this->loader.Instantiate(name);
+
+    auto engine =
+      gz::physics::RequestEngine3d<ConstructModelFeatures>::From(plugin);
+    ASSERT_NE(nullptr, engine);
+    EXPECT_TRUE(engine->GetName().find(this->PhysicsEngineName(name)) !=
+                std::string::npos);
+
+    sdf::Root root;
+    const sdf::Errors errors = root.Load(
+      gz::common::joinPaths(TEST_WORLD_DIR, "world_unsorted_links.world"));
+    EXPECT_TRUE(errors.empty()) << errors;
+    const sdf::World *sdfWorld = root.WorldByIndex(0);
+    EXPECT_NE(nullptr, sdfWorld);
+
+    // Per https://github.com/gazebosim/gz-physics/pull/562, there is a
+    // seg-fault in the bullet-featherstone implementation of ConstructSdfModel
+    // (called by ConstructSdfWorld). So loading the world successfully
+    // is enough for this test.
+    auto world = engine->ConstructWorld(*root.WorldByIndex(0));
+    EXPECT_NE(nullptr, world);
+  }
+}
+
+using WorldModelFeatureList = gz::physics::FeatureList<
+  GravityFeatures,
+  gz::physics::WorldModelFeature,
+  gz::physics::RemoveEntities,
+  gz::physics::GetNestedModelFromModel,
+  gz::physics::sdf::ConstructSdfLink,
+  gz::physics::sdf::ConstructSdfJoint,
+  gz::physics::sdf::ConstructSdfModel,
+  gz::physics::sdf::ConstructSdfNestedModel,
+  gz::physics::ConstructEmptyLinkFeature,
+  gz::physics::ConstructEmptyNestedModelFeature
+>;
 
 class WorldModelTest : public WorldFeaturesTest<WorldModelFeatureList>
 {

test/common_test/worlds/world_unsorted_links.world

@@ -0,0 +1,40 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <world name="default">
+    <plugin
+      filename="gz-sim-physics-system"
+      name="gz::sim::systems::v0::Physics">
+    </plugin>
+    <model name="unsorted_links">
+      <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>
+  </world>
+</sdf>