bullet-featherstone: Support nested models

bullet-featherstone/src/Base.hh

@@ -84,12 +84,14 @@
   Identity world;
   int indexInWorld;
   Eigen::Isometry3d baseInertiaToLinkFrame;
-  std::unique_ptr<btMultiBody> body;
+  std::shared_ptr<btMultiBody> body;
 
   std::vector<std::size_t> linkEntityIds;
   std::vector<std::size_t> jointEntityIds;
+  std::vector<std::size_t> nestedModelEntityIds;
   std::unordered_map<std::string, std::size_t> linkNameToEntityId;
   std::unordered_map<std::string, std::size_t> jointNameToEntityId;
+  std::unordered_map<std::string, std::size_t> nestedModelNameToEntityId;
 
   /// These are joints that connect this model to other models, e.g. fixed
   /// constraints.
@@ -99,7 +101,7 @@
     std::string _name,
     Identity _world,
     Eigen::Isometry3d _baseInertiaToLinkFrame,
-    std::unique_ptr<btMultiBody> _body)
+    std::shared_ptr<btMultiBody> _body)
     : name(std::move(_name)),
       world(std::move(_world)),
       baseInertiaToLinkFrame(_baseInertiaToLinkFrame),
@@ -273,14 +275,24 @@
     const auto id = this->GetNextEntity();
     auto world = std::make_shared<WorldInfo>(std::move(_worldInfo));
     this->worlds[id] = world;
