[dartsim] Add empty nested model construction and nested model entity…

… management (#228)

This adds two new features:

* `ConstructEmptyNestedModelFeature`, which allows users to create a nested model without having to use `ConstructSdfNestedModel`
* `GetNestedModelFromModel`, which allow retrieving a nested model from it's parent Model entity.

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>

Co-authored-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com>

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<std::shared_ptr<LinkInfo>> links {};
+  std::vector<std::size_t> nestedModels = {};
 };
 
 struct JointInfo
@@ -291,6 +293,15 @@ class Base : public Implements3d<FeatureList<Feature>>
     return std::forward_as_tuple(id, entry);
   }
 
+  public: inline std::tuple<std::size_t, ModelInfo &> 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)
   {

dartsim/src/Base_TEST.cc

@@ -64,7 +64,7 @@ TEST(BaseClass, RemoveModel)
     auto sn = pair.second->createShapeNodeWith<dart::dynamics::CollisionAspect>(
       boxShape);
 
-    auto res = base.AddModel({skel, frame, ""}, worldID);
+    auto res = base.AddModel({skel, name, 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);
@@ -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);

dartsim/src/EntityManagementFeatures.cc

@@ -238,7 +238,7 @@ Identity EntityManagementFeatures::GetModel(
 const std::string &EntityManagementFeatures::GetModelName(
     const Identity &_modelID) const
 {
-  return this->ReferenceInterface<ModelInfo>(_modelID)->model->getName();
+  return this->ReferenceInterface<ModelInfo>(_modelID)->localName;
 }
 
 /////////////////////////////////////////////////
@@ -267,6 +267,65 @@ Identity EntityManagementFeatures::GetWorldOfModel(
   }
 }
 
+/////////////////////////////////////////////////
+std::size_t EntityManagementFeatures::GetNestedModelCount(
+    const Identity &_modelID) const
+{
+  return this->ReferenceInterface<ModelInfo>(_modelID)->nestedModels.size();
+}
+
+/////////////////////////////////////////////////
+Identity EntityManagementFeatures::GetNestedModel(
+    const Identity &_modelID, const std::size_t _modelIndex) const
+{
+  const auto modelInfo = this->ReferenceInterface<ModelInfo>(_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
+{
+  const auto modelInfo = this->ReferenceInterface<ModelInfo>(_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 +695,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->AddNestedModel(
+      {model, _name, modelFrame, ""}, _modelID, worldID);  // NOLINT
 
   return this->GenerateIdentity(modelID, this->models.at(modelID));
 }

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;
 

dartsim/src/EntityManagement_TEST.cc

@@ -62,6 +62,20 @@ TEST(EntityManagement_TEST, ConstructEmptyWorld)
   EXPECT_EQ(world, model->GetWorld());
   EXPECT_NE(model, world->ConstructEmptyModel("dummy"));
 
+  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);
   EXPECT_EQ("empty link", link->GetName());

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()},
+          worldID);
+    }
+  }();
   model->setMobile(!_sdfModel.Static());
   model->setSelfCollisionCheck(_sdfModel.SelfCollide());
 

dartsim/src/SDFFeatures_TEST.cc

@@ -545,14 +545,15 @@ 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");
+  EXPECT_EQ(3u, parentModel->GetNestedModelCount());
+  auto nestedModel = parentModel->GetNestedModel("nested_model");
   ASSERT_NE(nullptr, nestedModel);
 
   auto nestedJoint = parentModel->GetJoint("nested_joint");
@@ -616,9 +617,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->GetNestedModel("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());
 

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<PolicyT, FeaturesT>;
 
     /// \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<PolicyT, FeaturesT>;
 
     /// \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,33 @@ class ConstructEmptyModelFeature : public virtual Feature
 };
 
 /////////////////////////////////////////////////
-/// \brief This feature constructs an empty link 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
+{
+  public: template <typename PolicyT, typename FeaturesT>
+  class Model : public virtual Feature::Model<PolicyT, FeaturesT>
+  {
+    public: using ModelPtrType = ModelPtr<PolicyT, FeaturesT>;
+
+    /// \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 ConstructEmptyNestedModel(const std::string &_name);
+  };
+
+  public: template <typename PolicyT>
+  class Implementation : public virtual Feature::Implementation<PolicyT>
+  {
+    public: virtual Identity ConstructEmptyNestedModel(
+        const Identity &_modelID, const std::string &_name) = 0;
+  };
+};
+
+/////////////////////////////////////////////////
+/// \brief This feature constructs an empty link and returns its pointer
 /// from the given model.
 class ConstructEmptyLinkFeature : public virtual Feature
 {
@@ -84,6 +114,8 @@ class ConstructEmptyLinkFeature : public virtual Feature
     public: using LinkPtrType = LinkPtr<PolicyT, FeaturesT>;
 
     /// \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);