From b54cd35001de3d0f09c75db004ce63ec50aef849 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 25 Feb 2021 09:18:37 -0600 Subject: [PATCH 01/17] Fix error caused when the same link name is used in different models Signed-off-by: Addisu Z. Taddese --- dartsim/src/Base.hh | 7 ++++++- dartsim/src/SDFFeatures.cc | 8 ++++---- dartsim/src/SDFFeatures.hh | 2 +- dartsim/src/SDFFeatures_TEST.cc | 2 +- dartsim/worlds/world_with_nested_model.sdf | 9 +++++++++ 5 files changed, 21 insertions(+), 7 deletions(-) diff --git a/dartsim/src/Base.hh b/dartsim/src/Base.hh index b0c39813c..e13292f46 100644 --- a/dartsim/src/Base.hh +++ b/dartsim/src/Base.hh @@ -398,7 +398,12 @@ class Base : public Implements3d> return matches.front(); else if (matches.size() > 1) { - ignerr << "Found " << matches.size() << " for " << _name << std::endl; + ignerr << "Found " << matches.size() << "skeletons that contain " << _name + << ":" << std::endl; + for (const auto &match : matches) + { + ignerr << match->getName() << "\n"; + } return nullptr; } return nullptr; diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index 6ee918f8b..ecec514e3 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -330,7 +330,7 @@ static ShapeAndTransform ConstructGeometry( } // namespace dart::dynamics::BodyNode *SDFFeatures::FindBodyNode( - dart::simulation::WorldPtr _world, const std::string _jointModelName, + dart::simulation::WorldPtr _world, const std::string &_jointModelName, const std::string &_entityFullName) { const auto[modelAbsoluteName, localName] = @@ -470,7 +470,7 @@ Identity SDFFeatures::ConstructSdfModelImpl( << "] of joint [" << sdfJoint->Name() << "] in model [" << modelName << "] could not be resolved. The joint will not be constructed\n"; - for (const auto error : errors) + for (const auto &error : errors) { ignerr << error << std::endl; } @@ -484,7 +484,7 @@ Identity SDFFeatures::ConstructSdfModelImpl( << "] of joint [" << sdfJoint->Name() << "] in model [" << modelName << "] could not be resolved. The joint will not be constructed\n"; - for (const auto error : errors) + for (const auto &error : errors) { ignerr << error << std::endl; } @@ -523,7 +523,7 @@ Identity SDFFeatures::ConstructSdfModelImpl( } auto * const child = - FindBodyNode(worlds[worldID], modelName, childSdfLink->Name()); + FindBodyNode(worlds[worldID], modelName, childLinkName); if (nullptr == child) { ignerr << "The child of joint [" << sdfJoint->Name() << "] in model [" diff --git a/dartsim/src/SDFFeatures.hh b/dartsim/src/SDFFeatures.hh index 25214035e..df2707636 100644 --- a/dartsim/src/SDFFeatures.hh +++ b/dartsim/src/SDFFeatures.hh @@ -120,7 +120,7 @@ class SDFFeatures : /// \returns The matched body node if exactly one match is found, otherwise /// a nullptr private: dart::dynamics::BodyNode *FindBodyNode( - dart::simulation::WorldPtr _world, const std::string _jointModelName, + dart::simulation::WorldPtr _world, const std::string &_jointModelName, const std::string &_entityFullName); }; diff --git a/dartsim/src/SDFFeatures_TEST.cc b/dartsim/src/SDFFeatures_TEST.cc index 3530b4f20..2568d7f08 100644 --- a/dartsim/src/SDFFeatures_TEST.cc +++ b/dartsim/src/SDFFeatures_TEST.cc @@ -327,7 +327,7 @@ TEST(SDFFeatures_TEST, WorldIsParentOrChild) TEST(SDFFeatures_TEST, WorldWithNestedModel) { World world = LoadWorld(TEST_WORLD_DIR "/world_with_nested_model.sdf"); - EXPECT_EQ(2u, world.GetModelCount()); + EXPECT_EQ(4u, world.GetModelCount()); dart::simulation::WorldPtr dartWorld = world.GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); diff --git a/dartsim/worlds/world_with_nested_model.sdf b/dartsim/worlds/world_with_nested_model.sdf index 7c0fcff9c..6ba6c4723 100644 --- a/dartsim/worlds/world_with_nested_model.sdf +++ b/dartsim/worlds/world_with_nested_model.sdf @@ -53,6 +53,15 @@ link1 nested_model::nested_link1 + + + + + + + + From 6c59b58ad65a7e2dac046125532fa83be004f6f4 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 3 Mar 2021 17:38:27 -0600 Subject: [PATCH 02/17] Make LoadWorld return WorldPtr Signed-off-by: Addisu Z. Taddese --- dartsim/src/SDFFeatures_TEST.cc | 57 ++++++++++++++++++--------------- 1 file changed, 32 insertions(+), 25 deletions(-) diff --git a/dartsim/src/SDFFeatures_TEST.cc b/dartsim/src/SDFFeatures_TEST.cc index 2568d7f08..051ddd3d5 100644 --- a/dartsim/src/SDFFeatures_TEST.cc +++ b/dartsim/src/SDFFeatures_TEST.cc @@ -76,7 +76,7 @@ auto LoadEngine() } ///////////////////////////////////////////////// -World LoadWorld(const std::string &_world) +WorldPtr LoadWorld(const std::string &_world) { auto engine = LoadEngine(); EXPECT_NE(nullptr, engine); @@ -95,16 +95,17 @@ World LoadWorld(const std::string &_world) auto world = engine->ConstructWorld(*sdfWorld); EXPECT_NE(nullptr, world); - return *world; + return world; } ///////////////////////////////////////////////// // Test that the dartsim plugin loaded all the relevant information correctly. TEST(SDFFeatures_TEST, CheckDartsimData) { - World world = LoadWorld(TEST_WORLD_DIR"/test.world"); + WorldPtr world = LoadWorld(TEST_WORLD_DIR"/test.world"); + ASSERT_NE(nullptr, world); - dart::simulation::WorldPtr dartWorld = world.GetDartsimWorld(); + dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); ASSERT_EQ(6u, dartWorld->getNumSkeletons()); @@ -207,12 +208,13 @@ TEST(SDFFeatures_TEST, CheckDartsimData) // Test that joint limits are working by running the simulation TEST(SDFFeatures_TEST, CheckJointLimitEnforcement) { - World world = LoadWorld(TEST_WORLD_DIR"/test.world"); + WorldPtr world = LoadWorld(TEST_WORLD_DIR"/test.world"); + ASSERT_NE(nullptr, world); - dart::simulation::WorldPtr dartWorld = world.GetDartsimWorld(); + dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); - const auto model = world.GetModel("joint_limit_test"); + const auto model = world->GetModel("joint_limit_test"); const dart::dynamics::SkeletonPtr skeleton = dartWorld->getSkeleton("joint_limit_test"); ASSERT_NE(nullptr, skeleton); @@ -326,22 +328,23 @@ TEST(SDFFeatures_TEST, WorldIsParentOrChild) ///////////////////////////////////////////////// TEST(SDFFeatures_TEST, WorldWithNestedModel) { - World world = LoadWorld(TEST_WORLD_DIR "/world_with_nested_model.sdf"); - EXPECT_EQ(4u, world.GetModelCount()); + WorldPtr world = LoadWorld(TEST_WORLD_DIR "/world_with_nested_model.sdf"); + ASSERT_NE(nullptr, world); + EXPECT_EQ(4u, world->GetModelCount()); - dart::simulation::WorldPtr dartWorld = world.GetDartsimWorld(); + dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); // check top level model - EXPECT_EQ("parent_model", world.GetModel(0)->GetName()); - EXPECT_EQ("parent_model::nested_model", world.GetModel(1)->GetName()); - auto parentModel = world.GetModel("parent_model"); + EXPECT_EQ("parent_model", world->GetModel(0)->GetName()); + EXPECT_EQ("parent_model::nested_model", world->GetModel(1)->GetName()); + auto parentModel = world->GetModel("parent_model"); ASSERT_NE(nullptr, parentModel); auto joint1 = parentModel->GetJoint("joint1"); EXPECT_NE(nullptr, joint1); - auto nestedModel = world.GetModel("parent_model::nested_model"); + auto nestedModel = world->GetModel("parent_model::nested_model"); ASSERT_NE(nullptr, nestedModel); auto nestedJoint = parentModel->GetJoint("nested_joint"); @@ -362,15 +365,16 @@ TEST(SDFFeatures_TEST, WorldWithNestedModel) ///////////////////////////////////////////////// TEST(SDFFeatures_TEST, WorldWithNestedModelJointToWorld) { - World world = + WorldPtr world = LoadWorld(TEST_WORLD_DIR "/world_with_nested_model_joint_to_world.sdf"); - EXPECT_EQ(2u, world.GetModelCount()); + ASSERT_NE(nullptr, world); + EXPECT_EQ(2u, world->GetModelCount()); - dart::simulation::WorldPtr dartWorld = world.GetDartsimWorld(); + dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); // check top level model - auto parentModel = world.GetModel("parent_model"); + auto parentModel = world->GetModel("parent_model"); ASSERT_NE(nullptr, parentModel); EXPECT_EQ("parent_model", parentModel->GetName()); EXPECT_EQ(1u, parentModel->GetJointCount()); @@ -383,7 +387,7 @@ TEST(SDFFeatures_TEST, WorldWithNestedModelJointToWorld) auto link1 = parentModel->GetLink("link1"); EXPECT_NE(nullptr, link1); - auto nestedModel = world.GetModel("parent_model::nested_model"); + auto nestedModel = world->GetModel("parent_model::nested_model"); ASSERT_NE(nullptr, nestedModel); EXPECT_EQ("parent_model::nested_model", nestedModel->GetName()); EXPECT_EQ(2u, nestedModel->GetLinkCount()); @@ -406,9 +410,10 @@ TEST(SDFFeatures_TEST, WorldWithNestedModelJointToWorld) // Test that joint type falls back to fixed if the type is not supported TEST(SDFFeatures_TEST, FallbackToFixedJoint) { - World world = LoadWorld(TEST_WORLD_DIR"/test.world"); + WorldPtr world = LoadWorld(TEST_WORLD_DIR"/test.world"); + ASSERT_NE(nullptr, world); - dart::simulation::WorldPtr dartWorld = world.GetDartsimWorld(); + dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); const dart::dynamics::SkeletonPtr skeleton = @@ -430,10 +435,11 @@ TEST(SDFFeatures_TEST, FallbackToFixedJoint) ///////////////////////////////////////////////// TEST(SDFFeatures_FrameSemantics, LinkRelativeTo) { - World world = LoadWorld(TEST_WORLD_DIR"/model_frames.sdf"); + WorldPtr world = LoadWorld(TEST_WORLD_DIR"/model_frames.sdf"); + ASSERT_NE(nullptr, world); const std::string modelName = "link_relative_to"; - dart::simulation::WorldPtr dartWorld = world.GetDartsimWorld(); + dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); const dart::dynamics::SkeletonPtr skeleton = @@ -461,10 +467,11 @@ TEST(SDFFeatures_FrameSemantics, LinkRelativeTo) ///////////////////////////////////////////////// TEST(SDFFeatures_FrameSemantics, CollisionRelativeTo) { - World world = LoadWorld(TEST_WORLD_DIR"/model_frames.sdf"); + WorldPtr world = LoadWorld(TEST_WORLD_DIR"/model_frames.sdf"); + ASSERT_NE(nullptr, world); const std::string modelName = "collision_relative_to"; - dart::simulation::WorldPtr dartWorld = world.GetDartsimWorld(); + dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); const dart::dynamics::SkeletonPtr skeleton = From d7257a49bb591950e2a8d97444a4cf2168989764 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 3 Mar 2021 23:54:11 -0600 Subject: [PATCH 03/17] Handle BodyNodes moving to other skeletons Signed-off-by: Addisu Z. Taddese --- dartsim/src/Base.hh | 13 +- dartsim/src/Base_TEST.cc | 8 +- dartsim/src/EntityManagementFeatures.cc | 16 +- dartsim/src/SDFFeatures.cc | 67 ++++--- dartsim/src/SDFFeatures.hh | 14 +- dartsim/src/SDFFeatures_TEST.cc | 229 ++++++++++++++++++++++-- 6 files changed, 295 insertions(+), 52 deletions(-) diff --git a/dartsim/src/Base.hh b/dartsim/src/Base.hh index e13292f46..a4d281095 100644 --- a/dartsim/src/Base.hh +++ b/dartsim/src/Base.hh @@ -288,7 +288,8 @@ class Base : public Implements3d> return std::forward_as_tuple(id, entry); } - public: inline std::size_t AddLink(DartBodyNode *_bn) + public: inline std::size_t AddLink(DartBodyNode *_bn, + const std::string &_fullName) { const std::size_t id = this->GetNextEntity(); this->links.idToObject[id] = std::make_shared(); @@ -299,6 +300,8 @@ class Base : public Implements3d> this->links.objectToID[_bn] = id; this->frames[id] = _bn; + this->linksByName[_fullName] = _bn; + return id; } @@ -326,6 +329,7 @@ class Base : public Implements3d> public: void RemoveModelImpl(const std::size_t _worldID, const std::size_t _modelID) { + // TODO (addisu) Handle removal of nested models const auto &world = this->worlds.at(_worldID); auto skel = this->models.at(_modelID)->model; // Remove the contents of the skeleton from local entity storage containers @@ -340,6 +344,8 @@ class Base : public Implements3d> this->shapes.RemoveEntity(sn); } this->links.RemoveEntity(bn); + this->linksByName.erase(::sdf::JoinName( + world->getName(), ::sdf::JoinName(skel->getName(), bn->getName()))); } this->models.RemoveEntity(skel); world->removeSkeleton(skel); @@ -415,6 +421,11 @@ class Base : public Implements3d> public: EntityStorage joints; public: EntityStorage shapes; public: std::unordered_map frames; + + /// \brief Map from the fully qualified link name (including the world name) + /// to the BodyNode object. This is useful for keeping track of BodyNodes even + /// as they move to other skeletons. + public: std::unordered_map linksByName; }; } diff --git a/dartsim/src/Base_TEST.cc b/dartsim/src/Base_TEST.cc index d18a84314..7dd9f3768 100644 --- a/dartsim/src/Base_TEST.cc +++ b/dartsim/src/Base_TEST.cc @@ -65,7 +65,10 @@ TEST(BaseClass, RemoveModel) boxShape); auto res = base.AddModel({skel, frame, ""}, worldID); - base.AddLink(pair.second); + const std::string fullName = ::sdf::JoinName( + world->getName(), + ::sdf::JoinName(skel->getName(), pair.second->getName())); + base.AddLink(pair.second, fullName); base.AddJoint(pair.first); base.AddShape({sn, name + "_shape"}); @@ -80,6 +83,7 @@ TEST(BaseClass, RemoveModel) EXPECT_EQ(5u, base.models.size()); EXPECT_EQ(5u, base.links.size()); + EXPECT_EQ(5u, base.linksByName.size()); EXPECT_EQ(5u, base.joints.size()); EXPECT_EQ(5u, base.shapes.size()); @@ -93,6 +97,7 @@ TEST(BaseClass, RemoveModel) // Check that other resouces (links, shapes, etc) are also removed EXPECT_EQ(4u, base.models.size()); EXPECT_EQ(4u, base.links.size()); + EXPECT_EQ(4u, base.linksByName.size()); EXPECT_EQ(4u, base.joints.size()); EXPECT_EQ(4u, base.shapes.size()); @@ -126,6 +131,7 @@ TEST(BaseClass, RemoveModel) --curSize; EXPECT_EQ(curSize, base.models.size()); EXPECT_EQ(curSize, base.links.size()); + EXPECT_EQ(curSize, base.linksByName.size()); EXPECT_EQ(curSize, base.joints.size()); EXPECT_EQ(curSize, base.shapes.size()); checkModelIndices(); diff --git a/dartsim/src/EntityManagementFeatures.cc b/dartsim/src/EntityManagementFeatures.cc index 69cc1ff77..da9f3f012 100644 --- a/dartsim/src/EntityManagementFeatures.cc +++ b/dartsim/src/EntityManagementFeatures.cc @@ -653,7 +653,21 @@ Identity EntityManagementFeatures::ConstructEmptyLink( model->createJointAndBodyNodePair( nullptr, prop_fj, prop_bn).second; - const std::size_t linkID = this->AddLink(bn); + auto worldIDIt = this->models.idToContainerID.find(_modelID); + if (worldIDIt == this->models.idToContainerID.end()) + { + ignerr << "World of model [" << model->getName() + << "] could not be found when creating link [" << _name + << "]\n"; + return this->GenerateInvalidId(); + } + + auto world = this->worlds.at(worldIDIt->second); + const std::string fullName = ::sdf::JoinName( + world->getName(), + ::sdf::JoinName(model->getName(), bn->getName())); + + const std::size_t linkID = this->AddLink(bn, fullName); return this->GenerateIdentity(linkID, this->links.at(linkID)); } diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index ecec514e3..a42b4fa4a 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -330,31 +330,22 @@ static ShapeAndTransform ConstructGeometry( } // namespace dart::dynamics::BodyNode *SDFFeatures::FindBodyNode( - dart::simulation::WorldPtr _world, const std::string &_jointModelName, - const std::string &_entityFullName) + const std::string &_worldName, const std::string &_jointModelName, + const std::string &_linkRelativeName) { - const auto[modelAbsoluteName, localName] = - ::sdf::SplitName(_entityFullName); - - if (localName == "world") - return nullptr; - - const auto modelSkeleton = _world->getSkeleton(_jointModelName); - - // First check the obvious place - if (modelSkeleton != nullptr - && modelSkeleton->getBodyNode(localName) != nullptr) - return modelSkeleton->getBodyNode(localName); - - // For nested situations where the entity's name doesn't contain the fully- - // qualified skeleton name (e.g. skeleton = "a::b::c", but the entity name - // contains "b::c") - auto matchedSkeleton = - this->FindContainingSkeletonFromName(_world, _entityFullName); - if (matchedSkeleton == nullptr) + if (_linkRelativeName == "world") return nullptr; - return matchedSkeleton->getBodyNode(localName); + const std::string fullName = ::sdf::JoinName( + _worldName, ::sdf::JoinName(_jointModelName, _linkRelativeName)); + auto it = this->linksByName.find(fullName); + if (it != this->linksByName.end()) + { + return it->second; + } + ignerr << "Could not find link " << _linkRelativeName << " in model " + << _jointModelName << std::endl; + return nullptr; } ///////////////////////////////////////////////// @@ -512,7 +503,7 @@ Identity SDFFeatures::ConstructSdfModelImpl( } auto * const parent = - FindBodyNode(this->worlds[worldID], modelName, parentLinkName); + FindBodyNode(this->worlds[worldID]->getName(), modelName, parentLinkName); if (nullptr == parent && parentLinkName != "world") { @@ -523,7 +514,7 @@ Identity SDFFeatures::ConstructSdfModelImpl( } auto * const child = - FindBodyNode(worlds[worldID], modelName, childLinkName); + FindBodyNode(worlds[worldID]->getName(), modelName, childLinkName); if (nullptr == child) { ignerr << "The child of joint [" << sdfJoint->Name() << "] in model [" @@ -586,7 +577,20 @@ Identity SDFFeatures::ConstructSdfLink( dart::dynamics::BodyNode * const bn = result.second; - const std::size_t linkID = this->AddLink(bn); + auto worldIDIt = this->models.idToContainerID.find(_modelID); + if (worldIDIt == this->models.idToContainerID.end()) + { + ignerr << "World of model [" << modelInfo.model->getName() + << "] could not be found when creating link [" << _sdfLink.Name() + << "]\n"; + return this->GenerateInvalidId(); + } + + auto world = this->worlds.at(worldIDIt->second); + const std::string fullName = ::sdf::JoinName( + world->getName(), + ::sdf::JoinName(modelInfo.model->getName(), bn->getName())); + const std::size_t linkID = this->AddLink(bn, fullName); this->AddJoint(joint); auto linkIdentity = this->GenerateIdentity(linkID, this->links.at(linkID)); @@ -630,6 +634,15 @@ Identity SDFFeatures::ConstructSdfJoint( { const auto &modelInfo = *this->ReferenceInterface(_modelID); + if (_sdfJoint.ChildLinkName() == "world") + { + ignerr << "Asked to create a joint with the world as the child in model " + << "[" << modelInfo.model->getName() << "]. This is currently not " + << "supported\n"; + + return this->GenerateInvalidId(); + } + // If we get an error here, one of two things has occured: // 1. The Model _sdfJoint came from is invalid, i.e, had errors during // sdf::Root::Load and ConstructSdfJoint was called regardless of the @@ -663,7 +676,7 @@ Identity SDFFeatures::ConstructSdfJoint( } dart::dynamics::BodyNode * const parent = - FindBodyNode(world, modelInfo.model->getName(), parentLinkName); + FindBodyNode(world->getName(), modelInfo.model->getName(), parentLinkName); std::string childLinkName; const auto childResolveErrors = _sdfJoint.ResolveChildLink(childLinkName); @@ -672,7 +685,7 @@ Identity SDFFeatures::ConstructSdfJoint( } dart::dynamics::BodyNode * const child = - FindBodyNode(world, modelInfo.model->getName(), childLinkName); + FindBodyNode(world->getName(), modelInfo.model->getName(), childLinkName); if (nullptr == parent && parentLinkName != "world") { diff --git a/dartsim/src/SDFFeatures.hh b/dartsim/src/SDFFeatures.hh index df2707636..60db0bce3 100644 --- a/dartsim/src/SDFFeatures.hh +++ b/dartsim/src/SDFFeatures.hh @@ -112,16 +112,18 @@ class SDFFeatures : private: Identity ConstructSdfModelImpl(std::size_t _parentID, const ::sdf::Model &_sdfModel); - /// \brief Find the dartsim BodyNode associated with the entity name - /// \param[in] _world Pointer to world to search through + /// \brief Find the dartsim BodyNode associated with the link name + /// \param[in] _worldName Name of world that contains the link /// \param[in] _jointModelName The name of the model associated with the joint - /// \param[in] _entityFullName The full name of the entity as specified in the - /// sdformat description + /// \param[in] _linkRelativeName The relative name of the link as specified in + /// the sdformat description in the scope of the model identified by + /// _jointModelName /// \returns The matched body node if exactly one match is found, otherwise /// a nullptr private: dart::dynamics::BodyNode *FindBodyNode( - dart::simulation::WorldPtr _world, const std::string &_jointModelName, - const std::string &_entityFullName); + const std::string &_worldName, + const std::string &_jointModelName, + const std::string &_linkRelativeName); }; } diff --git a/dartsim/src/SDFFeatures_TEST.cc b/dartsim/src/SDFFeatures_TEST.cc index 051ddd3d5..311245452 100644 --- a/dartsim/src/SDFFeatures_TEST.cc +++ b/dartsim/src/SDFFeatures_TEST.cc @@ -33,6 +33,7 @@ #include #include +#include #include #include #include @@ -41,6 +42,10 @@ #include +#include +#include +#include +#include #include #include @@ -51,6 +56,7 @@ struct TestFeatureList : ignition::physics::FeatureList< ignition::physics::GetBasicJointState, ignition::physics::SetBasicJointState, ignition::physics::dartsim::RetrieveWorld, + ignition::physics::sdf::ConstructSdfCollision, ignition::physics::sdf::ConstructSdfJoint, ignition::physics::sdf::ConstructSdfLink, ignition::physics::sdf::ConstructSdfModel, @@ -60,6 +66,8 @@ struct TestFeatureList : ignition::physics::FeatureList< using World = ignition::physics::World3d; using WorldPtr = ignition::physics::World3dPtr; +using ModelPtr = ignition::physics::Model3dPtr; +using LinkPtr = ignition::physics::Link3dPtr; ///////////////////////////////////////////////// auto LoadEngine() @@ -75,8 +83,14 @@ auto LoadEngine() return engine; } +enum class LoaderType +{ + Whole, + Piecemeal +}; + ///////////////////////////////////////////////// -WorldPtr LoadWorld(const std::string &_world) +WorldPtr LoadWorldWhole(const std::string &_world) { auto engine = LoadEngine(); EXPECT_NE(nullptr, engine); @@ -98,11 +112,189 @@ WorldPtr LoadWorld(const std::string &_world) return world; } +///////////////////////////////////////////////// +static ignition::math::Pose3d ResolveSdfPose(const ::sdf::SemanticPose &_semPose) +{ + ignition::math::Pose3d pose; + ::sdf::Errors errors = _semPose.Resolve(pose); + EXPECT_TRUE(errors.empty()) << errors; + return pose; +} + +static sdf::JointAxis ResolveJointAxis(const sdf::JointAxis &_unresolvedAxis) +{ + ignition::math::Vector3d axisXyz; + const sdf::Errors resolveAxisErrors = _unresolvedAxis.ResolveXyz(axisXyz); + EXPECT_TRUE (resolveAxisErrors.empty()) << resolveAxisErrors; + + sdf::JointAxis resolvedAxis = _unresolvedAxis; + + const sdf::Errors setXyzErrors = resolvedAxis.SetXyz(axisXyz); + EXPECT_TRUE(setXyzErrors.empty()) << setXyzErrors; + + resolvedAxis.SetXyzExpressedIn(""); + return resolvedAxis; +} + +///////////////////////////////////////////////// +/// Downstream applications, like ign-gazebo, use this way of world construction +WorldPtr LoadWorldPiecemeal(const std::string &_world) +{ + auto engine = LoadEngine(); + EXPECT_NE(nullptr, engine); + if (nullptr == engine) + return nullptr; + + sdf::Root root; + const sdf::Errors &errors = root.Load(_world); + EXPECT_EQ(0u, errors.size()) << errors; + + EXPECT_EQ(1u, root.WorldCount()); + const sdf::World *sdfWorld = root.WorldByIndex(0); + EXPECT_NE(nullptr, sdfWorld); + + sdf::World newWorld; + newWorld.SetName(sdfWorld->Name()); + newWorld.SetGravity(sdfWorld->Gravity()); + auto world = engine->ConstructWorld(newWorld); + if (nullptr == world) + return nullptr; + + std::unordered_map modelMap; + std::unordered_map linkMap; + + auto createModel = [&](const sdf::Model *_model, + const sdf::Model *_parentModel = nullptr) { + sdf::Model newSdfModel; + newSdfModel.SetName(_model->Name()); + newSdfModel.SetRawPose(ResolveSdfPose(_model->SemanticPose())); + newSdfModel.SetStatic(_model->Static()); + newSdfModel.SetSelfCollide(_model->SelfCollide()); + + ModelPtr newModel; + if (nullptr != _parentModel) + { + auto it = modelMap.find(_parentModel); + ASSERT_TRUE(it != modelMap.end()); + newModel = it->second->ConstructNestedModel(newSdfModel); + } + else + { + newModel = world->ConstructModel(newSdfModel); + } + + EXPECT_NE(nullptr, newModel); + if (nullptr != newModel) + { + modelMap[_model] = newModel; + } + }; + + for (uint64_t i = 0; i < sdfWorld->ModelCount(); ++i) + { + const auto *model = sdfWorld->ModelByIndex(i); + createModel(model); + for (uint64_t nestedInd = 0; nestedInd < model->ModelCount(); ++nestedInd) + { + createModel(model->ModelByIndex(nestedInd), model); + } + } + + for (auto [sdfModel, physModel] : modelMap) + { + for (uint64_t li = 0; li < sdfModel->LinkCount(); ++li) + { + const auto *link = sdfModel->LinkByIndex(li); + sdf::Link newSdfLink; + newSdfLink.SetName(link->Name()); + newSdfLink.SetRawPose(ResolveSdfPose(link->SemanticPose())); + newSdfLink.SetInertial(link->Inertial()); + + auto newLink = physModel->ConstructLink(newSdfLink); + EXPECT_NE(nullptr, newLink); + if (nullptr == newLink) + return nullptr; + + linkMap[link] = newLink; + } + } + + for (auto [sdfLink, physLink] : linkMap) + { + for (uint64_t ci = 0; ci < sdfLink->CollisionCount(); ++ci) + { + physLink->ConstructCollision(*sdfLink->CollisionByIndex(ci)); + } + } + + for (auto [sdfModel, physModel] : modelMap) + { + for (uint64_t ji = 0; ji < sdfModel->JointCount(); ++ji) + { + const auto *sdfJoint = sdfModel->JointByIndex(ji); + std::string resolvedParentLinkName; + const auto resolveParentErrors = + sdfJoint->ResolveParentLink(resolvedParentLinkName); + EXPECT_TRUE(resolveParentErrors.empty()) << resolveParentErrors; + + std::string resolvedChildLinkName; + const auto resolveChildErrors = + sdfJoint->ResolveChildLink(resolvedChildLinkName); + EXPECT_TRUE (resolveChildErrors.empty()) << resolveChildErrors; + + sdf::Joint newSdfJoint; + newSdfJoint.SetName(sdfJoint->Name()); + if (sdfJoint->Axis(0)) + { + newSdfJoint.SetAxis(0, ResolveJointAxis(*sdfJoint->Axis(0))); + } + if (sdfJoint->Axis(1)) + { + newSdfJoint.SetAxis(1, ResolveJointAxis(*sdfJoint->Axis(1))); + } + newSdfJoint.SetType(sdfJoint->Type()); + newSdfJoint.SetRawPose(ResolveSdfPose(sdfJoint->SemanticPose())); + newSdfJoint.SetThreadPitch(sdfJoint->ThreadPitch()); + + newSdfJoint.SetParentLinkName(resolvedParentLinkName); + newSdfJoint.SetChildLinkName(resolvedChildLinkName); + + physModel->ConstructJoint(newSdfJoint); + } + } + + return world; +} +///////////////////////////////////////////////// +WorldPtr LoadWorld(const std::string &_world, + LoaderType _loader = LoaderType::Whole) +{ + switch(_loader) + { + case LoaderType::Whole: + return LoadWorldWhole(_world); + case LoaderType::Piecemeal: + return LoadWorldPiecemeal(_world); + default: + return LoadWorldWhole(_world); + } +} + +///////////////////////////////////////////////// +class SDFFeatures_TEST : public ::testing::TestWithParam +{ +}; + +// Run with different load world functions +INSTANTIATE_TEST_CASE_P(LoadWorld, SDFFeatures_TEST, + ::testing::Values(LoaderType::Whole, + LoaderType::Piecemeal), ); + ///////////////////////////////////////////////// // Test that the dartsim plugin loaded all the relevant information correctly. -TEST(SDFFeatures_TEST, CheckDartsimData) +TEST_P(SDFFeatures_TEST, CheckDartsimData) { - WorldPtr world = LoadWorld(TEST_WORLD_DIR"/test.world"); + WorldPtr world = LoadWorld(TEST_WORLD_DIR"/test.world", this->GetParam()); ASSERT_NE(nullptr, world); dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); @@ -206,7 +398,7 @@ TEST(SDFFeatures_TEST, CheckDartsimData) ///////////////////////////////////////////////// // Test that joint limits are working by running the simulation -TEST(SDFFeatures_TEST, CheckJointLimitEnforcement) +TEST_P(SDFFeatures_TEST, CheckJointLimitEnforcement) { WorldPtr world = LoadWorld(TEST_WORLD_DIR"/test.world"); ASSERT_NE(nullptr, world); @@ -326,9 +518,10 @@ TEST(SDFFeatures_TEST, WorldIsParentOrChild) } ///////////////////////////////////////////////// -TEST(SDFFeatures_TEST, WorldWithNestedModel) +TEST_P(SDFFeatures_TEST, WorldWithNestedModel) { - WorldPtr world = LoadWorld(TEST_WORLD_DIR "/world_with_nested_model.sdf"); + WorldPtr world = LoadWorld(TEST_WORLD_DIR "/world_with_nested_model.sdf", + this->GetParam()); ASSERT_NE(nullptr, world); EXPECT_EQ(4u, world->GetModelCount()); @@ -363,10 +556,11 @@ TEST(SDFFeatures_TEST, WorldWithNestedModel) } ///////////////////////////////////////////////// -TEST(SDFFeatures_TEST, WorldWithNestedModelJointToWorld) +TEST_P(SDFFeatures_TEST, WorldWithNestedModelJointToWorld) { WorldPtr world = - LoadWorld(TEST_WORLD_DIR "/world_with_nested_model_joint_to_world.sdf"); + LoadWorld(TEST_WORLD_DIR "/world_with_nested_model_joint_to_world.sdf", + this->GetParam()); ASSERT_NE(nullptr, world); EXPECT_EQ(2u, world->GetModelCount()); @@ -408,9 +602,9 @@ TEST(SDFFeatures_TEST, WorldWithNestedModelJointToWorld) ///////////////////////////////////////////////// // Test that joint type falls back to fixed if the type is not supported -TEST(SDFFeatures_TEST, FallbackToFixedJoint) +TEST_P(SDFFeatures_TEST, FallbackToFixedJoint) { - WorldPtr world = LoadWorld(TEST_WORLD_DIR"/test.world"); + WorldPtr world = LoadWorld(TEST_WORLD_DIR"/test.world", this->GetParam()); ASSERT_NE(nullptr, world); dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); @@ -504,10 +698,11 @@ TEST(SDFFeatures_FrameSemantics, CollisionRelativeTo) ///////////////////////////////////////////////// TEST(SDFFeatures_FrameSemantics, ExplicitFramesWithLinks) { - World world = LoadWorld(TEST_WORLD_DIR"/model_frames.sdf"); + WorldPtr world = LoadWorld(TEST_WORLD_DIR"/model_frames.sdf"); + ASSERT_NE(nullptr, world); const std::string modelName = "explicit_frames_with_links"; - dart::simulation::WorldPtr dartWorld = world.GetDartsimWorld(); + dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); const dart::dynamics::SkeletonPtr skeleton = @@ -550,10 +745,11 @@ TEST(SDFFeatures_FrameSemantics, ExplicitFramesWithLinks) ///////////////////////////////////////////////// TEST(SDFFeatures_FrameSemantics, ExplicitFramesWithCollision) { - World world = LoadWorld(TEST_WORLD_DIR"/model_frames.sdf"); + WorldPtr world = LoadWorld(TEST_WORLD_DIR"/model_frames.sdf"); + ASSERT_NE(nullptr, world); const std::string modelName = "explicit_frames_with_collisions"; - dart::simulation::WorldPtr dartWorld = world.GetDartsimWorld(); + dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); const dart::dynamics::SkeletonPtr skeleton = @@ -586,10 +782,11 @@ TEST(SDFFeatures_FrameSemantics, ExplicitFramesWithCollision) ///////////////////////////////////////////////// TEST(SDFFeatures_FrameSemantics, ExplicitWorldFrames) { - World world = LoadWorld(TEST_WORLD_DIR"/world_frames.sdf"); + WorldPtr world = LoadWorld(TEST_WORLD_DIR"/world_frames.sdf"); + ASSERT_NE(nullptr, world); const std::string modelName = "M"; - dart::simulation::WorldPtr dartWorld = world.GetDartsimWorld(); + dart::simulation::WorldPtr dartWorld = world->GetDartsimWorld(); ASSERT_NE(nullptr, dartWorld); const dart::dynamics::SkeletonPtr skeleton = From bcfc9833b5c6c7a52860ff1f873dffaa20d2661e Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 4 Mar 2021 16:41:34 -0600 Subject: [PATCH 04/17] Codecheck Signed-off-by: Addisu Z. Taddese --- dartsim/src/Base.hh | 2 +- dartsim/src/SDFFeatures_TEST.cc | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/dartsim/src/Base.hh b/dartsim/src/Base.hh index a4d281095..30c62646c 100644 --- a/dartsim/src/Base.hh +++ b/dartsim/src/Base.hh @@ -329,7 +329,7 @@ class Base : public Implements3d> public: void RemoveModelImpl(const std::size_t _worldID, const std::size_t _modelID) { - // TODO (addisu) Handle removal of nested models + // TODO(addisu) Handle removal of nested models const auto &world = this->worlds.at(_worldID); auto skel = this->models.at(_modelID)->model; // Remove the contents of the skeleton from local entity storage containers diff --git a/dartsim/src/SDFFeatures_TEST.cc b/dartsim/src/SDFFeatures_TEST.cc index 311245452..67231c570 100644 --- a/dartsim/src/SDFFeatures_TEST.cc +++ b/dartsim/src/SDFFeatures_TEST.cc @@ -113,7 +113,8 @@ WorldPtr LoadWorldWhole(const std::string &_world) } ///////////////////////////////////////////////// -static ignition::math::Pose3d ResolveSdfPose(const ::sdf::SemanticPose &_semPose) +static ignition::math::Pose3d ResolveSdfPose( + const ::sdf::SemanticPose &_semPose) { ignition::math::Pose3d pose; ::sdf::Errors errors = _semPose.Resolve(pose); From baed7015ef77bf1c831fc6db07b2d71392421677 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 16 Mar 2021 18:25:14 -0500 Subject: [PATCH 05/17] Remove unnecessary functions Signed-off-by: Addisu Z. Taddese --- dartsim/src/Base.hh | 64 --------------------------------------------- 1 file changed, 64 deletions(-) diff --git a/dartsim/src/Base.hh b/dartsim/src/Base.hh index 30c62646c..b26b91fc9 100644 --- a/dartsim/src/Base.hh +++ b/dartsim/src/Base.hh @@ -351,70 +351,6 @@ class Base : public Implements3d> world->removeSkeleton(skel); } - static bool SkeletonNameEndsWithModelName( - const std::string &_skeletonName, const std::string &_modelName) - { - if (_modelName.empty()) - return true; - - if (_skeletonName.size() < _modelName.size()) - return false; - - // If _modelName comes from a nested model, it might actually be shorter - // than _skeletonName. E.g -> _skeletonName = a::b::c, _modelName = b::c - const size_t compareStartPosition = - _skeletonName.size() - _modelName.size(); - return _skeletonName.compare( - compareStartPosition, _modelName.size(), _modelName) == 0; - } - - /// \brief Finds the skeleton in the world that matches the qualified name - /// If more than one match exists, it will return null - /// \param[in] _worldID Which world to search. - /// \param[in] _name Name of entity. - /// \return Pointer to skeleton if exactly one match was found, otherwise null - public: DartSkeletonPtr FindContainingSkeletonFromName( - const dart::simulation::WorldPtr &_world, const std::string &_name) - { - if (_world == nullptr) - return nullptr; - - if (_name == "world") - return nullptr; - - const auto[skeletonName, entityName] = ::sdf::SplitName(_name); - std::vector matches; - for (size_t i = 0; i < _world->getNumSkeletons(); ++i) - { - auto candidateSkeleton = _world->getSkeleton(i); - if (SkeletonNameEndsWithModelName( - candidateSkeleton->getName(), skeletonName)) - { - // There may be multiple skeletons that match the model name, only match - // those that contain a matching entity - auto * node = candidateSkeleton->getBodyNode(entityName); - if (nullptr != node) - matches.push_back(candidateSkeleton); - } - } - - // It's possible there was more than 1 match - // (e.g. b::c matches both a::b::c and d::b::c) - if (matches.size() == 1) - return matches.front(); - else if (matches.size() > 1) - { - ignerr << "Found " << matches.size() << "skeletons that contain " << _name - << ":" << std::endl; - for (const auto &match : matches) - { - ignerr << match->getName() << "\n"; - } - return nullptr; - } - return nullptr; - } - public: EntityStorage worlds; public: EntityStorage models; public: EntityStorage links; From 372cd27b8267d2b31612b1984c27a24bfa242a34 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 16 Mar 2021 18:15:52 -0500 Subject: [PATCH 06/17] Ensure Link and Model APIs continue to work after joint creation We do this by keeping track of links separately from DART so that APIs such as `Model::GetLink()` and `Link::GetIndex` are not affected by BodyNode's moving from one skeleton to another when a joint is created between different (nested) models. Signed-off-by: Addisu Z. Taddese --- dartsim/src/Base.hh | 43 +++++++++++------ dartsim/src/Base_TEST.cc | 2 +- dartsim/src/EntityManagementFeatures.cc | 64 +++++++++++++------------ dartsim/src/JointFeatures_TEST.cc | 8 ++-- dartsim/src/SDFFeatures.cc | 4 +- dartsim/src/SDFFeatures_TEST.cc | 37 ++++++++++---- 6 files changed, 98 insertions(+), 60 deletions(-) diff --git a/dartsim/src/Base.hh b/dartsim/src/Base.hh index b26b91fc9..192e6b9fd 100644 --- a/dartsim/src/Base.hh +++ b/dartsim/src/Base.hh @@ -49,12 +49,6 @@ namespace dartsim { /// create a std::shared_ptr of the struct that wraps the corresponding DART /// shared pointer. -struct ModelInfo -{ - dart::dynamics::SkeletonPtr model; - dart::dynamics::SimpleFramePtr frame; - std::string canonicalLinkName; -}; struct LinkInfo { @@ -65,6 +59,14 @@ struct LinkInfo std::string name; }; +struct ModelInfo +{ + dart::dynamics::SkeletonPtr model; + dart::dynamics::SimpleFramePtr frame; + std::string canonicalLinkName; + std::vector> links {}; +}; + struct JointInfo { dart::dynamics::JointPtr joint; @@ -100,14 +102,15 @@ struct EntityStorage /// \brief The key represents the parent ID. The value represents a vector of /// the objects' IDs. The key of the vector is the object's index within its /// container. This is used by World and Model objects, which don't know their - /// own indices within their containers. + /// own indices within their containers as well as Links, whose indices might + /// change when constructing joints. /// /// The container type for World is Engine. /// The container type for Model is World. + /// The container type for Link is Model. /// - /// Links and Joints are contained in Models, but Links and Joints know their - /// own indices within their Models, so we do not need to use this field for - /// either of those types. + /// Joints are contained in Models, but they know their own indices within + /// their Models, so we do not need to use this field for Joints IndexMap indexInContainerToID; /// \brief Map from an entity ID to its index within its container @@ -289,18 +292,30 @@ class Base : public Implements3d> } public: inline std::size_t AddLink(DartBodyNode *_bn, - const std::string &_fullName) + const std::string &_fullName, std::size_t _modelID) { const std::size_t id = this->GetNextEntity(); - this->links.idToObject[id] = std::make_shared(); - this->links.idToObject[id]->link = _bn; + auto linkInfo = std::make_shared(); + this->links.idToObject[id] = linkInfo; + linkInfo->link = _bn; // The name of the BodyNode during creation is assumed to be the // Gazebo-specified name. - this->links.idToObject[id]->name = _bn->getName(); + linkInfo->name = _bn->getName(); this->links.objectToID[_bn] = id; this->frames[id] = _bn; this->linksByName[_fullName] = _bn; + this->models.at(_modelID)->links.push_back(linkInfo); + + // Even though DART keeps track of the index of this BodyNode in the + // skeleton, the BodyNode may be moved to another skeleton when a joint is + // constructed. Thus, we store the original index here. + this->links.idToIndexInContainer[id] = _bn->getIndexInSkeleton(); + std::vector &indexInContainerToID = + this->links.indexInContainerToID[_modelID]; + indexInContainerToID.push_back(id); + + this->links.idToContainerID[id] = _modelID; return id; } diff --git a/dartsim/src/Base_TEST.cc b/dartsim/src/Base_TEST.cc index 7dd9f3768..63242474b 100644 --- a/dartsim/src/Base_TEST.cc +++ b/dartsim/src/Base_TEST.cc @@ -68,7 +68,7 @@ TEST(BaseClass, RemoveModel) const std::string fullName = ::sdf::JoinName( world->getName(), ::sdf::JoinName(skel->getName(), pair.second->getName())); - base.AddLink(pair.second, fullName); + base.AddLink(pair.second, fullName, std::get<0>(res)); base.AddJoint(pair.first); base.AddShape({sn, name + "_shape"}); diff --git a/dartsim/src/EntityManagementFeatures.cc b/dartsim/src/EntityManagementFeatures.cc index da9f3f012..3dc2bfc59 100644 --- a/dartsim/src/EntityManagementFeatures.cc +++ b/dartsim/src/EntityManagementFeatures.cc @@ -272,22 +272,25 @@ std::size_t EntityManagementFeatures::GetLinkCount( const Identity &_modelID) const { return this->ReferenceInterface(_modelID) - ->model->getNumBodyNodes(); + ->links.size(); } ///////////////////////////////////////////////// Identity EntityManagementFeatures::GetLink( const Identity &_modelID, const std::size_t _linkIndex) const { - DartBodyNode *const bn = - this->ReferenceInterface(_modelID)->model->getBodyNode( - _linkIndex); + auto modelInfo = this->ReferenceInterface(_modelID); + + if (_linkIndex >= modelInfo->links.size()) + return this->GenerateInvalidId(); + + auto linkInfo = modelInfo->links[_linkIndex]; // If the link doesn't exist in "links", it means the containing entity has // been removed. - if (this->links.HasEntity(bn)) + if (this->links.HasEntity(linkInfo->link)) { - const std::size_t linkID = this->links.IdentityOf(bn); + const std::size_t linkID = this->links.IdentityOf(linkInfo->link); return this->GenerateIdentity(linkID, this->links.at(linkID)); } else @@ -304,25 +307,29 @@ Identity EntityManagementFeatures::GetLink( Identity EntityManagementFeatures::GetLink( const Identity &_modelID, const std::string &_linkName) const { - DartBodyNode *const bn = - this->ReferenceInterface(_modelID)->model->getBodyNode( - _linkName); - - // If the link doesn't exist in "links", it means the containing entity has - // been removed. - if (this->links.HasEntity(bn)) - { - const std::size_t linkID = this->links.IdentityOf(bn); - return this->GenerateIdentity(linkID, this->links.at(linkID)); - } - else + auto modelInfo = this->ReferenceInterface(_modelID); + for (const auto &linkInfo : modelInfo->links) { - // TODO(addisu) It's not clear what to do when `GetLink` is called on a - // model that has been removed. Right now we are returning an invalid - // identity, but that could cause a segfault if the use doesn't check if - // returned value before using it. - return this->GenerateInvalidId(); + if (_linkName == linkInfo->name) + { + // If the link doesn't exist in "links", it means the containing entity + // has been removed. + if (this->links.HasEntity(linkInfo->link)) + { + const std::size_t linkID = this->links.IdentityOf(linkInfo->link); + return this->GenerateIdentity(linkID, this->links.at(linkID)); + } + else + { + // TODO(addisu) It's not clear what to do when `GetLink` is called on a + // model that has been removed. Right now we are returning an invalid + // identity, but that could cause a segfault if the use doesn't check if + // returned value before using it. + return this->GenerateInvalidId(); + } + } } + return this->GenerateInvalidId(); } ///////////////////////////////////////////////// @@ -393,22 +400,19 @@ const std::string &EntityManagementFeatures::GetLinkName( std::size_t EntityManagementFeatures::GetLinkIndex( const Identity &_linkID) const { - return this->ReferenceInterface(_linkID) - ->link->getIndexInSkeleton(); + return this->links.idToIndexInContainer.at(_linkID); } ///////////////////////////////////////////////// Identity EntityManagementFeatures::GetModelOfLink( const Identity &_linkID) const { - const DartSkeletonPtr &model = - this->ReferenceInterface(_linkID)->link->getSkeleton(); + const std::size_t modelID = this->links.idToContainerID.at(_linkID); // If the model containing the link doesn't exist in "models", it means this // link belongs to a removed model. - if (this->models.HasEntity(model)) + if (this->models.HasEntity(modelID)) { - const std::size_t modelID = this->models.IdentityOf(model); return this->GenerateIdentity(modelID, this->models.at(modelID)); } else @@ -667,7 +671,7 @@ Identity EntityManagementFeatures::ConstructEmptyLink( world->getName(), ::sdf::JoinName(model->getName(), bn->getName())); - const std::size_t linkID = this->AddLink(bn, fullName); + const std::size_t linkID = this->AddLink(bn, fullName, _modelID); return this->GenerateIdentity(linkID, this->links.at(linkID)); } diff --git a/dartsim/src/JointFeatures_TEST.cc b/dartsim/src/JointFeatures_TEST.cc index ef77cecb3..7c354de5e 100644 --- a/dartsim/src/JointFeatures_TEST.cc +++ b/dartsim/src/JointFeatures_TEST.cc @@ -433,10 +433,8 @@ TEST_F(JointFeaturesFixture, LinkCountsInJointAttachDetach) // After attaching we expect each model to have 1 link, but the current // behavior is that there are 2 links in model1 and 0 in model2 - // EXPECT_EQ(1u, model1->GetLinkCount()); - // EXPECT_EQ(1u, model2->GetLinkCount()); - EXPECT_EQ(2u, model1->GetLinkCount()); - EXPECT_EQ(0u, model2->GetLinkCount()); + EXPECT_EQ(1u, model1->GetLinkCount()); + EXPECT_EQ(1u, model2->GetLinkCount()); // now detach joint and expect model2 to start moving again fixedJoint->Detach(); @@ -451,7 +449,7 @@ TEST_F(JointFeaturesFixture, LinkCountsInJointAttachDetach) auto model3Body = model3->GetLink(bodyName); auto fixedJoint2 = model3Body->AttachFixedJoint(model2Body); - EXPECT_EQ(2u, model2->GetLinkCount()); + EXPECT_EQ(1u, model2->GetLinkCount()); fixedJoint2->Detach(); // After detaching we expect each model to have 1 link EXPECT_EQ(1u, model2->GetLinkCount()); diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index a42b4fa4a..109a8c849 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -534,7 +534,7 @@ Identity SDFFeatures::ConstructSdfLink( const Identity &_modelID, const ::sdf::Link &_sdfLink) { - const auto &modelInfo = *this->ReferenceInterface(_modelID); + auto &modelInfo = *this->ReferenceInterface(_modelID); dart::dynamics::BodyNode::Properties bodyProperties; bodyProperties.mName = _sdfLink.Name(); @@ -590,7 +590,7 @@ Identity SDFFeatures::ConstructSdfLink( const std::string fullName = ::sdf::JoinName( world->getName(), ::sdf::JoinName(modelInfo.model->getName(), bn->getName())); - const std::size_t linkID = this->AddLink(bn, fullName); + const std::size_t linkID = this->AddLink(bn, fullName, _modelID); this->AddJoint(joint); auto linkIdentity = this->GenerateIdentity(linkID, this->links.at(linkID)); diff --git a/dartsim/src/SDFFeatures_TEST.cc b/dartsim/src/SDFFeatures_TEST.cc index 67231c570..ed0827866 100644 --- a/dartsim/src/SDFFeatures_TEST.cc +++ b/dartsim/src/SDFFeatures_TEST.cc @@ -544,16 +544,37 @@ TEST_P(SDFFeatures_TEST, WorldWithNestedModel) auto nestedJoint = parentModel->GetJoint("nested_joint"); EXPECT_NE(nullptr, nestedJoint); - EXPECT_EQ(3u, parentModel->GetLinkCount()); - EXPECT_EQ(0u, nestedModel->GetLinkCount()); - auto nestedLink1 = parentModel->GetLink("nested_link1"); - EXPECT_NE(nullptr, nestedLink1); + EXPECT_EQ(1u, parentModel->GetLinkCount()); + EXPECT_NE(nullptr, parentModel->GetLink("link1")); + EXPECT_EQ(nullptr, parentModel->GetLink("nested_link1")); + EXPECT_EQ(nullptr, parentModel->GetLink("nested_link2")); - auto nestedLink2 = parentModel->GetLink("nested_link2"); - EXPECT_NE(nullptr, nestedLink2); + ASSERT_EQ(2u, nestedModel->GetLinkCount()); + auto nestedLink1 = nestedModel->GetLink("nested_link1"); + ASSERT_NE(nullptr, nestedLink1); + EXPECT_EQ(0u, nestedLink1->GetIndex()); + EXPECT_EQ(nestedLink1, nestedModel->GetLink(0)); - auto link1 = parentModel->GetLink("link1"); - EXPECT_NE(nullptr, link1); + auto nestedLink2 = nestedModel->GetLink("nested_link2"); + ASSERT_NE(nullptr, nestedLink2); + EXPECT_EQ(1u, nestedLink2->GetIndex()); + EXPECT_EQ(nestedLink2, nestedModel->GetLink(1)); + + auto nestedModelSkel = dartWorld->getSkeleton("parent_model::nested_model"); + ASSERT_NE(nullptr, nestedModelSkel); + // nested_model::nested_link1 would have moved to the parent_model skeleton so + // we expect to not find it in the nested_model skeleton + EXPECT_EQ(nullptr, nestedModelSkel->getBodyNode("nested_link1")); + + auto nestedModel2 = world->GetModel("parent_model::nested_model2"); + ASSERT_NE(nullptr, nestedModel2); + EXPECT_EQ(1u, nestedModel2->GetLinkCount()); + EXPECT_NE(nullptr, nestedModel2->GetLink("nested_link1")); + + auto nestedModel3 = world->GetModel("parent_model::nested_model2"); + ASSERT_NE(nullptr, nestedModel3); + EXPECT_EQ(1u, nestedModel3->GetLinkCount()); + EXPECT_NE(nullptr, nestedModel3->GetLink("nested_link1")); } ///////////////////////////////////////////////// From 353410cfa75390a176989e04fd6f0effab63224a Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 16 Mar 2021 23:17:59 -0500 Subject: [PATCH 07/17] Add empty nested model construction and nested model entity management Signed-off-by: Addisu Z. Taddese --- dartsim/src/Base.hh | 11 +++ dartsim/src/Base_TEST.cc | 2 +- dartsim/src/EntityManagementFeatures.cc | 86 ++++++++++++++++++- dartsim/src/EntityManagementFeatures.hh | 13 +++ dartsim/src/EntityManagement_TEST.cc | 6 ++ dartsim/src/SDFFeatures.cc | 20 ++++- dartsim/src/SDFFeatures_TEST.cc | 8 +- include/ignition/physics/ConstructEmpty.hh | 24 ++++++ include/ignition/physics/GetEntities.hh | 51 +++++++++++ .../ignition/physics/detail/ConstructEmpty.hh | 10 +++ .../ignition/physics/detail/GetEntities.hh | 52 +++++++++++ 11 files changed, 272 insertions(+), 11 deletions(-) diff --git a/dartsim/src/Base.hh b/dartsim/src/Base.hh index 192e6b9fd..b8f2feb70 100644 --- a/dartsim/src/Base.hh +++ b/dartsim/src/Base.hh @@ -62,9 +62,11 @@ struct LinkInfo struct ModelInfo { dart::dynamics::SkeletonPtr model; + std::string localName; dart::dynamics::SimpleFramePtr frame; std::string canonicalLinkName; std::vector> links {}; + std::vector nestedModels = {}; }; struct JointInfo @@ -291,6 +293,15 @@ class Base : public Implements3d> return std::forward_as_tuple(id, entry); } + public: inline std::tuple AddNestedModel( + const ModelInfo &_info, const std::size_t _parentID, + const std::size_t _worldID) + { + auto [id, entry] = this->AddModel(_info, _worldID); + this->models.at(_parentID)->nestedModels.push_back(id); + return {id, entry}; + } + public: inline std::size_t AddLink(DartBodyNode *_bn, const std::string &_fullName, std::size_t _modelID) { diff --git a/dartsim/src/Base_TEST.cc b/dartsim/src/Base_TEST.cc index 63242474b..aa2dec7bd 100644 --- a/dartsim/src/Base_TEST.cc +++ b/dartsim/src/Base_TEST.cc @@ -64,7 +64,7 @@ TEST(BaseClass, RemoveModel) auto sn = pair.second->createShapeNodeWith( boxShape); - auto res = base.AddModel({skel, frame, ""}, worldID); + auto res = base.AddModel({skel, name, frame, ""}, worldID); const std::string fullName = ::sdf::JoinName( world->getName(), ::sdf::JoinName(skel->getName(), pair.second->getName())); diff --git a/dartsim/src/EntityManagementFeatures.cc b/dartsim/src/EntityManagementFeatures.cc index 3dc2bfc59..a7ca71331 100644 --- a/dartsim/src/EntityManagementFeatures.cc +++ b/dartsim/src/EntityManagementFeatures.cc @@ -238,7 +238,7 @@ Identity EntityManagementFeatures::GetModel( const std::string &EntityManagementFeatures::GetModelName( const Identity &_modelID) const { - return this->ReferenceInterface(_modelID)->model->getName(); + return this->ReferenceInterface(_modelID)->localName; } ///////////////////////////////////////////////// @@ -267,6 +267,64 @@ Identity EntityManagementFeatures::GetWorldOfModel( } } +///////////////////////////////////////////////// +std::size_t EntityManagementFeatures::GetNestedModelCount( + const Identity &_modelID) const +{ + return this->ReferenceInterface(_modelID)->nestedModels.size(); +} + +///////////////////////////////////////////////// +Identity EntityManagementFeatures::GetNestedModel( + const Identity &_modelID, const std::size_t _modelIndex) const +{ + auto modelInfo = this->ReferenceInterface(_modelID); + if (_modelIndex >= modelInfo->nestedModels.size()) + { + return this->GenerateInvalidId(); + } + + const auto nestedModelID = modelInfo->nestedModels[_modelIndex]; + + // If the model doesn't exist in "models", it means the containing entity has + // been removed. + if (this->models.HasEntity(nestedModelID)) + { + return this->GenerateIdentity(nestedModelID, + this->models.at(nestedModelID)); + } + else + { + return this->GenerateInvalidId(); + } +} + +///////////////////////////////////////////////// +Identity EntityManagementFeatures::GetNestedModel( + const Identity &_modelID, const std::string &_modelName) const +{ + auto modelInfo = this->ReferenceInterface(_modelID); + + const std::string fullName = + ::sdf::JoinName(modelInfo->model->getName(), _modelName); + + if (this->models.HasEntity(_modelID)) + { + auto worldID = this->models.idToContainerID.at(_modelID); + auto nestedSkel = this->worlds.at(worldID)->getSkeleton(fullName); + if (nullptr == nestedSkel) + { + return this->GenerateInvalidId(); + } + const std::size_t nestedModelID = this->models.IdentityOf(nestedSkel); + return this->GenerateIdentity(nestedModelID, + this->models.at(nestedModelID)); + } + else + { + return this->GenerateInvalidId(); + } +} ///////////////////////////////////////////////// std::size_t EntityManagementFeatures::GetLinkCount( const Identity &_modelID) const @@ -636,7 +694,31 @@ Identity EntityManagementFeatures::ConstructEmptyModel( dart::dynamics::Frame::World(), _name + "_frame"); - auto [modelID, modelInfo] = this->AddModel({model, modelFrame, ""}, _worldID); // NOLINT + auto [modelID, modelInfo] = + this->AddModel({model, _name, modelFrame, ""}, _worldID); // NOLINT + + return this->GenerateIdentity(modelID, this->models.at(modelID)); +} + +///////////////////////////////////////////////// +Identity EntityManagementFeatures::ConstructEmptyNestedModel( + const Identity &_modelID, const std::string &_name) +{ + // find the world assocated with the model + auto worldID = this->models.idToContainerID.at(_modelID); + const auto &skel = this->models.at(_modelID)->model; + const std::string modelFullName = ::sdf::JoinName(skel->getName(), _name); + + dart::dynamics::SkeletonPtr model = + dart::dynamics::Skeleton::create(modelFullName); + + dart::dynamics::SimpleFramePtr modelFrame = + dart::dynamics::SimpleFrame::createShared( + dart::dynamics::Frame::World(), + modelFullName + "_frame"); + + auto [modelID, modelInfo] = + this->AddModel({model, _name, modelFrame, ""}, worldID); // NOLINT return this->GenerateIdentity(modelID, this->models.at(modelID)); } diff --git a/dartsim/src/EntityManagementFeatures.hh b/dartsim/src/EntityManagementFeatures.hh index 58b2ae9c8..370f334c7 100644 --- a/dartsim/src/EntityManagementFeatures.hh +++ b/dartsim/src/EntityManagementFeatures.hh @@ -37,6 +37,7 @@ struct EntityManagementFeatureList : FeatureList< RemoveEntities, ConstructEmptyWorldFeature, ConstructEmptyModelFeature, + ConstructEmptyNestedModelFeature, ConstructEmptyLinkFeature, CollisionFilterMaskFeature > { }; @@ -81,6 +82,15 @@ class EntityManagementFeatures : public: Identity GetWorldOfModel(const Identity &_modelID) const override; + 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; + public: std::size_t GetLinkCount(const Identity &_modelID) const override; public: Identity GetLink( @@ -144,6 +154,9 @@ class EntityManagementFeatures : public: Identity ConstructEmptyModel( const Identity &_worldID, const std::string &_name) override; + public: Identity ConstructEmptyNestedModel( + const Identity &_modelID, const std::string &_name) override; + public: Identity ConstructEmptyLink( const Identity &_modelID, const std::string &_name) override; diff --git a/dartsim/src/EntityManagement_TEST.cc b/dartsim/src/EntityManagement_TEST.cc index 3fa84a53c..196e50661 100644 --- a/dartsim/src/EntityManagement_TEST.cc +++ b/dartsim/src/EntityManagement_TEST.cc @@ -62,6 +62,12 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) EXPECT_EQ(world, model->GetWorld()); EXPECT_NE(model, world->ConstructEmptyModel("dummy")); + auto nestedModel = model->ConstructEmptyModel("empty nested model"); + ASSERT_NE(nullptr, nestedModel); + EXPECT_EQ("empty nested model", nestedModel->GetName()); + EXPECT_EQ(world, nestedModel->GetWorld()); + EXPECT_NE(nestedModel, nestedModel->ConstructEmptyModel("dummy")); + auto link = model->ConstructEmptyLink("empty link"); ASSERT_NE(nullptr, link); EXPECT_EQ("empty link", link->GetName()); diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index 109a8c849..1bc7b13ec 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -398,8 +398,9 @@ Identity SDFFeatures::ConstructSdfModelImpl( { auto worldID = _parentID; std::string modelName = _sdfModel.Name(); + const bool isNested = this->models.HasEntity(_parentID); // If this is a nested model, find the world assocated with the model - if (this->models.HasEntity(_parentID)) + if (isNested) { worldID = this->models.idToContainerID.at(_parentID); const auto &skel = this->models.at(_parentID)->model; @@ -420,9 +421,20 @@ Identity SDFFeatures::ConstructSdfModelImpl( // TODO(anyone) This may not work correctly with nested models and will need // to be updated once multiple canonical links can exist in a nested model // https://github.com/ignitionrobotics/ign-physics/issues/209 - auto [modelID, modelInfo] = this->AddModel( // NOLINT - {model, modelFrame, _sdfModel.CanonicalLinkName()}, worldID); - + auto [modelID, modelInfo] = [&] { + if (isNested) + { + return this->AddNestedModel( // NOLINT + {model, _sdfModel.Name(), modelFrame, _sdfModel.CanonicalLinkName()}, + _parentID, worldID); + } + else + { + return this->AddModel( // NOLINT + {model, _sdfModel.Name(), modelFrame, _sdfModel.CanonicalLinkName()}, + _parentID); + } + }(); model->setMobile(!_sdfModel.Static()); model->setSelfCollisionCheck(_sdfModel.SelfCollide()); diff --git a/dartsim/src/SDFFeatures_TEST.cc b/dartsim/src/SDFFeatures_TEST.cc index ed0827866..8fe659025 100644 --- a/dartsim/src/SDFFeatures_TEST.cc +++ b/dartsim/src/SDFFeatures_TEST.cc @@ -531,14 +531,14 @@ TEST_P(SDFFeatures_TEST, WorldWithNestedModel) // check top level model EXPECT_EQ("parent_model", world->GetModel(0)->GetName()); - EXPECT_EQ("parent_model::nested_model", world->GetModel(1)->GetName()); + EXPECT_EQ("nested_model", world->GetModel(1)->GetName()); auto parentModel = world->GetModel("parent_model"); ASSERT_NE(nullptr, parentModel); auto joint1 = parentModel->GetJoint("joint1"); EXPECT_NE(nullptr, joint1); - auto nestedModel = world->GetModel("parent_model::nested_model"); + auto nestedModel = parentModel->GetModel("nested_model"); ASSERT_NE(nullptr, nestedModel); auto nestedJoint = parentModel->GetJoint("nested_joint"); @@ -603,9 +603,9 @@ TEST_P(SDFFeatures_TEST, WorldWithNestedModelJointToWorld) auto link1 = parentModel->GetLink("link1"); EXPECT_NE(nullptr, link1); - auto nestedModel = world->GetModel("parent_model::nested_model"); + auto nestedModel = parentModel->GetModel("nested_model"); ASSERT_NE(nullptr, nestedModel); - EXPECT_EQ("parent_model::nested_model", nestedModel->GetName()); + EXPECT_EQ("nested_model", nestedModel->GetName()); EXPECT_EQ(2u, nestedModel->GetLinkCount()); EXPECT_EQ(2u, nestedModel->GetJointCount()); diff --git a/include/ignition/physics/ConstructEmpty.hh b/include/ignition/physics/ConstructEmpty.hh index 8863fe581..342c03b62 100644 --- a/include/ignition/physics/ConstructEmpty.hh +++ b/include/ignition/physics/ConstructEmpty.hh @@ -73,6 +73,30 @@ class ConstructEmptyModelFeature : public virtual Feature }; }; +///////////////////////////////////////////////// +/// \brief This feature constructs an empty nested model and return its pointer +/// from the given world. +class ConstructEmptyNestedModelFeature : public virtual Feature +{ + public: template + class Model : public virtual Feature::Model + { + public: using ModelPtrType = ModelPtr; + + /// \brief Construct an empty model and attach a given name to it. + /// \return + /// The ModelPtrType of the constructed model. + public: ModelPtrType ConstructEmptyModel(const std::string &_name); + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: virtual Identity ConstructEmptyNestedModel( + const Identity &_modelID, const std::string &_name) = 0; + }; +}; + ///////////////////////////////////////////////// /// \brief This feature constructs an empty link and return its pointer /// from the given model. diff --git a/include/ignition/physics/GetEntities.hh b/include/ignition/physics/GetEntities.hh index 98c04beda..92d60d12a 100644 --- a/include/ignition/physics/GetEntities.hh +++ b/include/ignition/physics/GetEntities.hh @@ -213,6 +213,56 @@ namespace ignition }; }; + ///////////////////////////////////////////////// + /// \brief This feature retrieves the nested model pointer from the parent + /// model by specifying the model index and model index/name. + class IGNITION_PHYSICS_VISIBLE GetNestedModelFromModel + : public virtual FeatureWithRequirements + { + public: template + class Model : public virtual Feature::Model + { + // typedefs for the type of Model that this Model can return + public: using ModelPtrType = ModelPtr; + public: using ConstModelPtrType = ConstModelPtr; + + /// \brief Get the number of Models inside this Model. + public: std::size_t GetModelCount() const; + + /// \brief Get a Model that exists within this Model. + /// \param[in] _index + /// Index of the model within this model. + /// \return A model reference. If _index is GetModelCount() or higher, + /// this will be a nullptr. + public: ModelPtrType GetModel(std::size_t _index); + + /// \sa GetModel(std::size_t) + public: ConstModelPtrType GetModel(std::size_t _index) const; + + /// \brief Get a Model that exists within this Model. + /// \param[in] _name + /// Name of the model within this model. + /// \return A model reference. If a model named _name does not exist in + /// this model, this will be a nullptr. + public: ModelPtrType GetModel(const std::string &_name); + + /// \sa GetModel(const std::string &) + public: ConstModelPtrType GetModel(const std::string &_name) const; + }; + + public: template + class Implementation : public virtual Feature::Implementation + { + public: virtual std::size_t GetNestedModelCount( + const Identity &_modelID) const = 0; + + public: virtual Identity GetNestedModel( + const Identity &_modelID, std::size_t _modelIndex) const = 0; + + public: virtual Identity GetNestedModel( + const Identity &_modelID, const std::string &_modelName) const = 0; + }; + }; ///////////////////////////////////////////////// /// \brief This feature retrieves the link pointer from the model /// by specifying model index and link index/name. @@ -457,6 +507,7 @@ namespace ignition GetEngineInfo, GetWorldFromEngine, GetModelFromWorld, + GetNestedModelFromModel, GetLinkFromModel, GetJointFromModel, GetShapeFromLink diff --git a/include/ignition/physics/detail/ConstructEmpty.hh b/include/ignition/physics/detail/ConstructEmpty.hh index e226af640..fed87cb53 100644 --- a/include/ignition/physics/detail/ConstructEmpty.hh +++ b/include/ignition/physics/detail/ConstructEmpty.hh @@ -45,6 +45,16 @@ auto ConstructEmptyModelFeature::World ->ConstructEmptyModel(this->identity, _name)); } +///////////////////////////////////////////////// +template +auto ConstructEmptyNestedModelFeature::Model +::ConstructEmptyModel(const std::string &_name) -> ModelPtrType +{ + return ModelPtrType(this->pimpl, + this->template Interface() + ->ConstructEmptyNestedModel(this->identity, _name)); +} + ///////////////////////////////////////////////// template auto ConstructEmptyLinkFeature::Model diff --git a/include/ignition/physics/detail/GetEntities.hh b/include/ignition/physics/detail/GetEntities.hh index 402c132d2..fed54ccb9 100644 --- a/include/ignition/physics/detail/GetEntities.hh +++ b/include/ignition/physics/detail/GetEntities.hh @@ -214,6 +214,58 @@ namespace ignition ->GetWorldOfModel(this->identity)); } + ///////////////////////////////////////////////// + template + std::size_t + GetNestedModelFromModel::Model::GetModelCount() const + { + return this->template Interface() + ->GetNestedModelCount(this->identity); + } + + ///////////////////////////////////////////////// + template + auto GetNestedModelFromModel::Model::GetModel( + const std::size_t _index) -> ModelPtrType + { + return ModelPtrType( + this->pimpl, + this->template Interface()->GetNestedModel( + this->identity, _index)); + } + + ///////////////////////////////////////////////// + template + auto GetNestedModelFromModel::Model::GetModel( + const std::size_t _index) const -> ConstModelPtrType + { + return ConstModelPtrType( + this->pimpl, + this->template Interface()->GetNestedModel( + this->identity, _index)); + } + + ///////////////////////////////////////////////// + template + auto GetNestedModelFromModel::Model::GetModel( + const std::string &_name) -> ModelPtrType + { + return ModelPtrType( + this->pimpl, + this->template Interface()->GetNestedModel( + this->identity, _name)); + } + + ///////////////////////////////////////////////// + template + auto GetNestedModelFromModel::Model::GetModel( + const std::string &_name) const -> ConstModelPtrType + { + return ConstModelPtrType(this->pimpl, + this->template Interface()->GetNestedModel( + this->identity, _name)); + } + ///////////////////////////////////////////////// template std::size_t GetLinkFromModel::Model::GetLinkCount() From 96f46c847b6b575148ae979a6cfacf22a19001c7 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 16 Mar 2021 23:32:02 -0500 Subject: [PATCH 08/17] Add empty nested model construction and nested model entity management in TPE Signed-off-by: Addisu Z. Taddese --- tpe/lib/src/Model.cc | 23 ++++++++- tpe/lib/src/Model.hh | 8 +++ tpe/plugin/src/EntityManagementFeatures.cc | 60 +++++++++++++++++++++- tpe/plugin/src/EntityManagementFeatures.hh | 14 +++++ tpe/plugin/src/EntityManagement_TEST.cc | 11 ++++ 5 files changed, 114 insertions(+), 2 deletions(-) diff --git a/tpe/lib/src/Model.cc b/tpe/lib/src/Model.cc index eaec716b8..8eb5899b2 100644 --- a/tpe/lib/src/Model.cc +++ b/tpe/lib/src/Model.cc @@ -16,6 +16,7 @@ */ #include +#include #include #include @@ -64,7 +65,7 @@ Entity &Model::AddLink() { std::size_t linkId = Entity::GetNextId(); - if (this->GetChildren().empty()) + if (this->GetLinkCount() == 0u) this->dataPtr->firstLinkId = linkId; const auto[it, success] = this->GetChildren().insert( @@ -75,6 +76,16 @@ Entity &Model::AddLink() return *it->second.get(); } +////////////////////////////////////////////////// +std::size_t Model::GetLinkCount() const +{ + const auto &children = this->GetChildren(); + return std::count_if(children.begin(), children.end(), [](auto _child) + { + return std::dynamic_pointer_cast(_child.second); + }); +} + ////////////////////////////////////////////////// Entity &Model::AddModel() { @@ -87,6 +98,16 @@ Entity &Model::AddModel() return *it->second.get(); } +////////////////////////////////////////////////// +std::size_t Model::GetModelCount() const +{ + const auto &children = this->GetChildren(); + return std::count_if(children.begin(), children.end(), [](auto _child) + { + return std::dynamic_pointer_cast(_child.second); + }); +} + ////////////////////////////////////////////////// void Model::SetCanonicalLink(std::size_t linkId) { diff --git a/tpe/lib/src/Model.hh b/tpe/lib/src/Model.hh index 72d9fb852..3639b58dc 100644 --- a/tpe/lib/src/Model.hh +++ b/tpe/lib/src/Model.hh @@ -48,10 +48,18 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Model : public Entity /// \return Newly created Link public: Entity &AddLink(); + /// \brief Get the number of links in the model + /// \return Number of links in the model + public: std::size_t GetLinkCount() const; + /// \brief Add a nested model /// \return Newly created nested model public: Entity &AddModel(); + /// \brief Get the number of nested models in the model + /// \return Number of nested mdoels in the model + public: std::size_t GetModelCount() const; + /// \brief Set the canonical link of model public: void SetCanonicalLink( std::size_t linkId = kNullEntityId); diff --git a/tpe/plugin/src/EntityManagementFeatures.cc b/tpe/plugin/src/EntityManagementFeatures.cc index 60ac6e77c..2d377c679 100644 --- a/tpe/plugin/src/EntityManagementFeatures.cc +++ b/tpe/plugin/src/EntityManagementFeatures.cc @@ -172,11 +172,54 @@ Identity EntityManagementFeatures::GetWorldOfModel( return this->GenerateInvalidId(); } +///////////////////////////////////////////////// +std::size_t EntityManagementFeatures::GetNestedModelCount( + const Identity &_modelID) const +{ + return this->ReferenceInterface(_modelID)->model->GetModelCount(); +} + +///////////////////////////////////////////////// +Identity EntityManagementFeatures::GetNestedModel( + const Identity &_modelID, const std::size_t _modelIndex) const +{ + std::size_t modelId = this->indexInContainerToId(_modelID.id, _modelIndex); + auto it = this->models.find(modelId); + if (it != this->models.end() && it->second != nullptr) + { + return this->GenerateIdentity(modelId, it->second); + } + return this->GenerateInvalidId(); +} + +///////////////////////////////////////////////// +Identity EntityManagementFeatures::GetNestedModel( + const Identity &_modelID, const std::string &_modelName) const +{ + auto modelInfo = this->ReferenceInterface(_modelID); + if (modelInfo != nullptr) + { + tpelib::Entity &modelEnt = modelInfo->world->GetChildByName(_modelName); + for (const auto &[parentModelId, nestedModelInfo]: this->models) + { + if (nestedModelInfo != nullptr) + { + std::string name = nestedModelInfo->model->GetName(); + if (parentModelId == modelEnt.GetId() && name == modelEnt.GetName()) + { + return this->GenerateIdentity(parentModelId, nestedModelInfo); + } + } + } + } + return this->GenerateInvalidId(); +} + ///////////////////////////////////////////////// std::size_t EntityManagementFeatures::GetLinkCount( const Identity &_modelID) const { - return this->ReferenceInterface(_modelID)->model->GetChildCount(); + return this->ReferenceInterface(_modelID)->model->GetLinkCount(); } ///////////////////////////////////////////////// @@ -406,6 +449,21 @@ Identity EntityManagementFeatures::ConstructEmptyModel( return this->GenerateInvalidId(); } +///////////////////////////////////////////////// +Identity EntityManagementFeatures::ConstructEmptyNestedModel( + const Identity &_modelID, const std::string &_name) +{ + auto modelInfo = this->ReferenceInterface(_modelID); + if (modelInfo != nullptr) + { + auto &modelEnt = modelInfo->model->AddModel(); + modelEnt.SetName(_name); + tpelib::Model *model = static_cast(&modelEnt); + return this->AddModel(_modelID.id, *model); + } + return this->GenerateInvalidId(); +} + ///////////////////////////////////////////////// Identity EntityManagementFeatures::ConstructEmptyLink( const Identity &_modelID, const std::string &_name) diff --git a/tpe/plugin/src/EntityManagementFeatures.hh b/tpe/plugin/src/EntityManagementFeatures.hh index 8f28d50a3..3e5075349 100644 --- a/tpe/plugin/src/EntityManagementFeatures.hh +++ b/tpe/plugin/src/EntityManagementFeatures.hh @@ -36,11 +36,13 @@ struct EntityManagementFeatureList : FeatureList< GetEngineInfo, GetWorldFromEngine, GetModelFromWorld, + GetNestedModelFromModel, GetLinkFromModel, GetShapeFromLink, RemoveEntities, ConstructEmptyWorldFeature, ConstructEmptyModelFeature, + ConstructEmptyNestedModelFeature, ConstructEmptyLinkFeature, CollisionFilterMaskFeature > { }; @@ -87,6 +89,15 @@ class EntityManagementFeatures : public: Identity GetWorldOfModel(const Identity &_modelID) const override; + 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; + public: std::size_t GetLinkCount(const Identity &_modelID) const override; public: Identity GetLink( @@ -135,6 +146,9 @@ class EntityManagementFeatures : public: Identity ConstructEmptyModel( const Identity &_worldID, const std::string &_name) override; + public: Identity ConstructEmptyNestedModel( + const Identity &_modelID, const std::string &_name) override; + public: Identity ConstructEmptyLink( const Identity &_modelID, const std::string &_name) override; diff --git a/tpe/plugin/src/EntityManagement_TEST.cc b/tpe/plugin/src/EntityManagement_TEST.cc index bdc312eef..c4e864905 100644 --- a/tpe/plugin/src/EntityManagement_TEST.cc +++ b/tpe/plugin/src/EntityManagement_TEST.cc @@ -79,6 +79,17 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) EXPECT_EQ(model, link->GetModel()); EXPECT_EQ(link, model->GetLink(0)); EXPECT_EQ(link, model->GetLink("empty link")); + + EXPECT_EQ(0u, model->GetModelCount()); + auto nestedModel = model->ConstructEmptyModel("empty nested model"); + EXPECT_EQ(1u, model->GetModelCount()); + ASSERT_NE(nullptr, nestedModel); + EXPECT_EQ("empty nested model", nestedModel->GetName()); + EXPECT_EQ(0u, nestedModel->GetLinkCount()); + ASSERT_NE(nullptr, nestedModel->ConstructEmptyLink("empty link")); + EXPECT_EQ(1u, nestedModel->GetLinkCount()); + + EXPECT_EQ(2u, model->GetLinkCount()); } TEST(EntityManagement_TEST, RemoveEntities) From 0bb7d9e9d787d815f7fd3db15aa2da9a815c40b2 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 22 Mar 2021 10:31:12 -0500 Subject: [PATCH 09/17] Address reviewer feedback Signed-off-by: Addisu Z. Taddese --- dartsim/src/Base_TEST.cc | 11 ++++++++++- dartsim/src/EntityManagementFeatures.cc | 2 +- dartsim/src/EntityManagement_TEST.cc | 4 ++++ dartsim/src/JointFeatures_TEST.cc | 3 +-- dartsim/src/SDFFeatures.cc | 2 +- dartsim/src/SDFFeatures_TEST.cc | 4 ++-- 6 files changed, 19 insertions(+), 7 deletions(-) diff --git a/dartsim/src/Base_TEST.cc b/dartsim/src/Base_TEST.cc index 63242474b..4dbe68130 100644 --- a/dartsim/src/Base_TEST.cc +++ b/dartsim/src/Base_TEST.cc @@ -65,10 +65,19 @@ TEST(BaseClass, RemoveModel) boxShape); auto res = base.AddModel({skel, frame, ""}, worldID); + ASSERT_TRUE(base.models.HasEntity(std::get<0>(res))); + const auto &modelInfo = base.models.at(std::get<0>(res)); + EXPECT_EQ(skel, modelInfo->model); + const std::string fullName = ::sdf::JoinName( world->getName(), ::sdf::JoinName(skel->getName(), pair.second->getName())); - base.AddLink(pair.second, fullName, std::get<0>(res)); + auto linkID = base.AddLink(pair.second, fullName, std::get<0>(res)); + ASSERT_TRUE(base.links.HasEntity(linkID)); + const auto &linkInfo = base.links.at(linkID); + EXPECT_EQ(pair.second->getName(), linkInfo->name); + EXPECT_EQ(pair.second, linkInfo->link); + base.AddJoint(pair.first); base.AddShape({sn, name + "_shape"}); diff --git a/dartsim/src/EntityManagementFeatures.cc b/dartsim/src/EntityManagementFeatures.cc index 3dc2bfc59..fe8958f59 100644 --- a/dartsim/src/EntityManagementFeatures.cc +++ b/dartsim/src/EntityManagementFeatures.cc @@ -284,7 +284,7 @@ Identity EntityManagementFeatures::GetLink( if (_linkIndex >= modelInfo->links.size()) return this->GenerateInvalidId(); - auto linkInfo = modelInfo->links[_linkIndex]; + const auto &linkInfo = modelInfo->links[_linkIndex]; // If the link doesn't exist in "links", it means the containing entity has // been removed. diff --git a/dartsim/src/EntityManagement_TEST.cc b/dartsim/src/EntityManagement_TEST.cc index 3fa84a53c..b1cd6092b 100644 --- a/dartsim/src/EntityManagement_TEST.cc +++ b/dartsim/src/EntityManagement_TEST.cc @@ -67,6 +67,8 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) EXPECT_EQ("empty link", link->GetName()); EXPECT_EQ(model, link->GetModel()); EXPECT_NE(link, model->ConstructEmptyLink("dummy")); + EXPECT_EQ(0u, link->GetIndex()); + EXPECT_EQ(model, link->GetModel()); auto joint = link->AttachRevoluteJoint(nullptr); EXPECT_NEAR((Eigen::Vector3d::UnitX() - joint->GetAxis()).norm(), 0.0, 1e-6); @@ -76,6 +78,8 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) EXPECT_NEAR((Eigen::Vector3d::UnitZ() - joint->GetAxis()).norm(), 0.0, 1e-6); auto child = model->ConstructEmptyLink("child link"); + EXPECT_EQ(2u, child->GetIndex()); + EXPECT_EQ(model, child->GetModel()); const std::string boxName = "box"; const Eigen::Vector3d boxSize(0.1, 0.2, 0.3); diff --git a/dartsim/src/JointFeatures_TEST.cc b/dartsim/src/JointFeatures_TEST.cc index 7c354de5e..1732faba8 100644 --- a/dartsim/src/JointFeatures_TEST.cc +++ b/dartsim/src/JointFeatures_TEST.cc @@ -431,8 +431,7 @@ TEST_F(JointFeaturesFixture, LinkCountsInJointAttachDetach) auto fixedJoint = model2Body->AttachFixedJoint(model1Body); - // After attaching we expect each model to have 1 link, but the current - // behavior is that there are 2 links in model1 and 0 in model2 + // After attaching we expect each model to have 1 link EXPECT_EQ(1u, model1->GetLinkCount()); EXPECT_EQ(1u, model2->GetLinkCount()); diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index c62ae5163..c8aeb74b0 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -534,7 +534,7 @@ Identity SDFFeatures::ConstructSdfLink( const Identity &_modelID, const ::sdf::Link &_sdfLink) { - auto &modelInfo = *this->ReferenceInterface(_modelID); + const auto &modelInfo = *this->ReferenceInterface(_modelID); dart::dynamics::BodyNode::Properties bodyProperties; bodyProperties.mName = _sdfLink.Name(); diff --git a/dartsim/src/SDFFeatures_TEST.cc b/dartsim/src/SDFFeatures_TEST.cc index 131da1b16..ca031d18d 100644 --- a/dartsim/src/SDFFeatures_TEST.cc +++ b/dartsim/src/SDFFeatures_TEST.cc @@ -585,10 +585,10 @@ TEST_P(SDFFeatures_TEST, WorldWithNestedModel) EXPECT_EQ(1u, nestedModel2->GetLinkCount()); EXPECT_NE(nullptr, nestedModel2->GetLink("nested_link1")); - auto nestedModel3 = world->GetModel("parent_model::nested_model2"); + auto nestedModel3 = world->GetModel("parent_model::nested_model3"); ASSERT_NE(nullptr, nestedModel3); EXPECT_EQ(1u, nestedModel3->GetLinkCount()); - EXPECT_NE(nullptr, nestedModel3->GetLink("nested_link1")); + EXPECT_NE(nullptr, nestedModel3->GetLink("link1")); } ///////////////////////////////////////////////// From 16da5e14a35a9e3e437287a576e11f014425fe91 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 22 Mar 2021 10:52:11 -0500 Subject: [PATCH 10/17] Fix typo, add const ref Signed-off-by: Addisu Z. Taddese --- dartsim/src/EntityManagementFeatures.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/dartsim/src/EntityManagementFeatures.cc b/dartsim/src/EntityManagementFeatures.cc index fe8958f59..71cd71651 100644 --- a/dartsim/src/EntityManagementFeatures.cc +++ b/dartsim/src/EntityManagementFeatures.cc @@ -307,7 +307,7 @@ Identity EntityManagementFeatures::GetLink( Identity EntityManagementFeatures::GetLink( const Identity &_modelID, const std::string &_linkName) const { - auto modelInfo = this->ReferenceInterface(_modelID); + const auto &modelInfo = this->ReferenceInterface(_modelID); for (const auto &linkInfo : modelInfo->links) { if (_linkName == linkInfo->name) @@ -323,8 +323,8 @@ Identity EntityManagementFeatures::GetLink( { // TODO(addisu) It's not clear what to do when `GetLink` is called on a // model that has been removed. Right now we are returning an invalid - // identity, but that could cause a segfault if the use doesn't check if - // returned value before using it. + // identity, but that could cause a segfault if the user doesn't check + // the returned value before using it. return this->GenerateInvalidId(); } } From b836a0aa0fc7e9407e9f9d569491d3b9d32d9f82 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 23 Mar 2021 12:35:16 -0500 Subject: [PATCH 11/17] Address reviewer feedback Signed-off-by: Addisu Z. Taddese --- dartsim/src/EntityManagementFeatures.cc | 5 +++-- include/ignition/physics/ConstructEmpty.hh | 16 ++++++++++++---- include/ignition/physics/GetEntities.hh | 3 ++- 3 files changed, 17 insertions(+), 7 deletions(-) diff --git a/dartsim/src/EntityManagementFeatures.cc b/dartsim/src/EntityManagementFeatures.cc index e4b6c6f88..2c16acf56 100644 --- a/dartsim/src/EntityManagementFeatures.cc +++ b/dartsim/src/EntityManagementFeatures.cc @@ -278,7 +278,7 @@ std::size_t EntityManagementFeatures::GetNestedModelCount( Identity EntityManagementFeatures::GetNestedModel( const Identity &_modelID, const std::size_t _modelIndex) const { - auto modelInfo = this->ReferenceInterface(_modelID); + const auto modelInfo = this->ReferenceInterface(_modelID); if (_modelIndex >= modelInfo->nestedModels.size()) { return this->GenerateInvalidId(); @@ -303,7 +303,7 @@ Identity EntityManagementFeatures::GetNestedModel( Identity EntityManagementFeatures::GetNestedModel( const Identity &_modelID, const std::string &_modelName) const { - auto modelInfo = this->ReferenceInterface(_modelID); + const auto modelInfo = this->ReferenceInterface(_modelID); const std::string fullName = ::sdf::JoinName(modelInfo->model->getName(), _modelName); @@ -325,6 +325,7 @@ Identity EntityManagementFeatures::GetNestedModel( return this->GenerateInvalidId(); } } + ///////////////////////////////////////////////// std::size_t EntityManagementFeatures::GetLinkCount( const Identity &_modelID) const diff --git a/include/ignition/physics/ConstructEmpty.hh b/include/ignition/physics/ConstructEmpty.hh index 342c03b62..2b1f908da 100644 --- a/include/ignition/physics/ConstructEmpty.hh +++ b/include/ignition/physics/ConstructEmpty.hh @@ -26,7 +26,7 @@ namespace ignition { namespace physics { ///////////////////////////////////////////////// -/// \brief This feature constructs an empty world and return its pointer +/// \brief This feature constructs an empty world and returns its pointer /// from the current physics engine in use. class ConstructEmptyWorldFeature : public virtual Feature { @@ -36,6 +36,8 @@ class ConstructEmptyWorldFeature : public virtual Feature public: using WorldPtrType = WorldPtr; /// \brief Construct an empty world and attach a given name to it. + /// \param[in] _name + /// Name of the world. /// \return /// The WorldPtrType of the constructed world. public: WorldPtrType ConstructEmptyWorld(const std::string &_name); @@ -50,7 +52,7 @@ class ConstructEmptyWorldFeature : public virtual Feature }; ///////////////////////////////////////////////// -/// \brief This feature constructs an empty model and return its pointer +/// \brief This feature constructs an empty model and returns its pointer /// from the given world. class ConstructEmptyModelFeature : public virtual Feature { @@ -60,6 +62,8 @@ class ConstructEmptyModelFeature : public virtual Feature public: using ModelPtrType = ModelPtr; /// \brief Construct an empty model and attach a given name to it. + /// \param[in] _name + /// Name of the model. /// \return /// The ModelPtrType of the constructed model. public: ModelPtrType ConstructEmptyModel(const std::string &_name); @@ -74,7 +78,7 @@ class ConstructEmptyModelFeature : public virtual Feature }; ///////////////////////////////////////////////// -/// \brief This feature constructs an empty nested model and return its pointer +/// \brief This feature constructs an empty nested model and returns its pointer /// from the given world. class ConstructEmptyNestedModelFeature : public virtual Feature { @@ -84,6 +88,8 @@ class ConstructEmptyNestedModelFeature : public virtual Feature public: using ModelPtrType = ModelPtr; /// \brief Construct an empty model and attach a given name to it. + /// \param[in] _name + /// Name of the nested model. /// \return /// The ModelPtrType of the constructed model. public: ModelPtrType ConstructEmptyModel(const std::string &_name); @@ -98,7 +104,7 @@ class ConstructEmptyNestedModelFeature : public virtual Feature }; ///////////////////////////////////////////////// -/// \brief This feature constructs an empty link and return its pointer +/// \brief This feature constructs an empty link and returns its pointer /// from the given model. class ConstructEmptyLinkFeature : public virtual Feature { @@ -108,6 +114,8 @@ class ConstructEmptyLinkFeature : public virtual Feature public: using LinkPtrType = LinkPtr; /// \brief Construct an empty link and attach a given name to it. + /// \param[in] _name + /// Name of the link. /// \return /// The LinkPtrType of the constructed link. public: LinkPtrType ConstructEmptyLink(const std::string &_name); diff --git a/include/ignition/physics/GetEntities.hh b/include/ignition/physics/GetEntities.hh index 92d60d12a..58c6c5de5 100644 --- a/include/ignition/physics/GetEntities.hh +++ b/include/ignition/physics/GetEntities.hh @@ -215,7 +215,7 @@ namespace ignition ///////////////////////////////////////////////// /// \brief This feature retrieves the nested model pointer from the parent - /// model by specifying the model index and model index/name. + /// model by specifying the name or index of the nested model. class IGNITION_PHYSICS_VISIBLE GetNestedModelFromModel : public virtual FeatureWithRequirements { @@ -263,6 +263,7 @@ namespace ignition const Identity &_modelID, const std::string &_modelName) const = 0; }; }; + ///////////////////////////////////////////////// /// \brief This feature retrieves the link pointer from the model /// by specifying model index and link index/name. From d137bba029a5cea24d22ee5458ef32b740e32fcf Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 23 Mar 2021 12:46:50 -0500 Subject: [PATCH 12/17] Make nested model API more explicit by using "nested" in function names Signed-off-by: Addisu Z. Taddese --- dartsim/src/EntityManagement_TEST.cc | 4 +-- dartsim/src/SDFFeatures_TEST.cc | 4 +-- include/ignition/physics/ConstructEmpty.hh | 2 +- include/ignition/physics/GetEntities.hh | 33 ++++++++++--------- .../ignition/physics/detail/ConstructEmpty.hh | 2 +- .../ignition/physics/detail/GetEntities.hh | 12 +++---- 6 files changed, 29 insertions(+), 28 deletions(-) diff --git a/dartsim/src/EntityManagement_TEST.cc b/dartsim/src/EntityManagement_TEST.cc index d2c2ea30b..5cf68c936 100644 --- a/dartsim/src/EntityManagement_TEST.cc +++ b/dartsim/src/EntityManagement_TEST.cc @@ -62,11 +62,11 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) EXPECT_EQ(world, model->GetWorld()); EXPECT_NE(model, world->ConstructEmptyModel("dummy")); - auto nestedModel = model->ConstructEmptyModel("empty nested model"); + auto nestedModel = model->ConstructEmptyNestedModel("empty nested model"); ASSERT_NE(nullptr, nestedModel); EXPECT_EQ("empty nested model", nestedModel->GetName()); EXPECT_EQ(world, nestedModel->GetWorld()); - EXPECT_NE(nestedModel, nestedModel->ConstructEmptyModel("dummy")); + EXPECT_NE(nestedModel, nestedModel->ConstructEmptyNestedModel("dummy")); auto link = model->ConstructEmptyLink("empty link"); ASSERT_NE(nullptr, link); diff --git a/dartsim/src/SDFFeatures_TEST.cc b/dartsim/src/SDFFeatures_TEST.cc index b99968685..01374463b 100644 --- a/dartsim/src/SDFFeatures_TEST.cc +++ b/dartsim/src/SDFFeatures_TEST.cc @@ -552,7 +552,7 @@ TEST_P(SDFFeatures_TEST, WorldWithNestedModel) auto joint1 = parentModel->GetJoint("joint1"); EXPECT_NE(nullptr, joint1); - auto nestedModel = parentModel->GetModel("nested_model"); + auto nestedModel = parentModel->GetNestedModel("nested_model"); ASSERT_NE(nullptr, nestedModel); auto nestedJoint = parentModel->GetJoint("nested_joint"); @@ -616,7 +616,7 @@ TEST_P(SDFFeatures_TEST, WorldWithNestedModelJointToWorld) auto link1 = parentModel->GetLink("link1"); EXPECT_NE(nullptr, link1); - auto nestedModel = parentModel->GetModel("nested_model"); + auto nestedModel = parentModel->GetNestedModel("nested_model"); ASSERT_NE(nullptr, nestedModel); EXPECT_EQ("nested_model", nestedModel->GetName()); EXPECT_EQ(2u, nestedModel->GetLinkCount()); diff --git a/include/ignition/physics/ConstructEmpty.hh b/include/ignition/physics/ConstructEmpty.hh index 2b1f908da..4adbf97e9 100644 --- a/include/ignition/physics/ConstructEmpty.hh +++ b/include/ignition/physics/ConstructEmpty.hh @@ -92,7 +92,7 @@ class ConstructEmptyNestedModelFeature : public virtual Feature /// Name of the nested model. /// \return /// The ModelPtrType of the constructed model. - public: ModelPtrType ConstructEmptyModel(const std::string &_name); + public: ModelPtrType ConstructEmptyNestedModel(const std::string &_name); }; public: template diff --git a/include/ignition/physics/GetEntities.hh b/include/ignition/physics/GetEntities.hh index 58c6c5de5..ad43a6018 100644 --- a/include/ignition/physics/GetEntities.hh +++ b/include/ignition/physics/GetEntities.hh @@ -226,28 +226,29 @@ namespace ignition public: using ModelPtrType = ModelPtr; public: using ConstModelPtrType = ConstModelPtr; - /// \brief Get the number of Models inside this Model. - public: std::size_t GetModelCount() const; + /// \brief Get the number of nested Models inside this Model. + public: std::size_t GetNestedModelCount() const; - /// \brief Get a Model that exists within this Model. + /// \brief Get a nested Model that exists within this Model. /// \param[in] _index /// Index of the model within this model. - /// \return A model reference. If _index is GetModelCount() or higher, - /// this will be a nullptr. - public: ModelPtrType GetModel(std::size_t _index); + /// \return A model reference. If _index is GetNestedModelCount() or + /// higher, this will be a nullptr. + public: ModelPtrType GetNestedModel(std::size_t _index); - /// \sa GetModel(std::size_t) - public: ConstModelPtrType GetModel(std::size_t _index) const; + /// \sa GetNestedModel(std::size_t) + public: ConstModelPtrType GetNestedModel(std::size_t _index) const; - /// \brief Get a Model that exists within this Model. + /// \brief Get a nested Model that exists within this Model. /// \param[in] _name - /// Name of the model within this model. - /// \return A model reference. If a model named _name does not exist in - /// this model, this will be a nullptr. - public: ModelPtrType GetModel(const std::string &_name); - - /// \sa GetModel(const std::string &) - public: ConstModelPtrType GetModel(const std::string &_name) const; + /// Name of the nested model within this model. + /// \return A model reference. If a nested model named _name does not + /// exist in this model, this will be a nullptr. + public: ModelPtrType GetNestedModel(const std::string &_name); + + /// \sa GetNestedModel(const std::string &) + public: ConstModelPtrType GetNestedModel( + const std::string &_name) const; }; public: template diff --git a/include/ignition/physics/detail/ConstructEmpty.hh b/include/ignition/physics/detail/ConstructEmpty.hh index fed87cb53..e1a865018 100644 --- a/include/ignition/physics/detail/ConstructEmpty.hh +++ b/include/ignition/physics/detail/ConstructEmpty.hh @@ -48,7 +48,7 @@ auto ConstructEmptyModelFeature::World ///////////////////////////////////////////////// template auto ConstructEmptyNestedModelFeature::Model -::ConstructEmptyModel(const std::string &_name) -> ModelPtrType +::ConstructEmptyNestedModel(const std::string &_name) -> ModelPtrType { return ModelPtrType(this->pimpl, this->template Interface() diff --git a/include/ignition/physics/detail/GetEntities.hh b/include/ignition/physics/detail/GetEntities.hh index fed54ccb9..8a0864e5b 100644 --- a/include/ignition/physics/detail/GetEntities.hh +++ b/include/ignition/physics/detail/GetEntities.hh @@ -216,8 +216,8 @@ namespace ignition ///////////////////////////////////////////////// template - std::size_t - GetNestedModelFromModel::Model::GetModelCount() const + std::size_t GetNestedModelFromModel::Model< + PolicyT, FeaturesT>::GetNestedModelCount() const { return this->template Interface() ->GetNestedModelCount(this->identity); @@ -225,7 +225,7 @@ namespace ignition ///////////////////////////////////////////////// template - auto GetNestedModelFromModel::Model::GetModel( + auto GetNestedModelFromModel::Model::GetNestedModel( const std::size_t _index) -> ModelPtrType { return ModelPtrType( @@ -236,7 +236,7 @@ namespace ignition ///////////////////////////////////////////////// template - auto GetNestedModelFromModel::Model::GetModel( + auto GetNestedModelFromModel::Model::GetNestedModel( const std::size_t _index) const -> ConstModelPtrType { return ConstModelPtrType( @@ -247,7 +247,7 @@ namespace ignition ///////////////////////////////////////////////// template - auto GetNestedModelFromModel::Model::GetModel( + auto GetNestedModelFromModel::Model::GetNestedModel( const std::string &_name) -> ModelPtrType { return ModelPtrType( @@ -258,7 +258,7 @@ namespace ignition ///////////////////////////////////////////////// template - auto GetNestedModelFromModel::Model::GetModel( + auto GetNestedModelFromModel::Model::GetNestedModel( const std::string &_name) const -> ConstModelPtrType { return ConstModelPtrType(this->pimpl, From 212c941319d4a7227ef38ae6bdbf196f2cec576b Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 23 Mar 2021 12:59:41 -0500 Subject: [PATCH 13/17] Fix bug in nested model construction Signed-off-by: Addisu Z. Taddese --- dartsim/src/EntityManagementFeatures.cc | 4 ++-- dartsim/src/EntityManagement_TEST.cc | 8 ++++++++ dartsim/src/SDFFeatures.cc | 2 +- dartsim/src/SDFFeatures_TEST.cc | 1 + 4 files changed, 12 insertions(+), 3 deletions(-) diff --git a/dartsim/src/EntityManagementFeatures.cc b/dartsim/src/EntityManagementFeatures.cc index 2c16acf56..a85bfe6ab 100644 --- a/dartsim/src/EntityManagementFeatures.cc +++ b/dartsim/src/EntityManagementFeatures.cc @@ -718,8 +718,8 @@ Identity EntityManagementFeatures::ConstructEmptyNestedModel( dart::dynamics::Frame::World(), modelFullName + "_frame"); - auto [modelID, modelInfo] = - this->AddModel({model, _name, modelFrame, ""}, worldID); // NOLINT + auto [modelID, modelInfo] = this->AddNestedModel( + {model, _name, modelFrame, ""}, _modelID, worldID); // NOLINT return this->GenerateIdentity(modelID, this->models.at(modelID)); } diff --git a/dartsim/src/EntityManagement_TEST.cc b/dartsim/src/EntityManagement_TEST.cc index 5cf68c936..9cd49f412 100644 --- a/dartsim/src/EntityManagement_TEST.cc +++ b/dartsim/src/EntityManagement_TEST.cc @@ -65,8 +65,16 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) auto nestedModel = model->ConstructEmptyNestedModel("empty nested model"); ASSERT_NE(nullptr, nestedModel); EXPECT_EQ("empty nested model", nestedModel->GetName()); + EXPECT_EQ(1u, model->GetNestedModelCount()); EXPECT_EQ(world, nestedModel->GetWorld()); + EXPECT_EQ(0u, model->GetIndex()); + EXPECT_EQ(nestedModel, model->GetNestedModel(0)); + EXPECT_EQ(nestedModel, model->GetNestedModel("empty nested model")); EXPECT_NE(nestedModel, nestedModel->ConstructEmptyNestedModel("dummy")); + // This should remain 1 since we're adding a nested model in `nestedModel` not + // in `model`. + EXPECT_EQ(1u, model->GetNestedModelCount()); + EXPECT_EQ(1u, nestedModel->GetNestedModelCount()); auto link = model->ConstructEmptyLink("empty link"); ASSERT_NE(nullptr, link); diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index 8e947826b..2a89d8614 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -432,7 +432,7 @@ Identity SDFFeatures::ConstructSdfModelImpl( { return this->AddModel( // NOLINT {model, _sdfModel.Name(), modelFrame, _sdfModel.CanonicalLinkName()}, - _parentID); + worldID); } }(); model->setMobile(!_sdfModel.Static()); diff --git a/dartsim/src/SDFFeatures_TEST.cc b/dartsim/src/SDFFeatures_TEST.cc index 01374463b..09a2c77b0 100644 --- a/dartsim/src/SDFFeatures_TEST.cc +++ b/dartsim/src/SDFFeatures_TEST.cc @@ -552,6 +552,7 @@ TEST_P(SDFFeatures_TEST, WorldWithNestedModel) auto joint1 = parentModel->GetJoint("joint1"); EXPECT_NE(nullptr, joint1); + EXPECT_EQ(3u, parentModel->GetNestedModelCount()); auto nestedModel = parentModel->GetNestedModel("nested_model"); ASSERT_NE(nullptr, nestedModel); From a652ef2b94b139322aa4db718d4e6606099634e1 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 23 Mar 2021 13:53:34 -0500 Subject: [PATCH 14/17] Add a test for AddNestedModel Signed-off-by: Addisu Z. Taddese --- dartsim/src/Base_TEST.cc | 37 +++++++++++++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) diff --git a/dartsim/src/Base_TEST.cc b/dartsim/src/Base_TEST.cc index e6d1a6cce..40cfb83c5 100644 --- a/dartsim/src/Base_TEST.cc +++ b/dartsim/src/Base_TEST.cc @@ -148,6 +148,43 @@ TEST(BaseClass, RemoveModel) EXPECT_EQ(0u, curSize); } +TEST(BaseClass, AddNestedModel) +{ + dartsim::Base base; + base.InitiateEngine(0); + + dart::simulation::WorldPtr world = dart::simulation::World::create("default"); + + auto worldID = base.AddWorld(world, world->getName()); + EXPECT_TRUE(base.worlds.HasEntity(worldID)); + EXPECT_EQ(worldID, base.worlds.IdentityOf(world->getName())); + auto createSkel = [](const std::string &_skelName) + { + auto skel = dart::dynamics::Skeleton::create(_skelName); + auto frame = dart::dynamics::SimpleFrame::createShared( + dart::dynamics::Frame::World(), _skelName + "_frame"); + return dartsim::ModelInfo{skel, _skelName, frame, ""}; + }; + + const auto &[parentModelID, parentModelInfo] = + base.AddModel(createSkel("parent_model"), worldID); + EXPECT_EQ(0u, parentModelInfo.nestedModels.size()); + + const auto &[nestedModel1ID, nestedModel1Info] = base.AddNestedModel( + createSkel("parent_model::nested_model1"), parentModelID, worldID); + ASSERT_TRUE(base.models.HasEntity(nestedModel1ID)); + EXPECT_EQ(nestedModel1Info.model, base.models.at(nestedModel1ID)->model); + ASSERT_EQ(1u, parentModelInfo.nestedModels.size()); + EXPECT_EQ(nestedModel1ID, parentModelInfo.nestedModels[0]); + + const auto &[nestedModel2ID, nestedModel2Info] = base.AddNestedModel( + createSkel("parent_model::nested_model2"), parentModelID, worldID); + ASSERT_TRUE(base.models.HasEntity(nestedModel2ID)); + EXPECT_EQ(nestedModel2Info.model, base.models.at(nestedModel2ID)->model); + ASSERT_EQ(2u, parentModelInfo.nestedModels.size()); + EXPECT_EQ(nestedModel2ID, parentModelInfo.nestedModels[1]); +} + int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); From f0a849285efb0a501e89030cd318eb2ce7b7b175 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 23 Mar 2021 16:12:28 -0500 Subject: [PATCH 15/17] Fix function names after merge Signed-off-by: Addisu Z. Taddese --- tpe/plugin/src/EntityManagement_TEST.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/tpe/plugin/src/EntityManagement_TEST.cc b/tpe/plugin/src/EntityManagement_TEST.cc index c4e864905..61283ecbd 100644 --- a/tpe/plugin/src/EntityManagement_TEST.cc +++ b/tpe/plugin/src/EntityManagement_TEST.cc @@ -80,9 +80,9 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) EXPECT_EQ(link, model->GetLink(0)); EXPECT_EQ(link, model->GetLink("empty link")); - EXPECT_EQ(0u, model->GetModelCount()); - auto nestedModel = model->ConstructEmptyModel("empty nested model"); - EXPECT_EQ(1u, model->GetModelCount()); + EXPECT_EQ(0u, model->GetNestedModelCount()); + auto nestedModel = model->ConstructEmptyNestedModel("empty nested model"); + EXPECT_EQ(1u, model->GetNestedModelCount()); ASSERT_NE(nullptr, nestedModel); EXPECT_EQ("empty nested model", nestedModel->GetName()); EXPECT_EQ(0u, nestedModel->GetLinkCount()); From e17aed3bb5b6ae4e68658cb7ec39dbe4cdd72a16 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 23 Mar 2021 16:26:25 -0500 Subject: [PATCH 16/17] Address reviewer feedback Signed-off-by: Addisu Z. Taddese --- tpe/lib/src/Model.cc | 20 ++++++++++---------- tpe/plugin/src/EntityManagementFeatures.cc | 2 +- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/tpe/lib/src/Model.cc b/tpe/lib/src/Model.cc index 8eb5899b2..be98c1883 100644 --- a/tpe/lib/src/Model.cc +++ b/tpe/lib/src/Model.cc @@ -35,6 +35,12 @@ class ignition::physics::tpelib::ModelPrivate /// \brief First inserted link id; public: std::size_t firstLinkId = kNullEntityId; + + /// \brief Links in the model + public: std::vector linkIds; + + /// \brief Nested models links + public: std::vector nestedModelIds; }; using namespace ignition; @@ -70,6 +76,7 @@ Entity &Model::AddLink() const auto[it, success] = this->GetChildren().insert( {linkId, std::make_shared(linkId)}); + this->dataPtr->linkIds.push_back(linkId); it->second->SetParent(this); this->ChildrenChanged(); @@ -79,11 +86,7 @@ Entity &Model::AddLink() ////////////////////////////////////////////////// std::size_t Model::GetLinkCount() const { - const auto &children = this->GetChildren(); - return std::count_if(children.begin(), children.end(), [](auto _child) - { - return std::dynamic_pointer_cast(_child.second); - }); + return this->dataPtr->linkIds.size(); } ////////////////////////////////////////////////// @@ -92,6 +95,7 @@ Entity &Model::AddModel() std::size_t modelId = Entity::GetNextId(); const auto[it, success] = this->GetChildren().insert( {modelId, std::make_shared(modelId)}); + this->dataPtr->nestedModelIds.push_back(modelId); it->second->SetParent(this); this->ChildrenChanged(); @@ -101,11 +105,7 @@ Entity &Model::AddModel() ////////////////////////////////////////////////// std::size_t Model::GetModelCount() const { - const auto &children = this->GetChildren(); - return std::count_if(children.begin(), children.end(), [](auto _child) - { - return std::dynamic_pointer_cast(_child.second); - }); + return this->dataPtr->nestedModelIds.size(); } ////////////////////////////////////////////////// diff --git a/tpe/plugin/src/EntityManagementFeatures.cc b/tpe/plugin/src/EntityManagementFeatures.cc index 2d377c679..8189187d2 100644 --- a/tpe/plugin/src/EntityManagementFeatures.cc +++ b/tpe/plugin/src/EntityManagementFeatures.cc @@ -204,7 +204,7 @@ Identity EntityManagementFeatures::GetNestedModel( { if (nestedModelInfo != nullptr) { - std::string name = nestedModelInfo->model->GetName(); + const std::string &name = nestedModelInfo->model->GetName(); if (parentModelId == modelEnt.GetId() && name == modelEnt.GetName()) { return this->GenerateIdentity(parentModelId, nestedModelInfo); From e5679df5d6698b29f31fc34d37428cbceb8cfc52 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 23 Mar 2021 20:27:41 -0500 Subject: [PATCH 17/17] Fix entity by index lookup, address reviewer feedback Signed-off-by: Addisu Z. Taddese --- tpe/lib/src/Model.cc | 3 +- tpe/plugin/src/Base.hh | 30 ++++++++---- tpe/plugin/src/Base_TEST.cc | 16 ++++--- tpe/plugin/src/EntityManagementFeatures.cc | 53 ++++++++++------------ tpe/plugin/src/EntityManagement_TEST.cc | 2 + 5 files changed, 58 insertions(+), 46 deletions(-) diff --git a/tpe/lib/src/Model.cc b/tpe/lib/src/Model.cc index be98c1883..813db13c5 100644 --- a/tpe/lib/src/Model.cc +++ b/tpe/lib/src/Model.cc @@ -16,7 +16,6 @@ */ #include -#include #include #include @@ -39,7 +38,7 @@ class ignition::physics::tpelib::ModelPrivate /// \brief Links in the model public: std::vector linkIds; - /// \brief Nested models links + /// \brief Nested models public: std::vector nestedModelIds; }; diff --git a/tpe/plugin/src/Base.hh b/tpe/plugin/src/Base.hh index b8355b676..6b9e58eb6 100644 --- a/tpe/plugin/src/Base.hh +++ b/tpe/plugin/src/Base.hh @@ -88,26 +88,38 @@ class Base : public Implements3d> return -1; } - public: inline std::size_t indexInContainerToId( - const std::size_t _containerId, const std::size_t _index) const + public: template + inline std::pair indexInContainerToId( + const std::size_t _containerId, + const std::size_t _index, + const std::map &_idMap) const { std::size_t counter = 0; auto it = this->childIdToParentId.begin(); while (counter <= _index && it != this->childIdToParentId.end()) { - if (it->second == _containerId && counter == _index) + if (it->second == _containerId) { - return it->first; - } - else if (it->second == _containerId) - { - ++counter; + auto idMapIt = _idMap.find(it->first); + // only count if the entity is found in the idMap. This makes sure we're + // counting only the entities with the correct EntityType + if (idMapIt != _idMap.end()) + { + if (counter == _index) + { + return *idMapIt; + } + else + { + ++counter; + } + } } ++it; } // return invalid id if entity not found - return -1; + return {INVALID_ENTITY_ID, nullptr}; } diff --git a/tpe/plugin/src/Base_TEST.cc b/tpe/plugin/src/Base_TEST.cc index 551fe0241..3438833be 100644 --- a/tpe/plugin/src/Base_TEST.cc +++ b/tpe/plugin/src/Base_TEST.cc @@ -158,24 +158,28 @@ TEST(BaseClass, AddEntities) // check indices std::size_t modelInd1 = base.idToIndexInContainer(modelId1); EXPECT_EQ(0u, modelInd1); - EXPECT_EQ(modelId1, base.indexInContainerToId(worldId, 0u)); + EXPECT_EQ(modelId1, + base.indexInContainerToId(worldId, 0u, base.models).first); std::size_t modelInd2 = base.idToIndexInContainer(modelId2); EXPECT_EQ(1u, modelInd2); - EXPECT_EQ(modelId2, base.indexInContainerToId(worldId, 1u)); + EXPECT_EQ(modelId2, + base.indexInContainerToId(worldId, 1u, base.models).first); std::size_t linkInd1 = base.idToIndexInContainer(linkId1); EXPECT_EQ(0u, linkInd1); - EXPECT_EQ(linkId1, base.indexInContainerToId(modelId1, 0u)); + EXPECT_EQ(linkId1, base.indexInContainerToId(modelId1, 0u, base.links).first); std::size_t linkInd2 = base.idToIndexInContainer(linkId2); EXPECT_EQ(0u, linkInd2); - EXPECT_EQ(linkId2, base.indexInContainerToId(modelId2, 0u)); + EXPECT_EQ(linkId2, base.indexInContainerToId(modelId2, 0u, base.links).first); std::size_t boxInd = base.idToIndexInContainer(boxId); EXPECT_EQ(0u, boxInd); - EXPECT_EQ(boxId, base.indexInContainerToId(linkId1, 0u)); + EXPECT_EQ(boxId, + base.indexInContainerToId(linkId1, 0u, base.collisions).first); std::size_t cylinderInd = base.idToIndexInContainer(cylinderId); EXPECT_EQ(0u, cylinderInd); - EXPECT_EQ(cylinderId, base.indexInContainerToId(linkId2, 0u)); + EXPECT_EQ(cylinderId, + base.indexInContainerToId(linkId2, 0u, base.collisions).first); } int main(int argc, char *argv[]) diff --git a/tpe/plugin/src/EntityManagementFeatures.cc b/tpe/plugin/src/EntityManagementFeatures.cc index 8189187d2..a69a6a5f2 100644 --- a/tpe/plugin/src/EntityManagementFeatures.cc +++ b/tpe/plugin/src/EntityManagementFeatures.cc @@ -110,11 +110,11 @@ std::size_t EntityManagementFeatures::GetModelCount( Identity EntityManagementFeatures::GetModel( const Identity &_worldID, const std::size_t _modelIndex) const { - std::size_t modelId = this->indexInContainerToId(_worldID.id, _modelIndex); - auto it = this->models.find(modelId); - if (it != this->models.end() && it->second != nullptr) + const auto &[modelId, modelInfo] = + this->indexInContainerToId(_worldID.id, _modelIndex, this->models); + if (modelInfo != nullptr) { - return this->GenerateIdentity(modelId, it->second); + return this->GenerateIdentity(modelId, modelInfo); } return this->GenerateInvalidId(); } @@ -183,11 +183,11 @@ std::size_t EntityManagementFeatures::GetNestedModelCount( Identity EntityManagementFeatures::GetNestedModel( const Identity &_modelID, const std::size_t _modelIndex) const { - std::size_t modelId = this->indexInContainerToId(_modelID.id, _modelIndex); - auto it = this->models.find(modelId); - if (it != this->models.end() && it->second != nullptr) + const auto &[nestedModelId, nestedModelInfo] = + this->indexInContainerToId(_modelID.id, _modelIndex, this->models); + if (nestedModelInfo != nullptr) { - return this->GenerateIdentity(modelId, it->second); + return this->GenerateIdentity(nestedModelId, nestedModelInfo); } return this->GenerateInvalidId(); } @@ -196,20 +196,14 @@ Identity EntityManagementFeatures::GetNestedModel( Identity EntityManagementFeatures::GetNestedModel( const Identity &_modelID, const std::string &_modelName) const { - auto modelInfo = this->ReferenceInterface(_modelID); + auto modelInfo = this->ReferenceInterface(_modelID); if (modelInfo != nullptr) { - tpelib::Entity &modelEnt = modelInfo->world->GetChildByName(_modelName); - for (const auto &[parentModelId, nestedModelInfo]: this->models) + tpelib::Entity &modelEnt = modelInfo->model->GetChildByName(_modelName); + auto it = this->models.find(modelEnt.GetId()); + if (it != this->models.end() && it->second != nullptr) { - if (nestedModelInfo != nullptr) - { - const std::string &name = nestedModelInfo->model->GetName(); - if (parentModelId == modelEnt.GetId() && name == modelEnt.GetName()) - { - return this->GenerateIdentity(parentModelId, nestedModelInfo); - } - } + return this->GenerateIdentity(it->first, it->second); } } return this->GenerateInvalidId(); @@ -226,11 +220,11 @@ std::size_t EntityManagementFeatures::GetLinkCount( Identity EntityManagementFeatures::GetLink( const Identity &_modelID, const std::size_t _linkIndex) const { - std::size_t linkId = this->indexInContainerToId(_modelID.id, _linkIndex); - auto it = this->links.find(linkId); - if (it != this->links.end() && it->second != nullptr) + const auto &[linkId, linkInfo] = + this->indexInContainerToId(_modelID.id, _linkIndex, this->links); + if (linkInfo != nullptr) { - return this->GenerateIdentity(it->first, it->second); + return this->GenerateIdentity(linkId, linkInfo); } return this->GenerateInvalidId(); } @@ -299,11 +293,11 @@ std::size_t EntityManagementFeatures::GetShapeCount( Identity EntityManagementFeatures::GetShape( const Identity &_linkID, const std::size_t _shapeIndex) const { - std::size_t shapeId = this->indexInContainerToId(_linkID.id, _shapeIndex); - auto it = this->collisions.find(shapeId); - if (it != this->collisions.end() && it->second != nullptr) + const auto &[shapeId, shapeInfo] = + this->indexInContainerToId(_linkID.id, _shapeIndex, this->collisions); + if (shapeInfo != nullptr) { - return this->GenerateIdentity(it->first, it->second); + return this->GenerateIdentity(shapeId, shapeInfo); } return this->GenerateInvalidId(); } @@ -371,8 +365,9 @@ bool EntityManagementFeatures::RemoveModelByIndex( auto worldInfo = this->ReferenceInterface(_worldID); if (worldInfo != nullptr) { - auto modelId = this->indexInContainerToId(_worldID.id, _modelIndex); - if (this->models.find(modelId) != this->models.end()) + const auto [modelId, modelInfo] = + this->indexInContainerToId(_worldID.id, _modelIndex, this->models); + if (modelInfo != nullptr) { this->models.erase(modelId); this->childIdToParentId.erase(modelId); diff --git a/tpe/plugin/src/EntityManagement_TEST.cc b/tpe/plugin/src/EntityManagement_TEST.cc index 61283ecbd..8dccad7d9 100644 --- a/tpe/plugin/src/EntityManagement_TEST.cc +++ b/tpe/plugin/src/EntityManagement_TEST.cc @@ -85,6 +85,8 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld) EXPECT_EQ(1u, model->GetNestedModelCount()); ASSERT_NE(nullptr, nestedModel); EXPECT_EQ("empty nested model", nestedModel->GetName()); + EXPECT_EQ(nestedModel, model->GetNestedModel("empty nested model")); + EXPECT_EQ(nestedModel, model->GetNestedModel(0)); EXPECT_EQ(0u, nestedModel->GetLinkCount()); ASSERT_NE(nullptr, nestedModel->ConstructEmptyLink("empty link")); EXPECT_EQ(1u, nestedModel->GetLinkCount());