From 4b4731e8513145c6677dbd5f68408636730c3640 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 25 Jul 2022 21:53:33 +0800 Subject: [PATCH 1/2] Fix the entity management features for bullet classic Signed-off-by: Michael X. Grey --- bullet/src/Base.hh | 36 +++- bullet/src/EntityManagementFeatures.cc | 263 ++++++++++++++++++------- bullet/src/EntityManagementFeatures.hh | 16 +- bullet/src/SDFFeatures.cc | 2 +- 4 files changed, 228 insertions(+), 89 deletions(-) diff --git a/bullet/src/Base.hh b/bullet/src/Base.hh index c4ee8e98e..82fb14896 100644 --- a/bullet/src/Base.hh +++ b/bullet/src/Base.hh @@ -57,6 +57,8 @@ struct WorldInfo std::shared_ptr broadphase; std::shared_ptr solver; std::shared_ptr world; + std::vector models = {}; + std::unordered_map modelsByName = {}; }; struct ModelInfo @@ -67,6 +69,9 @@ struct ModelInfo bool fixed; math::Pose3d pose; std::vector links = {}; + std::unordered_map linksByName = {}; + std::vector joints = {}; + std::unordered_map jointsByName = {}; }; struct LinkInfo @@ -81,6 +86,7 @@ struct LinkInfo std::shared_ptr motionState; std::shared_ptr collisionShape; std::shared_ptr link; + std::vector shapes = {}; }; struct CollisionInfo @@ -201,7 +207,10 @@ class Base : public Implements3d> public: inline Identity AddWorld(WorldInfo _worldInfo) { const auto id = this->GetNextEntity(); - this->worlds[id] = std::make_shared(_worldInfo); + const auto world = std::make_shared(_worldInfo); + this->worlds[id] = world; + this->worldsByName[world->name] = id; + this->worldsByIndex.push_back(id); this->childIdToParentId.insert({id, -1}); return this->GenerateIdentity(id, this->worlds.at(id)); } @@ -209,7 +218,11 @@ class Base : public Implements3d> public: inline Identity AddModel(std::size_t _worldId, ModelInfo _modelInfo) { const auto id = this->GetNextEntity(); - this->models[id] = std::make_shared(_modelInfo); + const auto model = std::make_shared(_modelInfo); + this->models[id] = model; + const auto world = this->worlds.at(_worldId); + world->models.push_back(id); + world->modelsByName[model->name] = id; this->childIdToParentId.insert({id, _worldId}); return this->GenerateIdentity(id, this->models.at(id)); } @@ -217,10 +230,12 @@ class Base : public Implements3d> public: inline Identity AddLink(std::size_t _modelId, LinkInfo _linkInfo) { const auto id = this->GetNextEntity(); - this->links[id] = std::make_shared(_linkInfo); + const auto link = std::make_shared(_linkInfo); + this->links[id] = link; auto model = this->models.at(_linkInfo.model); model->links.push_back(id); + model->linksByName[link->name] = id; this->childIdToParentId.insert({id, _modelId}); return this->GenerateIdentity(id, this->links.at(id)); @@ -230,14 +245,23 @@ class Base : public Implements3d> { const auto id = this->GetNextEntity(); this->collisions[id] = std::make_shared(_collisionInfo); + this->links.at(_linkId)->shapes.push_back(id); this->childIdToParentId.insert({id, _linkId}); return this->GenerateIdentity(id, this->collisions.at(id)); } - public: inline Identity AddJoint(JointInfo _jointInfo) + public: inline Identity AddJoint( + std::optional _modelId, JointInfo _jointInfo) { const auto id = this->GetNextEntity(); - this->joints[id] = std::make_shared(_jointInfo); + const auto joint = std::make_shared(_jointInfo); + this->joints[id] = joint; + if (_modelId.has_value()) + { + const auto model = this->models.at(*_modelId); + model->joints.push_back(id); + model->jointsByName[joint->name] = id; + } return this->GenerateIdentity(id, this->joints.at(id)); } @@ -249,6 +273,8 @@ class Base : public Implements3d> public: using JointInfoPtr = std::shared_ptr; public: std::unordered_map worlds; + public: std::vector worldsByIndex; + public: std::unordered_map worldsByName; public: std::unordered_map models; public: std::unordered_map links; public: std::unordered_map collisions; diff --git a/bullet/src/EntityManagementFeatures.cc b/bullet/src/EntityManagementFeatures.cc index cf549d20c..27dade540 100644 --- a/bullet/src/EntityManagementFeatures.cc +++ b/bullet/src/EntityManagementFeatures.cc @@ -185,27 +185,48 @@ std::size_t EntityManagementFeatures::GetWorldCount(const Identity &) const } Identity EntityManagementFeatures::GetWorld( - const Identity &, std::size_t) const + const Identity &, const std::size_t _worldIndex) const { - return this->GenerateIdentity(0); + if (_worldIndex >= this->worldsByIndex.size()) + return this->GenerateInvalidId(); + + const auto worldID = this->worldsByIndex[_worldIndex]; + return this->GenerateIdentity(worldID, this->worlds.at(worldID)); } Identity EntityManagementFeatures::GetWorld( - const Identity &, const std::string &) const + const Identity &, const std::string &_name) const { - return this->GenerateIdentity(0); + const auto it = this->worldsByName.find(_name); + if (it == this->worldsByName.end()) + return this->GenerateInvalidId(); + + const auto id = it->second; + return this->GenerateIdentity(id, this->worlds.at(id)); } const std::string &EntityManagementFeatures::GetWorldName( const Identity &_worldID) const { - static const std::string worldName = this->worlds.at(_worldID)->name; - return worldName; + return this->ReferenceInterface(_worldID)->name; } -std::size_t EntityManagementFeatures::GetWorldIndex(const Identity &) const +std::size_t EntityManagementFeatures::GetWorldIndex( + const Identity &_worldID) const { - return 0; + const auto it = std::find( + this->worldsByIndex.begin(), + this->worldsByIndex.end(), + _worldID.id); + + if (it == this->worldsByIndex.end()) + { + throw std::runtime_error( + "World [" + std::to_string(_worldID.id) + "] cannot be found in engine " + "with " + std::to_string(this->worlds.size()) + " worlds"); + } + + return *it; } Identity EntityManagementFeatures::GetEngineOfWorld(const Identity &) const @@ -214,21 +235,29 @@ Identity EntityManagementFeatures::GetEngineOfWorld(const Identity &) const } std::size_t EntityManagementFeatures::GetModelCount( - const Identity &) const + const Identity &_worldId) const { - return 0; + return this->ReferenceInterface(_worldId)->models.size(); } Identity EntityManagementFeatures::GetModel( - const Identity &, std::size_t) const + const Identity &_worldID, const std::size_t index) const { - return this->GenerateIdentity(0); + const auto modelID = + this->ReferenceInterface(_worldID)->models.at(index); + return this->GenerateIdentity(modelID, this->models.at(modelID)); } Identity EntityManagementFeatures::GetModel( - const Identity &, const std::string &) const + const Identity &_worldID, const std::string &_name) const { - return this->GenerateIdentity(0); + const auto world = this->ReferenceInterface(_worldID); + const auto it = world->modelsByName.find(_name); + if (it == world->modelsByName.end()) + return this->GenerateInvalidId(); + + const auto id = it->second; + return this->GenerateIdentity(id, this->models.at(id)); } const std::string &EntityManagementFeatures::GetModelName( @@ -238,135 +267,223 @@ const std::string &EntityManagementFeatures::GetModelName( return modelName; } -std::size_t EntityManagementFeatures::GetModelIndex(const Identity &) const -{ - return 0; -} - -Identity EntityManagementFeatures::GetWorldOfModel(const Identity &) const +std::size_t EntityManagementFeatures::GetModelIndex( + const Identity &_modelID) const { - return this->GenerateIdentity(0); -} + const auto model = this->ReferenceInterface(_modelID); + const auto world = this->ReferenceInterface(model->world); + const auto it = + std::find(world->models.begin(), world->models.end(), _modelID.id); -std::size_t EntityManagementFeatures::GetNestedModelCount( - const Identity &) const -{ - return 0; -} + if (it == world->models.end()) + { + throw std::runtime_error( + "Model [" + std::to_string(_modelID.id) + "] cannot be found in " + "world [" + std::to_string(model->world.id) + "]"); + } -Identity EntityManagementFeatures::GetNestedModel( - const Identity &, std::size_t ) const -{ - return this->GenerateIdentity(0); + return *it; } -Identity EntityManagementFeatures::GetNestedModel( - const Identity &, const std::string &) const +Identity EntityManagementFeatures::GetWorldOfModel( + const Identity &_modelID) const { - return this->GenerateIdentity(0); + return this->ReferenceInterface(_modelID)->world; } -std::size_t EntityManagementFeatures::GetLinkCount(const Identity &) const +std::size_t EntityManagementFeatures::GetLinkCount( + const Identity &_modelID) const { - return 0; + return this->ReferenceInterface(_modelID)->links.size(); } Identity EntityManagementFeatures::GetLink( - const Identity &, std::size_t) const + const Identity &_modelID, std::size_t _linkIndex) const { - return this->GenerateIdentity(0); + const auto model = this->ReferenceInterface(_modelID); + if (_linkIndex < model->links.size()) + { + const auto id = model->links[_linkIndex]; + const auto link = this->links.at(id); + return this->GenerateIdentity(id, link); + } + + return this->GenerateInvalidId(); } Identity EntityManagementFeatures::GetLink( - const Identity &, const std::string &) const + const Identity &_modelID, const std::string &_name) const { - return this->GenerateIdentity(0); + const auto model = this->ReferenceInterface(_modelID); + const auto it = model->linksByName.find(_name); + if (it == model->linksByName.end()) + return this->GenerateInvalidId(); + + const auto linkID = it->second; + return this->GenerateIdentity(linkID, this->links.at(linkID)); } -std::size_t EntityManagementFeatures::GetJointCount(const Identity &) const +std::size_t EntityManagementFeatures::GetJointCount( + const Identity &_modelID) const { - return 0; + return this->ReferenceInterface(_modelID)->joints.size(); } Identity EntityManagementFeatures::GetJoint( - const Identity &, std::size_t ) const + const Identity &_modelID, std::size_t _jointIndex) const { - return this->GenerateIdentity(0); + const auto model = this->ReferenceInterface(_modelID); + if (_jointIndex < model->joints.size()) + { + const auto id = model->joints[_jointIndex]; + const auto joint = this->joints.at(id); + return this->GenerateIdentity(id, joint); + } + + return this->GenerateInvalidId(); } Identity EntityManagementFeatures::GetJoint( - const Identity &, const std::string &) const + const Identity &_modelID, const std::string &_name) const { - return this->GenerateIdentity(0); + const auto model = this->ReferenceInterface(_modelID); + const auto it = model->jointsByName.find(_name); + if (it == model->jointsByName.end()) + return this->GenerateInvalidId(); + + const auto jointID = it->second; + return this->GenerateIdentity(jointID, this->joints.at(jointID)); } const std::string &EntityManagementFeatures::GetLinkName( - const Identity &) const + const Identity &_linkID) const { - static const std::string linkName = "bulletLink"; - return linkName; + return this->ReferenceInterface(_linkID)->name; } -std::size_t EntityManagementFeatures::GetLinkIndex(const Identity &) const +std::size_t EntityManagementFeatures::GetLinkIndex( + const Identity &_linkID) const { - return 0; + const auto link = this->ReferenceInterface(_linkID); + const auto model = this->ReferenceInterface(link->model); + const auto it = + std::find(model->links.begin(), model->links.end(), _linkID.id); + + if (it == model->links.end()) + { + throw std::runtime_error( + "Link [" + std::to_string(_linkID.id) + "] cannot be found in " + "model [" + std::to_string(link->model.id) + "]"); + } + + return *it; } -Identity EntityManagementFeatures::GetModelOfLink(const Identity &) const +Identity EntityManagementFeatures::GetModelOfLink( + const Identity &_linkID) const { - return this->GenerateIdentity(0); + return this->ReferenceInterface(_linkID)->model; } -std::size_t EntityManagementFeatures::GetShapeCount(const Identity &) const +std::size_t EntityManagementFeatures::GetShapeCount( + const Identity &_linkID) const { - return 0; + return this->ReferenceInterface(_linkID)->shapes.size(); } Identity EntityManagementFeatures::GetShape( - const Identity &, std::size_t) const + const Identity &_linkID, std::size_t shapeIndex) const { - return this->GenerateIdentity(0); + const auto link = this->ReferenceInterface(_linkID); + if (link->shapes.size() >= shapeIndex) + { + throw std::runtime_error( + "Link [" + std::to_string(_linkID.id) + "] cannot be found in " + "model [" + std::to_string(link->model.id) + "]"); + } + + const auto shapeID = link->shapes[shapeIndex]; + return this->GenerateIdentity(shapeID, this->collisions.at(shapeID)); } Identity EntityManagementFeatures::GetShape( - const Identity &, const std::string &) const + const Identity &_linkID, const std::string &_name) const { - return this->GenerateIdentity(0); + const auto link = this->ReferenceInterface(_linkID); + const auto it = std::find_if( + link->shapes.begin(), + link->shapes.end(), + [this, &_name](const auto& shapeID) + { + return _name == this->collisions.at(shapeID)->name; + }); + + if (it == link->shapes.end()) + return this->GenerateInvalidId(); + + return this->GenerateIdentity(*it, this->collisions.at(*it)); } const std::string &EntityManagementFeatures::GetJointName( - const Identity &) const + const Identity &_jointID) const { - static const std::string jointName = "bulletJoint"; - return jointName; + return this->ReferenceInterface(_jointID)->name; } -std::size_t EntityManagementFeatures::GetJointIndex(const Identity &) const +std::size_t EntityManagementFeatures::GetJointIndex( + const Identity &_jointID) const { - return 0; + const auto joint = this->ReferenceInterface(_jointID); + const auto parentLink = this->links.at(joint->parentLinkId); + const auto model = this->ReferenceInterface(parentLink->model); + const auto it = + std::find(model->joints.begin(), model->joints.end(), _jointID.id); + if (it == model->joints.end()) + { + throw std::runtime_error( + "Joint [" + std::to_string(_jointID.id) + "] cannot be found in " + "model [" + std::to_string(parentLink->model.id) + "]"); + } + + return *it; } -Identity EntityManagementFeatures::GetModelOfJoint(const Identity &) const +Identity EntityManagementFeatures::GetModelOfJoint( + const Identity &_jointID) const { - return this->GenerateIdentity(0); + const auto joint = this->ReferenceInterface(_jointID); + const auto parentLink = this->links.at(joint->parentLinkId); + return parentLink->model; } const std::string &EntityManagementFeatures::GetShapeName( - const Identity &) const + const Identity &_shapeID) const { - static const std::string shapeName = "bulletShape"; - return shapeName; + return this->ReferenceInterface(_shapeID)->name; } -std::size_t EntityManagementFeatures::GetShapeIndex(const Identity &) const +std::size_t EntityManagementFeatures::GetShapeIndex( + const Identity &_shapeID) const { - return 0; + const auto shape = this->ReferenceInterface(_shapeID); + const auto link = this->ReferenceInterface(shape->link); + const auto it = + std::find(link->shapes.begin(), link->shapes.end(), _shapeID.id); + if (it == link->shapes.end()) + { + throw std::runtime_error( + "Shape [" + std::to_string(_shapeID.id) + "] cannot be found in " + "link [" + std::to_string(shape->link.id) + "]"); + } + + return *it; } -Identity EntityManagementFeatures::GetLinkOfShape(const Identity &) const +Identity EntityManagementFeatures::GetLinkOfShape( + const Identity &_shapeID) const { - return this->GenerateIdentity(0); + return this->ReferenceInterface(_shapeID)->link; } } // namespace bullet } // namespace physics diff --git a/bullet/src/EntityManagementFeatures.hh b/bullet/src/EntityManagementFeatures.hh index 51eac7175..146bb90c6 100644 --- a/bullet/src/EntityManagementFeatures.hh +++ b/bullet/src/EntityManagementFeatures.hh @@ -32,7 +32,12 @@ namespace physics { namespace bullet { struct EntityManagementFeatureList : gz::physics::FeatureList< - GetEntities, + GetEngineInfo, + GetWorldFromEngine, + GetModelFromWorld, + GetLinkFromModel, + GetJointFromModel, + GetShapeFromLink, RemoveModelFromWorld, ConstructEmptyWorldFeature > { }; @@ -93,15 +98,6 @@ 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( diff --git a/bullet/src/SDFFeatures.cc b/bullet/src/SDFFeatures.cc index d86b686e8..f11e9632f 100644 --- a/bullet/src/SDFFeatures.cc +++ b/bullet/src/SDFFeatures.cc @@ -460,7 +460,7 @@ Identity SDFFeatures::ConstructSdfJoint( // Generate an identity for it and return it auto identity = - this->AddJoint({_sdfJoint.Name(), joint, childId, parentId, + this->AddJoint(_modelID.id, {_sdfJoint.Name(), joint, childId, parentId, static_cast(type), axis}); return identity; } From db252423235210ee6a6f9b22756c56a38c1c3c0d Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 25 Jul 2022 22:16:26 +0800 Subject: [PATCH 2/2] Fix model removal for bullet classic Signed-off-by: Michael X. Grey --- bullet/src/Base.hh | 57 +----------- bullet/src/EntityManagementFeatures.cc | 115 +++++++------------------ bullet/src/SDFFeatures.cc | 2 +- 3 files changed, 32 insertions(+), 142 deletions(-) diff --git a/bullet/src/Base.hh b/bullet/src/Base.hh index 82fb14896..b3a98a175 100644 --- a/bullet/src/Base.hh +++ b/bullet/src/Base.hh @@ -160,50 +160,6 @@ class Base : public Implements3d> return this->GenerateIdentity(0); } - public: inline std::size_t idToIndexInContainer(std::size_t _id) const - { - auto it = this->childIdToParentId.find(_id); - if (it != this->childIdToParentId.end()) - { - std::size_t index = 0; - for (const auto &pair : this->childIdToParentId) - { - if (pair.first == _id && pair.second == it->second) - { - return index; - } - else if (pair.second == it->second) - { - ++index; - } - } - } - // return invalid index if not found in id map - return -1; - } - - public: inline std::size_t indexInContainerToId( - const std::size_t _containerId, const std::size_t _index) const - { - std::size_t counter = 0; - auto it = this->childIdToParentId.begin(); - - while (counter <= _index && it != this->childIdToParentId.end()) - { - if (it->second == _containerId && counter == _index) - { - return it->first; - } - else if (it->second == _containerId) - { - ++counter; - } - ++it; - } - // return invalid id if entity not found - return -1; - } - public: inline Identity AddWorld(WorldInfo _worldInfo) { const auto id = this->GetNextEntity(); @@ -211,7 +167,6 @@ class Base : public Implements3d> this->worlds[id] = world; this->worldsByName[world->name] = id; this->worldsByIndex.push_back(id); - this->childIdToParentId.insert({id, -1}); return this->GenerateIdentity(id, this->worlds.at(id)); } @@ -223,21 +178,18 @@ class Base : public Implements3d> const auto world = this->worlds.at(_worldId); world->models.push_back(id); world->modelsByName[model->name] = id; - this->childIdToParentId.insert({id, _worldId}); return this->GenerateIdentity(id, this->models.at(id)); } - public: inline Identity AddLink(std::size_t _modelId, LinkInfo _linkInfo) + public: inline Identity AddLink(LinkInfo _linkInfo) { const auto id = this->GetNextEntity(); const auto link = std::make_shared(_linkInfo); this->links[id] = link; - auto model = this->models.at(_linkInfo.model); + auto model = this->models.at(link->model); model->links.push_back(id); model->linksByName[link->name] = id; - - this->childIdToParentId.insert({id, _modelId}); return this->GenerateIdentity(id, this->links.at(id)); } public: inline Identity AddCollision( @@ -246,7 +198,6 @@ class Base : public Implements3d> const auto id = this->GetNextEntity(); this->collisions[id] = std::make_shared(_collisionInfo); this->links.at(_linkId)->shapes.push_back(id); - this->childIdToParentId.insert({id, _linkId}); return this->GenerateIdentity(id, this->collisions.at(id)); } @@ -279,10 +230,6 @@ class Base : public Implements3d> public: std::unordered_map links; public: std::unordered_map collisions; public: std::unordered_map joints; - - // childIdToParentId needs to be an ordered map so this iteration proceeds - // in ascending order of the keys of that map. Do not change. - public: std::map childIdToParentId; }; } // namespace bullet diff --git a/bullet/src/EntityManagementFeatures.cc b/bullet/src/EntityManagementFeatures.cc index 27dade540..0f91a74e1 100644 --- a/bullet/src/EntityManagementFeatures.cc +++ b/bullet/src/EntityManagementFeatures.cc @@ -56,16 +56,32 @@ Identity EntityManagementFeatures::ConstructEmptyWorld( ///////////////////////////////////////////////// bool EntityManagementFeatures::RemoveModel(const Identity &_modelID) { - // Check if the model exists - if (this->models.find(_modelID.id) == this->models.end()) + const auto model = this->ReferenceInterface(_modelID); + auto worldID = model->world; + auto bulletWorld = this->worlds.at(model->world)->world; + + // Clean up joints, this section considers both links in the joint + // are part of the same world + for (const auto jointID : model->joints) { - return false; + const auto joint = this->joints.at(jointID); + bulletWorld->removeConstraint(joint->joint.get()); + this->joints.erase(jointID); } - auto worldID = this->models.at(_modelID)->world; - auto modelIndex = idToIndexInContainer(_modelID); + for (const auto linkID : model->links) + { + const auto link = this->links.at(linkID); + for (const auto shapeID : link->shapes) + this->collisions.erase(shapeID); - return this->RemoveModelByIndex(worldID, modelIndex); + bulletWorld->removeRigidBody(link->link.get()); + this->links.erase(linkID); + } + + // Clean up model + this->models.erase(_modelID.id); + return true; } bool EntityManagementFeatures::ModelRemoved( @@ -77,95 +93,22 @@ bool EntityManagementFeatures::ModelRemoved( bool EntityManagementFeatures::RemoveModelByIndex( const Identity & _worldID, std::size_t _modelIndex) { + const auto modelID = this->GetModel(_worldID, _modelIndex); // Check if the model exists - auto _modelEntity = indexInContainerToId(_worldID, _modelIndex); - if (this->models.find(_modelEntity) == this->models.end()) - { + if (!modelID) return false; - } - - auto model = this->models.at(_modelEntity); - auto bulletWorld = this->worlds.at(model->world)->world; - // Clean up joints, this section considers both links in the joint - // are part of the same world - auto joint_it = this->joints.begin(); - while (joint_it != this->joints.end()) - { - const auto &jointInfo = joint_it->second; - const auto &childLinkInfo = this->links[jointInfo->childLinkId]; - if (childLinkInfo->model.id == _modelIndex) - { - bulletWorld->removeConstraint(jointInfo->joint.get()); - this->childIdToParentId.erase(joint_it->first); - joint_it = this->joints.erase(joint_it); - continue; - } - joint_it++; - } - - // Clean up collisions - auto collision_it = this->collisions.begin(); - while (collision_it != this->collisions.end()) - { - const auto &collisionInfo = collision_it->second; - if (collisionInfo->model.id == _modelIndex) - { - this->childIdToParentId.erase(collision_it->first); - collision_it = this->collisions.erase(collision_it); - continue; - } - collision_it++; - } - - // Clean up links - auto it = this->links.begin(); - while (it != this->links.end()) - { - const auto &linkInfo = it->second; - - if (linkInfo->model.id == _modelIndex) - { - bulletWorld->removeRigidBody(linkInfo->link.get()); - this->childIdToParentId.erase(it->first); - it = this->links.erase(it); - continue; - } - it++; - } - - // Clean up model - this->models.erase(_modelEntity); - this->childIdToParentId.erase(_modelIndex); - - return true; + return this->RemoveModel(modelID); } bool EntityManagementFeatures::RemoveModelByName( const Identity & _worldID, const std::string & _modelName ) { - // Check if there is a model with the requested name - bool found = false; - size_t entity = 0; - // We need a link to model relationship - for (const auto &model : this->models) - { - const auto &modelInfo = model.second; - if (modelInfo->name == _modelName) - { - found = true; - entity = model.first; - break; - } - } - - if (found) - { - auto modelIndex = idToIndexInContainer(entity); - return this->RemoveModelByIndex(_worldID, modelIndex); - } + const auto modelID = this->GetModel(_worldID, _modelName); + if (!modelID) + return false; - return false; + return this->RemoveModel(modelID); } const std::string &EntityManagementFeatures::GetEngineName( const Identity &) const diff --git a/bullet/src/SDFFeatures.cc b/bullet/src/SDFFeatures.cc index f11e9632f..4fced5fd7 100644 --- a/bullet/src/SDFFeatures.cc +++ b/bullet/src/SDFFeatures.cc @@ -188,7 +188,7 @@ Identity SDFFeatures::ConstructSdfLink( // Generate an identity for it const auto linkIdentity = - this->AddLink(_modelID, {name, _modelID, pose, inertialPose, + this->AddLink({name, _modelID, pose, inertialPose, mass, linkInertiaDiag, myMotionState, collisionShape, body}); // Create associated collisions to this model