-    return this->GenerateIdentity(id, world);
+    auto worldID = this->GenerateIdentity(id, world);
+
+    auto worldModel = std::make_shared<ModelInfo>(
+      world->name, worldID,
+      Eigen::Isometry3d::Identity(), nullptr);
+    this->models[id] = worldModel;
+    world->modelNameToEntityId[worldModel->name] = id;
+    worldModel->indexInWorld = -1;
+    world->modelIndexToEntityId[worldModel->indexInWorld] = id;
+
+    return worldID;
   }
 
   public: inline Identity AddModel(
     std::string _name,
     Identity _worldID,
     Eigen::Isometry3d _baseInertialToLinkFrame,
-    std::unique_ptr<btMultiBody> _body)
+    std::shared_ptr<btMultiBody> _body)
   {
     const auto id = this->GetNextEntity();
     auto model = std::make_shared<ModelInfo>(
@@ -292,6 +304,30 @@
     world->modelNameToEntityId[model->name] = id;
     model->indexInWorld = world->nextModelIndex++;
     world->modelIndexToEntityId[model->indexInWorld] = id;
+
+    auto worldModel = this->models.at(model->world);
+    worldModel->nestedModelEntityIds.push_back(id);
+    worldModel->nestedModelNameToEntityId[model->name] = id;
+
+    return this->GenerateIdentity(id, model);
+  }
+
+  public: inline Identity AddNestedModel(
+    std::string _name,
+    Identity _parentID,
+    Identity _worldID,
+    Eigen::Isometry3d _baseInertialToLinkFrame,
+    std::shared_ptr<btMultiBody> _body)
+  {
+    const auto id = this->GetNextEntity();
+    auto model = std::make_shared<ModelInfo>(
+      std::move(_name), std::move(_worldID),
+      std::move(_baseInertialToLinkFrame), std::move(_body));
+
+    this->models[id] = model;
+    const auto parentModel = this->models.at(_parentID);
+    parentModel->nestedModelEntityIds.push_back(id);
+    parentModel->nestedModelNameToEntityId[model->name] = id;
     return this->GenerateIdentity(id, model);
   }
 
@@ -303,13 +339,7 @@
 
     auto *model = this->ReferenceInterface<ModelInfo>(_linkInfo.model);
     model->linkNameToEntityId[link->name] = id;
-    if (link->indexInModel.has_value())
-    {
-      // We expect the links to be added in order
-      assert(static_cast<std::size_t>(*link->indexInModel + 1) ==
-             model->linkEntityIds.size());
-    }
-    else
+    if (!link->indexInModel.has_value())
     {
       // We are adding the root link. This means the model should not already
       // have a root link
@@ -357,6 +387,99 @@
     return this->GenerateIdentity(id, joint);
   }
 
+  public: bool RemoveModelImpl(const Identity &_parentID,
+                               const Identity &_modelID)
+  {
+    auto *model = this->ReferenceInterface<ModelInfo>(_modelID);
+    if (!model)
+      return false;
+
+    // Remove nested models
+    for (auto &nestedModelID : model->nestedModelEntityIds)
+    {
+      this->RemoveModelImpl(_modelID, this->GenerateIdentity(nestedModelID,
+                            this->models.at(nestedModelID)));
+    }
+    model->nestedModelEntityIds.clear();
+
+    // remove references in parent model or world model
+    auto parentModelIt = this->models.find(_parentID);
+    if (parentModelIt != this->models.end())
+    {
+      auto parentModel = parentModelIt->second;
+      auto nestedModelIt =
+          parentModel->nestedModelNameToEntityId.find(model->name);
+      if (nestedModelIt !=
+          parentModel->nestedModelNameToEntityId.end())
+      {
+        std::size_t nestedModelID = nestedModelIt->second;
+        parentModel->nestedModelNameToEntityId.erase(nestedModelIt);
+        parentModel->nestedModelEntityIds.erase(std::remove(
+            parentModel->nestedModelEntityIds.begin(),
+            parentModel->nestedModelEntityIds.end(), nestedModelID),
+            parentModel->nestedModelEntityIds.end());
+      }
+    }
+
+    // If nested, no need to remove multibody
+    // \todo(iche033) Remove links and joints in nested model
+    bool isNested =  this->worlds.find(_parentID) == this->worlds.end();
+    if (isNested)
+    {
+      return true;
+    }
+
+    // Remove model from world
+    auto *world = this->ReferenceInterface<WorldInfo>(model->world);
+    if (!world)
+      return false;
+    if (world->modelIndexToEntityId.erase(model->indexInWorld) == 0)
+    {
+      // The model has already been removed at some point
+      return false;
+    }
+    world->modelNameToEntityId.erase(model->name);
+
+    // Remove all constraints related to this model
+    for (const auto jointID : model->jointEntityIds)
+    {
+      const auto joint = this->joints.at(jointID);
+      if (joint->motor)
+      {
+        world->world->removeMultiBodyConstraint(joint->motor.get());
+      }
+      if (joint->fixedConstraint)
+      {
+        world->world->removeMultiBodyConstraint(joint->fixedConstraint.get());
+      }
+      if (joint->jointLimits)
+      {
+        world->world->removeMultiBodyConstraint(joint->jointLimits.get());
+      }
+      this->joints.erase(jointID);
+    }
+    // \todo(iche033) Remove external constraints related to this model
+    // (model->external_constraints) once this is supported
+
+    world->world->removeMultiBody(model->body.get());
+    for (const auto linkID : model->linkEntityIds)
+    {
+      const auto &link = this->links.at(linkID);
+      if (link->collider)
+      {
+        world->world->removeCollisionObject(link->collider.get());
+        for (const auto shapeID : link->collisionEntityIds)
+          this->collisions.erase(shapeID);
+      }
+
+      this->links.erase(linkID);
+    }
+
+    this->models.erase(_modelID);
+
+    return true;
+  }
+
   public: ~Base() override {
     // The order of destruction between meshesGImpact and triangleMeshes is
     // important.

bullet-featherstone/src/EntityManagementFeatures.cc

@@ -206,53 +206,16 @@
 bool EntityManagementFeatures::RemoveModel(const Identity &_modelID)
 {
   auto *model = this->ReferenceInterface<ModelInfo>(_modelID);
-  auto *world = this->ReferenceInterface<WorldInfo>(model->world);
-  if (world->modelIndexToEntityId.erase(model->indexInWorld) == 0)
-  {
-    // The model has already been removed at some point.
+  if (!model)
     return false;
-  }
-
-  world->modelNameToEntityId.erase(model->name);
-
-  // Remove all constraints related to this model
-  for (auto constraint_index : model->external_constraints)
-  {
-    const auto joint = this->joints.at(constraint_index);
-    const auto &constraint =
-      std::get<std::unique_ptr<btMultiBodyConstraint>>(joint->identifier);
-    world->world->removeMultiBodyConstraint(constraint.get());
-    this->joints.erase(constraint_index);
-  }
-
-  world->world->removeMultiBody(model->body.get());
-  for (const auto linkID : model->linkEntityIds)
-  {
-    const auto &link = this->links.at(linkID);
-    if (link->collider)
-    {
-      world->world->removeCollisionObject(link->collider.get());
-      for (const auto shapeID : link->collisionEntityIds)
-        this->collisions.erase(shapeID);
-    }
-
-    this->links.erase(linkID);
-  }
-
-  for (const auto jointID : model->jointEntityIds)
-    this->joints.erase(jointID);
-
-  this->models.erase(_modelID);
-  return true;
+  return this->RemoveModelImpl(model->world, _modelID);
 }
 
 /////////////////////////////////////////////////
 bool EntityManagementFeatures::ModelRemoved(
     const Identity &_modelID) const
 {
-  auto *model = this->ReferenceInterface<ModelInfo>(_modelID);
-  auto *world = this->ReferenceInterface<WorldInfo>(model->world);
-  return world->modelIndexToEntityId.count(model->indexInWorld) == 0;
+  return this->models.find(_modelID) == this->models.end();
 }
 
 /////////////////////////////////////////////////
@@ -283,6 +246,55 @@
     this->GenerateIdentity(it->second, this->models.at(it->second)));
 }
 
