Skip to content

Commit

Permalink
dartsim: Add support for joints in worlds (#501)
Browse files Browse the repository at this point in the history 
As of gazebosim/sdformat#1117, SDFormat supports joints under world (//world/joint in xpath). But in gz-physics, Entities such as joints are only available in a Model. To support world joints, instead of creating a new type of entity, we have chosen to implement a scheme where the world behaves like a model via the WorldModelFeature feature. Given a world, `w`, we can do `model = w->GetWorldModel()` to get the model proxy. We can then use any of the APIs supported by the features available in the physics engine, e.g., `model->GetJoint("j1")` where "j1" is a joint directly under <world> in the SDFormat.

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>
Co-authored-by: Marco A. Gutierrez <marcogg@marcogg.com>
  • Loading branch information
@azeey @marcoag
azeey and marcoag authored May 19, 2023
1 parent 63a7486 commit c80d4d7
Show file tree
Hide file tree
Showing 10 changed files with 595 additions and 146 deletions.
93 changes: 67 additions & 26 deletions dartsim/src/Base.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -161,6 +161,16 @@ struct EntityStorage
return idToObject.at(_id);
}

const std::optional<Value1> MaybeAt(const std::size_t _id) const
{
auto it = this->idToObject.find(_id);
if (it != this->idToObject.end())
{
return it->second;
}
return std::nullopt;
}

Value1 &at(const Key2 &_key)
{
return idToObject.at(objectToID.at(_key));
Expand Down Expand Up @@ -293,6 +303,12 @@ class Base : public Implements3d<FeatureList<Feature>>
this->worlds.AddEntity(id, _world, _name, 0);

this->frames[id] = dart::dynamics::Frame::World();
auto model = dart::dynamics::Skeleton::create("");

auto modelInfo = std::make_shared<ModelInfo>();
modelInfo->model = model;
modelInfo->localName = _name;
this->modelProxiesToWorld.AddEntity(id, modelInfo, _world, 0);

return id;
}
Expand All @@ -310,6 +326,8 @@ class Base : public Implements3d<FeatureList<Feature>>
{
this->frames[id] = _info.frame.get();
}
auto modelProxy = this->modelProxiesToWorld.at(_worldID);
modelProxy->nestedModels.push_back(id);

return std::forward_as_tuple(id, *entry);
}
Expand All @@ -327,7 +345,7 @@ class Base : public Implements3d<FeatureList<Feature>>
this->models.AddEntity(id, entry, _info.model, _parentID);
this->frames[id] = _info.frame.get();

auto parentModelInfo = this->models.at(_parentID);
auto parentModelInfo = this->GetModelInfo(_parentID);
parentModelInfo->nestedModels.push_back(id);
return {id, *entry};
}
Expand Down Expand Up @@ -482,7 +500,7 @@ class Base : public Implements3d<FeatureList<Feature>>
_joint->getTransformFromChildBodyNode());

this->jointsByName[_fullName] = _joint;
this->models.at(_modelID)->joints.push_back(jointInfo);
this->GetModelInfo(_modelID)->joints.push_back(jointInfo);
this->joints.idToObject[id]->frame = jointFrame;
this->frames[id] = this->joints.idToObject[id]->frame.get();

Expand Down Expand Up @@ -539,17 +557,16 @@ class Base : public Implements3d<FeatureList<Feature>>

// If this is a nested model, remove an entry from the parent models
// "nestedModels" vector
// Note, it is the responsibility of the caller to avoid calling this
// function when `_modelID` points to a proxy model to a world.
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);
}
const std::size_t modelIndex =
this->models.idToIndexInContainer.at(_modelID);
auto parentModelInfo = this->GetModelInfo(parentID);
if (modelIndex >= parentModelInfo->nestedModels.size())
return false;
parentModelInfo->nestedModels.erase(parentModelInfo->nestedModels.begin() +
modelIndex);
this->models.RemoveEntity(skel);
world->removeSkeleton(skel);
return true;
Expand All @@ -558,6 +575,12 @@ class Base : public Implements3d<FeatureList<Feature>>
public: inline std::size_t GetWorldOfModelImpl(
const std::size_t &_modelID) const
{
auto worldModelProxy = this->modelProxiesToWorld.MaybeAt(_modelID);
if (worldModelProxy.has_value())
{
return _modelID;
}

if (this->models.HasEntity(_modelID))
{
auto parentIt = this->models.idToContainerID.find(_modelID);
Expand Down Expand Up @@ -586,32 +609,49 @@ class Base : public Implements3d<FeatureList<Feature>>
}
};


