Remove nested models

dartsim/src/Base.hh

@@ -278,27 +278,42 @@ class Base : public Implements3d<FeatureList<Feature>>
 
     const dart::simulation::WorldPtr &world = worlds[_worldID];
 
-    const std::size_t indexInWorld = world->getNumSkeletons();
-    this->models.idToIndexInContainer[id] = indexInWorld;
     std::vector<std::size_t> &indexInContainerToID =
         this->models.indexInContainerToID[_worldID];
+    const std::size_t indexInWorld = indexInContainerToID.size();
+    this->models.idToIndexInContainer[id] = indexInWorld;
     indexInContainerToID.push_back(id);
     world->addSkeleton(entry.model);
 
     this->models.idToContainerID[id] = _worldID;
     this->frames[id] = _info.frame.get();
 
-    assert(indexInContainerToID.size() == world->getNumSkeletons());
-
     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);
+    const std::size_t id = this->GetNextEntity();
+    this->models.idToObject[id] = std::make_shared<ModelInfo>(_info);
+    ModelInfo &entry = *this->models.idToObject[id];
+    this->models.objectToID[_info.model] = id;
+
+    const dart::simulation::WorldPtr &world = worlds[_worldID];
+
+    auto parentModelInfo = this->models.at(_parentID);
+    const std::size_t indexInModel =
+        parentModelInfo->nestedModels.size();
+    this->models.idToIndexInContainer[id] = indexInModel;
+    std::vector<std::size_t> &indexInContainerToID =
+        this->models.indexInContainerToID[_parentID];
+    indexInContainerToID.push_back(id);
+    world->addSkeleton(entry.model);
+
+    this->models.idToContainerID[id] = _parentID;
+    this->frames[id] = _info.frame.get();
+    parentModelInfo->nestedModels.push_back(id);
     return {id, entry};
   }
 
@@ -352,13 +367,19 @@ class Base : public Implements3d<FeatureList<Feature>>
     return id;
   }
 
-  public: void RemoveModelImpl(const std::size_t _worldID,
+  public: bool 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;
+    auto modelInfo = this->models.at(_modelID);
+    auto skel = modelInfo->model;
     // Remove the contents of the skeleton from local entity storage containers
+    for (auto &nestedModel : modelInfo->nestedModels)
+    {
+      this->RemoveModelImpl(_worldID, nestedModel);
+    }
+    modelInfo->nestedModels.clear();
+
     for (auto &jt : skel->getJoints())
     {
       this->joints.RemoveEntity(jt);
@@ -373,8 +394,41 @@ class Base : public Implements3d<FeatureList<Feature>>
       this->linksByName.erase(::sdf::JoinName(
           world->getName(), ::sdf::JoinName(skel->getName(), bn->getName())));
     }
+
+    // If this is a nested model, remove an entry from the parent models
+    // "nestedModels" vector
+    auto parentID = this->models.idToContainerID.at(_modelID);
+    if (parentID != _worldID)
+    {
+      auto parentModelInfo = this->models.at(parentID);
+      const std::size_t modelIndex =
+          this->models.idToIndexInContainer.at(_modelID);
+      if (modelIndex >= parentModelInfo->nestedModels.size())
+        return false;
+      parentModelInfo->nestedModels.erase(
+          parentModelInfo->nestedModels.begin() + modelIndex);
+    }
     this->models.RemoveEntity(skel);
     world->removeSkeleton(skel);
+    return true;
+  }
+
+  public: inline std::size_t GetWorldOfModelImpl(
+              const std::size_t &_modelID) const
+  {
+    if (this->models.HasEntity(_modelID))
+    {
+      auto parentIt = this->models.idToContainerID.find(_modelID);
+      if (parentIt != this->models.idToContainerID.end())
+      {
+        if (this->worlds.HasEntity(parentIt->second))
+        {
+          return parentIt->second;
+        }
+        return this->GetWorldOfModelImpl(parentIt->second);
+      }
+    }
+    return this->GenerateInvalidId();
   }
 
   public: EntityStorage<DartWorldPtr, std::string> worlds;

dartsim/src/EntityManagementFeatures.cc

@@ -99,6 +99,10 @@ class BitmaskContactFilter : public dart::collision::BodyNodeCollisionFilter
 
 /// Utility functions
 /////////////////////////////////////////////////