+/////////////////////////////////////////////////
+bool EntityManagementFeatures::RemoveNestedModelByIndex(
+    const Identity &_modelID, std::size_t _nestedModelIndex)
+{
+  auto *model = this->ReferenceInterface<ModelInfo>(_modelID);
+  if (!model)
+    return false;
+  if (_nestedModelIndex >= model->nestedModelEntityIds.size())
+    return false;
+  const auto nestedModelID = model->nestedModelEntityIds[_nestedModelIndex];
+
+  auto modelIt =  this->models.find(nestedModelID);
+  if (modelIt != this->models.end())
+  {
+    return RemoveModelImpl(_modelID,
+        this->GenerateIdentity(modelIt->first, modelIt->second));
+  }
+  return false;
+}
+
+/////////////////////////////////////////////////
+bool EntityManagementFeatures::RemoveNestedModelByName(const Identity &_modelID,
+    const std::string &_modelName)
+{
+  auto *model = this->ReferenceInterface<ModelInfo>(_modelID);
+  if (!model)
+    return false;
+
+  unsigned int nestedModelID = 0u;
+  auto nestedModelIt =  model->nestedModelNameToEntityId.find(_modelName);
+  if (nestedModelIt != model->nestedModelNameToEntityId.end())
+  {
+    nestedModelID = nestedModelIt->second;
+  }
+  else
+  {
+    return false;
+  }
+
+  auto modelIt =  this->models.find(nestedModelID);
+  if (modelIt != this->models.end())
+  {
+    return RemoveModelImpl(_modelID,
+        this->GenerateIdentity(modelIt->first, modelIt->second));
+  }
+  return false;
+}
+
+
 /////////////////////////////////////////////////
 const std::string &EntityManagementFeatures::GetEngineName(
   const Identity &) const
@@ -359,9 +371,15 @@
 
 /////////////////////////////////////////////////
 std::size_t EntityManagementFeatures::GetModelCount(
-    const Identity &) const
+    const Identity &_worldID) const
 {
-  return this->models.size();
+  // Get world model and return its nested model count
+  auto modelIt = this->models.find(_worldID);
+  if (modelIt != this->models.end())
+  {
+    return modelIt->second->nestedModelEntityIds.size();
+  }
+  return 0u;
 }
 
 /////////////////////////////////////////////////
@@ -414,6 +432,58 @@
   return this->ReferenceInterface<ModelInfo>(_modelID)->world;
 }
 
+/////////////////////////////////////////////////
+std::size_t EntityManagementFeatures::GetNestedModelCount(
+    const Identity &_modelID) const
+{
+  return this->ReferenceInterface<ModelInfo>(
+      _modelID)->nestedModelEntityIds.size();
+}
+
+/////////////////////////////////////////////////
+Identity EntityManagementFeatures::GetNestedModel(
+    const Identity &_modelID, const std::size_t _modelIndex) const
+{
+  const auto modelInfo = this->ReferenceInterface<ModelInfo>(_modelID);
+  if (_modelIndex >= modelInfo->nestedModelEntityIds.size())
+  {
+    return this->GenerateInvalidId();
+  }
+
+  const auto nestedModelID = modelInfo->nestedModelEntityIds[_modelIndex];
+
+  // If the model doesn't exist in "models", it means the containing entity has
+  // been removed.
+  if (this->models.find(nestedModelID) != this->models.end())
+  {
+    return this->GenerateIdentity(nestedModelID,
+                                  this->models.at(nestedModelID));
+  }
+  else
+  {
+    return this->GenerateInvalidId();
+  }
+}
+
+/////////////////////////////////////////////////
+Identity EntityManagementFeatures::GetNestedModel(
+    const Identity &_modelID, const std::string &_modelName) const
+{
+  const auto modelInfo = this->ReferenceInterface<ModelInfo>(_modelID);
+  auto modelIt = modelInfo->nestedModelNameToEntityId.find(_modelName);
+  if (modelIt == modelInfo->nestedModelNameToEntityId.end())
+    return this->GenerateInvalidId();
+  auto nestedModelID = modelIt->second;
+  return this->GenerateIdentity(nestedModelID,
+      this->models.at(nestedModelID));
+}
+
+/////////////////////////////////////////////////
+Identity EntityManagementFeatures::GetWorldModel(const Identity &_worldID) const
+{
+  return this->GenerateIdentity(_worldID, this->models.at(_worldID));
+}
+
 }  // namespace bullet_featherstone
 }  // namespace physics
 }  // namespace gz