Skip to content

Commit

Permalink
bullet-featherstone: Support nested models (#574)
Browse files Browse the repository at this point in the history 
Extended bullet-featurestone implementation to
support these features related to nested models:

* ConstructSdfJoint (limited to creating fixed joints
  to the world)
* ConstructSdfNestedModel
* GetNestedModelFromModel
* WorldModelFeature
* RemoveEntities

The nested models are currently added to a single
multibody since bullet-featherstone does not currently
support joints between two different multibodies.
The nested model keeps a shared ptr to the root link's
btMultibody.

With the addition of the above features, more common
tests are now being run using bullet-feathersone plugin.

Closes #546, #547.

Signed-off-by: Shameek Ganguly <shameekarcanesphinx@gmail.com>
Signed-off-by: Steve Peters <scpeters@openrobotics.org>
Signed-off-by: Ian Chen <ichen@openrobotics.org>
Co-authored-by: Shameek Ganguly <shameekarcanesphinx@gmail.com>
Co-authored-by: Steve Peters <scpeters@openrobotics.org>
  • Loading branch information
@iche033 @shameekganguly @scpeters
3 people authored Mar 6, 2024
1 parent ea90ada commit 18e0dad
Show file tree
Hide file tree
Showing 12 changed files with 1,065 additions and 264 deletions.
145 changes: 134 additions & 11 deletions bullet-featherstone/src/Base.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -84,12 +84,14 @@ struct ModelInfo
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.
Expand All @@ -99,7 +101,7 @@ struct ModelInfo
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),
Expand Down Expand Up @@ -273,14 +275,24 @@ class Base : public Implements3d<FeatureList<Feature>>
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>(
Expand All @@ -292,6 +304,30 @@ class Base : public Implements3d<FeatureList<Feature>>
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);
}

Expand All @@ -303,13 +339,7 @@ class Base : public Implements3d<FeatureList<Feature>>

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
Expand Down Expand Up @@ -357,6 +387,99 @@ class Base : public Implements3d<FeatureList<Feature>>
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.
Expand Down
154 changes: 112 additions & 42 deletions bullet-featherstone/src/EntityManagementFeatures.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -206,53 +206,16 @@ Identity EntityManagementFeatures::ConstructEmptyWorld(
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();
}

/////////////////////////////////////////////////
Expand Down Expand Up @@ -283,6 +246,55 @@ bool EntityManagementFeatures::RemoveModelByName(
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
Expand Down Expand Up @@ -359,9 +371,15 @@ Identity EntityManagementFeatures::GetEngineOfWorld(

/////////////////////////////////////////////////
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;
}

/////////////////////////////////////////////////
Expand Down Expand Up @@ -414,6 +432,58 @@ Identity EntityManagementFeatures::GetWorldOfModel(
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
Loading

0 comments on commit 18e0dad

Please sign in to comment.