+/// TODO (addisu): There's a lot of code duplication in model removal code, such
+/// as RemoveModelByIndex, where we call GetFilterPtr followed by
+/// RemoveSkeletonCollisions(model). To de-duplicate this, move this logic into
+/// RemoveModelImpl. To do that, we need to move GetFilterPtr into Base.hh.
 static const std::shared_ptr<BitmaskContactFilter> GetFilterPtr(
     const EntityManagementFeatures* _emf, std::size_t _worldID)
 {
@@ -121,7 +125,7 @@ static std::size_t GetWorldOfShapeNode(const EntityManagementFeatures *_emf,
   // Now find the skeleton's model
   const std::size_t modelID = _emf->models.objectToID.at(skelPtr);
   // And the world containing the model
-  return _emf->models.idToContainerID.at(modelID);
+  return _emf->GetWorldOfModelImpl(modelID);
 }
 
 /////////////////////////////////////////////////
@@ -198,14 +202,19 @@ std::size_t EntityManagementFeatures::GetModelCount(
 Identity EntityManagementFeatures::GetModel(
     const Identity &_worldID, const std::size_t _modelIndex) const
 {
-  const DartSkeletonPtr &model =
-      this->ReferenceInterface<DartWorld>(_worldID)->getSkeleton(_modelIndex);
+  const auto &indexInContainerToID =
+      this->models.indexInContainerToID.at(_worldID);
+
+  if (_modelIndex >= indexInContainerToID.size())
+  {
+    return this->GenerateInvalidId();
+  }
+  const std::size_t modelID = indexInContainerToID[_modelIndex];
 
   // If the model doesn't exist in "models", it means the containing entity has
   // been removed.
-  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
@@ -255,16 +264,12 @@ std::size_t EntityManagementFeatures::GetModelIndex(
 Identity EntityManagementFeatures::GetWorldOfModel(
     const Identity &_modelID) const
 {
-  // If the model doesn't exist in "models", it it has been removed.
-  if (this->models.HasEntity(_modelID))
+  auto worldID = this->GetWorldOfModelImpl(_modelID);
+  if (worldID != INVALID_ENTITY_ID )
   {
-    const std::size_t worldID = this->models.idToContainerID.at(_modelID);
     return this->GenerateIdentity(worldID, this->worlds.at(worldID));
   }
-  else
-  {
-    return this->GenerateInvalidId();
-  }
+  return this->GenerateInvalidId();
 }
 
 /////////////////////////////////////////////////
@@ -310,7 +315,7 @@ Identity EntityManagementFeatures::GetNestedModel(
 
   if (this->models.HasEntity(_modelID))
   {
-    auto worldID = this->models.idToContainerID.at(_modelID);
+    auto worldID = this->GetWorldOfModelImpl(_modelID);
     auto nestedSkel = this->worlds.at(worldID)->getSkeleton(fullName);
     if (nullptr == nestedSkel)
     {
@@ -620,8 +625,7 @@ bool EntityManagementFeatures::RemoveModelByIndex(const Identity &_worldID,
   {
     auto filterPtr = GetFilterPtr(this, _worldID);
     filterPtr->RemoveSkeletonCollisions(model);
-    this->RemoveModelImpl(_worldID, this->models.IdentityOf(model));
-    return true;
+    return this->RemoveModelImpl(_worldID, this->models.IdentityOf(model));
   }
   return false;
 }
@@ -637,8 +641,7 @@ bool EntityManagementFeatures::RemoveModelByName(const Identity &_worldID,
   {
     auto filterPtr = GetFilterPtr(this, _worldID);
     filterPtr->RemoveSkeletonCollisions(model);
-    this->RemoveModelImpl(_worldID, this->models.IdentityOf(model));
-    return true;
+    return this->RemoveModelImpl(_worldID, this->models.IdentityOf(model));
   }
   return false;
 }
@@ -648,13 +651,13 @@ bool EntityManagementFeatures::RemoveModel(const Identity &_modelID)
 {
   if (this->models.HasEntity(_modelID))
   {
-    auto worldID = this->models.idToContainerID.at(_modelID);
+    auto worldID = this->GetWorldOfModelImpl(_modelID);
     auto model = this->models.at(_modelID)->model;
 
     auto filterPtr = GetFilterPtr(this, worldID);
     filterPtr->RemoveSkeletonCollisions(model);
-    this->RemoveModelImpl(this->models.idToContainerID.at(_modelID), _modelID);
-    return true;
+
+    return this->RemoveModelImpl(worldID, _modelID);
   }
   return false;
 }
@@ -665,6 +668,50 @@ bool EntityManagementFeatures::ModelRemoved(const Identity &_modelID) const
   return !this->models.HasEntity(_modelID);
 }
 
+/////////////////////////////////////////////////
+bool EntityManagementFeatures::RemoveNestedModelByIndex(
+    const Identity &_modelID, std::size_t _nestedModelIndex)
+{
+  auto modelInfo = this->ReferenceInterface<ModelInfo>(_modelID);
+  if (_nestedModelIndex >= modelInfo->nestedModels.size())
+  {
+    return this->GenerateInvalidId();
+  }
+  const auto nestedModelID = modelInfo->nestedModels[_nestedModelIndex];
+  if (this->models.HasEntity(nestedModelID))
+  {
+    const auto worldID = this->GetWorldOfModelImpl(nestedModelID);
+    const auto model = this->models.at(nestedModelID)->model;
+    const auto filterPtr = GetFilterPtr(this, worldID);
+    filterPtr->RemoveSkeletonCollisions(model);
+    return this->RemoveModelImpl(worldID, nestedModelID);
+  }
+  return false;
+}
+
+/////////////////////////////////////////////////
+bool EntityManagementFeatures::RemoveNestedModelByName(const Identity &_modelID,
+                                                 const std::string &_modelName)
+{
+  auto modelInfo = this->ReferenceInterface<ModelInfo>(_modelID);
+  const std::string fullName =
+      ::sdf::JoinName(modelInfo->model->getName(), _modelName);
+
+  if (this->models.HasEntity(_modelID))
+  {
+    auto worldID = this->GetWorldOfModelImpl(_modelID);
+    auto nestedSkel = this->worlds.at(worldID)->getSkeleton(fullName);
+    if (nullptr == nestedSkel || !this->models.HasEntity(nestedSkel))
+    {
+      return false;
+    }
+    const std::size_t nestedModelID = this->models.IdentityOf(nestedSkel);
+    const auto filterPtr = GetFilterPtr(this, worldID);
+    filterPtr->RemoveSkeletonCollisions(nestedSkel);
+    return this->RemoveModelImpl(worldID, nestedModelID);
+  }
+  return false;
+}
 /////////////////////////////////////////////////
 Identity EntityManagementFeatures::ConstructEmptyWorld(
     const Identity &/*_engineID*/, const std::string &_name)
@@ -695,19 +742,19 @@ Identity EntityManagementFeatures::ConstructEmptyModel(
         dart::dynamics::Frame::World(),
         _name + "_frame");
 
-  auto [modelID, modelInfo] =
+  const 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)
+    const Identity &_parentModelID, 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;
+  auto worldID = this->GetWorldOfModelImpl(_parentModelID);
+  const auto &skel = this->models.at(_parentModelID)->model;
   const std::string modelFullName = ::sdf::JoinName(skel->getName(), _name);
 
   dart::dynamics::SkeletonPtr model =
@@ -719,7 +766,7 @@ Identity EntityManagementFeatures::ConstructEmptyNestedModel(
         modelFullName + "_frame");
 
   auto [modelID, modelInfo] = this->AddNestedModel(
-      {model, _name, modelFrame, ""}, _modelID, worldID);  // NOLINT
+      {model, _name, modelFrame, ""}, _parentModelID, worldID);  // NOLINT
 
   return this->GenerateIdentity(modelID, this->models.at(modelID));
 }
@@ -740,16 +787,16 @@ Identity EntityManagementFeatures::ConstructEmptyLink(
       model->createJointAndBodyNodePair<dart::dynamics::FreeJoint>(
         nullptr, prop_fj, prop_bn).second;
 
-  auto worldIDIt = this->models.idToContainerID.find(_modelID);
-  if (worldIDIt == this->models.idToContainerID.end())
+  auto worldID = this->GetWorldOfModelImpl(_modelID);
+  if (worldID == INVALID_ENTITY_ID)
   {
     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);
+  auto world = this->worlds.at(worldID);
   const std::string fullName = ::sdf::JoinName(
       world->getName(),
       ::sdf::JoinName(model->getName(), bn->getName()));

dartsim/src/EntityManagementFeatures.hh

@@ -147,6 +147,12 @@ class EntityManagementFeatures :
 
   public: bool ModelRemoved(const Identity &_modelID) const override;
 
+  public: bool RemoveNestedModelByIndex(
+     const Identity &_modelID, std::size_t _modelIndex) override;
+
+  public: bool RemoveNestedModelByName(
+      const Identity &_modelID, const std::string &_modelName) override;
+
   // ----- Construct empty entities -----
   public: Identity ConstructEmptyWorld(
       const Identity &_engineID, const std::string &_name) override;