/// \brief Create a fully (world) scoped joint name.
/// \param _modelID Identity of the parent model of the joint's child link.
/// \param _name The unscoped joint name.
/// \return The fully (world) scoped joint name, or an empty string
/// if a world cannot be resolved.
public: inline std::string FullyScopedJointName(
const Identity &_modelID,
const std::size_t &_modelID,
const std::string &_name) const
{
const auto modelInfo = this->ReferenceInterface<ModelInfo>(_modelID);

auto worldID = this->GetWorldOfModelImpl(_modelID);
if (worldID == INVALID_ENTITY_ID)
if (this->modelProxiesToWorld.HasEntity(_modelID))
{
gzerr << "World of model [" << modelInfo->model->getName()
<< "] could not be found when creating joint [" << _name
<< "]\n";
return "";
auto world = this->worlds.at(_modelID);
return ::sdf::JoinName(world->getName(), _name);
}
else
{
const auto modelInfo = this->models.at(_modelID);

auto world = this->worlds.at(worldID);
const std::string fullJointName = ::sdf::JoinName(
world->getName(),
::sdf::JoinName(modelInfo->model->getName(), _name));
auto worldID = this->GetWorldOfModelImpl(_modelID);
if (worldID == INVALID_ENTITY_ID)
{
gzerr << "World of model [" << modelInfo->model->getName()
<< "] could not be found when creating the fully scoped name of "
"joint ["
<< _name << "]\n";
return "";
}
auto world = this->worlds.at(worldID);
return ::sdf::JoinName(
world->getName(),
::sdf::JoinName(modelInfo->model->getName(), _name));
}
}

return fullJointName;
public: ModelInfoPtr GetModelInfo(std::size_t _modelID) const
{
auto modelProxy = this->modelProxiesToWorld.MaybeAt(_modelID);
if (modelProxy)
{
return *modelProxy;
}
return this->models.at(_modelID);
}

public: EntityStorage<DartWorldPtr, std::string> worlds;
Expand All @@ -620,6 +660,7 @@ class Base : public Implements3d<FeatureList<Feature>>
public: EntityStorage<JointInfoPtr, const DartJoint*> joints;
public: EntityStorage<ShapeInfoPtr, const DartShapeNode*> shapes;
public: std::unordered_map<std::size_t, dart::dynamics::Frame*> frames;
public: EntityStorage<ModelInfoPtr, DartWorldPtr> modelProxiesToWorld;

