Skip to content

Commit

Permalink
Remove nested models (#230)
Browse files Browse the repository at this point in the history 
This adds a new feature, RemoveNestedModelFromModel, that allow users to remove a nested model via it's parent model entity. This PR also ensures that when a parent model is removed, all its nested models are removed as well.

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

Co-authored-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com>
  • Loading branch information
@azeey @adlarkin
azeey and adlarkin authored Mar 25, 2021
1 parent 3c2de8b commit f9641a5
Show file tree
Hide file tree
Showing 15 changed files with 495 additions and 68 deletions.
72 changes: 63 additions & 9 deletions dartsim/src/Base.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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};
}

Expand Down Expand Up @@ -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);
Expand All @@ -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;
Expand Down
103 changes: 75 additions & 28 deletions dartsim/src/EntityManagementFeatures.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand All @@ -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);
}

/////////////////////////////////////////////////
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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();
}

/////////////////////////////////////////////////
Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -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;
}
Expand All @@ -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;
}
Expand All @@ -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;
}
Expand All @@ -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)
Expand Down Expand Up @@ -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 =
Expand All @@ -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));
}
Expand All @@ -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()));
Expand Down
6 changes: 6 additions & 0 deletions dartsim/src/EntityManagementFeatures.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
Loading

0 comments on commit f9641a5

Please sign in to comment.