From b54cd35001de3d0f09c75db004ce63ec50aef849 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 25 Feb 2021 09:18:37 -0600 Subject: [PATCH 1/8] 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 2/8] 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 3/8] 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 4/8] 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 5/8] 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 6/8] 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 0bb7d9e9d787d815f7fd3db15aa2da9a815c40b2 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 22 Mar 2021 10:31:12 -0500 Subject: [PATCH 7/8] 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 8/8] 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(); } }