From 1260ef275d96ae4f9f360c80a8bf810fc7ca121f Mon Sep 17 00:00:00 2001 From: shameekganguly Date: Tue, 7 Nov 2023 19:34:34 -0800 Subject: [PATCH] bullet-featherstoneFix how links are flattened in ConstructSdfModel (#562) * Add test to show seg-fault with unsorted links Signed-off-by: Shameek Ganguly Signed-off-by: Steve Peters Signed-off-by: Ian Chen Co-authored-by: Steve Peters Co-authored-by: Ian Chen --- bullet-featherstone/src/SDFFeatures.cc | 87 +++++++++---------- test/common_test/world_features.cc | 75 ++++++++++++---- .../worlds/world_unsorted_links.sdf | 36 ++++++++ 3 files changed, 134 insertions(+), 64 deletions(-) create mode 100644 test/common_test/worlds/world_unsorted_links.sdf diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 5f63e5694..1ddce408d 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -135,9 +135,12 @@ std::optional 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> + 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,10 @@ std::optional 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) + { + linkTree[parent].push_back(child); + } } return true; @@ -231,11 +238,10 @@ std::optional ValidateModel(const ::sdf::Model &_sdfModel) return std::nullopt; } - std::vector flatLinks; - std::unordered_map 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 +260,35 @@ std::optional 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 +298,23 @@ std::optional 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 flatLinks; + std::function flattenLinkTree = + [&](const ::sdf::Link *link) { - // Every element in flatLinks should have a parent if the earlier validation - // was done correctly. - if (parentOf.size() == 0) + if (link != rootLink) { - break; + flatLinks.push_back(link); } - 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 (auto it = linkTree.find(link); it != linkTree.end()) { - // 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; - } - - auto &p_index = p_index_it->second; - if (i < p_index) - { - // 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(M.Mass()); diff --git a/test/common_test/world_features.cc b/test/common_test/world_features.cc index b9d7d0011..6c08ddb98 100644 --- a/test/common_test/world_features.cc +++ b/test/common_test/world_features.cc @@ -67,7 +67,7 @@ class WorldFeaturesTest: public: gz::plugin::Loader loader; }; -using GravityFeatures = gz::physics::FeatureList< +struct GravityFeatures : gz::physics::FeatureList< gz::physics::GetEngineInfo, gz::physics::Gravity, gz::physics::sdf::ConstructSdfWorld, @@ -75,15 +75,12 @@ using GravityFeatures = gz::physics::FeatureList< gz::physics::GetModelFromWorld, gz::physics::GetLinkFromModel, gz::physics::ForwardStep ->; +> { }; -using GravityFeaturesTestTypes = - ::testing::Types; -TYPED_TEST_SUITE(WorldFeaturesTest, - GravityFeatures,); +using WorldFeaturesTestGravity = WorldFeaturesTest; ///////////////////////////////////////////////// -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 +struct ConstructModelFeatures : gz::physics::FeatureList< + gz::physics::GetEngineInfo, + gz::physics::sdf::ConstructSdfWorld, + gz::physics::sdf::ConstructSdfModel, + gz::physics::GetModelFromWorld, + gz::physics::ForwardStep +> { }; + +using WorldFeaturesTestConstructModel = + WorldFeaturesTest; + +///////////////////////////////////////////////// +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::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.sdf")); + EXPECT_TRUE(errors.empty()) << errors; + const sdf::World *sdfWorld = root.WorldByIndex(0); + ASSERT_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(*sdfWorld); + EXPECT_NE(nullptr, world); + } +} + +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 +> { }; class WorldModelTest : public WorldFeaturesTest { diff --git a/test/common_test/worlds/world_unsorted_links.sdf b/test/common_test/worlds/world_unsorted_links.sdf new file mode 100644 index 000000000..fb2cd9c49 --- /dev/null +++ b/test/common_test/worlds/world_unsorted_links.sdf @@ -0,0 +1,36 @@ + + + + + + + + + + world + parent_link + + + parent_link + child_linkA + + 0 0 1 + + + + child_linkA + child_linkB + + 0 0 1 + + + + child_linkB + child_linkC + + 0 0 1 + + + + +