/// \brief Map from the fully qualified link name (including the world name)
/// to the BodyNode object. This is useful for keeping track of BodyNodes even
Expand Down
75 changes: 48 additions & 27 deletions dartsim/src/EntityManagementFeatures.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -258,6 +258,12 @@ const std::string &EntityManagementFeatures::GetModelName(
std::size_t EntityManagementFeatures::GetModelIndex(
const Identity &_modelID) const
{
// If this is a model proxy of a world, we just return 0.
auto modelProxy = this->modelProxiesToWorld.MaybeAt(_modelID);
if (modelProxy)
{
return 0;
}
// TODO(anyone) this will throw if the model has been removed. The alternative
// is to first check if the model exists, but what should we return if it
// doesn't exist
Expand Down Expand Up @@ -317,22 +323,20 @@ Identity EntityManagementFeatures::GetNestedModel(
const std::string fullName =
::sdf::JoinName(modelInfo->model->getName(), _modelName);

if (this->models.HasEntity(_modelID))
auto worldID = this->GetWorldOfModelImpl(_modelID);
if (!worldID)
{
auto worldID = this->GetWorldOfModelImpl(_modelID);
auto nestedSkel = this->worlds.at(worldID)->getSkeleton(fullName);
if (nullptr == nestedSkel)
{
return this->GenerateInvalidId();
}
const std::size_t nestedModelID = this->models.IdentityOf(nestedSkel);
return this->GenerateIdentity(nestedModelID,
this->models.at(nestedModelID));
return this->GenerateInvalidId();
}
else

auto nestedSkel = this->worlds.at(worldID)->getSkeleton(fullName);
if (nullptr == nestedSkel)
{
return this->GenerateInvalidId();
}
const std::size_t nestedModelID = this->models.IdentityOf(nestedSkel);
return this->GenerateIdentity(nestedModelID,
this->models.at(nestedModelID));
}

/////////////////////////////////////////////////
Expand Down Expand Up @@ -404,7 +408,7 @@ Identity EntityManagementFeatures::GetLink(
std::size_t EntityManagementFeatures::GetJointCount(
const Identity &_modelID) const
{
return this->ReferenceInterface<ModelInfo>(_modelID)->model->getNumJoints();
return this->ReferenceInterface<ModelInfo>(_modelID)->joints.size();
}

/////////////////////////////////////////////////
Expand Down Expand Up @@ -566,12 +570,14 @@ Identity EntityManagementFeatures::GetModelOfJoint(
const Identity &_jointID) const
{
const std::size_t modelID = this->joints.idToContainerID.at(_jointID);
auto modelInfo = this->GetModelInfo(modelID);


// If the model containing the joint doesn't exist in "models", it means this
// joint belongs to a removed model.
if (this->models.HasEntity(modelID))
if (modelInfo)
{
return this->GenerateIdentity(modelID, this->models.at(modelID));
return this->GenerateIdentity(modelID, modelInfo);
}
else
{
Expand Down Expand Up @@ -666,6 +672,11 @@ bool EntityManagementFeatures::RemoveModel(const Identity &_modelID)
/////////////////////////////////////////////////
bool EntityManagementFeatures::ModelRemoved(const Identity &_modelID) const
{
auto modelProxy = this->modelProxiesToWorld.MaybeAt(_modelID);
if (modelProxy)
{
return false;
}
return !this->models.HasEntity(_modelID);
}

Expand Down Expand Up @@ -698,20 +709,16 @@ bool EntityManagementFeatures::RemoveNestedModelByName(const Identity &_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))
{
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;
}
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);
}
/////////////////////////////////////////////////
Identity EntityManagementFeatures::ConstructEmptyWorld(
Expand Down Expand Up @@ -755,7 +762,7 @@ Identity EntityManagementFeatures::ConstructEmptyNestedModel(
{
// find the world assocated with the model
auto worldID = this->GetWorldOfModelImpl(_parentModelID);
const auto &skel = this->models.at(_parentModelID)->model;
const auto &skel = this->GetModelInfo(_parentModelID)->model;
const std::string modelFullName = ::sdf::JoinName(skel->getName(), _name);

dart::dynamics::SkeletonPtr model =
Expand All @@ -776,6 +783,11 @@ Identity EntityManagementFeatures::ConstructEmptyNestedModel(
Identity EntityManagementFeatures::ConstructEmptyLink(
const Identity &_modelID, const std::string &_name)
{
// Return early if model is a proxy to the world.
if (this->modelProxiesToWorld.MaybeAt(_modelID))
{
return this->GenerateInvalidId();
}
auto model = this->ReferenceInterface<ModelInfo>(_modelID)->model;

dart::dynamics::FreeJoint::Properties prop_fj;
Expand Down Expand Up @@ -833,6 +845,15 @@ void EntityManagementFeatures::RemoveCollisionFilterMask(
filterPtr->RemoveIgnoredCollision(shapeNode);
}

Identity EntityManagementFeatures::GetWorldModel(const Identity &_worldID) const
{
auto modelID = this->modelProxiesToWorld.MaybeAt(_worldID);
if (modelID)
{
return this->GenerateIdentity(_worldID, *modelID);
}
return this->GenerateInvalidId();
}
} // namespace dartsim
}
}
6 changes: 5 additions & 1 deletion dartsim/src/EntityManagementFeatures.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,8 @@ struct EntityManagementFeatureList : FeatureList<
ConstructEmptyModelFeature,
ConstructEmptyNestedModelFeature,
ConstructEmptyLinkFeature,
CollisionFilterMaskFeature
CollisionFilterMaskFeature,
WorldModelFeature
> { };

class EntityManagementFeatures :
Expand Down Expand Up @@ -174,6 +175,9 @@ class EntityManagementFeatures :
const Identity &_shapeID) const override;

public: void RemoveCollisionFilterMask(const Identity &_shapeID) override;

// ----- World model feature -----
public: Identity GetWorldModel(const Identity &_worldID) const override;
};

}
Expand Down
Loading

0 comments on commit c80d4d7

Please sign in to comment.