From 17cb3cdef389a2c865fda92c65691eb86060cfb5 Mon Sep 17 00:00:00 2001 From: Shameek Ganguly Date: Tue, 17 Oct 2023 00:13:08 -0700 Subject: [PATCH 01/29] Fix how links are flattened in bullet featherstone ConstructSdfModel Signed-off-by: Shameek Ganguly --- bullet-featherstone/src/SDFFeatures.cc | 84 +++++++++++--------------- 1 file changed, 36 insertions(+), 48 deletions(-) diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 5f63e5694..76d079a95 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -135,9 +135,11 @@ std::optional ValidateModel(const ::sdf::Model &_sdfModel) const ::sdf::Joint *rootJoint = nullptr; bool fixed = false; const std::string &rootModelName = _sdfModel.Name(); + 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 +219,13 @@ 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) { + if (linkTree.count(parent) == 0) { + linkTree[parent] = {child}; + } else { + linkTree[parent].push_back(child); + } + } } return true; @@ -231,11 +240,8 @@ 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 + const auto findRootLink = + [&rootLink, &parentOf, &rootModelName](const ::sdf::Model &model) -> bool { for (std::size_t i = 0; i < model.LinkCount(); ++i) { @@ -254,23 +260,30 @@ 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)) { return std::nullopt; + } 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 +293,19 @@ 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) - { - // 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 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; + std::vector flatLinks; + std::function flattenLinkTree = + [&](const ::sdf::Link *link) { + if (link != rootLink) { + flatLinks.push_back(link); } - - 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; + if (auto it = linkTree.find(link); it != linkTree.end()) { + 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()); From 4e7305671f113a9873be6fe2c1d6b9210313c934 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 2 Nov 2023 12:56:11 -0700 Subject: [PATCH 02/29] common_test/world_features: don't use TYPED_TEST Minor refactor based on #493. Signed-off-by: Steve Peters --- test/common_test/world_features.cc | 32 +++++++++++++----------------- 1 file changed, 14 insertions(+), 18 deletions(-) diff --git a/test/common_test/world_features.cc b/test/common_test/world_features.cc index b9d7d0011..b4d3b784d 100644 --- a/test/common_test/world_features.cc +++ b/test/common_test/world_features.cc @@ -77,13 +77,10 @@ using GravityFeatures = gz::physics::FeatureList< 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,18 @@ TYPED_TEST(WorldFeaturesTest, GravityFeatures) } } -struct WorldModelFeatureList - : gz::physics::FeatureList -{ -}; +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 { From 49de232c10afa76541b03221bd14778cfb29a9d6 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 2 Nov 2023 12:57:12 -0700 Subject: [PATCH 03/29] Add test to show seg-fault with unsorted links Test for #562. Signed-off-by: Steve Peters --- test/common_test/world_features.cc | 41 +++++++++++++++++++ .../worlds/world_unsorted_links.world | 40 ++++++++++++++++++ 2 files changed, 81 insertions(+) create mode 100644 test/common_test/worlds/world_unsorted_links.world diff --git a/test/common_test/world_features.cc b/test/common_test/world_features.cc index b4d3b784d..e9aa3436a 100644 --- a/test/common_test/world_features.cc +++ b/test/common_test/world_features.cc @@ -179,6 +179,47 @@ TEST_F(WorldFeaturesTestGravity, GravityFeatures) } } +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; + +///////////////////////////////////////////////// +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.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, diff --git a/test/common_test/worlds/world_unsorted_links.world b/test/common_test/worlds/world_unsorted_links.world new file mode 100644 index 000000000..667b62559 --- /dev/null +++ b/test/common_test/worlds/world_unsorted_links.world @@ -0,0 +1,40 @@ + + + + + + + + + + + + world + parent_link + + + parent_link + child_linkA + + 0 0 1 + + + + child_linkA + child_linkB + + 0 0 1 + + + + child_linkB + child_linkC + + 0 0 1 + + + + + From e689f8b9513c7aa3e94eea73b2979f883699cf3d Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 6 Nov 2023 23:19:06 +0000 Subject: [PATCH 04/29] add comments and fix style Signed-off-by: Ian Chen --- bullet-featherstone/src/SDFFeatures.cc | 36 +++++++++++++++++++------- 1 file changed, 26 insertions(+), 10 deletions(-) diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 76d079a95..a75e35927 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -135,6 +135,7 @@ 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; @@ -219,10 +220,14 @@ 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) { - if (linkTree.count(parent) == 0) { + if (parent != nullptr) + { + if (linkTree.count(parent) == 0) + { linkTree[parent] = {child}; - } else { + } + else + { linkTree[parent].push_back(child); } } @@ -240,6 +245,8 @@ std::optional ValidateModel(const ::sdf::Model &_sdfModel) return std::nullopt; } + // 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 { @@ -266,16 +273,21 @@ std::optional ValidateModel(const ::sdf::Model &_sdfModel) return true; }; - if (rootLink == nullptr && !findRootLink(_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 (rootLink != nullptr) { + if (rootLink != nullptr) + { break; } - if (!findRootLink(*_sdfModel.ModelByIndex(i))) { + if (!findRootLink(*_sdfModel.ModelByIndex(i))) + { return std::nullopt; } } @@ -295,12 +307,16 @@ std::optional ValidateModel(const ::sdf::Model &_sdfModel) // ordering is necessary. std::vector flatLinks; std::function flattenLinkTree = - [&](const ::sdf::Link *link) { - if (link != rootLink) { + [&](const ::sdf::Link *link) + { + if (link != rootLink) + { flatLinks.push_back(link); } - if (auto it = linkTree.find(link); it != linkTree.end()) { - for (const auto &child_link : it->second) { + if (auto it = linkTree.find(link); it != linkTree.end()) + { + for (const auto &child_link : it->second) + { flattenLinkTree(child_link); } } From 0a88d1d415fdb92710451d2f10b60b65f62f4104 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 8 Nov 2023 01:48:17 +0000 Subject: [PATCH 05/29] wip Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.hh | 21 +++++ .../src/EntityManagementFeatures.cc | 43 +++++++++++ .../src/EntityManagementFeatures.hh | 11 +++ bullet-featherstone/src/SDFFeatures.cc | 77 ++++++++++++++++++- bullet-featherstone/src/SDFFeatures.hh | 13 ++++ 5 files changed, 164 insertions(+), 1 deletion(-) diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index 184d958c2..25f878575 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -88,8 +88,10 @@ struct ModelInfo std::vector linkEntityIds; std::vector jointEntityIds; + std::vector nestedModelEntityIds; std::unordered_map linkNameToEntityId; std::unordered_map jointNameToEntityId; + std::unordered_map nestedModelNameToEntityId; /// These are joints that connect this model to other models, e.g. fixed /// constraints. @@ -295,6 +297,25 @@ class Base : public Implements3d> return this->GenerateIdentity(id, model); } + public: inline Identity AddNestedModel( + std::string _name, + Identity _parentID, + Identity _worldID, + Eigen::Isometry3d _baseInertialToLinkFrame, + std::unique_ptr _body) + { + const auto id = this->GetNextEntity(); + auto model = std::make_shared( + std::move(_name), std::move(_worldID), + std::move(_baseInertialToLinkFrame), std::move(_body)); + + this->models[id] = model; + const auto parentModel = this->models.at(_parentID); + parentModel->nestedModelEntityIds.push_back(id); + parentModel->nestedModelNameToEntityId[model->name] = id; + return this->GenerateIdentity(id, model); + } + public: inline Identity AddLink(LinkInfo _linkInfo) { const auto id = this->GetNextEntity(); diff --git a/bullet-featherstone/src/EntityManagementFeatures.cc b/bullet-featherstone/src/EntityManagementFeatures.cc index 3a491862a..5b8ca62e2 100644 --- a/bullet-featherstone/src/EntityManagementFeatures.cc +++ b/bullet-featherstone/src/EntityManagementFeatures.cc @@ -414,6 +414,49 @@ Identity EntityManagementFeatures::GetWorldOfModel( return this->ReferenceInterface(_modelID)->world; } +///////////////////////////////////////////////// +std::size_t EntityManagementFeatures::GetNestedModelCount( + const Identity &_modelID) const +{ + return this->ReferenceInterface( + _modelID)->nestedModelEntityIds.size(); +} + +///////////////////////////////////////////////// +Identity EntityManagementFeatures::GetNestedModel( + const Identity &_modelID, const std::size_t _modelIndex) const +{ + const auto modelInfo = this->ReferenceInterface(_modelID); + if (_modelIndex >= modelInfo->nestedModelEntityIds.size()) + { + return this->GenerateInvalidId(); + } + + const auto nestedModelID = modelInfo->nestedModelEntityIds[_modelIndex]; + + // If the model doesn't exist in "models", it means the containing entity has + // been removed. + if (this->models.find(nestedModelID) != this->models.end()) + { + return this->GenerateIdentity(nestedModelID, + this->models.at(nestedModelID)); + } + else + { + return this->GenerateInvalidId(); + } +} + +///////////////////////////////////////////////// +Identity EntityManagementFeatures::GetNestedModel( + const Identity &_modelID, const std::string &_modelName) const +{ + const auto modelInfo = this->ReferenceInterface(_modelID); + auto nestedModelID = modelInfo->nestedModelNameToEntityId.at(_modelName); + return this->GenerateIdentity(nestedModelID, + this->models.at(nestedModelID)); +} + } // namespace bullet_featherstone } // namespace physics } // namespace gz diff --git a/bullet-featherstone/src/EntityManagementFeatures.hh b/bullet-featherstone/src/EntityManagementFeatures.hh index 09d45bdba..81f87f0e4 100644 --- a/bullet-featherstone/src/EntityManagementFeatures.hh +++ b/bullet-featherstone/src/EntityManagementFeatures.hh @@ -37,6 +37,7 @@ struct EntityManagementFeatureList : gz::physics::FeatureList< GetJointFromModel, GetLinkFromModel, GetModelFromWorld, + GetNestedModelFromModel, GetShapeFromLink, GetWorldFromEngine, RemoveModelFromWorld @@ -158,6 +159,16 @@ class EntityManagementFeatures : // ----- Construct empty entities ----- public: Identity ConstructEmptyWorld( const Identity &_engineID, const std::string & _name) override; + + // ----- GetNestedModelFromModel ----- + public: std::size_t GetNestedModelCount( + const Identity &_modelID) const override; + + public: Identity GetNestedModel( + const Identity &_modelID, std::size_t _modelIndex) const override; + + public: Identity GetNestedModel( + const Identity &_modelID, const std::string &_modelName) const override; }; } // namespace bullet_featherstone diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index a75e35927..5e43111a3 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -125,6 +125,8 @@ struct Structure /// This contains all the links except the root link std::vector flatLinks; + + std::vector nestedStructure; }; ///////////////////////////////////////////////// @@ -342,6 +344,80 @@ std::optional ValidateModel(const ::sdf::Model &_sdfModel) rootLink, rootJoint, mass, inertia, fixed, parentOf, flatLinks}; } +///////////////////////////////////////////////// +Identity SDFFeatures::ConstructSdfNestedModel(const Identity &_parentID, + const ::sdf::Model &_sdfModel) +{ + return this->ConstructSdfModelImpl(_parentID, _sdfModel); +} + +///////////////////////////////////////////////// +Identity SDFFeatures::ConstructSdfModelImpl( + std::size_t _parentID, + const ::sdf::Model &_sdfModel) +{ + const auto validation = ValidateModel(_sdfModel); + if (!validation.has_value()) + return this->GenerateInvalidId(); + + const auto &structure = *validation; + const bool isStatic = _sdfModel.Static(); + + // Identity worldIdentity; + auto parentModelIt = this->models.find(_parentID); + const bool isNested = (parentModelIt != this->models.end()); + + // auto worldID = _parentID; + // WorldInfo *world = nullptr; + const auto &worldIdentity = isNested ? + parentModelIt->second->world : + this->GenerateIdentity(_parentID, this->worlds[_parentID]); +/* else + { + const WorldInfoPtr &worldInfo - this->worlds.at(_parentID); + } +*/ + // const auto *world = this->ReferenceInterface(worldIdentity); + // auto worldIdentity = this->GenerateIdentity(worldID, this->worlds[worldID]); + + const auto rootInertialToLink = + gz::math::eigen3::convert(structure.rootLink->Inertial().Pose()).inverse(); + + auto modelID = [&] { + if (isNested) + { + auto modelIdentity = this->GenerateIdentity(_parentID, this->models[_parentID]); + return this->AddNestedModel( + _sdfModel.Name(), modelIdentity, worldIdentity, rootInertialToLink, + std::make_unique( + static_cast(structure.flatLinks.size()), + structure.mass, + structure.inertia, + structure.fixedBase || isStatic, + true)); + } + else + { + return this->AddModel( + _sdfModel.Name(), worldIdentity, rootInertialToLink, + std::make_unique( + static_cast(structure.flatLinks.size()), + structure.mass, + structure.inertia, + structure.fixedBase || isStatic, + true)); + } + }(); + + const auto rootID = + this->AddLink(LinkInfo{ + structure.rootLink->Name(), std::nullopt, modelID, rootInertialToLink + }); + const auto *model = this->ReferenceInterface(modelID); + + return modelID; +} + ///////////////////////////////////////////////// Identity SDFFeatures::ConstructSdfModel( const Identity &_worldID, @@ -599,7 +675,6 @@ Identity SDFFeatures::ConstructSdfModel( } } - model->body->setHasSelfCollision(_sdfModel.SelfCollide()); model->body->finalizeMultiDof(); diff --git a/bullet-featherstone/src/SDFFeatures.hh b/bullet-featherstone/src/SDFFeatures.hh index 2285c1a4b..22dd01288 100644 --- a/bullet-featherstone/src/SDFFeatures.hh +++ b/bullet-featherstone/src/SDFFeatures.hh @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -35,6 +36,7 @@ namespace bullet_featherstone { struct SDFFeatureList : gz::physics::FeatureList< sdf::ConstructSdfModel, + sdf::ConstructSdfNestedModel, sdf::ConstructSdfWorld, sdf::ConstructSdfCollision > { }; @@ -51,6 +53,10 @@ class SDFFeatures : const Identity &_worldID, const ::sdf::Model &_sdfModel) override; + public: Identity ConstructSdfNestedModel( + const Identity &_parentID, + const ::sdf::Model &_sdfModel) override; + public: bool AddSdfCollision( const Identity &_linkID, const ::sdf::Collision &_collision, @@ -59,6 +65,13 @@ class SDFFeatures : private: Identity ConstructSdfCollision( const Identity &_linkID, const ::sdf::Collision &_collision) override; + + /// \brief Construct a bullet entity from a sdf::model + /// \param[in] _parentID Parent identity + /// \param[in] _sdfModel sdf::Model to construct entity from + /// \return The entity identity if constructed otherwise an invalid identity + private: Identity ConstructSdfModelImpl(std::size_t _parentID, + const ::sdf::Model &_sdfModel); }; } // namespace bullet_featherstone From ef0ef821a75253f96b54eb1375e3f67eb1b9692b Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 8 Nov 2023 01:59:37 +0000 Subject: [PATCH 06/29] change to use struct, rename world file to sdf, code cleanup Signed-off-by: Ian Chen --- bullet-featherstone/src/SDFFeatures.cc | 9 +-------- test/common_test/world_features.cc | 18 +++++++++--------- ...ed_links.world => world_unsorted_links.sdf} | 4 ---- 3 files changed, 10 insertions(+), 21 deletions(-) rename test/common_test/worlds/{world_unsorted_links.world => world_unsorted_links.sdf} (89%) diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index a75e35927..1ddce408d 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -222,14 +222,7 @@ std::optional ValidateModel(const ::sdf::Model &_sdfModel) } if (parent != nullptr) { - if (linkTree.count(parent) == 0) - { - linkTree[parent] = {child}; - } - else - { - linkTree[parent].push_back(child); - } + linkTree[parent].push_back(child); } } diff --git a/test/common_test/world_features.cc b/test/common_test/world_features.cc index e9aa3436a..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,7 +75,7 @@ using GravityFeatures = gz::physics::FeatureList< gz::physics::GetModelFromWorld, gz::physics::GetLinkFromModel, gz::physics::ForwardStep ->; +> { }; using WorldFeaturesTestGravity = WorldFeaturesTest; @@ -179,13 +179,13 @@ TEST_F(WorldFeaturesTestGravity, GravityFeatures) } } -using ConstructModelFeatures = 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; @@ -206,21 +206,21 @@ TEST_F(WorldFeaturesTestConstructModel, ConstructModelUnsortedLinks) sdf::Root root; const sdf::Errors errors = root.Load( - gz::common::joinPaths(TEST_WORLD_DIR, "world_unsorted_links.world")); + gz::common::joinPaths(TEST_WORLD_DIR, "world_unsorted_links.sdf")); EXPECT_TRUE(errors.empty()) << errors; const sdf::World *sdfWorld = root.WorldByIndex(0); - EXPECT_NE(nullptr, sdfWorld); + 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(*root.WorldByIndex(0)); + auto world = engine->ConstructWorld(*sdfWorld); EXPECT_NE(nullptr, world); } } -using WorldModelFeatureList = gz::physics::FeatureList< +struct WorldModelFeatureList : gz::physics::FeatureList< GravityFeatures, gz::physics::WorldModelFeature, gz::physics::RemoveEntities, @@ -231,7 +231,7 @@ using WorldModelFeatureList = gz::physics::FeatureList< gz::physics::sdf::ConstructSdfNestedModel, gz::physics::ConstructEmptyLinkFeature, gz::physics::ConstructEmptyNestedModelFeature ->; +> { }; class WorldModelTest : public WorldFeaturesTest { diff --git a/test/common_test/worlds/world_unsorted_links.world b/test/common_test/worlds/world_unsorted_links.sdf similarity index 89% rename from test/common_test/worlds/world_unsorted_links.world rename to test/common_test/worlds/world_unsorted_links.sdf index 667b62559..fb2cd9c49 100644 --- a/test/common_test/worlds/world_unsorted_links.world +++ b/test/common_test/worlds/world_unsorted_links.sdf @@ -1,10 +1,6 @@ - - From 811bb1381f935343c5c0ac69e4f62a7af48d645a Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 10 Nov 2023 02:09:30 +0000 Subject: [PATCH 07/29] working Signed-off-by: Ian Chen --- bullet-featherstone/src/SDFFeatures.cc | 797 +++++++++++++++++++++++-- dartsim/src/SDFFeatures.cc | 14 + 2 files changed, 763 insertions(+), 48 deletions(-) diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 8d337a9c7..fd8eb9cfd 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -77,6 +77,121 @@ static std::optional ResolveSdfPose( return math::eigen3::convert(pose); } +///////////////////////////////////////////////// +::sdf::Errors LinkPoseInModelTree(math::Pose3d &_pose, + const std::string &_linkName, + const ::sdf::Model *_model) +{ + ::sdf::Errors errors; + size_t idx = _linkName.find("::"); + if (idx != std::string::npos) + { + if (_model->ModelCount() > 0) + { + std::string nestedModelName = _linkName.substr(0, idx); + std::string nestedLinkName = _linkName.substr(idx + 2); + const auto *nestedModel = _model->ModelByName(nestedModelName); + if (!nestedModel) + { + gzerr << "Unable to find nested model " << nestedModelName << std::endl; + return errors; + } + + math::Pose3d p; + errors = nestedModel->SemanticPose().Resolve(p); + if (!errors.empty()) + return errors; + _pose = _pose * p; + return LinkPoseInModelTree(_pose, nestedLinkName, nestedModel); + } + } + else + { + const auto *link = _model->LinkByName(_linkName); + if (!link) + { + gzerr << "Unable to find link " << _linkName << std::endl; + return errors; + } + math::Pose3d p; + errors = link->SemanticPose().Resolve(p); + if (!errors.empty()) + return errors; + + _pose = _pose * p; + } + return errors; +} + +///////////////////////////////////////////////// +/// \brief Get pose of joint relative to link +/// \param[out] _resolvePose Pose of joint relative to link +/// \param[in] _model Parent model of joint +/// \param[in] _joint Joint name +/// \param[in] _link Scoped link name +::sdf::Errors ResolveJointPoseRelToLink(math::Pose3d &_resolvedPose, + const ::sdf::Model *_model, + const std::string &_joint, const std::string &_link) +{ + ::sdf::Errors errors; + const auto *joint = _model->JointByName(_joint); + if (!joint) + { + gzerr << "No joint [" << _joint << "] found in model [" + << _model->Name() << "]" << std::endl; + return errors; + } + + math::Pose3d jointPose; + errors = joint->SemanticPose().Resolve(jointPose); + if (!errors.empty()) + return errors; + + // joint pose is expressed relative to child link + if (joint->ChildName() == _link) + { + _resolvedPose = jointPose; + return errors; + } + + // pose of parent link + const auto *link = _model->LinkByName(_link); + if (!link) + { + gzerr << "No link [" << _link << "] found in model [" + << _model->Name() << "]" << std::endl; + return errors; + } + + math::Pose3d parentLinkPose; + errors = LinkPoseInModelTree(parentLinkPose, _link, _model); + std::cerr << " === parent link pose in model tree " << _link << " " + << parentLinkPose + << std::endl; + if (!errors.empty()) + return errors; + + // pose of child link + math::Pose3d childLinkPose; + errors = LinkPoseInModelTree(childLinkPose, joint->ChildName(), _model); + std::cerr << " === child link pose in model tree " << joint->ChildName() << " " + << childLinkPose + << std::endl; + if (!errors.empty()) + return errors; + + auto jointPoseInModel = childLinkPose * jointPose; + + std::cerr << "joint erros " << errors.size() << std::endl; + // std::cerr << "link pose " << linkPose << std::endl; + std::cerr << "joint pose " << jointPose << std::endl; + _resolvedPose = parentLinkPose.Inverse() * jointPoseInModel; + // _resolvedPose = jointPose.Inverse() * linkPose; + std::cerr << "out pose " << _resolvedPose << std::endl; + + return errors; +} + ///////////////////////////////////////////////// Identity SDFFeatures::ConstructSdfWorld( const Identity &_engine, @@ -106,6 +221,8 @@ struct ParentInfo { const ::sdf::Joint *joint; const ::sdf::Model *model; + const ::sdf::Link *link; + const ::sdf::Link *parent; }; ///////////////////////////////////////////////// @@ -113,6 +230,7 @@ struct Structure { /// The root link of the model const ::sdf::Link *rootLink; + const ::sdf::Model *model; const ::sdf::Joint *rootJoint; btScalar mass; btVector3 inertia; @@ -125,10 +243,252 @@ struct Structure /// This contains all the links except the root link std::vector flatLinks; - - std::vector nestedStructure; }; + +///////////////////////////////////////////////// +bool BuildTrees(const ::sdf::Model *_sdfModel, + // std::unordered_map &_nodes, + std::unordered_map &_parentOf, + std::unordered_map> &_linkTree) +{ + for (std::size_t i = 0; i < _sdfModel->JointCount(); ++i) + { + const auto *joint = _sdfModel->JointByIndex(i); + const auto &parentLinkName = joint->ParentName(); + const auto &childLinkName = joint->ChildName(); + const std::string &modelName = _sdfModel->Name(); +// const auto *parent = FindLinkInModelTree(parentLinkName, _sdfModel); +// const auto *child = FindLinkInModelTree(childLinkName, _sdfModel); + const auto *parent = _sdfModel->LinkByName(parentLinkName); + const auto *child = _sdfModel->LinkByName(childLinkName); + + std::cerr << "parent link name " << parentLinkName << " " << parent << std::endl; + std::cerr << "child link name " << childLinkName << " " << child << std::endl; + + switch (joint->Type()) + { + case ::sdf::JointType::FIXED: + case ::sdf::JointType::REVOLUTE: + case ::sdf::JointType::PRISMATIC: + case ::sdf::JointType::BALL: + break; + default: + gzerr << "Joint type [" << (std::size_t)(joint->Type()) + << "] is not supported by " + << "gz-physics-bullet-featherstone-plugin. " + << "Replaced by a fixed joint.\n"; + } + if (child == parent) + { + gzerr << "The Link [" << parentLinkName << "] is being attached to " + << "itself by Joint [" << joint->Name() << "] in Model [" + << modelName << "]. That is not allowed.\n"; + return false; + } + if (nullptr == parent && parentLinkName != "world") + { + gzerr << "The link [" << parentLinkName << "] cannot be found in " + << "Model [" << modelName << "], but joint [" + << joint->Name() << "] wants to use it as its parent link\n"; + return false; + } + else if (nullptr == parent) + { + // This link is attached to the world, making it the root +/* if (nullptr != rootLink) + { + // A root already exists for this model + gzerr << "Two root links were found for Model [" << modelName + << "]: [" << rootLink->Name() << "] and [" << childLinkName + << "], but gz-physics-bullet-featherstone-plugin only " + << "supports one root per Model.\n"; + return false; + } +*/ + + if (joint->Type() != ::sdf::JointType::FIXED) + { + gzerr << "Link [" << child->Name() << "] in Model [" + << modelName << "] is being connected to the " + << "world by Joint [" << joint->Name() << "] with a [" + << (std::size_t)(joint->Type()) << "] joint type, but only " + << "Fixed (" << (std::size_t)(::sdf::JointType::FIXED) + << ") is supported by " + << "gz-physics-bullet-featherstone-plugin\n"; + return false; + } + + // TODO determine this later in a separate function? + // rootLink = child; + // rootJoint = joint; + // fixed = true; + + // Do not add the root link to the set of links that have parents + // continue; + } + + if (!_parentOf.insert( + std::make_pair(child, ParentInfo{joint, _sdfModel, child, parent})).second) + { + gzerr << "The Link [" << childLinkName << "] in Model [" + << modelName << "] has multiple parent joints. That is not " + << "supported by the gz-physics-bullet-featherstone plugin.\n"; + } + if (parent != nullptr) + { + _linkTree[parent].push_back(child); + } + std::cerr << "parent of inserted " << child->Name() << std::endl; + std::cerr << "parent of size " << _parentOf.size() << std::endl; + } + + // Recursively build tree from nested models + for (std::size_t i = 0; i < _sdfModel->ModelCount(); ++i) + { + const auto *model = _sdfModel->ModelByIndex(i); + if (!BuildTrees(model, _parentOf, _linkTree)) + return false; + } + return true; +} + +///////////////////////////////////////////////// +void FindRootLinks(const ::sdf::Model *_sdfModel, + const std::unordered_map &_parentOf, + std::unordered_map &_rootLinks) +{ + std::cerr << "find root links parentof size " << _parentOf.size() << std::endl; + for (auto p : _parentOf) + std::cerr << " parentOf[i] " << p.first << std::endl; + std::cerr << "model link count " << _sdfModel->Name() << " " << _sdfModel->LinkCount() << std::endl; + for (std::size_t i = 0; i < _sdfModel->LinkCount(); ++i) + { + const auto *link = _sdfModel->LinkByIndex(i); + if (_parentOf.count(link) == 0) + { + std::cerr << "root link inserting " << link->Name() << " " << link <ModelCount(); ++i) + { + const auto *model = _sdfModel->ModelByIndex(i); + FindRootLinks(model, _parentOf, _rootLinks); + } +} + +///////////////////////////////////////////////// +std::optional BuildStructure( + const ::sdf::Link * _rootLink, + const ::sdf::Model * _model, + const std::unordered_map &_parentOf, + std::unordered_map> + &_linkTree) +{ + bool fixed = false; + const ::sdf::Joint *rootJoint = nullptr; + // rootJoint only exists if root link is connected to world by a joint + auto linkIt = _parentOf.find(_rootLink); + if (linkIt != _parentOf.end()) + { + const auto &parentInfo = linkIt->second; + rootJoint = parentInfo.joint; + fixed = true; + } + + // The documentation for bullet does not mention whether parent links must + // have a lower index than their child links, but the Featherstone Algorithm + // needs to crawl up and down the tree systematically, and so the flattened + // tree structures used by the algorithm usually do expect the parents to + // 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. + std::vector flatLinks; + std::function flattenLinkTree = + [&](const ::sdf::Link *link) + { + if (link != _rootLink) + { + flatLinks.push_back(link); + } + if (auto it = _linkTree.find(link); it != _linkTree.end()) + { + for (const auto &childLink : it->second) + { + flattenLinkTree(childLink); + } + } + }; + flattenLinkTree(_rootLink); + + // TODO transform to appropriate frame for nested models? + const auto &M = _rootLink->Inertial().MassMatrix(); + const auto mass = static_cast(M.Mass()); + btVector3 inertia(convertVec(M.DiagonalMoments())); + for (const double &I : {M.Ixy(), M.Ixz(), M.Iyz()}) + { + if (std::abs(I) > 1e-3) + { + gzerr << "The base link of the model is required to have a diagonal " + << "inertia matrix by gz-physics-bullet-featherstone, but the " + << "Link [" << _rootLink->Name() << "] has a non-zero diagonal " + << "value: " << I << "\n"; + return std::nullopt; + } + } + std::cerr << " structure: " << std::endl; + std::cerr << " " << _rootLink->Name() << std::endl; + std::cerr << " " << ((rootJoint) ? rootJoint->Name() : "N/A") << std::endl; + std::cerr << " " << mass << std::endl; + std::cerr << " " << fixed << std::endl; + std::cerr << " " << flatLinks.size() << std::endl; + + return Structure{ + _rootLink, _model, rootJoint, mass, inertia, fixed, _parentOf, flatLinks}; +} + +///////////////////////////////////////////////// +std::vector ValidateModel2(const ::sdf::Model &_sdfModel) +{ + // std::unordered_map nodes; + // a map of child link and its parent info + std::unordered_map parentOf; + + // a map of parent link to a vector of its child links + std::unordered_map> + linkTree; + + // A map of root link of its parent model + std::unordered_map rootLinks; + + BuildTrees(&_sdfModel, parentOf, linkTree); + FindRootLinks(&_sdfModel, parentOf, rootLinks); + + std::vector structures; + if (rootLinks.empty()) + { + // No root link was found in this model + gzerr << "No root links are found in this model" << std::endl; + return structures; + } + + // Build subtrees + for (const auto &rootLinkIt : rootLinks) + { + auto structure = BuildStructure(rootLinkIt.first, rootLinkIt.second, + parentOf, linkTree); + if (structure.has_value()) + { + structures.push_back(*structure); + } + } + + return structures; +} + ///////////////////////////////////////////////// std::optional ValidateModel(const ::sdf::Model &_sdfModel) { @@ -139,7 +499,7 @@ std::optional ValidateModel(const ::sdf::Model &_sdfModel) const std::string &rootModelName = _sdfModel.Name(); // a map of parent link to a vector of its child links std::unordered_map> - linkTree; + linkTree; const auto checkModel = [&rootLink, &rootJoint, &fixed, &parentOf, &rootModelName, &linkTree]( @@ -332,9 +692,8 @@ std::optional ValidateModel(const ::sdf::Model &_sdfModel) return std::nullopt; } } - return Structure{ - rootLink, rootJoint, mass, inertia, fixed, parentOf, flatLinks}; + rootLink, &_sdfModel, rootJoint, mass, inertia, fixed, parentOf, flatLinks}; } ///////////////////////////////////////////////// @@ -349,58 +708,90 @@ Identity SDFFeatures::ConstructSdfModelImpl( std::size_t _parentID, const ::sdf::Model &_sdfModel) { - const auto validation = ValidateModel(_sdfModel); - if (!validation.has_value()) - return this->GenerateInvalidId(); - - const auto &structure = *validation; - const bool isStatic = _sdfModel.Static(); + // This function constructs the entire sdf model tree, including nested models + // So return the nested model identity if it is constructed already + if (this->models.find(_parentID) != this->models.end()) + { + return this->GenerateIdentity(_parentID, this->models[_parentID]); + } - // Identity worldIdentity; - auto parentModelIt = this->models.find(_parentID); - const bool isNested = (parentModelIt != this->models.end()); + auto structures = ValidateModel2(_sdfModel); + if (structures.empty()) + return this->GenerateInvalidId(); - // auto worldID = _parentID; - // WorldInfo *world = nullptr; - const auto &worldIdentity = isNested ? - parentModelIt->second->world : - this->GenerateIdentity(_parentID, this->worlds[_parentID]); -/* else + if (structures.size() > 1) { - const WorldInfoPtr &worldInfo - this->worlds.at(_parentID); + // multiple subt-trees detected in model + // \todo(iche033) support multiple sub-tree floating bodies + gzerr << "Multiple subt-trees / floating bodies are not supported yet. " + << std::endl; } -*/ - // const auto *world = this->ReferenceInterface(worldIdentity); - // auto worldIdentity = this->GenerateIdentity(worldID, this->worlds[worldID]); + auto structure = structures[0]; + const bool isStatic = _sdfModel.Static(); + WorldInfo *world = nullptr; const auto rootInertialToLink = gz::math::eigen3::convert(structure.rootLink->Inertial().Pose()).inverse(); + std::size_t rootModelID = 0u; - auto modelID = [&] { - if (isNested) - { - auto modelIdentity = this->GenerateIdentity(_parentID, this->models[_parentID]); - return this->AddNestedModel( - _sdfModel.Name(), modelIdentity, worldIdentity, rootInertialToLink, - std::make_unique( - static_cast(structure.flatLinks.size()), - structure.mass, - structure.inertia, - structure.fixedBase || isStatic, - true)); - } - else + std::cerr <<" 0 ========================================== " << std::endl; + + // recursively create models in SDFFeatures + auto addModels = [&](std::size_t _modelOrWorldID, const ::sdf::Model *_model, + auto &&_addModels) { - return this->AddModel( - _sdfModel.Name(), worldIdentity, rootInertialToLink, - std::make_unique( - static_cast(structure.flatLinks.size()), - structure.mass, - structure.inertia, - structure.fixedBase || isStatic, - true)); - } - }(); + if (!_model) + return false; + + auto modelIt = this->models.find(_modelOrWorldID); + const bool isNested = (modelIt != this->models.end()); + + auto modelID = [&] { + if (isNested) + { + auto worldIdentity = modelIt->second->world; + auto modelIdentity = + this->GenerateIdentity(_modelOrWorldID, this->models[_modelOrWorldID]); + return this->AddNestedModel( + _model->Name(), modelIdentity, worldIdentity, rootInertialToLink, + // multibody is null because we are using a single multiple for the + // entire model tree + nullptr); + } + else + { + auto worldIdentity = this->GenerateIdentity( + _modelOrWorldID, this->worlds[_modelOrWorldID]); + world = this->ReferenceInterface(worldIdentity); + auto id = this->AddModel( + _model->Name(), worldIdentity, rootInertialToLink, + std::make_unique( + static_cast(structure.flatLinks.size()), + structure.mass, + structure.inertia, + structure.fixedBase || isStatic, + true)); + rootModelID = id; + return id; + } + }(); + + // recursively add nested models + for (std::size_t i = 0; i < _model->ModelCount(); ++i) + { + if (!_addModels(modelID, _model->ModelByIndex(i), _addModels)) + return false; + } + return true; + }; + if (!addModels(_parentID, &_sdfModel, addModels)) + { + return this->GenerateInvalidId(); + } + + // Get the root model identity + auto modelID = + this->GenerateIdentity(rootModelID, this->models[rootModelID]); const auto rootID = this->AddLink(LinkInfo{ @@ -408,6 +799,314 @@ Identity SDFFeatures::ConstructSdfModelImpl( }); const auto *model = this->ReferenceInterface(modelID); + if (structure.rootJoint) + { + // Add world joint + this->AddJoint( + JointInfo{ + structure.rootJoint->Name(), + RootJoint{}, + std::nullopt, + rootID, + Eigen::Isometry3d::Identity(), + Eigen::Isometry3d::Identity(), + modelID + }); + } + + std::cerr <<" 1 ========================================== " << std::endl; + + model->body->setLinearDamping(0.0); + model->body->setAngularDamping(0.0); + + std::unordered_map linkIDs; + linkIDs.insert(std::make_pair(structure.rootLink, rootID)); + + for (int i = 0; i < static_cast(structure.flatLinks.size()); ++i) + { + const auto *link = structure.flatLinks[static_cast(i)]; + const Eigen::Isometry3d linkToComTf = gz::math::eigen3::convert( + link->Inertial().Pose()); + + if (linkIDs.find(link) == linkIDs.end()) + { + const auto linkID = this->AddLink( + LinkInfo{link->Name(), i, modelID, linkToComTf.inverse()}); + linkIDs.insert(std::make_pair(link, linkID)); + std::cerr << " addlink " << link->Name() << " " << i << std::endl; + } + + const auto &M = link->Inertial().MassMatrix(); + const btScalar mass = static_cast(M.Mass()); + const auto inertia = btVector3(convertVec(M.DiagonalMoments())); + + for (const double I : {M.Ixy(), M.Ixz(), M.Iyz()}) + { + if (std::abs(I) > 1e-3) + { + gzerr << "Links are required to have a diagonal inertia matrix in " + << "gz-physics-bullet-featherstone, but Link [" << link->Name() + << "] in Model [" << model->name << "] has a non-zero off " + << "diagonal value in its inertia matrix\n"; + return this->GenerateInvalidId(); + } + } + + if (structure.parentOf.size()) + { + const auto &parentInfo = structure.parentOf.at(link); + const auto *joint = parentInfo.joint; + const auto &parentLinkID = linkIDs.at( + parentInfo.model->LinkByName(joint->ParentName())); + const auto *parentLinkInfo = this->ReferenceInterface( + parentLinkID); + + int parentIndex = -1; + if (parentLinkInfo->indexInModel.has_value()) + parentIndex = *parentLinkInfo->indexInModel; + + // TODO + std::cerr << " model name " << parentInfo.model->Name() << std::endl; + std::cerr << " joint p name " << joint->ParentName() << std::endl; + std::cerr << " parent link index " << parentIndex << std::endl; + + Eigen::Isometry3d poseParentLinkToJoint; + Eigen::Isometry3d poseParentComToJoint; + { + gz::math::Pose3d gzPoseParentToJoint; + const auto errors = ResolveJointPoseRelToLink(gzPoseParentToJoint, + parentInfo.model, joint->Name(), joint->ParentName()); + // const auto errors = joint->SemanticPose().Resolve( + // gzPoseParentToJoint, joint->ParentName()); + + if (!errors.empty()) + { + gzerr << "An error occurred while resolving the transform of Joint [" + << joint->Name() << "] in Model [" << parentInfo.model->Name() << "]:\n"; + for (const auto &error : errors) + { + gzerr << error << "\n"; + } + + return this->GenerateInvalidId(); + } + + + poseParentLinkToJoint = gz::math::eigen3::convert(gzPoseParentToJoint); + poseParentComToJoint = + poseParentLinkToJoint * parentLinkInfo->inertiaToLinkFrame; + } + + Eigen::Isometry3d poseJointToChild; + { + gz::math::Pose3d gzPoseChildToJoint; + // const auto errors = + // link->SemanticPose().Resolve(gzPoseJointToChild, joint->Name()); + // this retrieves the joint pose relative to link + const auto errors = ResolveJointPoseRelToLink(gzPoseChildToJoint, + parentInfo.model, joint->Name(), joint->ChildName()); + + if (!errors.empty()) + { + gzerr << "An error occured while resolving the transform of Link [" + << link->Name() << "]:\n"; + for (const auto &error : errors) + { + gzerr << error << "\n"; + } + + return this->GenerateInvalidId(); + } + gz::math::Pose3d gzPoseJointToChild; + gzPoseJointToChild = gzPoseChildToJoint.Inverse(); + poseJointToChild = gz::math::eigen3::convert(gzPoseJointToChild); + } + + btQuaternion btRotParentComToJoint; + convertMat(poseParentComToJoint.linear()) + .getRotation(btRotParentComToJoint); + + // TODO + std::cerr << " model name " << _sdfModel.Name() << std::endl; + std::cerr << " joint p name " << joint->ParentName() << std::endl; + std::cerr << " joint c name " << joint->ChildName() << std::endl; + // + + // auto linkParent = _sdfModel.LinkByName(joint->ParentName()); + // gz::math::Pose3d parent2joint = gzPoseParentToJoint.Inverse(); + //const auto errors2 = linkParent->SemanticPose().Resolve( + // parent2joint, joint->Name()); // X_JP + + // std::cerr << "parent2joint " << parent2joint << " vs " << gzPoseParentToJoint.Inverse() << std::endl;; + + btTransform parentLocalInertialFrame = convertTf( + parentLinkInfo->inertiaToLinkFrame); + btTransform parent2jointBt = convertTf( + // gz::math::eigen3::convert(parent2joint.Inverse())); // X_PJ + poseParentLinkToJoint); // X_PJ + + // offsetInABt = parent COM to pivot (X_IpJ) + // offsetInBBt = current COM to pivot (X_IcJ) + // + // X_PIp + // X_PJ + // X_IpJ = X_PIp^-1 * X_PJ + // + // X_IcJ = X_CIc^-1 * X_CJ + btTransform offsetInABt, offsetInBBt; + offsetInABt = parentLocalInertialFrame * parent2jointBt; + offsetInBBt = + convertTf(linkToComTf.inverse() * poseJointToChild.inverse()); + // R_IcJ * R_IpJ ^ -1 = R_IcIp; + btQuaternion parentRotToThis = + offsetInBBt.getRotation() * offsetInABt.inverse().getRotation(); + + auto jointID = this->AddJoint( + JointInfo{ + joint->Name(), + InternalJoint{i}, + model->linkEntityIds[static_cast(parentIndex+1)], + linkIDs.find(link)->second, + poseParentLinkToJoint, + poseJointToChild, + modelID + }); + auto jointInfo = this->ReferenceInterface(jointID); + + if (::sdf::JointType::PRISMATIC == joint->Type() || + ::sdf::JointType::REVOLUTE == joint->Type() || + ::sdf::JointType::BALL == joint->Type()) + { + if (::sdf::JointType::REVOLUTE == joint->Type()) + { + const auto axis = convertVec(joint->Axis()->Xyz()); + model->body->setupRevolute( + i, mass, inertia, parentIndex, + parentRotToThis, + quatRotate(offsetInBBt.getRotation(), axis), + offsetInABt.getOrigin(), + -offsetInBBt.getOrigin(), + true); + } + else if (::sdf::JointType::PRISMATIC == joint->Type()) + { + const auto axis = convertVec(joint->Axis()->Xyz()); + model->body->setupPrismatic( + i, mass, inertia, parentIndex, + parentRotToThis, + quatRotate(offsetInBBt.getRotation(), axis), + offsetInABt.getOrigin(), + -offsetInBBt.getOrigin(), + true); + } + else if (::sdf::JointType::BALL == joint->Type()) + { + model->body->setupSpherical( + i, mass, inertia, parentIndex, + parentRotToThis, + offsetInABt.getOrigin(), + -offsetInBBt.getOrigin(), + true); + } + } + else + { + model->body->setupFixed( + i, mass, inertia, parentIndex, + parentRotToThis, + offsetInABt.getOrigin(), + -offsetInBBt.getOrigin()); + } + + if (::sdf::JointType::PRISMATIC == joint->Type() || + ::sdf::JointType::REVOLUTE == joint->Type()) + { + model->body->getLink(i).m_jointLowerLimit = + static_cast(joint->Axis()->Lower()); + model->body->getLink(i).m_jointUpperLimit = + static_cast(joint->Axis()->Upper()); + model->body->getLink(i).m_jointDamping = + static_cast(joint->Axis()->Damping()); + model->body->getLink(i).m_jointFriction = + static_cast(joint->Axis()->Friction()); + model->body->getLink(i).m_jointMaxVelocity = + static_cast(joint->Axis()->MaxVelocity()); + model->body->getLink(i).m_jointMaxForce = + static_cast(joint->Axis()->Effort()); + jointInfo->effort = static_cast(joint->Axis()->Effort()); + + jointInfo->jointLimits = + std::make_shared( + model->body.get(), i, static_cast(joint->Axis()->Lower()), + static_cast(joint->Axis()->Upper())); + world->world->addMultiBodyConstraint(jointInfo->jointLimits.get()); + } + + jointInfo->jointFeedback = std::make_shared(); + jointInfo->jointFeedback->m_reactionForces.setZero(); + model->body->getLink(i).m_jointFeedback = jointInfo->jointFeedback.get(); + } + } + std::cerr << " ======================= done 0 " << std::endl; + + model->body->setHasSelfCollision(_sdfModel.SelfCollide()); + model->body->finalizeMultiDof(); + + const auto worldToModel = ResolveSdfPose(_sdfModel.SemanticPose()); + if (!worldToModel) + return this->GenerateInvalidId(); + + std::cerr << " ======================= done 1 " << std::endl; + const auto modelToRootLink = + ResolveSdfPose(structure.rootLink->SemanticPose()); + if (!modelToRootLink) + return this->GenerateInvalidId(); + + const auto worldToRootCom = + *worldToModel * *modelToRootLink * rootInertialToLink.inverse(); + + model->body->setBaseWorldTransform(convertTf(worldToRootCom)); + model->body->setBaseVel(btVector3(0, 0, 0)); + model->body->setBaseOmega(btVector3(0, 0, 0)); + + { + const auto *link = structure.rootLink; + const auto &M = link->Inertial().MassMatrix(); + const btScalar mass = static_cast(M.Mass()); + const auto inertia = convertVec(M.DiagonalMoments()); + + for (const double I : {M.Ixy(), M.Ixz(), M.Iyz()}) + { + if (std::abs(I) > 1e-3) + { + gzerr << "Links are required to have a diagonal inertia matrix in " + << "gz-physics-bullet-featherstone, but Link [" << link->Name() + << "] in Model [" << model->name << "] has a non-zero off " + << "diagonal value in its inertia matrix\n"; + } + else + { + model->body->setBaseMass(mass); + model->body->setBaseInertia(inertia); + } + } + } + + + std::cerr << " ======================= done 2 " << std::endl; + world->world->addMultiBody(model->body.get()); + + for (const auto& [linkSdf, linkID] : linkIDs) + { + for (std::size_t c = 0; c < linkSdf->CollisionCount(); ++c) + { + // If we fail to add the collision, just keep building the model. It may + // need to be constructed outside of the SDF generation pipeline, e.g. + // with AttachHeightmap. + this->AddSdfCollision(linkID, *linkSdf->CollisionByIndex(c), isStatic); + } + } + return modelID; } @@ -510,6 +1209,7 @@ Identity SDFFeatures::ConstructSdfModel( gz::math::Pose3d gzPoseParentToJoint; const auto errors = joint->SemanticPose().Resolve( gzPoseParentToJoint, joint->ParentName()); + if (!errors.empty()) { gzerr << "An error occurred while resolving the transform of Joint [" @@ -532,6 +1232,7 @@ Identity SDFFeatures::ConstructSdfModel( gz::math::Pose3d gzPoseJointToChild; const auto errors = link->SemanticPose().Resolve(gzPoseJointToChild, joint->Name()); + if (!errors.empty()) { gzerr << "An error occured while resolving the transform of Link [" diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index c2fc54de8..66b6252d4 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -534,6 +534,7 @@ Identity SDFFeatures::ConstructSdfModelImpl( std::size_t _parentID, const ::sdf::Model &_sdfModel) { + std::cerr << "======= construct nested model ---- " << _sdfModel.Name() << std::endl; auto worldID = _parentID; std::string modelName = _sdfModel.Name(); const bool isNested = this->models.HasEntity(_parentID); @@ -1058,6 +1059,19 @@ Identity SDFFeatures::ConstructSdfJoint( // joint is connected to the world bool worldParent = (!_parent && _sdfJoint.ParentName() == "world"); bool worldChild = (!_child && _sdfJoint.ChildName() == "world"); +/* + std::cerr << "parent link name " << _sdfJoint.ParentName() << std::endl; + std::cerr << "child link name " << _sdfJoint.ChildName() << std::endl; + + size_t idx = _sdfJoint.ChildName().find("::"); + if (idx != std::string::npos) + { + std::string modelName = _sdfJoint.ChildName().substr(0, idx); + std::string linkName = _sdfJoint.ChildName().substr(idx + 2); + std::cerr << "model name " << modelName << std::endl; + std::cerr << "link name " << linkName << std::endl; + } +*/ if (worldChild) { From 65770b5be716f89adca39cddf34328da99fbd3ae Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Sat, 11 Nov 2023 02:29:14 +0000 Subject: [PATCH 08/29] Fix link/joint to model ref, tests passing Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.hh | 88 ++++++++++- .../src/EntityManagementFeatures.cc | 88 ++++++----- .../src/EntityManagementFeatures.hh | 7 + bullet-featherstone/src/FreeGroupFeatures.cc | 24 ++- bullet-featherstone/src/SDFFeatures.cc | 145 ++++++++++++------ test/common_test/free_joint_features.cc | 22 +++ test/common_test/world_features.cc | 138 +++++++++++++++++ 7 files changed, 417 insertions(+), 95 deletions(-) diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index 25f878575..c97ecfb4e 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -84,7 +84,7 @@ struct ModelInfo Identity world; int indexInWorld; Eigen::Isometry3d baseInertiaToLinkFrame; - std::unique_ptr body; + std::shared_ptr body; std::vector linkEntityIds; std::vector jointEntityIds; @@ -101,7 +101,7 @@ struct ModelInfo std::string _name, Identity _world, Eigen::Isometry3d _baseInertiaToLinkFrame, - std::unique_ptr _body) + std::shared_ptr _body) : name(std::move(_name)), world(std::move(_world)), baseInertiaToLinkFrame(_baseInertiaToLinkFrame), @@ -282,7 +282,7 @@ class Base : public Implements3d> std::string _name, Identity _worldID, Eigen::Isometry3d _baseInertialToLinkFrame, - std::unique_ptr _body) + std::shared_ptr _body) { const auto id = this->GetNextEntity(); auto model = std::make_shared( @@ -302,7 +302,7 @@ class Base : public Implements3d> Identity _parentID, Identity _worldID, Eigen::Isometry3d _baseInertialToLinkFrame, - std::unique_ptr _body) + std::shared_ptr _body) { const auto id = this->GetNextEntity(); auto model = std::make_shared( @@ -327,8 +327,8 @@ class Base : public Implements3d> if (link->indexInModel.has_value()) { // We expect the links to be added in order - assert(static_cast(*link->indexInModel + 1) == - model->linkEntityIds.size()); + // assert(static_cast(*link->indexInModel + 1) == + // model->linkEntityIds.size()); } else { @@ -378,6 +378,82 @@ class Base : public Implements3d> return this->GenerateIdentity(id, joint); } + public: bool RemoveModelImpl(const Identity &_parentID, const Identity &_modelID) + { + auto *model = this->ReferenceInterface(_modelID); + if (!model) + return false; + + auto parentIt = this->models.find(_parentID); + bool isNested = parentIt != this->models.end(); + + // Remove nested models + for (auto &nestedModelID : model->nestedModelEntityIds) + { + this->RemoveModelImpl(_modelID, + this->GenerateIdentity(nestedModelID, this->models.at(nestedModelID))); + } + model->nestedModelEntityIds.clear(); + + // If this is a nested model, remove references in parent model + if (isNested) + { + auto *parentModel = this->ReferenceInterface(_parentID); + auto nestedModelIt = parentModel->nestedModelNameToEntityId.find(model->name); + if (nestedModelIt != parentModel->nestedModelNameToEntityId.end()) + { + std::size_t nestedModelID = nestedModelIt->second; + parentModel->nestedModelNameToEntityId.erase(nestedModelIt); + parentModel->nestedModelEntityIds.erase(std::remove( + parentModel->nestedModelEntityIds.begin(), + parentModel->nestedModelEntityIds.end(), nestedModelID), + parentModel->nestedModelEntityIds.end()); + } + return true; + } + + auto *world = this->ReferenceInterface(model->world); + if (!world) + return false; + + if (world->modelIndexToEntityId.erase(model->indexInWorld) == 0) + { + // The model has already been removed at some point + return false; + } + world->modelNameToEntityId.erase(model->name); + + // Remove all constraints related to this model + for (auto constraint_index : model->external_constraints) + { + const auto joint = this->joints.at(constraint_index); + const auto &constraint = + std::get>(joint->identifier); + world->world->removeMultiBodyConstraint(constraint.get()); + this->joints.erase(constraint_index); + } + + world->world->removeMultiBody(model->body.get()); + for (const auto linkID : model->linkEntityIds) + { + const auto &link = this->links.at(linkID); + if (link->collider) + { + world->world->removeCollisionObject(link->collider.get()); + for (const auto shapeID : link->collisionEntityIds) + this->collisions.erase(shapeID); + } + + this->links.erase(linkID); + } + + for (const auto jointID : model->jointEntityIds) + this->joints.erase(jointID); + + this->models.erase(_modelID); + return true; + } + public: ~Base() override { // The order of destruction between meshesGImpact and triangleMeshes is // important. diff --git a/bullet-featherstone/src/EntityManagementFeatures.cc b/bullet-featherstone/src/EntityManagementFeatures.cc index 5b8ca62e2..be9891ee5 100644 --- a/bullet-featherstone/src/EntityManagementFeatures.cc +++ b/bullet-featherstone/src/EntityManagementFeatures.cc @@ -206,44 +206,9 @@ Identity EntityManagementFeatures::ConstructEmptyWorld( bool EntityManagementFeatures::RemoveModel(const Identity &_modelID) { auto *model = this->ReferenceInterface(_modelID); - auto *world = this->ReferenceInterface(model->world); - if (world->modelIndexToEntityId.erase(model->indexInWorld) == 0) - { - // The model has already been removed at some point. + if (!model) return false; - } - - world->modelNameToEntityId.erase(model->name); - - // Remove all constraints related to this model - for (auto constraint_index : model->external_constraints) - { - const auto joint = this->joints.at(constraint_index); - const auto &constraint = - std::get>(joint->identifier); - world->world->removeMultiBodyConstraint(constraint.get()); - this->joints.erase(constraint_index); - } - - world->world->removeMultiBody(model->body.get()); - for (const auto linkID : model->linkEntityIds) - { - const auto &link = this->links.at(linkID); - if (link->collider) - { - world->world->removeCollisionObject(link->collider.get()); - for (const auto shapeID : link->collisionEntityIds) - this->collisions.erase(shapeID); - } - - this->links.erase(linkID); - } - - for (const auto jointID : model->jointEntityIds) - this->joints.erase(jointID); - - this->models.erase(_modelID); - return true; + return this->RemoveModelImpl(model->world, _modelID); } ///////////////////////////////////////////////// @@ -283,6 +248,55 @@ bool EntityManagementFeatures::RemoveModelByName( this->GenerateIdentity(it->second, this->models.at(it->second))); } +///////////////////////////////////////////////// +bool EntityManagementFeatures::RemoveNestedModelByIndex( + const Identity &_modelID, std::size_t _nestedModelIndex) +{ + auto *model = this->ReferenceInterface(_modelID); + if (!model) + return false; + if (_nestedModelIndex >= model->nestedModelEntityIds.size()) + return false; + const auto nestedModelID = model->nestedModelEntityIds[_nestedModelIndex]; + + auto modelIt = this->models.find(nestedModelID); + if (modelIt != this->models.end()) + { + return RemoveModelImpl(_modelID, + this->GenerateIdentity(modelIt->first, modelIt->second)); + } + return false; +} + +///////////////////////////////////////////////// +bool EntityManagementFeatures::RemoveNestedModelByName(const Identity &_modelID, + const std::string &_modelName) +{ + auto *model = this->ReferenceInterface(_modelID); + if (!model) + return false; + + unsigned int nestedModelID = 0u; + auto nestedModelIt = model->nestedModelNameToEntityId.find(_modelName); + if (nestedModelIt != model->nestedModelNameToEntityId.end()) + { + nestedModelID = nestedModelIt->second; + } + else + { + return false; + } + + auto modelIt = this->models.find(nestedModelID); + if (modelIt != this->models.end()) + { + return RemoveModelImpl(_modelID, + this->GenerateIdentity(modelIt->first, modelIt->second)); + } + return false; +} + + ///////////////////////////////////////////////// const std::string &EntityManagementFeatures::GetEngineName( const Identity &) const diff --git a/bullet-featherstone/src/EntityManagementFeatures.hh b/bullet-featherstone/src/EntityManagementFeatures.hh index 81f87f0e4..a6c3367da 100644 --- a/bullet-featherstone/src/EntityManagementFeatures.hh +++ b/bullet-featherstone/src/EntityManagementFeatures.hh @@ -40,6 +40,7 @@ struct EntityManagementFeatureList : gz::physics::FeatureList< GetNestedModelFromModel, GetShapeFromLink, GetWorldFromEngine, + RemoveEntities, RemoveModelFromWorld > { }; @@ -156,6 +157,12 @@ class EntityManagementFeatures : public: bool ModelRemoved(const Identity &_modelID) const override; + public: bool RemoveNestedModelByIndex( + const Identity &_modelID, std::size_t _nestedModelIndex) override; + + public: bool RemoveNestedModelByName( + const Identity &_modelID, const std::string &_modelName) override; + // ----- Construct empty entities ----- public: Identity ConstructEmptyWorld( const Identity &_engineID, const std::string & _name) override; diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc index c557d6465..cc0ca8933 100644 --- a/bullet-featherstone/src/FreeGroupFeatures.cc +++ b/bullet-featherstone/src/FreeGroupFeatures.cc @@ -33,6 +33,11 @@ Identity FreeGroupFeatures::FindFreeGroupForModel( if (model->body->hasFixedBase()) return this->GenerateInvalidId(); + // bullet-featherstone does not allow floating bodies so if a joint exists + // the multibody does not quality as a FreeGroup + if (model->body->getNumDofs() > 0) + return this->GenerateInvalidId(); + return _modelID; } @@ -45,6 +50,11 @@ Identity FreeGroupFeatures::FindFreeGroupForLink( if (model->body->hasFixedBase()) return this->GenerateInvalidId(); + // bullet-featherstone does not allow floating bodies so if a joint exists + // the multibody does not quality as a FreeGroup + if (model->body->getNumDofs() > 0) + return this->GenerateInvalidId(); + return link->model; } @@ -54,8 +64,11 @@ Identity FreeGroupFeatures::GetFreeGroupRootLink(const Identity &_groupID) const // Free groups in bullet-featherstone are always represented by ModelInfo const auto *model = this->ReferenceInterface(_groupID); + // btMultiBody user index stores the gz-phsics model root link id + std::size_t rootID = static_cast(model->body->getUserIndex()); + // The first link entity in the model is always the root link - const std::size_t rootID = model->linkEntityIds.front(); + // const std::size_t rootID = model->linkEntityIds.front(); return this->GenerateIdentity(rootID, this->links.at(rootID)); } @@ -66,7 +79,7 @@ void FreeGroupFeatures::SetFreeGroupWorldAngularVelocity( // Free groups in bullet-featherstone are always represented by ModelInfo const auto *model = this->ReferenceInterface(_groupID); - if (model != nullptr) + if (model) { model->body->setBaseOmega(convertVec(_angularVelocity)); } @@ -79,7 +92,7 @@ void FreeGroupFeatures::SetFreeGroupWorldLinearVelocity( // Free groups in bullet-featherstone are always represented by ModelInfo const auto *model = this->ReferenceInterface(_groupID); // Set Base Vel - if(model) + if (model) { model->body->setBaseVel(convertVec(_linearVelocity)); } @@ -91,7 +104,10 @@ void FreeGroupFeatures::SetFreeGroupWorldPose( const PoseType &_pose) { const auto *model = this->ReferenceInterface(_groupID); - model->body->setBaseWorldTransform(convertTf(_pose)); + if (model) + { + model->body->setBaseWorldTransform(convertTf(_pose)); + } } } // namespace bullet_featherstone diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index fa22dd27a..9f8294f41 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -273,13 +273,11 @@ bool BuildTrees(const ::sdf::Model *_sdfModel, const auto &parentLinkName = joint->ParentName(); const auto &childLinkName = joint->ChildName(); const std::string &modelName = _sdfModel->Name(); -// const auto *parent = FindLinkInModelTree(parentLinkName, _sdfModel); -// const auto *child = FindLinkInModelTree(childLinkName, _sdfModel); const auto *parent = _sdfModel->LinkByName(parentLinkName); const auto *child = _sdfModel->LinkByName(childLinkName); - std::cerr << "parent link name " << parentLinkName << " " << parent << std::endl; - std::cerr << "child link name " << childLinkName << " " << child << std::endl; + // std::cerr << "parent link name " << parentLinkName << " " << parent << std::endl; + // std::cerr << "child link name " << childLinkName << " " << child << std::endl; switch (joint->Type()) { @@ -343,6 +341,7 @@ bool BuildTrees(const ::sdf::Model *_sdfModel, // continue; } + std::cerr << " parent of inserting " << child << " " << parent << std::endl; if (!_parentOf.insert( std::make_pair(child, ParentInfo{joint, _sdfModel, child, parent})).second) { @@ -354,8 +353,8 @@ bool BuildTrees(const ::sdf::Model *_sdfModel, { _linkTree[parent].push_back(child); } - std::cerr << "parent of inserted " << child->Name() << std::endl; - std::cerr << "parent of size " << _parentOf.size() << std::endl; + // std::cerr << "parent of inserted " << child->Name() << std::endl; + // std::cerr << "parent of size " << _parentOf.size() << std::endl; } // Recursively build tree from nested models @@ -371,20 +370,22 @@ bool BuildTrees(const ::sdf::Model *_sdfModel, ///////////////////////////////////////////////// void FindRootLinks(const ::sdf::Model *_sdfModel, const std::unordered_map &_parentOf, - std::unordered_map &_rootLinks) + std::vector> &_rootLinks) { - std::cerr << "find root links parentof size " << _parentOf.size() << std::endl; - for (auto p : _parentOf) - std::cerr << " parentOf[i] " << p.first << std::endl; - std::cerr << "model link count " << _sdfModel->Name() << " " << _sdfModel->LinkCount() << std::endl; + // std::cerr << "find root links parentof size " << _parentOf.size() << std::endl; + // for (auto p : _parentOf) + // std::cerr << " parentOf[i] " << p.first << std::endl; + // std::cerr << "model link count " << _sdfModel->Name() << " " << _sdfModel->LinkCount() << std::endl; for (std::size_t i = 0; i < _sdfModel->LinkCount(); ++i) { const auto *link = _sdfModel->LinkByIndex(i); - if (_parentOf.count(link) == 0) + auto parentOfIt = _parentOf.find(link); + if (parentOfIt == _parentOf.end() || !parentOfIt->second.parent) { - std::cerr << "root link inserting " << link->Name() << " " << link <Name() << " " << link < BuildStructure( extractInertial(_rootLink->Inertial(), mass, inertia, linkToPrincipalAxesPose); std::cerr << " structure: " << std::endl; + std::cerr << " " << _model->Name() << std::endl; std::cerr << " " << _rootLink->Name() << std::endl; std::cerr << " " << ((rootJoint) ? rootJoint->Name() : "N/A") << std::endl; std::cerr << " " << mass << std::endl; @@ -468,8 +470,9 @@ std::vector ValidateModel2(const ::sdf::Model &_sdfModel) std::unordered_map> linkTree; - // A map of root link of its parent model - std::unordered_map rootLinks; + // A vector of root link of its parent model + // Use a vector to preseve order of entities defined in sdf + std::vector> rootLinks; BuildTrees(&_sdfModel, parentOf, linkTree); FindRootLinks(&_sdfModel, parentOf, rootLinks); @@ -656,7 +659,7 @@ std::optional ValidateModel(const ::sdf::Model &_sdfModel) if (!rootLink) { - gzerr << "No root link was found for model [" << _sdfModel.Name() << "\n"; + gzerr << "No root link was found for model [" << _sdfModel.Name() << "]\n"; return std::nullopt; } @@ -707,11 +710,15 @@ Identity SDFFeatures::ConstructSdfModelImpl( std::size_t _parentID, const ::sdf::Model &_sdfModel) { + std::cerr << " ---------- constructing " << _sdfModel.Name() << std::endl; // This function constructs the entire sdf model tree, including nested models // So return the nested model identity if it is constructed already - if (this->models.find(_parentID) != this->models.end()) + auto pIt = this->models.find(_parentID); + if (pIt != this->models.end()) { - return this->GenerateIdentity(_parentID, this->models[_parentID]); + std::size_t nestedModelID = pIt->second->nestedModelNameToEntityId.at( + _sdfModel.Name()); + return this->GenerateIdentity(nestedModelID, this->models[nestedModelID]); } auto structures = ValidateModel2(_sdfModel); @@ -721,22 +728,28 @@ Identity SDFFeatures::ConstructSdfModelImpl( if (structures.size() > 1) { // multiple subt-trees detected in model - // \todo(iche033) support multiple sub-tree floating bodies - gzerr << "Multiple subt-trees / floating bodies are not supported yet. " + // \todo(iche033) support multiple sub-tree kinematic trees and + // multiple floating links in a single model + gzerr << "Multiple subt-trees / floating links detected in a model. " + << "This is not supported in bullet-featherstone implementation yet." << std::endl; } + // Create model for the first structure. auto structure = structures[0]; + std::cerr << " Adding structure " << structure.model->Name() << ": " << std::endl; const bool isStatic = _sdfModel.Static(); WorldInfo *world = nullptr; const auto rootInertialToLink = gz::math::eigen3::convert(structure.linkToPrincipalAxesPose).inverse(); - std::size_t rootModelID = 0u; - - std::cerr <<" 0 ========================================== " << std::endl; + // A map of link sdf to parent model identity + std::unordered_map linkParentModelIds; - // recursively create models in SDFFeatures + std::unordered_map modelIDs; + std::size_t rootModelID = 0u; + std::shared_ptr rootMultiBody; + // Add all models, including nested models auto addModels = [&](std::size_t _modelOrWorldID, const ::sdf::Model *_model, auto &&_addModels) { @@ -754,28 +767,37 @@ Identity SDFFeatures::ConstructSdfModelImpl( this->GenerateIdentity(_modelOrWorldID, this->models[_modelOrWorldID]); return this->AddNestedModel( _model->Name(), modelIdentity, worldIdentity, rootInertialToLink, - // multibody is null because we are using a single multiple for the - // entire model tree - nullptr); + rootMultiBody); } else { auto worldIdentity = this->GenerateIdentity( _modelOrWorldID, this->worlds[_modelOrWorldID]); - world = this->ReferenceInterface(worldIdentity); - auto id = this->AddModel( - _model->Name(), worldIdentity, rootInertialToLink, - std::make_unique( + rootMultiBody = std::make_shared( static_cast(structure.flatLinks.size()), structure.mass, structure.inertia, structure.fixedBase || isStatic, - true)); + true); + world = this->ReferenceInterface(worldIdentity); + auto id = this->AddModel( + _model->Name(), worldIdentity, rootInertialToLink, + rootMultiBody); rootModelID = id; return id; } }(); + // build link to model map for use later when adding links + for (std::size_t i = 0; i < _model->LinkCount(); ++i) + { + const ::sdf::Link *link = _model->LinkByIndex(i); + linkParentModelIds[link] = modelID; + } + modelIDs.insert(std::make_pair(_model, modelID)); + std::cerr << " Add model " << _model->Name() << " ID: " + << std::size_t(modelID) << std::endl; + // recursively add nested models for (std::size_t i = 0; i < _model->ModelCount(); ++i) { @@ -790,18 +812,24 @@ Identity SDFFeatures::ConstructSdfModelImpl( } // Get the root model identity + // New links and joints added are associated with this modelID + // so we do not break other features. auto modelID = this->GenerateIdentity(rootModelID, this->models[rootModelID]); + std::cerr << " adding root link " << structure.rootLink->Name() + << " from " << _sdfModel.Name() << std::endl; + // Add base link const auto rootID = this->AddLink(LinkInfo{ structure.rootLink->Name(), std::nullopt, modelID, rootInertialToLink }); + rootMultiBody->setUserIndex(std::size_t(rootID)); const auto *model = this->ReferenceInterface(modelID); + // Add world joint if (structure.rootJoint) { - // Add world joint this->AddJoint( JointInfo{ structure.rootJoint->Name(), @@ -819,9 +847,13 @@ Identity SDFFeatures::ConstructSdfModelImpl( model->body->setLinearDamping(0.0); model->body->setAngularDamping(0.0); + std::cerr <<" 2 ========================================== flatlink size " + << structure.flatLinks.size() << std::endl; + std::unordered_map linkIDs; linkIDs.insert(std::make_pair(structure.rootLink, rootID)); + // Add all links and joints for (int i = 0; i < static_cast(structure.flatLinks.size()); ++i) { const auto *link = structure.flatLinks[static_cast(i)]; @@ -834,8 +866,14 @@ Identity SDFFeatures::ConstructSdfModelImpl( if (linkIDs.find(link) == linkIDs.end()) { + std::size_t parentModelID = linkParentModelIds[link]; + std::cerr << " === = == = = parent model id " << parentModelID << " " + << (this->models.find(parentModelID) != this->models.end() )<< std::endl; const auto linkID = this->AddLink( - LinkInfo{link->Name(), i, modelID, linkToComTf.inverse()}); + LinkInfo{link->Name(), i, + // modelID, + this->GenerateIdentity(parentModelID, this->models.at(parentModelID)), + linkToComTf.inverse()}); linkIDs.insert(std::make_pair(link, linkID)); std::cerr << " addlink " << link->Name() << " " << i << std::endl; } @@ -854,9 +892,9 @@ Identity SDFFeatures::ConstructSdfModelImpl( parentIndex = *parentLinkInfo->indexInModel; // TODO - std::cerr << " model name " << parentInfo.model->Name() << std::endl; - std::cerr << " joint p name " << joint->ParentName() << std::endl; - std::cerr << " parent link index " << parentIndex << std::endl; + // std::cerr << " model name " << parentInfo.model->Name() << std::endl; + // std::cerr << " joint p name " << joint->ParentName() << std::endl; + // std::cerr << " parent link index " << parentIndex << std::endl; Eigen::Isometry3d poseParentLinkToJoint; Eigen::Isometry3d poseParentComToJoint; @@ -915,9 +953,9 @@ Identity SDFFeatures::ConstructSdfModelImpl( .getRotation(btRotParentComToJoint); // TODO - std::cerr << " model name " << _sdfModel.Name() << std::endl; - std::cerr << " joint p name " << joint->ParentName() << std::endl; - std::cerr << " joint c name " << joint->ChildName() << std::endl; + // std::cerr << " model name " << _sdfModel.Name() << std::endl; + // std::cerr << " joint p name " << joint->ParentName() << std::endl; + // std::cerr << " joint c name " << joint->ChildName() << std::endl; // // auto linkParent = _sdfModel.LinkByName(joint->ParentName()); @@ -953,12 +991,17 @@ Identity SDFFeatures::ConstructSdfModelImpl( JointInfo{ joint->Name(), InternalJoint{i}, - model->linkEntityIds[static_cast(parentIndex+1)], + // model->linkEntityIds[static_cast(parentIndex+1)], + linkIDs.find(parentInfo.parent)->second, linkIDs.find(link)->second, poseParentLinkToJoint, poseJointToChild, - modelID + // modelID + modelIDs.find(parentInfo.model)->second }); + std::cerr << " ================= add joint " << joint->Name() + << " " << std::size_t(linkIDs.find(parentInfo.parent)->second) << " " << + std::size_t(linkIDs.find(link)->second) << std::endl; auto jointInfo = this->ReferenceInterface(jointID); if (::sdf::JointType::PRISMATIC == joint->Type() || @@ -1035,16 +1078,18 @@ Identity SDFFeatures::ConstructSdfModelImpl( model->body->getLink(i).m_jointFeedback = jointInfo->jointFeedback.get(); } } - std::cerr << " ======================= done 0 " << std::endl; model->body->setHasSelfCollision(_sdfModel.SelfCollide()); model->body->finalizeMultiDof(); + + std::cerr << " body num links " << model->name << ": " + << model->body->getNumDofs() << std::endl; + const auto worldToModel = ResolveSdfPose(_sdfModel.SemanticPose()); if (!worldToModel) return this->GenerateInvalidId(); - std::cerr << " ======================= done 1 " << std::endl; const auto modelToRootLink = ResolveSdfPose(structure.rootLink->SemanticPose()); if (!modelToRootLink) @@ -1067,7 +1112,6 @@ Identity SDFFeatures::ConstructSdfModelImpl( model->body->setBaseInertia(inertia); } - std::cerr << " ======================= done 2 " << std::endl; world->world->addMultiBody(model->body.get()); for (const auto& [linkSdf, linkID] : linkIDs) @@ -1081,6 +1125,7 @@ Identity SDFFeatures::ConstructSdfModelImpl( } } + std::cerr <<" done ========================================== " << std::endl; return modelID; } @@ -1089,7 +1134,10 @@ Identity SDFFeatures::ConstructSdfModel( const Identity &_worldID, const ::sdf::Model &_sdfModel) { - const auto validation = ValidateModel(_sdfModel); + return this->ConstructSdfModelImpl(_worldID, _sdfModel); + +////////////////////////////// +/* const auto validation = ValidateModel(_sdfModel); if (!validation.has_value()) return this->GenerateInvalidId(); @@ -1372,6 +1420,7 @@ Identity SDFFeatures::ConstructSdfModel( } return modelID; +*/ } ///////////////////////////////////////////////// diff --git a/test/common_test/free_joint_features.cc b/test/common_test/free_joint_features.cc index 9292fe66a..89fd49f13 100644 --- a/test/common_test/free_joint_features.cc +++ b/test/common_test/free_joint_features.cc @@ -135,6 +135,18 @@ TEST_F(FreeGroupFeaturesTest, NestedFreeGroup) return testing::AssertionSuccess(); }; + if (engine->GetName() == "bullet-featherstone") + { + // https://github.com/gazebosim/gz-physics/issues/550 + // bullet-feathersone does not support multiple kinematic trees in + // a model so nested_model2 and nested_model3 are not constructed. + // \todo(iche033) Remove this block once the feature is supported. + EXPECT_FALSE(checkFreeGroupForModel("parent_model")); + EXPECT_FALSE(checkFreeGroupForModel("parent_model::nested_model2")); + EXPECT_FALSE(checkFreeGroupForModel("parent_model::nested_model3")); + GTEST_SKIP(); + } + EXPECT_TRUE(checkFreeGroupForModel("parent_model")); if (engine->GetName() == "tpe") { @@ -147,6 +159,7 @@ TEST_F(FreeGroupFeaturesTest, NestedFreeGroup) // Expect false because the link in nested_model is referenced by a joint. EXPECT_FALSE(checkFreeGroupForModel("parent_model::nested_model")); } + EXPECT_TRUE(checkFreeGroupForModel("parent_model::nested_model2")); EXPECT_TRUE(checkFreeGroupForModel("parent_model::nested_model3")); } @@ -173,6 +186,15 @@ TEST_F(FreeGroupFeaturesTest, NestedFreeGroupSetWorldPose) auto world = engine->ConstructWorld(*sdfWorld); EXPECT_NE(nullptr, world); + if (engine->GetName() == "bullet-featherstone") + { + // https://github.com/gazebosim/gz-physics/issues/550 + // bullet-feathersone does not support multiple kinematic trees in + // a model so nested_model2 and nested_model3 are not constructed. + // \todo(iche033) Remove this block once the feature is supported. + GTEST_SKIP(); + } + gz::math::Pose3d parentModelNewPose(0, 0, 2, 0, 0, 0); { auto parentModel = world->GetModel("parent_model"); diff --git a/test/common_test/world_features.cc b/test/common_test/world_features.cc index 6c08ddb98..81d6b820a 100644 --- a/test/common_test/world_features.cc +++ b/test/common_test/world_features.cc @@ -356,6 +356,144 @@ TEST_F(WorldModelTest, WorldModelAPI) } } +struct WorldNestedModelFeatureList : 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 WorldNestedModelTest : public WorldFeaturesTest +{ + public: gz::physics::World3dPtr LoadWorld( + const std::string &_pluginName) + { + gz::plugin::PluginPtr plugin = this->loader.Instantiate(_pluginName); + + auto engine = + gz::physics::RequestEngine3d::From(plugin); + + sdf::Root root; + const sdf::Errors errors = root.Load( + gz::common::joinPaths(TEST_WORLD_DIR, "world_nested_model_test.sdf")); + EXPECT_TRUE(errors.empty()) << errors; + if (errors.empty()) + { + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + return world; + } + return nullptr; + } +}; + +TEST_F(WorldModelTest, WorldConstructNestedModel) +{ + for (const std::string &name : this->pluginNames) + { + auto world = this->LoadWorld(name); + ASSERT_NE(nullptr, world); + + auto worldModel = world->GetWorldModel(); + ASSERT_NE(nullptr, worldModel); + EXPECT_EQ(world, worldModel->GetWorld()); + EXPECT_EQ("default", worldModel->GetName()); + EXPECT_EQ(0, worldModel->GetLinkCount()); + EXPECT_EQ(nullptr, worldModel->GetLink(0)); + EXPECT_EQ(nullptr, worldModel->GetLink("nonexistent_link")); + EXPECT_EQ(0, worldModel->GetIndex()); + EXPECT_EQ(world->GetModelCount(), worldModel->GetNestedModelCount()); + const auto nestedModel = worldModel->GetNestedModel(0); + ASSERT_NE(nullptr, nestedModel); + EXPECT_EQ("m1", nestedModel->GetName()); + EXPECT_NE(nullptr, worldModel->GetNestedModel("m2")); + + // Check that removing a World model proxy is not allowed + EXPECT_FALSE(worldModel->Remove()); + EXPECT_TRUE(worldModel.Valid()); + EXPECT_FALSE(worldModel->Removed()); + ASSERT_NE(nullptr, worldModel); + EXPECT_EQ(world, worldModel->GetWorld()); + + auto worldModel2 = world->GetWorldModel(); + ASSERT_NE(nullptr, worldModel2); + EXPECT_EQ(world, worldModel2->GetWorld()); + + EXPECT_EQ(2u, world->GetModelCount()); + EXPECT_EQ(2u, worldModel->GetNestedModelCount()); + + // Check that we can remove models via RemoveNestedModel + EXPECT_TRUE(worldModel->RemoveNestedModel(0)); + EXPECT_TRUE(nestedModel->Removed()); + EXPECT_EQ(1u, world->GetModelCount()); + EXPECT_EQ(1u, worldModel->GetNestedModelCount()); + EXPECT_NE(nullptr, worldModel->GetNestedModel(0)); + EXPECT_EQ(nullptr, worldModel->GetNestedModel("m1")); + + const auto m2 = worldModel->GetNestedModel("m2"); + ASSERT_NE(nullptr, m2); + EXPECT_TRUE(worldModel->RemoveNestedModel("m2")); + EXPECT_TRUE(m2->Removed()); + EXPECT_EQ(0u, world->GetModelCount()); + EXPECT_EQ(0u, worldModel->GetNestedModelCount()); + EXPECT_EQ(nullptr, worldModel->GetNestedModel("m2")); + + // Check that we can construct nested models and joints, but not links + auto m3 = worldModel->ConstructEmptyNestedModel("m3"); + ASSERT_TRUE(m3); + EXPECT_EQ(m3, world->GetModel("m3")); + EXPECT_EQ(m3, worldModel->GetNestedModel("m3")); + EXPECT_FALSE(worldModel->ConstructEmptyLink("test_link")); + + sdf::Link sdfLink; + sdfLink.SetName("link3"); + EXPECT_FALSE(worldModel->ConstructLink(sdfLink)); + + // Create a link in m3 for testing joint creation. + EXPECT_TRUE(m3->ConstructLink(sdfLink)); + sdf::Joint sdfJoint; + sdfJoint.SetName("test_joint"); + sdfJoint.SetType(sdf::JointType::FIXED); + sdfJoint.SetParentName("m2::link2"); + sdfJoint.SetChildName("m3::link3"); + EXPECT_TRUE(worldModel->ConstructJoint(sdfJoint)); + + sdf::Model sdfModel; + sdfModel.SetName("m4"); + sdfModel.AddLink(sdfLink); + EXPECT_TRUE(worldModel->ConstructNestedModel(sdfModel)); + EXPECT_TRUE(world->GetModel("m4")); + EXPECT_TRUE(worldModel->GetNestedModel("m4")); + + EXPECT_EQ(2u, world->GetModelCount()); + EXPECT_EQ(2u, worldModel->GetNestedModelCount()); + // Check that we can remove created via the World model proxy + EXPECT_EQ(m3, worldModel->GetNestedModel(0)); + EXPECT_TRUE(worldModel->RemoveNestedModel(0)); + EXPECT_TRUE(m3->Removed()); + EXPECT_EQ(1u, world->GetModelCount()); + EXPECT_EQ(1u, worldModel->GetNestedModelCount()); + EXPECT_EQ(nullptr, worldModel->GetNestedModel("m3")); + EXPECT_EQ(nullptr, world->GetModel("m3")); + + const auto m4 = worldModel->GetNestedModel("m4"); + ASSERT_NE(nullptr, m4); + EXPECT_TRUE(worldModel->RemoveNestedModel("m4")); + EXPECT_EQ(0u, world->GetModelCount()); + EXPECT_EQ(0u, worldModel->GetNestedModelCount()); + EXPECT_TRUE(m4->Removed()); + EXPECT_EQ(nullptr, worldModel->GetNestedModel("m4")); + EXPECT_EQ(nullptr, world->GetModel("m4")); + } +} + + int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); From b228f17672960845fe2aeee6f08b001bfcab3578 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 13 Nov 2023 22:55:17 +0000 Subject: [PATCH 09/29] support world model feature Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.hh | 57 ++++++++---- .../src/EntityManagementFeatures.cc | 31 +++++-- .../src/EntityManagementFeatures.hh | 5 +- bullet-featherstone/src/SDFFeatures.cc | 89 +++++++++++++----- dartsim/src/SDFFeatures.cc | 4 + test/common_test/free_joint_features.cc | 31 +++---- test/common_test/joint_features.cc | 3 + test/common_test/world_features.cc | 90 ++----------------- .../worlds/world_single_nested_model.sdf | 58 ++++++++++++ 9 files changed, 218 insertions(+), 150 deletions(-) create mode 100644 test/common_test/worlds/world_single_nested_model.sdf diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index c97ecfb4e..92ab30b43 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -275,7 +275,18 @@ class Base : public Implements3d> const auto id = this->GetNextEntity(); auto world = std::make_shared(std::move(_worldInfo)); this->worlds[id] = world; - return this->GenerateIdentity(id, world); + auto worldID = this->GenerateIdentity(id, world); + + auto worldModel = std::make_shared( + world->name, worldID, + Eigen::Isometry3d::Identity(), nullptr); + std::cerr << " === Add world model " << _worldInfo.name << " vs " << world << std::endl; + this->models[id] = worldModel; + world->modelNameToEntityId[worldModel->name] = id; + worldModel->indexInWorld = -1; + world->modelIndexToEntityId[worldModel->indexInWorld] = id; + + return worldID; } public: inline Identity AddModel( @@ -294,6 +305,11 @@ class Base : public Implements3d> world->modelNameToEntityId[model->name] = id; model->indexInWorld = world->nextModelIndex++; world->modelIndexToEntityId[model->indexInWorld] = id; + + auto worldModel = this->models.at(model->world); + worldModel->nestedModelEntityIds.push_back(id); + worldModel->nestedModelNameToEntityId[model->name] = id; + return this->GenerateIdentity(id, model); } @@ -380,12 +396,14 @@ class Base : public Implements3d> public: bool RemoveModelImpl(const Identity &_parentID, const Identity &_modelID) { + std::cerr << " ============= remove model impl " << std::endl; auto *model = this->ReferenceInterface(_modelID); if (!model) return false; + std::cerr << " ============= remove model impl 1 " << std::endl; - auto parentIt = this->models.find(_parentID); - bool isNested = parentIt != this->models.end(); + bool isNested = this->worlds.find(_parentID) == this->worlds.end(); + std::cerr << " ============= remove model impl 2 nested " << isNested << std::endl; // Remove nested models for (auto &nestedModelID : model->nestedModelEntityIds) @@ -394,28 +412,33 @@ class Base : public Implements3d> this->GenerateIdentity(nestedModelID, this->models.at(nestedModelID))); } model->nestedModelEntityIds.clear(); + std::cerr << " ============= remove model impl 3 " << std::endl; + + // remove references in parent model or world model + auto *parentModel = this->ReferenceInterface(_parentID); + auto nestedModelIt = parentModel->nestedModelNameToEntityId.find(model->name); + if (nestedModelIt != parentModel->nestedModelNameToEntityId.end()) + { + std::size_t nestedModelID = nestedModelIt->second; + parentModel->nestedModelNameToEntityId.erase(nestedModelIt); + parentModel->nestedModelEntityIds.erase(std::remove( + parentModel->nestedModelEntityIds.begin(), + parentModel->nestedModelEntityIds.end(), nestedModelID), + parentModel->nestedModelEntityIds.end()); + } - // If this is a nested model, remove references in parent model + std::cerr << " ============= remove model impl 4 " << std::endl; + + // If nested, we are done here. No need to remove multibody if (isNested) { - auto *parentModel = this->ReferenceInterface(_parentID); - auto nestedModelIt = parentModel->nestedModelNameToEntityId.find(model->name); - if (nestedModelIt != parentModel->nestedModelNameToEntityId.end()) - { - std::size_t nestedModelID = nestedModelIt->second; - parentModel->nestedModelNameToEntityId.erase(nestedModelIt); - parentModel->nestedModelEntityIds.erase(std::remove( - parentModel->nestedModelEntityIds.begin(), - parentModel->nestedModelEntityIds.end(), nestedModelID), - parentModel->nestedModelEntityIds.end()); - } return true; } + // Remove model from world auto *world = this->ReferenceInterface(model->world); if (!world) return false; - if (world->modelIndexToEntityId.erase(model->indexInWorld) == 0) { // The model has already been removed at some point @@ -451,6 +474,8 @@ class Base : public Implements3d> this->joints.erase(jointID); this->models.erase(_modelID); + + std::cerr << " ============= remove model impl done " << std::endl; return true; } diff --git a/bullet-featherstone/src/EntityManagementFeatures.cc b/bullet-featherstone/src/EntityManagementFeatures.cc index be9891ee5..84f1d579c 100644 --- a/bullet-featherstone/src/EntityManagementFeatures.cc +++ b/bullet-featherstone/src/EntityManagementFeatures.cc @@ -215,9 +215,11 @@ bool EntityManagementFeatures::RemoveModel(const Identity &_modelID) bool EntityManagementFeatures::ModelRemoved( const Identity &_modelID) const { - auto *model = this->ReferenceInterface(_modelID); - auto *world = this->ReferenceInterface(model->world); - return world->modelIndexToEntityId.count(model->indexInWorld) == 0; +// auto *model = this->ReferenceInterface(_modelID); +// auto *world = this->ReferenceInterface(model->world); +// return world->modelIndexToEntityId.count(model->indexInWorld) == 0; + + return this->models.find(_modelID) == this->models.end(); } ///////////////////////////////////////////////// @@ -373,9 +375,15 @@ Identity EntityManagementFeatures::GetEngineOfWorld( ///////////////////////////////////////////////// std::size_t EntityManagementFeatures::GetModelCount( - const Identity &) const + const Identity &_worldID) const { - return this->models.size(); + // Get world model and return its nested model count + auto modelIt = this->models.find(_worldID); + if (modelIt != this->models.end()) + { + return modelIt->second->nestedModelEntityIds.size(); + } + return 0u; } ///////////////////////////////////////////////// @@ -466,11 +474,22 @@ Identity EntityManagementFeatures::GetNestedModel( const Identity &_modelID, const std::string &_modelName) const { const auto modelInfo = this->ReferenceInterface(_modelID); - auto nestedModelID = modelInfo->nestedModelNameToEntityId.at(_modelName); + auto modelIt = modelInfo->nestedModelNameToEntityId.find(_modelName); + if (modelIt == modelInfo->nestedModelNameToEntityId.end()) + return this->GenerateInvalidId(); + auto nestedModelID = modelIt->second; return this->GenerateIdentity(nestedModelID, this->models.at(nestedModelID)); } +///////////////////////////////////////////////// +Identity EntityManagementFeatures::GetWorldModel(const Identity &_worldID) const +{ + std::cerr << "get world model " << std::endl; + std::cerr << " got " << this->models.at(_worldID) << std::endl; + return this->GenerateIdentity(_worldID, this->models.at(_worldID)); +} + } // namespace bullet_featherstone } // namespace physics } // namespace gz diff --git a/bullet-featherstone/src/EntityManagementFeatures.hh b/bullet-featherstone/src/EntityManagementFeatures.hh index a6c3367da..1cbb47813 100644 --- a/bullet-featherstone/src/EntityManagementFeatures.hh +++ b/bullet-featherstone/src/EntityManagementFeatures.hh @@ -41,7 +41,7 @@ struct EntityManagementFeatureList : gz::physics::FeatureList< GetShapeFromLink, GetWorldFromEngine, RemoveEntities, - RemoveModelFromWorld + WorldModelFeature > { }; class EntityManagementFeatures : @@ -176,6 +176,9 @@ class EntityManagementFeatures : public: Identity GetNestedModel( const Identity &_modelID, const std::string &_modelName) const override; + + // ----- World model feature ----- + public: Identity GetWorldModel(const Identity &_worldID) const override; }; } // namespace bullet_featherstone diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 9f8294f41..3476f7444 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -148,7 +148,14 @@ ::sdf::Errors ResolveJointPoseRelToLink(math::Pose3d &_resolvedPose, return errors; // joint pose is expressed relative to child link - if (joint->ChildName() == _link) + std::string childLinkName; + errors = joint->ResolveChildLink(childLinkName); + if (!errors.empty()) + { + childLinkName = joint->ChildName(); + } + + if (childLinkName == _link) { _resolvedPose = jointPose; return errors; @@ -173,8 +180,8 @@ ::sdf::Errors ResolveJointPoseRelToLink(math::Pose3d &_resolvedPose, // pose of child link math::Pose3d childLinkPose; - errors = LinkPoseInModelTree(childLinkPose, joint->ChildName(), _model); - std::cerr << " === child link pose in model tree " << joint->ChildName() << " " + errors = LinkPoseInModelTree(childLinkPose, childLinkName, _model); + std::cerr << " === child link pose in model tree " << childLinkName << " " << childLinkPose << std::endl; if (!errors.empty()) @@ -270,8 +277,18 @@ bool BuildTrees(const ::sdf::Model *_sdfModel, for (std::size_t i = 0; i < _sdfModel->JointCount(); ++i) { const auto *joint = _sdfModel->JointByIndex(i); - const auto &parentLinkName = joint->ParentName(); - const auto &childLinkName = joint->ChildName(); + std::string parentLinkName; + ::sdf::Errors errors = joint->ResolveParentLink(parentLinkName); + if (!errors.empty()) + { + parentLinkName = joint->ParentName(); + } + std::string childLinkName; + errors = joint->ResolveChildLink(childLinkName); + if (!errors.empty()) + { + childLinkName = joint->ChildName(); + } const std::string &modelName = _sdfModel->Name(); const auto *parent = _sdfModel->LinkByName(parentLinkName); const auto *child = _sdfModel->LinkByName(childLinkName); @@ -518,9 +535,19 @@ std::optional ValidateModel(const ::sdf::Model &_sdfModel) for (std::size_t i = 0; i < model.JointCount(); ++i) { const auto *joint = model.JointByIndex(i); - const auto &parentLinkName = joint->ParentName(); + std::string parentLinkName; + ::sdf::Errors errors = joint->ResolveParentLink(parentLinkName); + if (!errors.empty()) + { + parentLinkName = joint->ParentName(); + } + std::string childLinkName; + errors = joint->ResolveChildLink(childLinkName); + if (!errors.empty()) + { + childLinkName = joint->ChildName(); + } const auto *parent = model.LinkByName(parentLinkName); - const auto &childLinkName = joint->ChildName(); const auto *child = model.LinkByName(childLinkName); switch (joint->Type()) @@ -711,17 +738,21 @@ Identity SDFFeatures::ConstructSdfModelImpl( const ::sdf::Model &_sdfModel) { std::cerr << " ---------- constructing " << _sdfModel.Name() << std::endl; - // This function constructs the entire sdf model tree, including nested models - // So return the nested model identity if it is constructed already - auto pIt = this->models.find(_parentID); - if (pIt != this->models.end()) + // The ConstructSDFModelImpl function constructs the entire sdf model tree, + // including nested models So return the nested model identity if it is + // constructed already + auto wIt = this->worlds.find(_parentID); + if (wIt == this->worlds.end()) { - std::size_t nestedModelID = pIt->second->nestedModelNameToEntityId.at( + auto mIt = this->models.find(_parentID); + std::size_t nestedModelID = mIt->second->nestedModelNameToEntityId.at( _sdfModel.Name()); - return this->GenerateIdentity(nestedModelID, this->models[nestedModelID]); + return this->GenerateIdentity(nestedModelID, this->models.at(nestedModelID)); } + std::cerr << " ---------- constructing 1 " << _sdfModel.Name() << std::endl; auto structures = ValidateModel2(_sdfModel); + std::cerr << " ---------- constructing 2 " << _sdfModel.Name() << std::endl; if (structures.empty()) return this->GenerateInvalidId(); @@ -756,15 +787,15 @@ Identity SDFFeatures::ConstructSdfModelImpl( if (!_model) return false; - auto modelIt = this->models.find(_modelOrWorldID); - const bool isNested = (modelIt != this->models.end()); - + auto worldIt = this->worlds.find(_modelOrWorldID); + const bool isNested = worldIt == this->worlds.end(); auto modelID = [&] { if (isNested) { + auto modelIt = this->models.find(_modelOrWorldID); auto worldIdentity = modelIt->second->world; auto modelIdentity = - this->GenerateIdentity(_modelOrWorldID, this->models[_modelOrWorldID]); + this->GenerateIdentity(_modelOrWorldID, modelIt->second); return this->AddNestedModel( _model->Name(), modelIdentity, worldIdentity, rootInertialToLink, rootMultiBody); @@ -772,7 +803,7 @@ Identity SDFFeatures::ConstructSdfModelImpl( else { auto worldIdentity = this->GenerateIdentity( - _modelOrWorldID, this->worlds[_modelOrWorldID]); + _modelOrWorldID, worldIt->second); rootMultiBody = std::make_shared( static_cast(structure.flatLinks.size()), structure.mass, @@ -882,8 +913,14 @@ Identity SDFFeatures::ConstructSdfModelImpl( { const auto &parentInfo = structure.parentOf.at(link); const auto *joint = parentInfo.joint; + std::string parentLinkName; + ::sdf::Errors errors = joint->ResolveParentLink(parentLinkName); + if (!errors.empty()) + { + parentLinkName = joint->ParentName(); + } const auto &parentLinkID = linkIDs.at( - parentInfo.model->LinkByName(joint->ParentName())); + parentInfo.model->LinkByName(parentLinkName)); const auto *parentLinkInfo = this->ReferenceInterface( parentLinkID); @@ -900,8 +937,8 @@ Identity SDFFeatures::ConstructSdfModelImpl( Eigen::Isometry3d poseParentComToJoint; { gz::math::Pose3d gzPoseParentToJoint; - const auto errors = ResolveJointPoseRelToLink(gzPoseParentToJoint, - parentInfo.model, joint->Name(), joint->ParentName()); + errors = ResolveJointPoseRelToLink(gzPoseParentToJoint, + parentInfo.model, joint->Name(), parentLinkName); // const auto errors = joint->SemanticPose().Resolve( // gzPoseParentToJoint, joint->ParentName()); @@ -929,8 +966,14 @@ Identity SDFFeatures::ConstructSdfModelImpl( // const auto errors = // link->SemanticPose().Resolve(gzPoseJointToChild, joint->Name()); // this retrieves the joint pose relative to link - const auto errors = ResolveJointPoseRelToLink(gzPoseChildToJoint, - parentInfo.model, joint->Name(), joint->ChildName()); + std::string childLinkName; + errors = joint->ResolveChildLink(childLinkName); + if (!errors.empty()) + { + childLinkName = joint->ChildName(); + } + errors = ResolveJointPoseRelToLink(gzPoseChildToJoint, + parentInfo.model, joint->Name(), childLinkName); if (!errors.empty()) { diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index 66b6252d4..f4c0ee5f0 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -405,6 +405,9 @@ SDFFeatures::FindParentAndChildOfJoint(std::size_t _worldID, // Resolve parent and child frames to links std::string parentLinkName; ::sdf::Errors errors = _sdfJoint->ResolveParentLink(parentLinkName); + + std::cerr << " joint parent " << _sdfJoint->ParentName() << " resl " << parentLinkName << std::endl; + if (!errors.empty()) { gzerr << "The link of the parent frame [" << _sdfJoint->ParentName() @@ -431,6 +434,7 @@ SDFFeatures::FindParentAndChildOfJoint(std::size_t _worldID, } return {}; } + std::cerr << " joint child " << _sdfJoint->ChildName() << " resl " << childLinkName << std::endl; // When calling `FindBodyNode`, we need to check wheter the parent entity // (different from parent link/frame) of the joint is a model or world. If it diff --git a/test/common_test/free_joint_features.cc b/test/common_test/free_joint_features.cc index 89fd49f13..1ad2f2077 100644 --- a/test/common_test/free_joint_features.cc +++ b/test/common_test/free_joint_features.cc @@ -105,6 +105,11 @@ TEST_F(FreeGroupFeaturesTest, NestedFreeGroup) { for (const std::string &name : pluginNames) { + // https://github.com/gazebosim/gz-physics/issues/550 + // bullet-feathersone does not support multiple kinematic trees in + // a model so nested_model2 and nested_model3 are not constructed. + CHECK_UNSUPPORTED_ENGINE(name, "bullet-featherstone") + std::cout << "Testing plugin: " << name << std::endl; gz::plugin::PluginPtr plugin = loader.Instantiate(name); @@ -135,18 +140,6 @@ TEST_F(FreeGroupFeaturesTest, NestedFreeGroup) return testing::AssertionSuccess(); }; - if (engine->GetName() == "bullet-featherstone") - { - // https://github.com/gazebosim/gz-physics/issues/550 - // bullet-feathersone does not support multiple kinematic trees in - // a model so nested_model2 and nested_model3 are not constructed. - // \todo(iche033) Remove this block once the feature is supported. - EXPECT_FALSE(checkFreeGroupForModel("parent_model")); - EXPECT_FALSE(checkFreeGroupForModel("parent_model::nested_model2")); - EXPECT_FALSE(checkFreeGroupForModel("parent_model::nested_model3")); - GTEST_SKIP(); - } - EXPECT_TRUE(checkFreeGroupForModel("parent_model")); if (engine->GetName() == "tpe") { @@ -169,6 +162,11 @@ TEST_F(FreeGroupFeaturesTest, NestedFreeGroupSetWorldPose) { for (const std::string &name : pluginNames) { + // https://github.com/gazebosim/gz-physics/issues/550 + // bullet-feathersone does not support multiple kinematic trees in + // a model so nested_model2 and nested_model3 are not constructed. + CHECK_UNSUPPORTED_ENGINE(name, "bullet-featherstone") + std::cout << "Testing plugin: " << name << std::endl; gz::plugin::PluginPtr plugin = loader.Instantiate(name); @@ -186,15 +184,6 @@ TEST_F(FreeGroupFeaturesTest, NestedFreeGroupSetWorldPose) auto world = engine->ConstructWorld(*sdfWorld); EXPECT_NE(nullptr, world); - if (engine->GetName() == "bullet-featherstone") - { - // https://github.com/gazebosim/gz-physics/issues/550 - // bullet-feathersone does not support multiple kinematic trees in - // a model so nested_model2 and nested_model3 are not constructed. - // \todo(iche033) Remove this block once the feature is supported. - GTEST_SKIP(); - } - gz::math::Pose3d parentModelNewPose(0, 0, 2, 0, 0, 0); { auto parentModel = world->GetModel("parent_model"); diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index 2ceb48b83..8433f4e2a 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -1526,6 +1526,9 @@ TEST_F(WorldModelTest, JointSetCommand) { for (const std::string &name : this->pluginNames) { + // bullet-feathersone does not support joints between models yet + CHECK_UNSUPPORTED_ENGINE(name, "bullet-featherstone") + auto world = this->LoadWorld(name); ASSERT_NE(nullptr, world); diff --git a/test/common_test/world_features.cc b/test/common_test/world_features.cc index 81d6b820a..96e296b2c 100644 --- a/test/common_test/world_features.cc +++ b/test/common_test/world_features.cc @@ -361,12 +361,8 @@ struct WorldNestedModelFeatureList : gz::physics::FeatureList< 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 > { }; @@ -382,7 +378,7 @@ class WorldNestedModelTest : public WorldFeaturesTestpluginNames) { @@ -403,97 +399,25 @@ TEST_F(WorldModelTest, WorldConstructNestedModel) auto worldModel = world->GetWorldModel(); ASSERT_NE(nullptr, worldModel); EXPECT_EQ(world, worldModel->GetWorld()); - EXPECT_EQ("default", worldModel->GetName()); + EXPECT_EQ("nested_model_world", worldModel->GetName()); EXPECT_EQ(0, worldModel->GetLinkCount()); - EXPECT_EQ(nullptr, worldModel->GetLink(0)); - EXPECT_EQ(nullptr, worldModel->GetLink("nonexistent_link")); EXPECT_EQ(0, worldModel->GetIndex()); + EXPECT_EQ(1u, world->GetModelCount()); EXPECT_EQ(world->GetModelCount(), worldModel->GetNestedModelCount()); const auto nestedModel = worldModel->GetNestedModel(0); ASSERT_NE(nullptr, nestedModel); - EXPECT_EQ("m1", nestedModel->GetName()); - EXPECT_NE(nullptr, worldModel->GetNestedModel("m2")); - - // Check that removing a World model proxy is not allowed - EXPECT_FALSE(worldModel->Remove()); - EXPECT_TRUE(worldModel.Valid()); - EXPECT_FALSE(worldModel->Removed()); - ASSERT_NE(nullptr, worldModel); - EXPECT_EQ(world, worldModel->GetWorld()); - - auto worldModel2 = world->GetWorldModel(); - ASSERT_NE(nullptr, worldModel2); - EXPECT_EQ(world, worldModel2->GetWorld()); - - EXPECT_EQ(2u, world->GetModelCount()); - EXPECT_EQ(2u, worldModel->GetNestedModelCount()); + EXPECT_EQ("parent_model", nestedModel->GetName()); // Check that we can remove models via RemoveNestedModel EXPECT_TRUE(worldModel->RemoveNestedModel(0)); EXPECT_TRUE(nestedModel->Removed()); - EXPECT_EQ(1u, world->GetModelCount()); - EXPECT_EQ(1u, worldModel->GetNestedModelCount()); - EXPECT_NE(nullptr, worldModel->GetNestedModel(0)); - EXPECT_EQ(nullptr, worldModel->GetNestedModel("m1")); - - const auto m2 = worldModel->GetNestedModel("m2"); - ASSERT_NE(nullptr, m2); - EXPECT_TRUE(worldModel->RemoveNestedModel("m2")); - EXPECT_TRUE(m2->Removed()); EXPECT_EQ(0u, world->GetModelCount()); EXPECT_EQ(0u, worldModel->GetNestedModelCount()); - EXPECT_EQ(nullptr, worldModel->GetNestedModel("m2")); - - // Check that we can construct nested models and joints, but not links - auto m3 = worldModel->ConstructEmptyNestedModel("m3"); - ASSERT_TRUE(m3); - EXPECT_EQ(m3, world->GetModel("m3")); - EXPECT_EQ(m3, worldModel->GetNestedModel("m3")); - EXPECT_FALSE(worldModel->ConstructEmptyLink("test_link")); - - sdf::Link sdfLink; - sdfLink.SetName("link3"); - EXPECT_FALSE(worldModel->ConstructLink(sdfLink)); - - // Create a link in m3 for testing joint creation. - EXPECT_TRUE(m3->ConstructLink(sdfLink)); - sdf::Joint sdfJoint; - sdfJoint.SetName("test_joint"); - sdfJoint.SetType(sdf::JointType::FIXED); - sdfJoint.SetParentName("m2::link2"); - sdfJoint.SetChildName("m3::link3"); - EXPECT_TRUE(worldModel->ConstructJoint(sdfJoint)); - - sdf::Model sdfModel; - sdfModel.SetName("m4"); - sdfModel.AddLink(sdfLink); - EXPECT_TRUE(worldModel->ConstructNestedModel(sdfModel)); - EXPECT_TRUE(world->GetModel("m4")); - EXPECT_TRUE(worldModel->GetNestedModel("m4")); - - EXPECT_EQ(2u, world->GetModelCount()); - EXPECT_EQ(2u, worldModel->GetNestedModelCount()); - // Check that we can remove created via the World model proxy - EXPECT_EQ(m3, worldModel->GetNestedModel(0)); - EXPECT_TRUE(worldModel->RemoveNestedModel(0)); - EXPECT_TRUE(m3->Removed()); - EXPECT_EQ(1u, world->GetModelCount()); - EXPECT_EQ(1u, worldModel->GetNestedModelCount()); - EXPECT_EQ(nullptr, worldModel->GetNestedModel("m3")); - EXPECT_EQ(nullptr, world->GetModel("m3")); - - const auto m4 = worldModel->GetNestedModel("m4"); - ASSERT_NE(nullptr, m4); - EXPECT_TRUE(worldModel->RemoveNestedModel("m4")); - EXPECT_EQ(0u, world->GetModelCount()); - EXPECT_EQ(0u, worldModel->GetNestedModelCount()); - EXPECT_TRUE(m4->Removed()); - EXPECT_EQ(nullptr, worldModel->GetNestedModel("m4")); - EXPECT_EQ(nullptr, world->GetModel("m4")); + EXPECT_EQ(nullptr, worldModel->GetNestedModel(0)); + EXPECT_EQ(nullptr, worldModel->GetNestedModel("parent_model")); } } - int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); diff --git a/test/common_test/worlds/world_single_nested_model.sdf b/test/common_test/worlds/world_single_nested_model.sdf new file mode 100644 index 000000000..7c0fcff9c --- /dev/null +++ b/test/common_test/worlds/world_single_nested_model.sdf @@ -0,0 +1,58 @@ + + + + + + 1 2 2 0 0 0 + + 3 1 1 0 0 1.5707 + + + + 2 + + + + + + 0 1 0 1.5707 0 0 + + + + 2 + + + + + + nested_link1 + nested_link2 + + 1 0 0 + + + + + + + + 1 2 3 + + + + + + + 1 2 3 + + + + + + + link1 + nested_model::nested_link1 + + + + From d335865aa8b01d6a9fb2b99844c767c0d93bdf52 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 13 Nov 2023 23:23:02 +0000 Subject: [PATCH 10/29] clean up Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.hh | 8 - .../src/EntityManagementFeatures.cc | 2 - bullet-featherstone/src/SDFFeatures.cc | 840 +++--------------- dartsim/src/SDFFeatures.cc | 18 - 4 files changed, 140 insertions(+), 728 deletions(-) diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index 92ab30b43..e6907f8bb 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -280,7 +280,6 @@ class Base : public Implements3d> auto worldModel = std::make_shared( world->name, worldID, Eigen::Isometry3d::Identity(), nullptr); - std::cerr << " === Add world model " << _worldInfo.name << " vs " << world << std::endl; this->models[id] = worldModel; world->modelNameToEntityId[worldModel->name] = id; worldModel->indexInWorld = -1; @@ -396,14 +395,11 @@ class Base : public Implements3d> public: bool RemoveModelImpl(const Identity &_parentID, const Identity &_modelID) { - std::cerr << " ============= remove model impl " << std::endl; auto *model = this->ReferenceInterface(_modelID); if (!model) return false; - std::cerr << " ============= remove model impl 1 " << std::endl; bool isNested = this->worlds.find(_parentID) == this->worlds.end(); - std::cerr << " ============= remove model impl 2 nested " << isNested << std::endl; // Remove nested models for (auto &nestedModelID : model->nestedModelEntityIds) @@ -412,7 +408,6 @@ class Base : public Implements3d> this->GenerateIdentity(nestedModelID, this->models.at(nestedModelID))); } model->nestedModelEntityIds.clear(); - std::cerr << " ============= remove model impl 3 " << std::endl; // remove references in parent model or world model auto *parentModel = this->ReferenceInterface(_parentID); @@ -427,8 +422,6 @@ class Base : public Implements3d> parentModel->nestedModelEntityIds.end()); } - std::cerr << " ============= remove model impl 4 " << std::endl; - // If nested, we are done here. No need to remove multibody if (isNested) { @@ -475,7 +468,6 @@ class Base : public Implements3d> this->models.erase(_modelID); - std::cerr << " ============= remove model impl done " << std::endl; return true; } diff --git a/bullet-featherstone/src/EntityManagementFeatures.cc b/bullet-featherstone/src/EntityManagementFeatures.cc index 84f1d579c..c86f5440e 100644 --- a/bullet-featherstone/src/EntityManagementFeatures.cc +++ b/bullet-featherstone/src/EntityManagementFeatures.cc @@ -485,8 +485,6 @@ Identity EntityManagementFeatures::GetNestedModel( ///////////////////////////////////////////////// Identity EntityManagementFeatures::GetWorldModel(const Identity &_worldID) const { - std::cerr << "get world model " << std::endl; - std::cerr << " got " << this->models.at(_worldID) << std::endl; return this->GenerateIdentity(_worldID, this->models.at(_worldID)); } diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 3476f7444..0241f5d06 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -78,7 +78,79 @@ static std::optional ResolveSdfPose( } ///////////////////////////////////////////////// -::sdf::Errors LinkPoseInModelTree(math::Pose3d &_pose, +Identity SDFFeatures::ConstructSdfWorld( + const Identity &_engine, + const ::sdf::World &_sdfWorld) +{ + const Identity worldID = this->ConstructEmptyWorld(_engine, _sdfWorld.Name()); + + const WorldInfoPtr &worldInfo = this->worlds.at(worldID); + + worldInfo->world->setGravity(convertVec(_sdfWorld.Gravity())); + + for (std::size_t i = 0; i < _sdfWorld.ModelCount(); ++i) + { + const ::sdf::Model *model = _sdfWorld.ModelByIndex(i); + + if (!model) + continue; + + this->ConstructSdfModel(worldID, *model); + } + + return worldID; +} + +///////////////////////////////////////////////// +struct ParentInfo +{ + const ::sdf::Joint *joint; + const ::sdf::Model *model; + const ::sdf::Link *link; + const ::sdf::Link *parent; +}; + +///////////////////////////////////////////////// +struct Structure +{ + /// The root link of the model + const ::sdf::Link *rootLink; + const ::sdf::Model *model; + const ::sdf::Joint *rootJoint; + btScalar mass; + btVector3 inertia; + math::Pose3d linkToPrincipalAxesPose; + + /// Is the root link fixed + bool fixedBase; + + /// Get the parent joint of the link + std::unordered_map parentOf; + + /// This contains all the links except the root link + std::vector flatLinks; +}; + +///////////////////////////////////////////////// +void extractInertial( + const math::Inertiald &_inertial, + btScalar &_mass, + btVector3 &_principalInertiaMoments, + math::Pose3d &_linkToPrincipalAxesPose) +{ + const auto &M = _inertial.MassMatrix(); + _mass = static_cast(M.Mass()); + _principalInertiaMoments = convertVec(M.PrincipalMoments()); + _linkToPrincipalAxesPose = _inertial.Pose(); + _linkToPrincipalAxesPose.Rot() *= M.PrincipalAxesOffset(); +} + +///////////////////////////////////////////////// +/// \brief Get pose of link in the model tree +/// \param[out] _pose Pose of link relative to model +/// \param[in] _linkName Scoped name of link +/// \param[in] _model Model SDF +::sdf::Errors linkPoseInModelTree(math::Pose3d &_pose, const std::string &_linkName, const ::sdf::Model *_model) { @@ -102,7 +174,7 @@ ::sdf::Errors LinkPoseInModelTree(math::Pose3d &_pose, if (!errors.empty()) return errors; _pose = _pose * p; - return LinkPoseInModelTree(_pose, nestedLinkName, nestedModel); + return linkPoseInModelTree(_pose, nestedLinkName, nestedModel); } } else @@ -125,11 +197,11 @@ ::sdf::Errors LinkPoseInModelTree(math::Pose3d &_pose, ///////////////////////////////////////////////// /// \brief Get pose of joint relative to link -/// \param[out] _resolvePose Pose of joint relative to link +/// \param[out] _resolvedPose Pose of joint relative to link /// \param[in] _model Parent model of joint /// \param[in] _joint Joint name /// \param[in] _link Scoped link name -::sdf::Errors ResolveJointPoseRelToLink(math::Pose3d &_resolvedPose, +::sdf::Errors resolveJointPoseRelToLink(math::Pose3d &_resolvedPose, const ::sdf::Model *_model, const std::string &_joint, const std::string &_link) { @@ -171,108 +243,32 @@ ::sdf::Errors ResolveJointPoseRelToLink(math::Pose3d &_resolvedPose, } math::Pose3d parentLinkPose; - errors = LinkPoseInModelTree(parentLinkPose, _link, _model); - std::cerr << " === parent link pose in model tree " << _link << " " - << parentLinkPose - << std::endl; + errors = linkPoseInModelTree(parentLinkPose, _link, _model); if (!errors.empty()) return errors; // pose of child link math::Pose3d childLinkPose; - errors = LinkPoseInModelTree(childLinkPose, childLinkName, _model); - std::cerr << " === child link pose in model tree " << childLinkName << " " - << childLinkPose - << std::endl; + errors = linkPoseInModelTree(childLinkPose, childLinkName, _model); if (!errors.empty()) return errors; auto jointPoseInModel = childLinkPose * jointPose; - std::cerr << "joint erros " << errors.size() << std::endl; - // std::cerr << "link pose " << linkPose << std::endl; - std::cerr << "joint pose " << jointPose << std::endl; _resolvedPose = parentLinkPose.Inverse() * jointPoseInModel; - // _resolvedPose = jointPose.Inverse() * linkPose; - std::cerr << "out pose " << _resolvedPose << std::endl; return errors; } ///////////////////////////////////////////////// -Identity SDFFeatures::ConstructSdfWorld( - const Identity &_engine, - const ::sdf::World &_sdfWorld) -{ - const Identity worldID = this->ConstructEmptyWorld(_engine, _sdfWorld.Name()); - - const WorldInfoPtr &worldInfo = this->worlds.at(worldID); - - worldInfo->world->setGravity(convertVec(_sdfWorld.Gravity())); - - for (std::size_t i = 0; i < _sdfWorld.ModelCount(); ++i) - { - const ::sdf::Model *model = _sdfWorld.ModelByIndex(i); - - if (!model) - continue; - - this->ConstructSdfModel(worldID, *model); - } - - return worldID; -} - -///////////////////////////////////////////////// -struct ParentInfo -{ - const ::sdf::Joint *joint; - const ::sdf::Model *model; - const ::sdf::Link *link; - const ::sdf::Link *parent; -}; - -///////////////////////////////////////////////// -struct Structure -{ - /// The root link of the model - const ::sdf::Link *rootLink; - const ::sdf::Model *model; - const ::sdf::Joint *rootJoint; - btScalar mass; - btVector3 inertia; - math::Pose3d linkToPrincipalAxesPose; - - /// Is the root link fixed - bool fixedBase; - - /// Get the parent joint of the link - std::unordered_map parentOf; - - /// This contains all the links except the root link - std::vector flatLinks; -}; - -///////////////////////////////////////////////// -void extractInertial( - const math::Inertiald &_inertial, - btScalar &_mass, - btVector3 &_principalInertiaMoments, - math::Pose3d &_linkToPrincipalAxesPose) -{ - const auto &M = _inertial.MassMatrix(); - _mass = static_cast(M.Mass()); - _principalInertiaMoments = convertVec(M.PrincipalMoments()); - _linkToPrincipalAxesPose = _inertial.Pose(); - _linkToPrincipalAxesPose.Rot() *= M.PrincipalAxesOffset(); -} - - -///////////////////////////////////////////////// -bool BuildTrees(const ::sdf::Model *_sdfModel, - // std::unordered_map &_nodes, +/// \brief Recursively build a tree of parent-child data structures from the +/// \param[in] _sdfModel input Model SDF. +/// \param[out] _parentOf A map of child link to its parent +/// \param[out] _linkTree A map of parent link to its child links +bool buildTrees(const ::sdf::Model *_sdfModel, std::unordered_map &_parentOf, - std::unordered_map> &_linkTree) + std::unordered_map> &_linkTree) { for (std::size_t i = 0; i < _sdfModel->JointCount(); ++i) { @@ -293,9 +289,6 @@ bool BuildTrees(const ::sdf::Model *_sdfModel, const auto *parent = _sdfModel->LinkByName(parentLinkName); const auto *child = _sdfModel->LinkByName(childLinkName); - // std::cerr << "parent link name " << parentLinkName << " " << parent << std::endl; - // std::cerr << "child link name " << childLinkName << " " << child << std::endl; - switch (joint->Type()) { case ::sdf::JointType::FIXED: @@ -326,17 +319,6 @@ bool BuildTrees(const ::sdf::Model *_sdfModel, else if (nullptr == parent) { // This link is attached to the world, making it the root -/* if (nullptr != rootLink) - { - // A root already exists for this model - gzerr << "Two root links were found for Model [" << modelName - << "]: [" << rootLink->Name() << "] and [" << childLinkName - << "], but gz-physics-bullet-featherstone-plugin only " - << "supports one root per Model.\n"; - return false; - } -*/ - if (joint->Type() != ::sdf::JointType::FIXED) { gzerr << "Link [" << child->Name() << "] in Model [" @@ -348,17 +330,8 @@ bool BuildTrees(const ::sdf::Model *_sdfModel, << "gz-physics-bullet-featherstone-plugin\n"; return false; } - - // TODO determine this later in a separate function? - // rootLink = child; - // rootJoint = joint; - // fixed = true; - - // Do not add the root link to the set of links that have parents - // continue; } - std::cerr << " parent of inserting " << child << " " << parent << std::endl; if (!_parentOf.insert( std::make_pair(child, ParentInfo{joint, _sdfModel, child, parent})).second) { @@ -370,36 +343,33 @@ bool BuildTrees(const ::sdf::Model *_sdfModel, { _linkTree[parent].push_back(child); } - // std::cerr << "parent of inserted " << child->Name() << std::endl; - // std::cerr << "parent of size " << _parentOf.size() << std::endl; } // Recursively build tree from nested models for (std::size_t i = 0; i < _sdfModel->ModelCount(); ++i) { const auto *model = _sdfModel->ModelByIndex(i); - if (!BuildTrees(model, _parentOf, _linkTree)) + if (!buildTrees(model, _parentOf, _linkTree)) return false; } return true; } ///////////////////////////////////////////////// -void FindRootLinks(const ::sdf::Model *_sdfModel, +/// \brief Find all the root links given a model SDF +/// \param[in] _sdfModel Model SDF +/// \param[in] _parentOf A map of child link to its parent info +/// \param[out] _rootLinks A vector of root links and its immediate parent model +void findRootLinks(const ::sdf::Model *_sdfModel, const std::unordered_map &_parentOf, std::vector> &_rootLinks) { - // std::cerr << "find root links parentof size " << _parentOf.size() << std::endl; - // for (auto p : _parentOf) - // std::cerr << " parentOf[i] " << p.first << std::endl; - // std::cerr << "model link count " << _sdfModel->Name() << " " << _sdfModel->LinkCount() << std::endl; for (std::size_t i = 0; i < _sdfModel->LinkCount(); ++i) { const auto *link = _sdfModel->LinkByIndex(i); auto parentOfIt = _parentOf.find(link); if (parentOfIt == _parentOf.end() || !parentOfIt->second.parent) { - // std::cerr << "root link inserting " << link->Name() << " " << link <ModelCount(); ++i) { const auto *model = _sdfModel->ModelByIndex(i); - FindRootLinks(model, _parentOf, _rootLinks); + findRootLinks(model, _parentOf, _rootLinks); } } ///////////////////////////////////////////////// -std::optional BuildStructure( +/// \brief Build a structure for each root link +/// \param[in] _rootLink Root link in a model tree +/// \param[in] _parentOf A map of child link to its parent +/// \param[in] _linkTree A map of parent link to its child links +std::optional buildStructure( const ::sdf::Link * _rootLink, const ::sdf::Model * _model, const std::unordered_map &_parentOf, @@ -463,13 +437,14 @@ std::optional BuildStructure( math::Pose3d linkToPrincipalAxesPose; extractInertial(_rootLink->Inertial(), mass, inertia, linkToPrincipalAxesPose); - std::cerr << " structure: " << std::endl; - std::cerr << " " << _model->Name() << std::endl; - std::cerr << " " << _rootLink->Name() << std::endl; - std::cerr << " " << ((rootJoint) ? rootJoint->Name() : "N/A") << std::endl; - std::cerr << " " << mass << std::endl; - std::cerr << " " << fixed << std::endl; - std::cerr << " " << flatLinks.size() << std::endl; + // Uncomment to debug structure + // std::cout << "Structure: " << std::endl; + // std::cout << " model: " << _model->Name() << std::endl; + // std::cout << " root link: " << _rootLink->Name() << std::endl; + // std::cout << " root joint: " << ((rootJoint) ? rootJoint->Name() : "N/A") << std::endl; + // std::cout << " mass: " << mass << std::endl; + // std::cout << " fixed: " << fixed << std::endl; + // std::cout << " flatLinks size: " << flatLinks.size() << std::endl; return Structure{ _rootLink, _model, rootJoint, mass, inertia, linkToPrincipalAxesPose, fixed, @@ -477,13 +452,16 @@ std::optional BuildStructure( } ///////////////////////////////////////////////// -std::vector ValidateModel2(const ::sdf::Model &_sdfModel) +/// \brief Validate input model SDF and build a vector of structures. +/// Each structure represents a single kinematic tree in the model +/// \param[in] _sdfModel Input model SDF +/// \return A vector of structures +std::vector validateModel(const ::sdf::Model &_sdfModel) { - // std::unordered_map nodes; - // a map of child link and its parent info + // A map of child link and its parent info std::unordered_map parentOf; - // a map of parent link to a vector of its child links + // A map of parent link to a vector of its child links std::unordered_map> linkTree; @@ -491,8 +469,8 @@ std::vector ValidateModel2(const ::sdf::Model &_sdfModel) // Use a vector to preseve order of entities defined in sdf std::vector> rootLinks; - BuildTrees(&_sdfModel, parentOf, linkTree); - FindRootLinks(&_sdfModel, parentOf, rootLinks); + buildTrees(&_sdfModel, parentOf, linkTree); + findRootLinks(&_sdfModel, parentOf, rootLinks); std::vector structures; if (rootLinks.empty()) @@ -505,7 +483,7 @@ std::vector ValidateModel2(const ::sdf::Model &_sdfModel) // Build subtrees for (const auto &rootLinkIt : rootLinks) { - auto structure = BuildStructure(rootLinkIt.first, rootLinkIt.second, + auto structure = buildStructure(rootLinkIt.first, rootLinkIt.second, parentOf, linkTree); if (structure.has_value()) { @@ -516,215 +494,6 @@ std::vector ValidateModel2(const ::sdf::Model &_sdfModel) return structures; } -///////////////////////////////////////////////// -std::optional ValidateModel(const ::sdf::Model &_sdfModel) -{ - std::unordered_map parentOf; - const ::sdf::Link *rootLink = nullptr; - 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, &linkTree]( - const ::sdf::Model &model) -> bool - { - for (std::size_t i = 0; i < model.JointCount(); ++i) - { - const auto *joint = model.JointByIndex(i); - std::string parentLinkName; - ::sdf::Errors errors = joint->ResolveParentLink(parentLinkName); - if (!errors.empty()) - { - parentLinkName = joint->ParentName(); - } - std::string childLinkName; - errors = joint->ResolveChildLink(childLinkName); - if (!errors.empty()) - { - childLinkName = joint->ChildName(); - } - const auto *parent = model.LinkByName(parentLinkName); - const auto *child = model.LinkByName(childLinkName); - - switch (joint->Type()) - { - case ::sdf::JointType::FIXED: - case ::sdf::JointType::REVOLUTE: - case ::sdf::JointType::PRISMATIC: - case ::sdf::JointType::BALL: - break; - default: - gzerr << "Joint type [" << (std::size_t)(joint->Type()) - << "] is not supported by " - << "gz-physics-bullet-featherstone-plugin. " - << "Replaced by a fixed joint.\n"; - } - - if (child == parent) - { - gzerr << "The Link [" << parentLinkName << "] is being attached to " - << "itself by Joint [" << joint->Name() << "] in Model [" - << rootModelName << "]. That is not allowed.\n"; - return false; - } - - if (nullptr == parent && parentLinkName != "world") - { - gzerr << "The link [" << parentLinkName << "] cannot be found in " - << "Model [" << rootModelName << "], but joint [" - << joint->Name() << "] wants to use it as its parent link\n"; - return false; - } - else if (nullptr == parent) - { - // This link is attached to the world, making it the root - if (nullptr != rootLink) - { - // A root already exists for this model - gzerr << "Two root links were found for Model [" << rootModelName - << "]: [" << rootLink->Name() << "] and [" << childLinkName - << "], but gz-physics-bullet-featherstone-plugin only " - << "supports one root per Model.\n"; - return false; - } - - if (joint->Type() != ::sdf::JointType::FIXED) - { - gzerr << "Link [" << child->Name() << "] in Model [" - << rootModelName << "] is being connected to the " - << "world by Joint [" << joint->Name() << "] with a [" - << (std::size_t)(joint->Type()) << "] joint type, but only " - << "Fixed (" << (std::size_t)(::sdf::JointType::FIXED) - << ") is supported by " - << "gz-physics-bullet-featherstone-plugin\n"; - return false; - } - - rootLink = child; - rootJoint = joint; - fixed = true; - - // Do not add the root link to the set of links that have parents - continue; - } - - if (!parentOf.insert( - std::make_pair(child, ParentInfo{joint, &model})).second) - { - gzerr << "The Link [" << childLinkName << "] in Model [" - << 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; - }; - - if (!checkModel(_sdfModel)) - return std::nullopt; - - for (std::size_t i = 0; i < _sdfModel.ModelCount(); ++i) - { - if (!checkModel(*_sdfModel.ModelByIndex(i))) - return std::nullopt; - } - - // 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) - { - const auto *link = model.LinkByIndex(i); - if (parentOf.count(link) == 0) - { - // This link must be the root. If a different link was already - // identified as the root then we have a conflict. - if (rootLink && rootLink != link) - { - gzerr << "Two root links were found for Model [" << rootModelName - << "]: [" << rootLink->Name() << "] and [" << link->Name() - << "]. The Link [" << link->Name() << "] is implicitly a " - << "root because it has no parent joint.\n"; - return false; - } - - rootLink = link; - } - } - - return true; - }; - - 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 (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 - // have a lower index than their child links, but the Featherstone Algorithm - // needs to crawl up and down the tree systematically, and so the flattened - // tree structures used by the algorithm usually do expect the parents to - // 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. - std::vector flatLinks; - std::function flattenLinkTree = - [&](const ::sdf::Link *link) - { - if (link != rootLink) - { - flatLinks.push_back(link); - } - if (auto it = linkTree.find(link); it != linkTree.end()) - { - for (const auto &child_link : it->second) - { - flattenLinkTree(child_link); - } - } - }; - flattenLinkTree(rootLink); - - btScalar mass; - btVector3 inertia; - math::Pose3d linkToPrincipalAxesPose; - extractInertial(rootLink->Inertial(), mass, inertia, linkToPrincipalAxesPose); - - return Structure{ - rootLink, &_sdfModel, rootJoint, mass, inertia, linkToPrincipalAxesPose, fixed, - parentOf, flatLinks}; -} - ///////////////////////////////////////////////// Identity SDFFeatures::ConstructSdfNestedModel(const Identity &_parentID, const ::sdf::Model &_sdfModel) @@ -737,7 +506,6 @@ Identity SDFFeatures::ConstructSdfModelImpl( std::size_t _parentID, const ::sdf::Model &_sdfModel) { - std::cerr << " ---------- constructing " << _sdfModel.Name() << std::endl; // The ConstructSDFModelImpl function constructs the entire sdf model tree, // including nested models So return the nested model identity if it is // constructed already @@ -749,17 +517,15 @@ Identity SDFFeatures::ConstructSdfModelImpl( _sdfModel.Name()); return this->GenerateIdentity(nestedModelID, this->models.at(nestedModelID)); } - std::cerr << " ---------- constructing 1 " << _sdfModel.Name() << std::endl; - auto structures = ValidateModel2(_sdfModel); - std::cerr << " ---------- constructing 2 " << _sdfModel.Name() << std::endl; + auto structures = validateModel(_sdfModel); if (structures.empty()) return this->GenerateInvalidId(); if (structures.size() > 1) { // multiple subt-trees detected in model - // \todo(iche033) support multiple sub-tree kinematic trees and + // \todo(iche033) support multiple kinematic trees and // multiple floating links in a single model gzerr << "Multiple subt-trees / floating links detected in a model. " << "This is not supported in bullet-featherstone implementation yet." @@ -767,7 +533,6 @@ Identity SDFFeatures::ConstructSdfModelImpl( } // Create model for the first structure. auto structure = structures[0]; - std::cerr << " Adding structure " << structure.model->Name() << ": " << std::endl; const bool isStatic = _sdfModel.Static(); WorldInfo *world = nullptr; @@ -826,8 +591,6 @@ Identity SDFFeatures::ConstructSdfModelImpl( linkParentModelIds[link] = modelID; } modelIDs.insert(std::make_pair(_model, modelID)); - std::cerr << " Add model " << _model->Name() << " ID: " - << std::size_t(modelID) << std::endl; // recursively add nested models for (std::size_t i = 0; i < _model->ModelCount(); ++i) @@ -848,8 +611,6 @@ Identity SDFFeatures::ConstructSdfModelImpl( auto modelID = this->GenerateIdentity(rootModelID, this->models[rootModelID]); - std::cerr << " adding root link " << structure.rootLink->Name() - << " from " << _sdfModel.Name() << std::endl; // Add base link const auto rootID = this->AddLink(LinkInfo{ @@ -873,14 +634,9 @@ Identity SDFFeatures::ConstructSdfModelImpl( }); } - std::cerr <<" 1 ========================================== " << std::endl; - model->body->setLinearDamping(0.0); model->body->setAngularDamping(0.0); - std::cerr <<" 2 ========================================== flatlink size " - << structure.flatLinks.size() << std::endl; - std::unordered_map linkIDs; linkIDs.insert(std::make_pair(structure.rootLink, rootID)); @@ -898,15 +654,12 @@ Identity SDFFeatures::ConstructSdfModelImpl( if (linkIDs.find(link) == linkIDs.end()) { std::size_t parentModelID = linkParentModelIds[link]; - std::cerr << " === = == = = parent model id " << parentModelID << " " - << (this->models.find(parentModelID) != this->models.end() )<< std::endl; const auto linkID = this->AddLink( LinkInfo{link->Name(), i, // modelID, this->GenerateIdentity(parentModelID, this->models.at(parentModelID)), linkToComTf.inverse()}); linkIDs.insert(std::make_pair(link, linkID)); - std::cerr << " addlink " << link->Name() << " " << i << std::endl; } if (structure.parentOf.size()) @@ -928,16 +681,11 @@ Identity SDFFeatures::ConstructSdfModelImpl( if (parentLinkInfo->indexInModel.has_value()) parentIndex = *parentLinkInfo->indexInModel; - // TODO - // std::cerr << " model name " << parentInfo.model->Name() << std::endl; - // std::cerr << " joint p name " << joint->ParentName() << std::endl; - // std::cerr << " parent link index " << parentIndex << std::endl; - Eigen::Isometry3d poseParentLinkToJoint; Eigen::Isometry3d poseParentComToJoint; { gz::math::Pose3d gzPoseParentToJoint; - errors = ResolveJointPoseRelToLink(gzPoseParentToJoint, + errors = resolveJointPoseRelToLink(gzPoseParentToJoint, parentInfo.model, joint->Name(), parentLinkName); // const auto errors = joint->SemanticPose().Resolve( // gzPoseParentToJoint, joint->ParentName()); @@ -972,7 +720,7 @@ Identity SDFFeatures::ConstructSdfModelImpl( { childLinkName = joint->ChildName(); } - errors = ResolveJointPoseRelToLink(gzPoseChildToJoint, + errors = resolveJointPoseRelToLink(gzPoseChildToJoint, parentInfo.model, joint->Name(), childLinkName); if (!errors.empty()) @@ -995,46 +743,32 @@ Identity SDFFeatures::ConstructSdfModelImpl( convertMat(poseParentComToJoint.linear()) .getRotation(btRotParentComToJoint); - // TODO - // std::cerr << " model name " << _sdfModel.Name() << std::endl; - // std::cerr << " joint p name " << joint->ParentName() << std::endl; - // std::cerr << " joint c name " << joint->ChildName() << std::endl; - // - - // auto linkParent = _sdfModel.LinkByName(joint->ParentName()); - // gz::math::Pose3d parent2joint = gzPoseParentToJoint.Inverse(); - //const auto errors2 = linkParent->SemanticPose().Resolve( - // parent2joint, joint->Name()); // X_JP - - // std::cerr << "parent2joint " << parent2joint << " vs " << gzPoseParentToJoint.Inverse() << std::endl;; - - btTransform parentLocalInertialFrame = convertTf( - parentLinkInfo->inertiaToLinkFrame); - btTransform parent2jointBt = convertTf( - // gz::math::eigen3::convert(parent2joint.Inverse())); // X_PJ - poseParentLinkToJoint); // X_PJ - - // offsetInABt = parent COM to pivot (X_IpJ) - // offsetInBBt = current COM to pivot (X_IcJ) - // - // X_PIp - // X_PJ - // X_IpJ = X_PIp^-1 * X_PJ - // - // X_IcJ = X_CIc^-1 * X_CJ - btTransform offsetInABt, offsetInBBt; - offsetInABt = parentLocalInertialFrame * parent2jointBt; - offsetInBBt = - convertTf(linkToComTf.inverse() * poseJointToChild.inverse()); - // R_IcJ * R_IpJ ^ -1 = R_IcIp; - btQuaternion parentRotToThis = - offsetInBBt.getRotation() * offsetInABt.inverse().getRotation(); + btTransform parentLocalInertialFrame = convertTf( + parentLinkInfo->inertiaToLinkFrame); + btTransform parent2jointBt = convertTf( + // gz::math::eigen3::convert(parent2joint.Inverse())); // X_PJ + poseParentLinkToJoint); // X_PJ + + // offsetInABt = parent COM to pivot (X_IpJ) + // offsetInBBt = current COM to pivot (X_IcJ) + // + // X_PIp + // X_PJ + // X_IpJ = X_PIp^-1 * X_PJ + // + // X_IcJ = X_CIc^-1 * X_CJ + btTransform offsetInABt, offsetInBBt; + offsetInABt = parentLocalInertialFrame * parent2jointBt; + offsetInBBt = + convertTf(linkToComTf.inverse() * poseJointToChild.inverse()); + // R_IcJ * R_IpJ ^ -1 = R_IcIp; + btQuaternion parentRotToThis = + offsetInBBt.getRotation() * offsetInABt.inverse().getRotation(); auto jointID = this->AddJoint( JointInfo{ joint->Name(), InternalJoint{i}, - // model->linkEntityIds[static_cast(parentIndex+1)], linkIDs.find(parentInfo.parent)->second, linkIDs.find(link)->second, poseParentLinkToJoint, @@ -1042,9 +776,6 @@ Identity SDFFeatures::ConstructSdfModelImpl( // modelID modelIDs.find(parentInfo.model)->second }); - std::cerr << " ================= add joint " << joint->Name() - << " " << std::size_t(linkIDs.find(parentInfo.parent)->second) << " " << - std::size_t(linkIDs.find(link)->second) << std::endl; auto jointInfo = this->ReferenceInterface(jointID); if (::sdf::JointType::PRISMATIC == joint->Type() || @@ -1125,10 +856,6 @@ Identity SDFFeatures::ConstructSdfModelImpl( model->body->setHasSelfCollision(_sdfModel.SelfCollide()); model->body->finalizeMultiDof(); - - std::cerr << " body num links " << model->name << ": " - << model->body->getNumDofs() << std::endl; - const auto worldToModel = ResolveSdfPose(_sdfModel.SemanticPose()); if (!worldToModel) return this->GenerateInvalidId(); @@ -1168,7 +895,6 @@ Identity SDFFeatures::ConstructSdfModelImpl( } } - std::cerr <<" done ========================================== " << std::endl; return modelID; } @@ -1178,292 +904,6 @@ Identity SDFFeatures::ConstructSdfModel( const ::sdf::Model &_sdfModel) { return this->ConstructSdfModelImpl(_worldID, _sdfModel); - -////////////////////////////// -/* const auto validation = ValidateModel(_sdfModel); - if (!validation.has_value()) - return this->GenerateInvalidId(); - - const auto &structure = *validation; - const bool isStatic = _sdfModel.Static(); - - const auto *world = this->ReferenceInterface(_worldID); - - const auto rootInertialToLink = - gz::math::eigen3::convert(structure.linkToPrincipalAxesPose).inverse(); - const auto modelID = this->AddModel( - _sdfModel.Name(), _worldID, rootInertialToLink, - std::make_unique( - static_cast(structure.flatLinks.size()), - structure.mass, - structure.inertia, - structure.fixedBase || isStatic, - true)); - - const auto rootID = - this->AddLink(LinkInfo{ - structure.rootLink->Name(), std::nullopt, modelID, rootInertialToLink - }); - const auto *model = this->ReferenceInterface(modelID); - - if (structure.rootJoint) - { - this->AddJoint( - JointInfo{ - structure.rootJoint->Name(), - RootJoint{}, - std::nullopt, - rootID, - Eigen::Isometry3d::Identity(), - Eigen::Isometry3d::Identity(), - modelID - }); - } - - model->body->setLinearDamping(0.0); - model->body->setAngularDamping(0.0); - - std::unordered_map linkIDs; - linkIDs.insert(std::make_pair(structure.rootLink, rootID)); - - for (int i = 0; i < static_cast(structure.flatLinks.size()); ++i) - { - const auto *link = structure.flatLinks[static_cast(i)]; - btScalar mass; - btVector3 inertia; - math::Pose3d linkToPrincipalAxesPose; - extractInertial(link->Inertial(), mass, inertia, linkToPrincipalAxesPose); - const Eigen::Isometry3d linkToComTf = gz::math::eigen3::convert( - linkToPrincipalAxesPose); - - if (linkIDs.find(link) == linkIDs.end()) - { - const auto linkID = this->AddLink( - LinkInfo{link->Name(), i, modelID, linkToComTf.inverse()}); - linkIDs.insert(std::make_pair(link, linkID)); - } - - if (structure.parentOf.size()) - { - const auto &parentInfo = structure.parentOf.at(link); - const auto *joint = parentInfo.joint; - const auto &parentLinkID = linkIDs.at( - parentInfo.model->LinkByName(joint->ParentName())); - const auto *parentLinkInfo = this->ReferenceInterface( - parentLinkID); - - int parentIndex = -1; - if (parentLinkInfo->indexInModel.has_value()) - parentIndex = *parentLinkInfo->indexInModel; - - Eigen::Isometry3d poseParentLinkToJoint; - Eigen::Isometry3d poseParentComToJoint; - { - gz::math::Pose3d gzPoseParentToJoint; - const auto errors = joint->SemanticPose().Resolve( - gzPoseParentToJoint, joint->ParentName()); - if (!errors.empty()) - { - gzerr << "An error occurred while resolving the transform of Joint [" - << joint->Name() << "] in Model [" << model->name << "]:\n"; - for (const auto &error : errors) - { - gzerr << error << "\n"; - } - - return this->GenerateInvalidId(); - } - - poseParentLinkToJoint = gz::math::eigen3::convert(gzPoseParentToJoint); - poseParentComToJoint = - poseParentLinkToJoint * parentLinkInfo->inertiaToLinkFrame; - } - - Eigen::Isometry3d poseJointToChild; - { - gz::math::Pose3d gzPoseJointToChild; - const auto errors = - link->SemanticPose().Resolve(gzPoseJointToChild, joint->Name()); - if (!errors.empty()) - { - gzerr << "An error occured while resolving the transform of Link [" - << link->Name() << "]:\n"; - for (const auto &error : errors) - { - gzerr << error << "\n"; - } - - return this->GenerateInvalidId(); - } - - poseJointToChild = gz::math::eigen3::convert(gzPoseJointToChild); - } - - btQuaternion btRotParentComToJoint; - convertMat(poseParentComToJoint.linear()) - .getRotation(btRotParentComToJoint); - - auto linkParent = _sdfModel.LinkByName(joint->ParentName()); - gz::math::Pose3d parentTransformInWorldSpace; - const auto errors = linkParent->SemanticPose().Resolve( - parentTransformInWorldSpace); - - gz::math::Pose3d parent2joint; - const auto errors2 = linkParent->SemanticPose().Resolve( - parent2joint, joint->Name()); // X_JP - - btTransform parentLocalInertialFrame = convertTf( - parentLinkInfo->inertiaToLinkFrame); - btTransform parent2jointBt = convertTf( - gz::math::eigen3::convert(parent2joint.Inverse())); // X_PJ - - // offsetInABt = parent COM to pivot (X_IpJ) - // offsetInBBt = current COM to pivot (X_IcJ) - // - // X_PIp - // X_PJ - // X_IpJ = X_PIp^-1 * X_PJ - // - // X_IcJ = X_CIc^-1 * X_CJ - btTransform offsetInABt, offsetInBBt; - offsetInABt = parentLocalInertialFrame * parent2jointBt; - offsetInBBt = - convertTf(linkToComTf.inverse() * poseJointToChild.inverse()); - // R_IcJ * R_IpJ ^ -1 = R_IcIp; - btQuaternion parentRotToThis = - offsetInBBt.getRotation() * offsetInABt.inverse().getRotation(); - - auto jointID = this->AddJoint( - JointInfo{ - joint->Name(), - InternalJoint{i}, - model->linkEntityIds[static_cast(parentIndex+1)], - linkIDs.find(link)->second, - poseParentLinkToJoint, - poseJointToChild, - modelID - }); - auto jointInfo = this->ReferenceInterface(jointID); - - if (::sdf::JointType::PRISMATIC == joint->Type() || - ::sdf::JointType::REVOLUTE == joint->Type() || - ::sdf::JointType::BALL == joint->Type()) - { - if (::sdf::JointType::REVOLUTE == joint->Type()) - { - const auto axis = convertVec(joint->Axis()->Xyz()); - model->body->setupRevolute( - i, mass, inertia, parentIndex, - parentRotToThis, - quatRotate(offsetInBBt.getRotation(), axis), - offsetInABt.getOrigin(), - -offsetInBBt.getOrigin(), - true); - } - else if (::sdf::JointType::PRISMATIC == joint->Type()) - { - const auto axis = convertVec(joint->Axis()->Xyz()); - model->body->setupPrismatic( - i, mass, inertia, parentIndex, - parentRotToThis, - quatRotate(offsetInBBt.getRotation(), axis), - offsetInABt.getOrigin(), - -offsetInBBt.getOrigin(), - true); - } - else if (::sdf::JointType::BALL == joint->Type()) - { - model->body->setupSpherical( - i, mass, inertia, parentIndex, - parentRotToThis, - offsetInABt.getOrigin(), - -offsetInBBt.getOrigin(), - true); - } - } - else - { - model->body->setupFixed( - i, mass, inertia, parentIndex, - parentRotToThis, - offsetInABt.getOrigin(), - -offsetInBBt.getOrigin()); - } - - if (::sdf::JointType::PRISMATIC == joint->Type() || - ::sdf::JointType::REVOLUTE == joint->Type()) - { - model->body->getLink(i).m_jointLowerLimit = - static_cast(joint->Axis()->Lower()); - model->body->getLink(i).m_jointUpperLimit = - static_cast(joint->Axis()->Upper()); - model->body->getLink(i).m_jointDamping = - static_cast(joint->Axis()->Damping()); - model->body->getLink(i).m_jointFriction = - static_cast(joint->Axis()->Friction()); - model->body->getLink(i).m_jointMaxVelocity = - static_cast(joint->Axis()->MaxVelocity()); - model->body->getLink(i).m_jointMaxForce = - static_cast(joint->Axis()->Effort()); - jointInfo->effort = static_cast(joint->Axis()->Effort()); - - jointInfo->jointLimits = - std::make_shared( - model->body.get(), i, static_cast(joint->Axis()->Lower()), - static_cast(joint->Axis()->Upper())); - world->world->addMultiBodyConstraint(jointInfo->jointLimits.get()); - } - - jointInfo->jointFeedback = std::make_shared(); - jointInfo->jointFeedback->m_reactionForces.setZero(); - model->body->getLink(i).m_jointFeedback = jointInfo->jointFeedback.get(); - } - } - - model->body->setHasSelfCollision(_sdfModel.SelfCollide()); - model->body->finalizeMultiDof(); - - const auto worldToModel = ResolveSdfPose(_sdfModel.SemanticPose()); - if (!worldToModel) - return this->GenerateInvalidId(); - - const auto modelToRootLink = - ResolveSdfPose(structure.rootLink->SemanticPose()); - if (!modelToRootLink) - return this->GenerateInvalidId(); - - const auto worldToRootCom = - *worldToModel * *modelToRootLink * rootInertialToLink.inverse(); - - model->body->setBaseWorldTransform(convertTf(worldToRootCom)); - model->body->setBaseVel(btVector3(0, 0, 0)); - model->body->setBaseOmega(btVector3(0, 0, 0)); - - { - const auto *link = structure.rootLink; - btScalar mass; - btVector3 inertia; - math::Pose3d linkToPrincipalAxesPose; - extractInertial(link->Inertial(), mass, inertia, linkToPrincipalAxesPose); - model->body->setBaseMass(mass); - model->body->setBaseInertia(inertia); - } - - world->world->addMultiBody(model->body.get()); - - for (const auto& [linkSdf, linkID] : linkIDs) - { - for (std::size_t c = 0; c < linkSdf->CollisionCount(); ++c) - { - // If we fail to add the collision, just keep building the model. It may - // need to be constructed outside of the SDF generation pipeline, e.g. - // with AttachHeightmap. - this->AddSdfCollision(linkID, *linkSdf->CollisionByIndex(c), isStatic); - } - } - - return modelID; -*/ } ///////////////////////////////////////////////// diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index f4c0ee5f0..6723286cc 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -406,8 +406,6 @@ SDFFeatures::FindParentAndChildOfJoint(std::size_t _worldID, std::string parentLinkName; ::sdf::Errors errors = _sdfJoint->ResolveParentLink(parentLinkName); - std::cerr << " joint parent " << _sdfJoint->ParentName() << " resl " << parentLinkName << std::endl; - if (!errors.empty()) { gzerr << "The link of the parent frame [" << _sdfJoint->ParentName() @@ -434,7 +432,6 @@ SDFFeatures::FindParentAndChildOfJoint(std::size_t _worldID, } return {}; } - std::cerr << " joint child " << _sdfJoint->ChildName() << " resl " << childLinkName << std::endl; // When calling `FindBodyNode`, we need to check wheter the parent entity // (different from parent link/frame) of the joint is a model or world. If it @@ -538,7 +535,6 @@ Identity SDFFeatures::ConstructSdfModelImpl( std::size_t _parentID, const ::sdf::Model &_sdfModel) { - std::cerr << "======= construct nested model ---- " << _sdfModel.Name() << std::endl; auto worldID = _parentID; std::string modelName = _sdfModel.Name(); const bool isNested = this->models.HasEntity(_parentID); @@ -1063,20 +1059,6 @@ Identity SDFFeatures::ConstructSdfJoint( // joint is connected to the world bool worldParent = (!_parent && _sdfJoint.ParentName() == "world"); bool worldChild = (!_child && _sdfJoint.ChildName() == "world"); -/* - std::cerr << "parent link name " << _sdfJoint.ParentName() << std::endl; - std::cerr << "child link name " << _sdfJoint.ChildName() << std::endl; - - size_t idx = _sdfJoint.ChildName().find("::"); - if (idx != std::string::npos) - { - std::string modelName = _sdfJoint.ChildName().substr(0, idx); - std::string linkName = _sdfJoint.ChildName().substr(idx + 2); - std::cerr << "model name " << modelName << std::endl; - std::cerr << "link name " << linkName << std::endl; - } -*/ - if (worldChild) { gzerr << "Asked to create a joint with the world as the child in model " From 679ed5d17564286f39e4e0205a80428604b1a736 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 14 Nov 2023 00:39:29 +0000 Subject: [PATCH 11/29] more clean up Signed-off-by: Ian Chen --- bullet-featherstone/src/SDFFeatures.cc | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 0241f5d06..507f00759 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -146,7 +146,8 @@ void extractInertial( } ///////////////////////////////////////////////// -/// \brief Get pose of link in the model tree +/// \brief Get the pose of a link relative to the input model +/// The input link can be a nested link. /// \param[out] _pose Pose of link relative to model /// \param[in] _linkName Scoped name of link /// \param[in] _model Model SDF @@ -431,7 +432,6 @@ std::optional buildStructure( }; flattenLinkTree(_rootLink); - // TODO transform to appropriate frame for nested models? btScalar mass; btVector3 inertia; math::Pose3d linkToPrincipalAxesPose; @@ -656,7 +656,6 @@ Identity SDFFeatures::ConstructSdfModelImpl( std::size_t parentModelID = linkParentModelIds[link]; const auto linkID = this->AddLink( LinkInfo{link->Name(), i, - // modelID, this->GenerateIdentity(parentModelID, this->models.at(parentModelID)), linkToComTf.inverse()}); linkIDs.insert(std::make_pair(link, linkID)); @@ -687,8 +686,6 @@ Identity SDFFeatures::ConstructSdfModelImpl( gz::math::Pose3d gzPoseParentToJoint; errors = resolveJointPoseRelToLink(gzPoseParentToJoint, parentInfo.model, joint->Name(), parentLinkName); - // const auto errors = joint->SemanticPose().Resolve( - // gzPoseParentToJoint, joint->ParentName()); if (!errors.empty()) { @@ -711,8 +708,6 @@ Identity SDFFeatures::ConstructSdfModelImpl( Eigen::Isometry3d poseJointToChild; { gz::math::Pose3d gzPoseChildToJoint; - // const auto errors = - // link->SemanticPose().Resolve(gzPoseJointToChild, joint->Name()); // this retrieves the joint pose relative to link std::string childLinkName; errors = joint->ResolveChildLink(childLinkName); @@ -773,7 +768,6 @@ Identity SDFFeatures::ConstructSdfModelImpl( linkIDs.find(link)->second, poseParentLinkToJoint, poseJointToChild, - // modelID modelIDs.find(parentInfo.model)->second }); auto jointInfo = this->ReferenceInterface(jointID); @@ -856,7 +850,10 @@ Identity SDFFeatures::ConstructSdfModelImpl( model->body->setHasSelfCollision(_sdfModel.SelfCollide()); model->body->finalizeMultiDof(); - const auto worldToModel = ResolveSdfPose(_sdfModel.SemanticPose()); + // Note: this assumes the root link is in the top level model + // \todo(iche033) consider handling the case when the root link is + // in a nested model + const auto worldToModel = ResolveSdfPose(structure.model->SemanticPose()); if (!worldToModel) return this->GenerateInvalidId(); From ab31c26be6c49cb7d6de5873e35364a91d00fed8 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 14 Nov 2023 00:58:38 +0000 Subject: [PATCH 12/29] more cleanup Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.hh | 8 +------- bullet-featherstone/src/FreeGroupFeatures.cc | 3 --- dartsim/src/SDFFeatures.cc | 2 +- 3 files changed, 2 insertions(+), 11 deletions(-) diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index e6907f8bb..2201171fc 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -339,13 +339,7 @@ class Base : public Implements3d> auto *model = this->ReferenceInterface(_linkInfo.model); model->linkNameToEntityId[link->name] = id; - if (link->indexInModel.has_value()) - { - // We expect the links to be added in order - // assert(static_cast(*link->indexInModel + 1) == - // model->linkEntityIds.size()); - } - else + if (!link->indexInModel.has_value()) { // We are adding the root link. This means the model should not already // have a root link diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc index cc0ca8933..56bcc23c8 100644 --- a/bullet-featherstone/src/FreeGroupFeatures.cc +++ b/bullet-featherstone/src/FreeGroupFeatures.cc @@ -66,9 +66,6 @@ Identity FreeGroupFeatures::GetFreeGroupRootLink(const Identity &_groupID) const // btMultiBody user index stores the gz-phsics model root link id std::size_t rootID = static_cast(model->body->getUserIndex()); - - // The first link entity in the model is always the root link - // const std::size_t rootID = model->linkEntityIds.front(); return this->GenerateIdentity(rootID, this->links.at(rootID)); } diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index 6723286cc..c2fc54de8 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -405,7 +405,6 @@ SDFFeatures::FindParentAndChildOfJoint(std::size_t _worldID, // Resolve parent and child frames to links std::string parentLinkName; ::sdf::Errors errors = _sdfJoint->ResolveParentLink(parentLinkName); - if (!errors.empty()) { gzerr << "The link of the parent frame [" << _sdfJoint->ParentName() @@ -1059,6 +1058,7 @@ Identity SDFFeatures::ConstructSdfJoint( // joint is connected to the world bool worldParent = (!_parent && _sdfJoint.ParentName() == "world"); bool worldChild = (!_child && _sdfJoint.ChildName() == "world"); + if (worldChild) { gzerr << "Asked to create a joint with the world as the child in model " From 00dba936efe4dc332abc9a4f84f8ce8f0b4e9f52 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 14 Nov 2023 18:41:57 +0000 Subject: [PATCH 13/29] lint Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.hh | 10 ++++++---- bullet-featherstone/src/SDFFeatures.cc | 15 ++++++++++----- 2 files changed, 16 insertions(+), 9 deletions(-) diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index 2201171fc..ae0f8fbec 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -387,7 +387,8 @@ class Base : public Implements3d> return this->GenerateIdentity(id, joint); } - public: bool RemoveModelImpl(const Identity &_parentID, const Identity &_modelID) + public: bool RemoveModelImpl(const Identity &_parentID, + const Identity &_modelID) { auto *model = this->ReferenceInterface(_modelID); if (!model) @@ -398,14 +399,15 @@ class Base : public Implements3d> // Remove nested models for (auto &nestedModelID : model->nestedModelEntityIds) { - this->RemoveModelImpl(_modelID, - this->GenerateIdentity(nestedModelID, this->models.at(nestedModelID))); + this->RemoveModelImpl(_modelID, this->GenerateIdentity(nestedModelID, + this->models.at(nestedModelID))); } model->nestedModelEntityIds.clear(); // remove references in parent model or world model auto *parentModel = this->ReferenceInterface(_parentID); - auto nestedModelIt = parentModel->nestedModelNameToEntityId.find(model->name); + auto nestedModelIt = + parentModel->nestedModelNameToEntityId.find(model->name); if (nestedModelIt != parentModel->nestedModelNameToEntityId.end()) { std::size_t nestedModelID = nestedModelIt->second; diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 507f00759..2dbb9334a 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -334,7 +334,8 @@ bool buildTrees(const ::sdf::Model *_sdfModel, } if (!_parentOf.insert( - std::make_pair(child, ParentInfo{joint, _sdfModel, child, parent})).second) + std::make_pair(child, ParentInfo{joint, _sdfModel, child, parent})) + .second) { gzerr << "The Link [" << childLinkName << "] in Model [" << modelName << "] has multiple parent joints. That is not " @@ -435,13 +436,15 @@ std::optional buildStructure( btScalar mass; btVector3 inertia; math::Pose3d linkToPrincipalAxesPose; - extractInertial(_rootLink->Inertial(), mass, inertia, linkToPrincipalAxesPose); + extractInertial(_rootLink->Inertial(), mass, inertia, + linkToPrincipalAxesPose); // Uncomment to debug structure // std::cout << "Structure: " << std::endl; // std::cout << " model: " << _model->Name() << std::endl; // std::cout << " root link: " << _rootLink->Name() << std::endl; - // std::cout << " root joint: " << ((rootJoint) ? rootJoint->Name() : "N/A") << std::endl; + // std::cout << " root joint: " << ((rootJoint) ? rootJoint->Name() : "N/A") + // << std::endl; // std::cout << " mass: " << mass << std::endl; // std::cout << " fixed: " << fixed << std::endl; // std::cout << " flatLinks size: " << flatLinks.size() << std::endl; @@ -515,7 +518,8 @@ Identity SDFFeatures::ConstructSdfModelImpl( auto mIt = this->models.find(_parentID); std::size_t nestedModelID = mIt->second->nestedModelNameToEntityId.at( _sdfModel.Name()); - return this->GenerateIdentity(nestedModelID, this->models.at(nestedModelID)); + return this->GenerateIdentity(nestedModelID, + this->models.at(nestedModelID)); } auto structures = validateModel(_sdfModel); @@ -690,7 +694,8 @@ Identity SDFFeatures::ConstructSdfModelImpl( if (!errors.empty()) { gzerr << "An error occurred while resolving the transform of Joint [" - << joint->Name() << "] in Model [" << parentInfo.model->Name() << "]:\n"; + << joint->Name() << "] in Model [" << parentInfo.model->Name() + << "]:\n"; for (const auto &error : errors) { gzerr << error << "\n"; From 1e2bcdd18c96cfc6fb14e12c7656b2b6945fa410 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 14 Nov 2023 21:50:57 +0000 Subject: [PATCH 14/29] update remove logic, cleanup Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.hh | 25 +++++++++++-------- .../src/EntityManagementFeatures.cc | 4 --- 2 files changed, 15 insertions(+), 14 deletions(-) diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index ae0f8fbec..53197d7ee 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -405,17 +405,22 @@ class Base : public Implements3d> model->nestedModelEntityIds.clear(); // remove references in parent model or world model - auto *parentModel = this->ReferenceInterface(_parentID); - auto nestedModelIt = - parentModel->nestedModelNameToEntityId.find(model->name); - if (nestedModelIt != parentModel->nestedModelNameToEntityId.end()) + auto parentModelIt = this->models.find(_parentID); + if (parentModelIt != this->models.end()) { - std::size_t nestedModelID = nestedModelIt->second; - parentModel->nestedModelNameToEntityId.erase(nestedModelIt); - parentModel->nestedModelEntityIds.erase(std::remove( - parentModel->nestedModelEntityIds.begin(), - parentModel->nestedModelEntityIds.end(), nestedModelID), - parentModel->nestedModelEntityIds.end()); + auto parentModel = parentModelIt->second; + auto nestedModelIt = + parentModel->nestedModelNameToEntityId.find(model->name); + if (nestedModelIt != + parentModel->nestedModelNameToEntityId.end()) + { + std::size_t nestedModelID = nestedModelIt->second; + parentModel->nestedModelNameToEntityId.erase(nestedModelIt); + parentModel->nestedModelEntityIds.erase(std::remove( + parentModel->nestedModelEntityIds.begin(), + parentModel->nestedModelEntityIds.end(), nestedModelID), + parentModel->nestedModelEntityIds.end()); + } } // If nested, we are done here. No need to remove multibody diff --git a/bullet-featherstone/src/EntityManagementFeatures.cc b/bullet-featherstone/src/EntityManagementFeatures.cc index c86f5440e..99e40b827 100644 --- a/bullet-featherstone/src/EntityManagementFeatures.cc +++ b/bullet-featherstone/src/EntityManagementFeatures.cc @@ -215,10 +215,6 @@ bool EntityManagementFeatures::RemoveModel(const Identity &_modelID) bool EntityManagementFeatures::ModelRemoved( const Identity &_modelID) const { -// auto *model = this->ReferenceInterface(_modelID); -// auto *world = this->ReferenceInterface(model->world); -// return world->modelIndexToEntityId.count(model->indexInWorld) == 0; - return this->models.find(_modelID) == this->models.end(); } From 53e1ee54d961a98df088ae8c9fe6d222342bfc87 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 15 Nov 2023 13:30:42 +0000 Subject: [PATCH 15/29] fix removing model with constraints in bullet-featherstone (#577) Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.hh | 26 ++++++++++++------- test/common_test/world_features.cc | 20 +++++++++++--- .../worlds/world_single_nested_model.sdf | 9 ++++--- 3 files changed, 40 insertions(+), 15 deletions(-) diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index 53197d7ee..231ddf0c0 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -441,14 +441,25 @@ class Base : public Implements3d> world->modelNameToEntityId.erase(model->name); // Remove all constraints related to this model - for (auto constraint_index : model->external_constraints) + for (const auto jointID : model->jointEntityIds) { - const auto joint = this->joints.at(constraint_index); - const auto &constraint = - std::get>(joint->identifier); - world->world->removeMultiBodyConstraint(constraint.get()); - this->joints.erase(constraint_index); + const auto joint = this->joints.at(jointID); + if (joint->motor) + { + world->world->removeMultiBodyConstraint(joint->motor.get()); + } + if (joint->fixedConstraint) + { + world->world->removeMultiBodyConstraint(joint->fixedConstraint.get()); + } + if (joint->jointLimits) + { + world->world->removeMultiBodyConstraint(joint->jointLimits.get()); + } + this->joints.erase(jointID); } + // \todo(iche033) Remove external constraints related to this model + // (model->external_constraints) once this is supported world->world->removeMultiBody(model->body.get()); for (const auto linkID : model->linkEntityIds) @@ -464,9 +475,6 @@ class Base : public Implements3d> this->links.erase(linkID); } - for (const auto jointID : model->jointEntityIds) - this->joints.erase(jointID); - this->models.erase(_modelID); return true; diff --git a/test/common_test/world_features.cc b/test/common_test/world_features.cc index 96e296b2c..7c501aa96 100644 --- a/test/common_test/world_features.cc +++ b/test/common_test/world_features.cc @@ -358,11 +358,12 @@ TEST_F(WorldModelTest, WorldModelAPI) struct WorldNestedModelFeatureList : gz::physics::FeatureList< GravityFeatures, - gz::physics::WorldModelFeature, - gz::physics::RemoveEntities, + gz::physics::ForwardStep, gz::physics::GetNestedModelFromModel, gz::physics::sdf::ConstructSdfModel, - gz::physics::sdf::ConstructSdfNestedModel + gz::physics::sdf::ConstructSdfNestedModel, + gz::physics::RemoveEntities, + gz::physics::WorldModelFeature > { }; @@ -408,6 +409,10 @@ TEST_F(WorldNestedModelTest, WorldConstructNestedModel) ASSERT_NE(nullptr, nestedModel); EXPECT_EQ("parent_model", nestedModel->GetName()); + gz::physics::ForwardStep::Input input; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Output output; + // Check that we can remove models via RemoveNestedModel EXPECT_TRUE(worldModel->RemoveNestedModel(0)); EXPECT_TRUE(nestedModel->Removed()); @@ -415,6 +420,15 @@ TEST_F(WorldNestedModelTest, WorldConstructNestedModel) EXPECT_EQ(0u, worldModel->GetNestedModelCount()); EXPECT_EQ(nullptr, worldModel->GetNestedModel(0)); EXPECT_EQ(nullptr, worldModel->GetNestedModel("parent_model")); + + // verify we can step the world after model removal + const size_t numSteps = 1000; + for (size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + } + EXPECT_EQ(0u, world->GetModelCount()); + EXPECT_EQ(0u, worldModel->GetNestedModelCount()); } } diff --git a/test/common_test/worlds/world_single_nested_model.sdf b/test/common_test/worlds/world_single_nested_model.sdf index 7c0fcff9c..14f724f2b 100644 --- a/test/common_test/worlds/world_single_nested_model.sdf +++ b/test/common_test/worlds/world_single_nested_model.sdf @@ -49,9 +49,12 @@ - - link1 - nested_model::nested_link1 + + link1 + nested_model::nested_link1 + + 0 0 1 + From 2c85f5ea8b8357d16f450fad9b2057bfe486157c Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 17 Nov 2023 05:35:59 +0000 Subject: [PATCH 16/29] Support ConstructSdfJoint feature and add all links Signed-off-by: Ian Chen --- bullet-featherstone/src/SDFFeatures.cc | 122 +++++++++++++++++++++++-- bullet-featherstone/src/SDFFeatures.hh | 6 ++ 2 files changed, 118 insertions(+), 10 deletions(-) diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 2dbb9334a..98e8fe13b 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -509,7 +509,7 @@ Identity SDFFeatures::ConstructSdfModelImpl( std::size_t _parentID, const ::sdf::Model &_sdfModel) { - // The ConstructSDFModelImpl function constructs the entire sdf model tree, + // The ConstructSdfModelImpl function constructs the entire sdf model tree, // including nested models So return the nested model identity if it is // constructed already auto wIt = this->worlds.find(_parentID); @@ -609,23 +609,24 @@ Identity SDFFeatures::ConstructSdfModelImpl( return this->GenerateInvalidId(); } - // Get the root model identity - // New links and joints added are associated with this modelID - // so we do not break other features. - auto modelID = - this->GenerateIdentity(rootModelID, this->models[rootModelID]); - // Add base link + std::size_t baseLinkModelID = linkParentModelIds.at(structure.rootLink); const auto rootID = this->AddLink(LinkInfo{ - structure.rootLink->Name(), std::nullopt, modelID, rootInertialToLink + structure.rootLink->Name(), std::nullopt, + this->GenerateIdentity(baseLinkModelID, this->models.at(baseLinkModelID)), + rootInertialToLink }); rootMultiBody->setUserIndex(std::size_t(rootID)); + + auto modelID = + this->GenerateIdentity(rootModelID, this->models[rootModelID]); const auto *model = this->ReferenceInterface(modelID); // Add world joint if (structure.rootJoint) { + const auto &parentInfo = structure.parentOf.at(structure.rootLink); this->AddJoint( JointInfo{ structure.rootJoint->Name(), @@ -634,7 +635,7 @@ Identity SDFFeatures::ConstructSdfModelImpl( rootID, Eigen::Isometry3d::Identity(), Eigen::Isometry3d::Identity(), - modelID + modelIDs.find(parentInfo.model)->second }); } @@ -665,7 +666,7 @@ Identity SDFFeatures::ConstructSdfModelImpl( linkIDs.insert(std::make_pair(link, linkID)); } - if (structure.parentOf.size()) + if (!structure.parentOf.empty()) { const auto &parentInfo = structure.parentOf.at(link); const auto *joint = parentInfo.joint; @@ -897,6 +898,40 @@ Identity SDFFeatures::ConstructSdfModelImpl( } } + // Add the remaining links in the model without constructing the bullet objects. + // These are dummy links for book-keeping purposes + // \todo(iche033) The code will need to be updated when multiple subtrees in + // a single model is supported. + for (std::size_t i = 1u; i < structures.size(); ++i) + { + auto otherStructure = structures[i]; + // Add base link + std::size_t rootLinkModelID = linkParentModelIds[otherStructure.rootLink]; + auto rootLinkModelInfo = this->models.at(rootLinkModelID); + this->AddLink(LinkInfo{ + otherStructure.rootLink->Name(), + std::nullopt, + this->GenerateIdentity(rootLinkModelID, rootLinkModelInfo), + gz::math::eigen3::convert( + otherStructure.linkToPrincipalAxesPose).inverse() + }); + // other links + for (int j = 0; j < static_cast(otherStructure.flatLinks.size()); ++j) + { + const auto *link = otherStructure.flatLinks[static_cast(j)]; + std::size_t parentModelID = linkParentModelIds[link]; + btScalar mass; + btVector3 inertia; + math::Pose3d linkToPrincipalAxesPose; + extractInertial(link->Inertial(), mass, inertia, linkToPrincipalAxesPose); + auto parentModelInfo = this->models.at(parentModelID); + const auto linkID = this->AddLink( + LinkInfo{link->Name(), std::nullopt, + this->GenerateIdentity(parentModelID, parentModelInfo), + gz::math::eigen3::convert(linkToPrincipalAxesPose).inverse()}); + } + } + return modelID; } @@ -1227,6 +1262,73 @@ Identity SDFFeatures::ConstructSdfCollision( return this->GenerateInvalidId(); } +///////////////////////////////////////////////// +Identity SDFFeatures::ConstructSdfJoint( + const Identity &_modelID, + const ::sdf::Joint &_sdfJoint) +{ + auto modelInfo = this->ReferenceInterface(_modelID); + if (_sdfJoint.ChildName() == "world") + { + gzerr << "Asked to create a joint with the world as the child in model " + << "[" << modelInfo->name << "]. This is currently not " + << "supported\n"; + + return this->GenerateInvalidId(); + } + + std::string parentLinkName; + const auto resolveParentErrors = _sdfJoint.ResolveParentLink(parentLinkName); + if (!resolveParentErrors.empty()) { + parentLinkName = _sdfJoint.ParentName(); + } + std::string childLinkName; + const auto childResolveErrors = _sdfJoint.ResolveChildLink(childLinkName); + if (!childResolveErrors.empty()) { + childLinkName = _sdfJoint.ChildName(); + } + + // Currently only supports constructing joint with world + if (parentLinkName == "world" && _sdfJoint.Type() == ::sdf::JointType::FIXED) + { + auto worldModelIt = this->models.find(_modelID); + if (worldModelIt == this->models.end()) + return this->GenerateInvalidId(); + const auto worldModel = worldModelIt->second; + std::size_t idx = childLinkName.find("::"); + if (idx == std::string::npos) + return this->GenerateInvalidId(); + + const std::string modelName = childLinkName.substr(0, idx); + std::size_t modelID = worldModel->nestedModelNameToEntityId.at(modelName); + auto model = this->models.at(modelID); + + model->body->setFixedBase(true); + std::size_t linkID = model->body->getUserIndex(); + auto rootID = this->GenerateIdentity(linkID, this->links.at(linkID)); + return this->AddJoint( + JointInfo{ + _sdfJoint.Name(), + RootJoint{}, + std::nullopt, + rootID, + Eigen::Isometry3d::Identity(), + Eigen::Isometry3d::Identity(), + _modelID + }); + } + + // \todo(iche) Support fixed joint between 2 different models + gzerr << "Unable to create joint between parent: " << parentLinkName << " " + << "and child: " << childLinkName << ". " + << "ConstructSdfJoint in bullet-featherstone implementation currently " + << "only supports creating a fixed joint with the world as parent link." + << std::endl; + + return this->GenerateInvalidId(); +} + + } // namespace bullet_featherstone } // namespace physics } // namespace gz diff --git a/bullet-featherstone/src/SDFFeatures.hh b/bullet-featherstone/src/SDFFeatures.hh index 22dd01288..b190c986c 100644 --- a/bullet-featherstone/src/SDFFeatures.hh +++ b/bullet-featherstone/src/SDFFeatures.hh @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -35,6 +36,7 @@ namespace physics { namespace bullet_featherstone { struct SDFFeatureList : gz::physics::FeatureList< + sdf::ConstructSdfJoint, sdf::ConstructSdfModel, sdf::ConstructSdfNestedModel, sdf::ConstructSdfWorld, @@ -62,6 +64,10 @@ class SDFFeatures : const ::sdf::Collision &_collision, bool isStatic); + public: Identity ConstructSdfJoint( + const Identity &_modelID, + const ::sdf::Joint &_sdfJoint) override; + private: Identity ConstructSdfCollision( const Identity &_linkID, const ::sdf::Collision &_collision) override; From 06016b79d2837522d7fee8907bc67d28a1d17ea1 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 17 Nov 2023 05:53:26 +0000 Subject: [PATCH 17/29] add warning msg for floating bodies / subtrees Signed-off-by: Ian Chen --- bullet-featherstone/src/SDFFeatures.cc | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 98e8fe13b..8d2ae338d 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -915,6 +915,9 @@ Identity SDFFeatures::ConstructSdfModelImpl( gz::math::eigen3::convert( otherStructure.linkToPrincipalAxesPose).inverse() }); + gzwarn << "Floating body / sub-tree detected. Disabling link: '" + << otherStructure.rootLink->Name() << "' " + << "from model '" << rootLinkModelInfo->name << "'." << std::endl; // other links for (int j = 0; j < static_cast(otherStructure.flatLinks.size()); ++j) { @@ -929,6 +932,10 @@ Identity SDFFeatures::ConstructSdfModelImpl( LinkInfo{link->Name(), std::nullopt, this->GenerateIdentity(parentModelID, parentModelInfo), gz::math::eigen3::convert(linkToPrincipalAxesPose).inverse()}); + gzwarn << "Floating body / sub-tree detected. Disabling link: '" + << link->Name() << "' " << "from model '" << parentModelInfo->name + << "'." << std::endl; + } } From bc1219aa10e067ac56677b62265400f19a9d463c Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 17 Nov 2023 06:02:52 +0000 Subject: [PATCH 18/29] typo and style Signed-off-by: Ian Chen --- bullet-featherstone/src/SDFFeatures.cc | 4 ++-- bullet-featherstone/src/SDFFeatures.hh | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 8d2ae338d..e0dd17365 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -898,8 +898,8 @@ Identity SDFFeatures::ConstructSdfModelImpl( } } - // Add the remaining links in the model without constructing the bullet objects. - // These are dummy links for book-keeping purposes + // Add the remaining links in the model without constructing the bullet + // objects. These are dummy links for book-keeping purposes // \todo(iche033) The code will need to be updated when multiple subtrees in // a single model is supported. for (std::size_t i = 1u; i < structures.size(); ++i) diff --git a/bullet-featherstone/src/SDFFeatures.hh b/bullet-featherstone/src/SDFFeatures.hh index b190c986c..bea5faa70 100644 --- a/bullet-featherstone/src/SDFFeatures.hh +++ b/bullet-featherstone/src/SDFFeatures.hh @@ -72,7 +72,7 @@ class SDFFeatures : const Identity &_linkID, const ::sdf::Collision &_collision) override; - /// \brief Construct a bullet entity from a sdf::model + /// \brief Construct a bullet entity from a sdf::Model /// \param[in] _parentID Parent identity /// \param[in] _sdfModel sdf::Model to construct entity from /// \return The entity identity if constructed otherwise an invalid identity From 88962e84cda332c69e27fa5e83e2dfc2d1427d16 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 17 Nov 2023 20:16:47 +0000 Subject: [PATCH 19/29] add ifdef for bullet version in 20.04 Signed-off-by: Ian Chen --- bullet-featherstone/src/SDFFeatures.cc | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index e0dd17365..289cc7557 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -1274,6 +1274,19 @@ Identity SDFFeatures::ConstructSdfJoint( const Identity &_modelID, const ::sdf::Joint &_sdfJoint) { +#if BT_BULLET_VERSION < 289 + // The btMultiBody::setFixedBase function is only available in versions + // >= 2.89. This is needed for dynamically creating world joints, + // i.e. setting the btMultiBody to be fixed. So output an error letting + // users know the joint will not be created. + // \todo(iche033) An workaround for this is to loop through all the joints + // in the world first in ConstructSdfWorld, keep track of the models who are + // a child of the world joint, then when creating the btMultiBody + // in ConstructSdfModelImpl, pass fixedBase as true in its constructor. + gzerr << "ConstructSdfJoint feature is not supported for bullet version " + << "less than 2.89. Joint '" << sdfJoint->Name() << "' will not " + << "be created." << std::endl; +#else auto modelInfo = this->ReferenceInterface(_modelID); if (_sdfJoint.ChildName() == "world") { @@ -1325,12 +1338,13 @@ Identity SDFFeatures::ConstructSdfJoint( }); } - // \todo(iche) Support fixed joint between 2 different models + // \todo(iche033) Support fixed joint between 2 different models gzerr << "Unable to create joint between parent: " << parentLinkName << " " << "and child: " << childLinkName << ". " << "ConstructSdfJoint in bullet-featherstone implementation currently " << "only supports creating a fixed joint with the world as parent link." << std::endl; +#endif return this->GenerateInvalidId(); } From 1292f73986db6ee612040ed43a9e572b34493437 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 17 Nov 2023 21:24:00 +0000 Subject: [PATCH 20/29] fix build and warning Signed-off-by: Ian Chen --- bullet-featherstone/src/SDFFeatures.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 289cc7557..3929733a5 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -1283,8 +1283,9 @@ Identity SDFFeatures::ConstructSdfJoint( // in the world first in ConstructSdfWorld, keep track of the models who are // a child of the world joint, then when creating the btMultiBody // in ConstructSdfModelImpl, pass fixedBase as true in its constructor. + (void)_modelID; gzerr << "ConstructSdfJoint feature is not supported for bullet version " - << "less than 2.89. Joint '" << sdfJoint->Name() << "' will not " + << "less than 2.89. Joint '" << _sdfJoint.Name() << "' will not " << "be created." << std::endl; #else auto modelInfo = this->ReferenceInterface(_modelID); From 26471eb4da6a791a741a7716de706d1ac79f07b1 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 29 Nov 2023 21:13:21 +0000 Subject: [PATCH 21/29] typo Signed-off-by: Ian Chen --- bullet-featherstone/src/SDFFeatures.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 3929733a5..ca87ce02c 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -528,10 +528,10 @@ Identity SDFFeatures::ConstructSdfModelImpl( if (structures.size() > 1) { - // multiple subt-trees detected in model + // multiple sub-trees detected in model // \todo(iche033) support multiple kinematic trees and // multiple floating links in a single model - gzerr << "Multiple subt-trees / floating links detected in a model. " + gzerr << "Multiple sub-trees / floating links detected in a model. " << "This is not supported in bullet-featherstone implementation yet." << std::endl; } From 15aab54d1e02a0b1e618b346ed19e48726ae6a6c Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 30 Nov 2023 17:37:08 +0000 Subject: [PATCH 22/29] expand test Signed-off-by: Ian Chen --- test/common_test/world_features.cc | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/test/common_test/world_features.cc b/test/common_test/world_features.cc index 7c501aa96..c228768a2 100644 --- a/test/common_test/world_features.cc +++ b/test/common_test/world_features.cc @@ -360,6 +360,7 @@ struct WorldNestedModelFeatureList : gz::physics::FeatureList< GravityFeatures, gz::physics::ForwardStep, gz::physics::GetNestedModelFromModel, + gz::physics::sdf::ConstructSdfJoint, gz::physics::sdf::ConstructSdfModel, gz::physics::sdf::ConstructSdfNestedModel, gz::physics::RemoveEntities, @@ -409,6 +410,16 @@ TEST_F(WorldNestedModelTest, WorldConstructNestedModel) ASSERT_NE(nullptr, nestedModel); EXPECT_EQ("parent_model", nestedModel->GetName()); + // Test joint creation + sdf::Joint joint; + joint.SetName("world_joint"); + joint.SetType(sdf::JointType::FIXED); + joint.SetParentName("world"); + joint.SetChildName("invalid_link"); + EXPECT_FALSE(worldModel->ConstructJoint(joint)); + joint.SetChildName("parent_model::link1"); + EXPECT_TRUE(worldModel->ConstructJoint(joint)); + gz::physics::ForwardStep::Input input; gz::physics::ForwardStep::State state; gz::physics::ForwardStep::Output output; From 2681ea0ce7c8a59088c84b1efc827d03e684d0db Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 30 Nov 2023 18:55:58 +0000 Subject: [PATCH 23/29] add bullet ver check in test Signed-off-by: Ian Chen --- test/common_test/world_features.cc | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/test/common_test/world_features.cc b/test/common_test/world_features.cc index c228768a2..98a9f3faf 100644 --- a/test/common_test/world_features.cc +++ b/test/common_test/world_features.cc @@ -16,6 +16,8 @@ */ #include +#include + #include #include @@ -417,8 +419,12 @@ TEST_F(WorldNestedModelTest, WorldConstructNestedModel) joint.SetParentName("world"); joint.SetChildName("invalid_link"); EXPECT_FALSE(worldModel->ConstructJoint(joint)); - joint.SetChildName("parent_model::link1"); - EXPECT_TRUE(worldModel->ConstructJoint(joint)); + if (PhysicsEngineName(name) != "bullet-featherstone" || + btGetVersion() >= 289) + { + joint.SetChildName("parent_model::link1"); + EXPECT_TRUE(worldModel->ConstructJoint(joint)); + } gz::physics::ForwardStep::Input input; gz::physics::ForwardStep::State state; From 575e73b587c4f4523bdc97b00be7423dd17391c5 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Sat, 2 Dec 2023 02:01:08 +0000 Subject: [PATCH 24/29] update bullet ver check in test, simplify joint to link pose function, fix model transform Signed-off-by: Ian Chen --- bullet-featherstone/src/FreeGroupFeatures.cc | 10 -- bullet-featherstone/src/SDFFeatures.cc | 133 ++++++++---------- test/common_test/CMakeLists.txt | 9 +- .../joint_transmitted_wrench_features.cc | 17 +-- test/common_test/world_features.cc | 13 +- 5 files changed, 77 insertions(+), 105 deletions(-) diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc index 56bcc23c8..22e219528 100644 --- a/bullet-featherstone/src/FreeGroupFeatures.cc +++ b/bullet-featherstone/src/FreeGroupFeatures.cc @@ -33,11 +33,6 @@ Identity FreeGroupFeatures::FindFreeGroupForModel( if (model->body->hasFixedBase()) return this->GenerateInvalidId(); - // bullet-featherstone does not allow floating bodies so if a joint exists - // the multibody does not quality as a FreeGroup - if (model->body->getNumDofs() > 0) - return this->GenerateInvalidId(); - return _modelID; } @@ -50,11 +45,6 @@ Identity FreeGroupFeatures::FindFreeGroupForLink( if (model->body->hasFixedBase()) return this->GenerateInvalidId(); - // bullet-featherstone does not allow floating bodies so if a joint exists - // the multibody does not quality as a FreeGroup - if (model->body->getNumDofs() > 0) - return this->GenerateInvalidId(); - return link->model; } diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index ca87ce02c..118b9ef50 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -145,57 +145,6 @@ void extractInertial( _linkToPrincipalAxesPose.Rot() *= M.PrincipalAxesOffset(); } -///////////////////////////////////////////////// -/// \brief Get the pose of a link relative to the input model -/// The input link can be a nested link. -/// \param[out] _pose Pose of link relative to model -/// \param[in] _linkName Scoped name of link -/// \param[in] _model Model SDF -::sdf::Errors linkPoseInModelTree(math::Pose3d &_pose, - const std::string &_linkName, - const ::sdf::Model *_model) -{ - ::sdf::Errors errors; - size_t idx = _linkName.find("::"); - if (idx != std::string::npos) - { - if (_model->ModelCount() > 0) - { - std::string nestedModelName = _linkName.substr(0, idx); - std::string nestedLinkName = _linkName.substr(idx + 2); - const auto *nestedModel = _model->ModelByName(nestedModelName); - if (!nestedModel) - { - gzerr << "Unable to find nested model " << nestedModelName << std::endl; - return errors; - } - - math::Pose3d p; - errors = nestedModel->SemanticPose().Resolve(p); - if (!errors.empty()) - return errors; - _pose = _pose * p; - return linkPoseInModelTree(_pose, nestedLinkName, nestedModel); - } - } - else - { - const auto *link = _model->LinkByName(_linkName); - if (!link) - { - gzerr << "Unable to find link " << _linkName << std::endl; - return errors; - } - math::Pose3d p; - errors = link->SemanticPose().Resolve(p); - if (!errors.empty()) - return errors; - - _pose = _pose * p; - } - return errors; -} - ///////////////////////////////////////////////// /// \brief Get pose of joint relative to link /// \param[out] _resolvedPose Pose of joint relative to link @@ -215,12 +164,6 @@ ::sdf::Errors resolveJointPoseRelToLink(math::Pose3d &_resolvedPose, return errors; } - math::Pose3d jointPose; - errors = joint->SemanticPose().Resolve(jointPose); - if (!errors.empty()) - return errors; - - // joint pose is expressed relative to child link std::string childLinkName; errors = joint->ResolveChildLink(childLinkName); if (!errors.empty()) @@ -228,13 +171,15 @@ ::sdf::Errors resolveJointPoseRelToLink(math::Pose3d &_resolvedPose, childLinkName = joint->ChildName(); } + // joint pose is expressed relative to child link so return joint pose + // if input link is the child link if (childLinkName == _link) { - _resolvedPose = jointPose; + errors = joint->SemanticPose().Resolve(_resolvedPose); return errors; } - // pose of parent link + // compute joint pose relative to parent link const auto *link = _model->LinkByName(_link); if (!link) { @@ -243,20 +188,16 @@ ::sdf::Errors resolveJointPoseRelToLink(math::Pose3d &_resolvedPose, return errors; } - math::Pose3d parentLinkPose; - errors = linkPoseInModelTree(parentLinkPose, _link, _model); - if (!errors.empty()) - return errors; - - // pose of child link - math::Pose3d childLinkPose; - errors = linkPoseInModelTree(childLinkPose, childLinkName, _model); - if (!errors.empty()) - return errors; + math::Pose3d jointPoseRelToModel; + errors = _model->SemanticPose().Resolve(jointPoseRelToModel, + _model->Name() + "::" + _joint); + jointPoseRelToModel = jointPoseRelToModel.Inverse(); - auto jointPoseInModel = childLinkPose * jointPose; + math::Pose3d modelPoseRelToLink; + errors = _model->SemanticPose().Resolve(modelPoseRelToLink, + _model->Name() + "::" + _link); - _resolvedPose = parentLinkPose.Inverse() * jointPoseInModel; + _resolvedPose = modelPoseRelToLink * jointPoseRelToModel; return errors; } @@ -856,20 +797,60 @@ Identity SDFFeatures::ConstructSdfModelImpl( model->body->setHasSelfCollision(_sdfModel.SelfCollide()); model->body->finalizeMultiDof(); - // Note: this assumes the root link is in the top level model - // \todo(iche033) consider handling the case when the root link is - // in a nested model - const auto worldToModel = ResolveSdfPose(structure.model->SemanticPose()); + const auto worldToModel = ResolveSdfPose(_sdfModel.SemanticPose()); if (!worldToModel) return this->GenerateInvalidId(); + auto modelToNestedModel = Eigen::Isometry3d::Identity(); + // if the model of the root link is in a top level model, compute + // its pose relative to top level model + if (structure.model != &_sdfModel) + { + // get the scoped name of the model + // This is used to resolve the model pose + std::string modelScopedName; + auto getModelScopedName = [&](const ::sdf::Model *_targetModel, + const ::sdf::Model *_parentModel, const std::string &_prefix, + auto &&_getModelScopedName) + { + for (std::size_t i = 0u; i < _parentModel->ModelCount(); ++i) + { + auto childModel =_parentModel->ModelByIndex(i); + if (childModel == _targetModel) + { + modelScopedName = _prefix + childModel->Name(); + return true; + } + else + { + if (_getModelScopedName(_targetModel, childModel, + _prefix + childModel->Name() + "::", _getModelScopedName)) + return true; + } + } + return false; + }; + + math::Pose3d modelPose; + if (!getModelScopedName(structure.model, &_sdfModel, + _sdfModel.Name() + "::", getModelScopedName)) + return this->GenerateInvalidId(); + + auto errors = _sdfModel.SemanticPose().Resolve(modelPose, + modelScopedName); + if (!errors.empty()) + return this->GenerateInvalidId(); + modelToNestedModel = math::eigen3::convert(modelPose).inverse(); + } + const auto modelToRootLink = ResolveSdfPose(structure.rootLink->SemanticPose()); if (!modelToRootLink) return this->GenerateInvalidId(); const auto worldToRootCom = - *worldToModel * *modelToRootLink * rootInertialToLink.inverse(); + *worldToModel * modelToNestedModel * *modelToRootLink * + rootInertialToLink.inverse(); model->body->setBaseWorldTransform(convertTf(worldToRootCom)); model->body->setBaseVel(btVector3(0, 0, 0)); diff --git a/test/common_test/CMakeLists.txt b/test/common_test/CMakeLists.txt index 2e38a0c25..251d96bd3 100644 --- a/test/common_test/CMakeLists.txt +++ b/test/common_test/CMakeLists.txt @@ -1,5 +1,4 @@ set(TEST_TYPE "COMMON_TEST") -include_directories(${BULLET_INCLUDE_DIRS}) set(tests added_mass @@ -47,6 +46,13 @@ endfunction() set(GZ_PHYSICS_RESOURCE_DIR "${CMAKE_SOURCE_DIR}/resources") +# Get bullet version using pkg_check_modules as it is not available +# through the cmake module +gz_pkg_check_modules_quiet(bullet_version_check "bullet") +if (bullet_version_check_FOUND) + string(REPLACE "." "" GzBullet_VERSION ${bullet_version_check_VERSION}) +endif() + foreach(test ${tests}) set(test_executable "${TEST_TYPE}_${test}") add_executable(${test_executable} ${test}.cc) @@ -65,6 +71,7 @@ foreach(test ${tests}) target_compile_definitions(${test_executable} PRIVATE "TEST_WORLD_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/worlds/\"" "GZ_PHYSICS_RESOURCE_DIR=\"${GZ_PHYSICS_RESOURCE_DIR}\"" + "BT_BULLET_VERSION=${GzBullet_VERSION}" ) install(TARGETS ${test_executable} DESTINATION ${TEST_INSTALL_DIR}) diff --git a/test/common_test/joint_transmitted_wrench_features.cc b/test/common_test/joint_transmitted_wrench_features.cc index 8d1563b2d..be35f02e8 100644 --- a/test/common_test/joint_transmitted_wrench_features.cc +++ b/test/common_test/joint_transmitted_wrench_features.cc @@ -17,7 +17,6 @@ #include #include -#include #include #include @@ -225,12 +224,10 @@ TYPED_TEST(JointTransmittedWrenchFixture, PendulumAtZeroAngle) TYPED_TEST(JointTransmittedWrenchFixture, PendulumInMotion) { // This test requires https://github.com/bulletphysics/bullet3/pull/4462 - // When removing this check, also remove - // `#include ` at the top of this file, and - // `include_directories(${BULLET_INCLUDE_DIRS})` from - // test/common_test/CMakeLists.txt - if (this->engineName == "bullet-featherstone" && btGetVersion() <= 325) +#if BT_BULLET_VERSION <= 325 + if (this->engineName == "bullet-featherstone") GTEST_SKIP(); +#endif // Start pendulum at 90° (parallel to the ground) and stop at about 40° // so that we have non-trivial test expectations. @@ -418,12 +415,10 @@ TYPED_TEST(JointTransmittedWrenchFixture, JointLosses) TYPED_TEST(JointTransmittedWrenchFixture, ContactForces) { // This test requires https://github.com/bulletphysics/bullet3/pull/4462 - // When removing this check, also remove - // `#include ` at the top of this file, and - // `include_directories(${BULLET_INCLUDE_DIRS})` from - // test/common_test/CMakeLists.txt - if (this->engineName == "bullet-featherstone" && btGetVersion() <= 325) +#if BT_BULLET_VERSION <= 325 + if (this->engineName == "bullet-featherstone") GTEST_SKIP(); +#endif auto box = this->world->GetModel("box"); ASSERT_NE(nullptr, box); diff --git a/test/common_test/world_features.cc b/test/common_test/world_features.cc index 98a9f3faf..22909241b 100644 --- a/test/common_test/world_features.cc +++ b/test/common_test/world_features.cc @@ -16,8 +16,6 @@ */ #include -#include - #include #include @@ -419,12 +417,13 @@ TEST_F(WorldNestedModelTest, WorldConstructNestedModel) joint.SetParentName("world"); joint.SetChildName("invalid_link"); EXPECT_FALSE(worldModel->ConstructJoint(joint)); - if (PhysicsEngineName(name) != "bullet-featherstone" || - btGetVersion() >= 289) - { - joint.SetChildName("parent_model::link1"); + joint.SetChildName("parent_model::link1"); + if (PhysicsEngineName(name) != "bullet-featherstone") EXPECT_TRUE(worldModel->ConstructJoint(joint)); - } + else +#if BT_BULLET_VERSION >= 289 + EXPECT_TRUE(worldModel->ConstructJoint(joint)); +#endif gz::physics::ForwardStep::Input input; gz::physics::ForwardStep::State state; From a2837786bedf22b5fb9c055a37ab3f885abdf54c Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Sat, 2 Dec 2023 02:04:48 +0000 Subject: [PATCH 25/29] style Signed-off-by: Ian Chen --- bullet-featherstone/src/SDFFeatures.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 118b9ef50..e1a53a40e 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -815,7 +815,7 @@ Identity SDFFeatures::ConstructSdfModelImpl( { for (std::size_t i = 0u; i < _parentModel->ModelCount(); ++i) { - auto childModel =_parentModel->ModelByIndex(i); + auto childModel = _parentModel->ModelByIndex(i); if (childModel == _targetModel) { modelScopedName = _prefix + childModel->Name(); From b963af17b666bcbe5c8ea10a16f6cbe07d8ad651 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 4 Dec 2023 17:28:41 +0000 Subject: [PATCH 26/29] update bullet ver target def Signed-off-by: Ian Chen --- test/common_test/CMakeLists.txt | 16 ++++++++++++---- .../joint_transmitted_wrench_features.cc | 4 ++-- test/common_test/world_features.cc | 6 +++++- 3 files changed, 19 insertions(+), 7 deletions(-) diff --git a/test/common_test/CMakeLists.txt b/test/common_test/CMakeLists.txt index 251d96bd3..b347dd325 100644 --- a/test/common_test/CMakeLists.txt +++ b/test/common_test/CMakeLists.txt @@ -49,9 +49,6 @@ set(GZ_PHYSICS_RESOURCE_DIR "${CMAKE_SOURCE_DIR}/resources") # Get bullet version using pkg_check_modules as it is not available # through the cmake module gz_pkg_check_modules_quiet(bullet_version_check "bullet") -if (bullet_version_check_FOUND) - string(REPLACE "." "" GzBullet_VERSION ${bullet_version_check_VERSION}) -endif() foreach(test ${tests}) set(test_executable "${TEST_TYPE}_${test}") @@ -71,9 +68,20 @@ foreach(test ${tests}) target_compile_definitions(${test_executable} PRIVATE "TEST_WORLD_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/worlds/\"" "GZ_PHYSICS_RESOURCE_DIR=\"${GZ_PHYSICS_RESOURCE_DIR}\"" - "BT_BULLET_VERSION=${GzBullet_VERSION}" ) + if (bullet_version_check_VERSION VERSION_GREATER_EQUAL 2.89) + target_compile_definitions(${test_executable} PRIVATE + "BT_BULLET_VERSION_GE_289" + ) + endif() + if (bullet_version_check_VERSION VERSION_LESS_EQUAL 3.25) + target_compile_definitions(${test_executable} PRIVATE + "BT_BULLET_VERSION_LE_325" + ) + endif() + + install(TARGETS ${test_executable} DESTINATION ${TEST_INSTALL_DIR}) configure_common_test("bullet" ${test_executable}) diff --git a/test/common_test/joint_transmitted_wrench_features.cc b/test/common_test/joint_transmitted_wrench_features.cc index be35f02e8..21fb52576 100644 --- a/test/common_test/joint_transmitted_wrench_features.cc +++ b/test/common_test/joint_transmitted_wrench_features.cc @@ -224,7 +224,7 @@ TYPED_TEST(JointTransmittedWrenchFixture, PendulumAtZeroAngle) TYPED_TEST(JointTransmittedWrenchFixture, PendulumInMotion) { // This test requires https://github.com/bulletphysics/bullet3/pull/4462 -#if BT_BULLET_VERSION <= 325 +#ifdef BT_BULLET_VERSION_LE_325 if (this->engineName == "bullet-featherstone") GTEST_SKIP(); #endif @@ -415,7 +415,7 @@ TYPED_TEST(JointTransmittedWrenchFixture, JointLosses) TYPED_TEST(JointTransmittedWrenchFixture, ContactForces) { // This test requires https://github.com/bulletphysics/bullet3/pull/4462 -#if BT_BULLET_VERSION <= 325 +#if BT_BULLET_VERSION_LE_325 if (this->engineName == "bullet-featherstone") GTEST_SKIP(); #endif diff --git a/test/common_test/world_features.cc b/test/common_test/world_features.cc index 22909241b..de6433e70 100644 --- a/test/common_test/world_features.cc +++ b/test/common_test/world_features.cc @@ -419,11 +419,15 @@ TEST_F(WorldNestedModelTest, WorldConstructNestedModel) EXPECT_FALSE(worldModel->ConstructJoint(joint)); joint.SetChildName("parent_model::link1"); if (PhysicsEngineName(name) != "bullet-featherstone") + { EXPECT_TRUE(worldModel->ConstructJoint(joint)); + } else -#if BT_BULLET_VERSION >= 289 + { +#ifdef BT_BULLET_VERSION_GE_289 EXPECT_TRUE(worldModel->ConstructJoint(joint)); #endif + } gz::physics::ForwardStep::Input input; gz::physics::ForwardStep::State state; From 8e991167823429d8363c563b11cb8d3d659f609c Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 30 Jan 2024 19:56:02 +0000 Subject: [PATCH 27/29] fix merge Signed-off-by: Ian Chen --- test/common_test/Worlds.hh | 2 ++ test/common_test/world_features.cc | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/test/common_test/Worlds.hh b/test/common_test/Worlds.hh index d57aa32d3..6ad145f8f 100644 --- a/test/common_test/Worlds.hh +++ b/test/common_test/Worlds.hh @@ -48,6 +48,8 @@ const auto kStringPendulumSdf = CommonTestWorld("string_pendulum.sdf"); const auto kTestWorld = CommonTestWorld("test.world"); const auto kWorldJointTestSdf = CommonTestWorld("world_joint_test.sdf"); const auto kWorldUnsortedLinksSdf = CommonTestWorld("world_unsorted_links.sdf"); +const auto kWorldSingleNestedModelSdf = + CommonTestWorld("world_single_nested_model.sdf"); const auto kWorldWithNestedModelSdf = CommonTestWorld("world_with_nested_model.sdf"); } // namespace common_test::worlds diff --git a/test/common_test/world_features.cc b/test/common_test/world_features.cc index 709865fdc..640e185cc 100644 --- a/test/common_test/world_features.cc +++ b/test/common_test/world_features.cc @@ -381,7 +381,7 @@ class WorldNestedModelTest : public WorldFeaturesTest Date: Tue, 5 Mar 2024 01:34:07 +0000 Subject: [PATCH 28/29] typos, comments, style Signed-off-by: Ian Chen --- bullet-featherstone/src/Base.hh | 6 +++--- .../src/EntityManagementFeatures.cc | 2 -- bullet-featherstone/src/SDFFeatures.cc | 18 ++++++++++-------- 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index 231ddf0c0..4e9bb4338 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -394,8 +394,6 @@ class Base : public Implements3d> if (!model) return false; - bool isNested = this->worlds.find(_parentID) == this->worlds.end(); - // Remove nested models for (auto &nestedModelID : model->nestedModelEntityIds) { @@ -423,7 +421,9 @@ class Base : public Implements3d> } } - // If nested, we are done here. No need to remove multibody + // If nested, no need to remove multibody + // \todo(iche033) Remove links and joints in nested model + bool isNested = this->worlds.find(_parentID) == this->worlds.end(); if (isNested) { return true; diff --git a/bullet-featherstone/src/EntityManagementFeatures.cc b/bullet-featherstone/src/EntityManagementFeatures.cc index 99e40b827..775c2948b 100644 --- a/bullet-featherstone/src/EntityManagementFeatures.cc +++ b/bullet-featherstone/src/EntityManagementFeatures.cc @@ -206,8 +206,6 @@ Identity EntityManagementFeatures::ConstructEmptyWorld( bool EntityManagementFeatures::RemoveModel(const Identity &_modelID) { auto *model = this->ReferenceInterface(_modelID); - if (!model) - return false; return this->RemoveModelImpl(model->world, _modelID); } diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 464c5cb6e..0bbdfc11e 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -179,7 +179,7 @@ ::sdf::Errors resolveJointPoseRelToLink(math::Pose3d &_resolvedPose, return errors; } - // compute joint pose relative to parent link + // compute joint pose relative to specified link const auto *link = _model->LinkByName(_link); if (!link) { @@ -204,6 +204,7 @@ ::sdf::Errors resolveJointPoseRelToLink(math::Pose3d &_resolvedPose, ///////////////////////////////////////////////// /// \brief Recursively build a tree of parent-child data structures from the +/// input Model SDF. /// \param[in] _sdfModel input Model SDF. /// \param[out] _parentOf A map of child link to its parent /// \param[out] _linkTree A map of parent link to its child links @@ -302,7 +303,8 @@ bool buildTrees(const ::sdf::Model *_sdfModel, /// \brief Find all the root links given a model SDF /// \param[in] _sdfModel Model SDF /// \param[in] _parentOf A map of child link to its parent info -/// \param[out] _rootLinks A vector of root links and its immediate parent model +/// \param[out] _rootLinks A vector of root links paired with their +/// immediate parent model void findRootLinks(const ::sdf::Model *_sdfModel, const std::unordered_map &_parentOf, std::vector> &_rootLinks) @@ -472,12 +474,13 @@ Identity SDFFeatures::ConstructSdfModelImpl( // multiple sub-trees detected in model // \todo(iche033) support multiple kinematic trees and // multiple floating links in a single model + // https://github.com/gazebosim/gz-physics/issues/550 gzerr << "Multiple sub-trees / floating links detected in a model. " << "This is not supported in bullet-featherstone implementation yet." << std::endl; } // Create model for the first structure. - auto structure = structures[0]; + auto &structure = structures[0]; const bool isStatic = _sdfModel.Static(); WorldInfo *world = nullptr; @@ -688,7 +691,6 @@ Identity SDFFeatures::ConstructSdfModelImpl( btTransform parentLocalInertialFrame = convertTf( parentLinkInfo->inertiaToLinkFrame); btTransform parent2jointBt = convertTf( - // gz::math::eigen3::convert(parent2joint.Inverse())); // X_PJ poseParentLinkToJoint); // X_PJ // offsetInABt = parent COM to pivot (X_IpJ) @@ -702,7 +704,7 @@ Identity SDFFeatures::ConstructSdfModelImpl( btTransform offsetInABt, offsetInBBt; offsetInABt = parentLocalInertialFrame * parent2jointBt; offsetInBBt = - convertTf(linkToComTf.inverse() * poseJointToChild.inverse()); + convertTf(linkToComTf.inverse() * poseJointToChild.inverse()); // R_IcJ * R_IpJ ^ -1 = R_IcIp; btQuaternion parentRotToThis = offsetInBBt.getRotation() * offsetInABt.inverse().getRotation(); @@ -802,7 +804,7 @@ Identity SDFFeatures::ConstructSdfModelImpl( return this->GenerateInvalidId(); auto modelToNestedModel = Eigen::Isometry3d::Identity(); - // if the model of the root link is in a top level model, compute + // if the model of the root link is nested in a top level model, compute // its pose relative to top level model if (structure.model != &_sdfModel) { @@ -1267,7 +1269,7 @@ Identity SDFFeatures::ConstructSdfJoint( // >= 2.89. This is needed for dynamically creating world joints, // i.e. setting the btMultiBody to be fixed. So output an error letting // users know the joint will not be created. - // \todo(iche033) An workaround for this is to loop through all the joints + // \todo(iche033) A workaround for this is to loop through all the joints // in the world first in ConstructSdfWorld, keep track of the models who are // a child of the world joint, then when creating the btMultiBody // in ConstructSdfModelImpl, pass fixedBase as true in its constructor. @@ -1297,7 +1299,7 @@ Identity SDFFeatures::ConstructSdfJoint( childLinkName = _sdfJoint.ChildName(); } - // Currently only supports constructing joint with world + // Currently only supports constructing fixed joint with world as parent if (parentLinkName == "world" && _sdfJoint.Type() == ::sdf::JointType::FIXED) { auto worldModelIt = this->models.find(_modelID); From d08d0ef83a4e4cc638ff11eb792877be90a0ea9f Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 5 Mar 2024 17:32:47 +0000 Subject: [PATCH 29/29] check null model, update test Signed-off-by: Ian Chen --- bullet-featherstone/src/EntityManagementFeatures.cc | 2 ++ test/common_test/world_features.cc | 4 ++++ 2 files changed, 6 insertions(+) diff --git a/bullet-featherstone/src/EntityManagementFeatures.cc b/bullet-featherstone/src/EntityManagementFeatures.cc index 775c2948b..99e40b827 100644 --- a/bullet-featherstone/src/EntityManagementFeatures.cc +++ b/bullet-featherstone/src/EntityManagementFeatures.cc @@ -206,6 +206,8 @@ Identity EntityManagementFeatures::ConstructEmptyWorld( bool EntityManagementFeatures::RemoveModel(const Identity &_modelID) { auto *model = this->ReferenceInterface(_modelID); + if (!model) + return false; return this->RemoveModelImpl(model->world, _modelID); } diff --git a/test/common_test/world_features.cc b/test/common_test/world_features.cc index 640e185cc..e1e68c0f0 100644 --- a/test/common_test/world_features.cc +++ b/test/common_test/world_features.cc @@ -434,6 +434,10 @@ TEST_F(WorldNestedModelTest, WorldConstructNestedModel) gz::physics::ForwardStep::State state; gz::physics::ForwardStep::Output output; + // Check invalid input to RemoveNestedModel method + EXPECT_FALSE(worldModel->RemoveNestedModel(1)); + EXPECT_FALSE(worldModel->RemoveNestedModel("invalid")); + // Check that we can remove models via RemoveNestedModel EXPECT_TRUE(worldModel->RemoveNestedModel(0)); EXPECT_TRUE(nestedModel->Removed());