From 18e0daddd6a3f034cc3b6d7bcded899e815af036 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 5 Mar 2024 20:07:52 -0800 Subject: [PATCH] bullet-featherstone: Support nested models (#574) 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 Signed-off-by: Steve Peters Signed-off-by: Ian Chen Co-authored-by: Shameek Ganguly Co-authored-by: Steve Peters --- bullet-featherstone/src/Base.hh | 145 +++- .../src/EntityManagementFeatures.cc | 154 +++- .../src/EntityManagementFeatures.hh | 23 +- bullet-featherstone/src/FreeGroupFeatures.cc | 13 +- bullet-featherstone/src/SDFFeatures.cc | 793 +++++++++++++----- bullet-featherstone/src/SDFFeatures.hh | 19 + test/common_test/CMakeLists.txt | 5 + test/common_test/Worlds.hh | 2 + test/common_test/free_joint_features.cc | 11 + test/common_test/joint_features.cc | 3 + test/common_test/world_features.cc | 100 +++ .../worlds/world_single_nested_model.sdf | 61 ++ 12 files changed, 1065 insertions(+), 264 deletions(-) create mode 100644 test/common_test/worlds/world_single_nested_model.sdf diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index 184d958c2..4e9bb4338 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -84,12 +84,14 @@ struct ModelInfo Identity world; int indexInWorld; Eigen::Isometry3d baseInertiaToLinkFrame; - std::unique_ptr body; + std::shared_ptr body; std::vector linkEntityIds; std::vector jointEntityIds; + std::vector nestedModelEntityIds; std::unordered_map linkNameToEntityId; std::unordered_map jointNameToEntityId; + std::unordered_map nestedModelNameToEntityId; /// These are joints that connect this model to other models, e.g. fixed /// constraints. @@ -99,7 +101,7 @@ struct ModelInfo std::string _name, Identity _world, Eigen::Isometry3d _baseInertiaToLinkFrame, - std::unique_ptr _body) + std::shared_ptr _body) : name(std::move(_name)), world(std::move(_world)), baseInertiaToLinkFrame(_baseInertiaToLinkFrame), @@ -273,14 +275,24 @@ class Base : public Implements3d> const auto id = this->GetNextEntity(); auto world = std::make_shared(std::move(_worldInfo)); this->worlds[id] = world; - return this->GenerateIdentity(id, world); + auto worldID = this->GenerateIdentity(id, world); + + auto worldModel = std::make_shared( + 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 _body) + std::shared_ptr _body) { const auto id = this->GetNextEntity(); auto model = std::make_shared( @@ -292,6 +304,30 @@ class Base : public Implements3d> 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 _body) + { + const auto id = this->GetNextEntity(); + auto model = std::make_shared( + 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 @@ class Base : public Implements3d> auto *model = this->ReferenceInterface(_linkInfo.model); model->linkNameToEntityId[link->name] = id; - if (link->indexInModel.has_value()) - { - // We expect the links to be added in order - assert(static_cast(*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 @@ class Base : public Implements3d> return this->GenerateIdentity(id, joint); } + public: bool RemoveModelImpl(const Identity &_parentID, + const Identity &_modelID) + { + auto *model = this->ReferenceInterface(_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(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. diff --git a/bullet-featherstone/src/EntityManagementFeatures.cc b/bullet-featherstone/src/EntityManagementFeatures.cc index 3a491862a..99e40b827 100644 --- a/bullet-featherstone/src/EntityManagementFeatures.cc +++ b/bullet-featherstone/src/EntityManagementFeatures.cc @@ -206,53 +206,16 @@ Identity EntityManagementFeatures::ConstructEmptyWorld( bool EntityManagementFeatures::RemoveModel(const Identity &_modelID) { auto *model = this->ReferenceInterface(_modelID); - auto *world = this->ReferenceInterface(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>(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(_modelID); - auto *world = this->ReferenceInterface(model->world); - return world->modelIndexToEntityId.count(model->indexInWorld) == 0; + return this->models.find(_modelID) == this->models.end(); } ///////////////////////////////////////////////// @@ -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(_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(_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 @@ 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; } ///////////////////////////////////////////////// @@ -414,6 +432,58 @@ Identity EntityManagementFeatures::GetWorldOfModel( return this->ReferenceInterface(_modelID)->world; } +///////////////////////////////////////////////// +std::size_t EntityManagementFeatures::GetNestedModelCount( + const Identity &_modelID) const +{ + return this->ReferenceInterface( + _modelID)->nestedModelEntityIds.size(); +} + +///////////////////////////////////////////////// +Identity EntityManagementFeatures::GetNestedModel( + const Identity &_modelID, const std::size_t _modelIndex) const +{ + const auto modelInfo = this->ReferenceInterface(_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(_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 diff --git a/bullet-featherstone/src/EntityManagementFeatures.hh b/bullet-featherstone/src/EntityManagementFeatures.hh index 09d45bdba..1cbb47813 100644 --- a/bullet-featherstone/src/EntityManagementFeatures.hh +++ b/bullet-featherstone/src/EntityManagementFeatures.hh @@ -37,9 +37,11 @@ struct EntityManagementFeatureList : gz::physics::FeatureList< GetJointFromModel, GetLinkFromModel, GetModelFromWorld, + GetNestedModelFromModel, GetShapeFromLink, GetWorldFromEngine, - RemoveModelFromWorld + RemoveEntities, + WorldModelFeature > { }; class EntityManagementFeatures : @@ -155,9 +157,28 @@ class EntityManagementFeatures : public: bool ModelRemoved(const Identity &_modelID) const override; + public: bool RemoveNestedModelByIndex( + const Identity &_modelID, std::size_t _nestedModelIndex) 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; + + // ----- GetNestedModelFromModel ----- + 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; + + // ----- World model feature ----- + public: Identity GetWorldModel(const Identity &_worldID) const override; }; } // namespace bullet_featherstone diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc index c557d6465..22e219528 100644 --- a/bullet-featherstone/src/FreeGroupFeatures.cc +++ b/bullet-featherstone/src/FreeGroupFeatures.cc @@ -54,8 +54,8 @@ Identity FreeGroupFeatures::GetFreeGroupRootLink(const Identity &_groupID) const // Free groups in bullet-featherstone are always represented by ModelInfo const auto *model = this->ReferenceInterface(_groupID); - // The first link entity in the model is always the root link - const std::size_t rootID = model->linkEntityIds.front(); + // btMultiBody user index stores the gz-phsics model root link id + std::size_t rootID = static_cast(model->body->getUserIndex()); return this->GenerateIdentity(rootID, this->links.at(rootID)); } @@ -66,7 +66,7 @@ void FreeGroupFeatures::SetFreeGroupWorldAngularVelocity( // Free groups in bullet-featherstone are always represented by ModelInfo const auto *model = this->ReferenceInterface(_groupID); - if (model != nullptr) + if (model) { model->body->setBaseOmega(convertVec(_angularVelocity)); } @@ -79,7 +79,7 @@ void FreeGroupFeatures::SetFreeGroupWorldLinearVelocity( // Free groups in bullet-featherstone are always represented by ModelInfo const auto *model = this->ReferenceInterface(_groupID); // Set Base Vel - if(model) + if (model) { model->body->setBaseVel(convertVec(_linearVelocity)); } @@ -91,7 +91,10 @@ void FreeGroupFeatures::SetFreeGroupWorldPose( const PoseType &_pose) { const auto *model = this->ReferenceInterface(_groupID); - model->body->setBaseWorldTransform(convertTf(_pose)); + if (model) + { + model->body->setBaseWorldTransform(convertTf(_pose)); + } } } // namespace bullet_featherstone diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index da528ae35..0bbdfc11e 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -106,6 +106,8 @@ struct ParentInfo { const ::sdf::Joint *joint; const ::sdf::Model *model; + const ::sdf::Link *link; + const ::sdf::Link *parent; }; ///////////////////////////////////////////////// @@ -113,6 +115,7 @@ struct Structure { /// The root link of the model const ::sdf::Link *rootLink; + const ::sdf::Model *model; const ::sdf::Joint *rootJoint; btScalar mass; btVector3 inertia; @@ -143,167 +146,209 @@ void extractInertial( } ///////////////////////////////////////////////// -std::optional ValidateModel(const ::sdf::Model &_sdfModel) +/// \brief Get pose of joint relative to link +/// \param[out] _resolvedPose Pose of joint relative to link +/// \param[in] _model Parent model of joint +/// \param[in] _joint Joint name +/// \param[in] _link Scoped link name +::sdf::Errors resolveJointPoseRelToLink(math::Pose3d &_resolvedPose, + const ::sdf::Model *_model, + const std::string &_joint, const std::string &_link) { - std::unordered_map parentOf; - const ::sdf::Link *rootLink = nullptr; - const ::sdf::Joint *rootJoint = nullptr; - bool fixed = false; - const std::string &rootModelName = _sdfModel.Name(); - // a map of parent link to a vector of its child links - std::unordered_map> - linkTree; - - const auto checkModel = - [&rootLink, &rootJoint, &fixed, &parentOf, &rootModelName, &linkTree]( - const ::sdf::Model &model) -> bool - { - for (std::size_t i = 0; i < model.JointCount(); ++i) - { - const auto *joint = model.JointByIndex(i); - const auto &parentLinkName = joint->ParentName(); - const auto *parent = model.LinkByName(parentLinkName); - const auto &childLinkName = joint->ChildName(); - const auto *child = model.LinkByName(childLinkName); - - switch (joint->Type()) - { - case ::sdf::JointType::FIXED: - case ::sdf::JointType::REVOLUTE: - case ::sdf::JointType::PRISMATIC: - case ::sdf::JointType::BALL: - break; - default: - gzerr << "Joint type [" << (std::size_t)(joint->Type()) - << "] is not supported by " - << "gz-physics-bullet-featherstone-plugin. " - << "Replaced by a fixed joint.\n"; - } - - if (child == parent) - { - gzerr << "The Link [" << parentLinkName << "] is being attached to " - << "itself by Joint [" << joint->Name() << "] in Model [" - << rootModelName << "]. That is not allowed.\n"; - return false; - } + ::sdf::Errors errors; + const auto *joint = _model->JointByName(_joint); + if (!joint) + { + gzerr << "No joint [" << _joint << "] found in model [" + << _model->Name() << "]" << std::endl; + return errors; + } - if (nullptr == parent && parentLinkName != "world") - { - gzerr << "The link [" << parentLinkName << "] cannot be found in " - << "Model [" << rootModelName << "], but joint [" - << joint->Name() << "] wants to use it as its parent link\n"; - return false; - } - else if (nullptr == parent) - { - // This link is attached to the world, making it the root - if (nullptr != rootLink) - { - // A root already exists for this model - gzerr << "Two root links were found for Model [" << rootModelName - << "]: [" << rootLink->Name() << "] and [" << childLinkName - << "], but gz-physics-bullet-featherstone-plugin only " - << "supports one root per Model.\n"; - return false; - } + std::string childLinkName; + errors = joint->ResolveChildLink(childLinkName); + if (!errors.empty()) + { + childLinkName = joint->ChildName(); + } - if (joint->Type() != ::sdf::JointType::FIXED) - { - gzerr << "Link [" << child->Name() << "] in Model [" - << rootModelName << "] is being connected to the " - << "world by Joint [" << joint->Name() << "] with a [" - << (std::size_t)(joint->Type()) << "] joint type, but only " - << "Fixed (" << (std::size_t)(::sdf::JointType::FIXED) - << ") is supported by " - << "gz-physics-bullet-featherstone-plugin\n"; - return false; - } + // joint pose is expressed relative to child link so return joint pose + // if input link is the child link + if (childLinkName == _link) + { + errors = joint->SemanticPose().Resolve(_resolvedPose); + return errors; + } - rootLink = child; - rootJoint = joint; - fixed = true; + // compute joint pose relative to specified link + const auto *link = _model->LinkByName(_link); + if (!link) + { + gzerr << "No link [" << _link << "] found in model [" + << _model->Name() << "]" << std::endl; + return errors; + } - // Do not add the root link to the set of links that have parents - continue; - } + math::Pose3d jointPoseRelToModel; + errors = _model->SemanticPose().Resolve(jointPoseRelToModel, + _model->Name() + "::" + _joint); + jointPoseRelToModel = jointPoseRelToModel.Inverse(); - if (!parentOf.insert( - std::make_pair(child, ParentInfo{joint, &model})).second) - { - gzerr << "The Link [" << childLinkName << "] in Model [" - << rootModelName << "] has multiple parent joints. That is not " - << "supported by the gz-physics-bullet-featherstone plugin.\n"; - } - if (parent != nullptr) - { - linkTree[parent].push_back(child); - } - } + math::Pose3d modelPoseRelToLink; + errors = _model->SemanticPose().Resolve(modelPoseRelToLink, + _model->Name() + "::" + _link); - return true; - }; + _resolvedPose = modelPoseRelToLink * jointPoseRelToModel; - if (!checkModel(_sdfModel)) - return std::nullopt; + return errors; +} - for (std::size_t i = 0; i < _sdfModel.ModelCount(); ++i) +///////////////////////////////////////////////// +/// \brief Recursively build a tree of parent-child data structures from the +/// input Model SDF. +/// \param[in] _sdfModel input Model SDF. +/// \param[out] _parentOf A map of child link to its parent +/// \param[out] _linkTree A map of parent link to its child links +bool buildTrees(const ::sdf::Model *_sdfModel, + std::unordered_map &_parentOf, + std::unordered_map> &_linkTree) +{ + for (std::size_t i = 0; i < _sdfModel->JointCount(); ++i) { - if (!checkModel(*_sdfModel.ModelByIndex(i))) - return std::nullopt; - } + const auto *joint = _sdfModel->JointByIndex(i); + std::string parentLinkName; + ::sdf::Errors errors = joint->ResolveParentLink(parentLinkName); + if (!errors.empty()) + { + parentLinkName = joint->ParentName(); + } + std::string childLinkName; + errors = joint->ResolveChildLink(childLinkName); + if (!errors.empty()) + { + childLinkName = joint->ChildName(); + } + const std::string &modelName = _sdfModel->Name(); + const auto *parent = _sdfModel->LinkByName(parentLinkName); + const auto *child = _sdfModel->LinkByName(childLinkName); - // Find root link in model and verify that there is only one root link in - // the model. Returns false if more than one root link is found - const auto findRootLink = - [&rootLink, &parentOf, &rootModelName](const ::sdf::Model &model) -> bool + switch (joint->Type()) + { + case ::sdf::JointType::FIXED: + case ::sdf::JointType::REVOLUTE: + case ::sdf::JointType::PRISMATIC: + case ::sdf::JointType::BALL: + break; + default: + gzerr << "Joint type [" << (std::size_t)(joint->Type()) + << "] is not supported by " + << "gz-physics-bullet-featherstone-plugin. " + << "Replaced by a fixed joint.\n"; + } + if (child == parent) + { + gzerr << "The Link [" << parentLinkName << "] is being attached to " + << "itself by Joint [" << joint->Name() << "] in Model [" + << modelName << "]. That is not allowed.\n"; + return false; + } + if (nullptr == parent && parentLinkName != "world") + { + gzerr << "The link [" << parentLinkName << "] cannot be found in " + << "Model [" << modelName << "], but joint [" + << joint->Name() << "] wants to use it as its parent link\n"; + return false; + } + else if (nullptr == parent) { - for (std::size_t i = 0; i < model.LinkCount(); ++i) + // This link is attached to the world, making it the root + if (joint->Type() != ::sdf::JointType::FIXED) { - const auto *link = model.LinkByIndex(i); - if (parentOf.count(link) == 0) - { - // This link must be the root. If a different link was already - // identified as the root then we have a conflict. - if (rootLink && rootLink != link) - { - gzerr << "Two root links were found for Model [" << rootModelName - << "]: [" << rootLink->Name() << "] and [" << link->Name() - << "]. The Link [" << link->Name() << "] is implicitly a " - << "root because it has no parent joint.\n"; - return false; - } - - rootLink = link; - } + gzerr << "Link [" << child->Name() << "] in Model [" + << modelName << "] is being connected to the " + << "world by Joint [" << joint->Name() << "] with a [" + << (std::size_t)(joint->Type()) << "] joint type, but only " + << "Fixed (" << (std::size_t)(::sdf::JointType::FIXED) + << ") is supported by " + << "gz-physics-bullet-featherstone-plugin\n"; + return false; } + } - return true; - }; + if (!_parentOf.insert( + std::make_pair(child, ParentInfo{joint, _sdfModel, child, parent})) + .second) + { + gzerr << "The Link [" << childLinkName << "] in Model [" + << modelName << "] has multiple parent joints. That is not " + << "supported by the gz-physics-bullet-featherstone plugin.\n"; + } + if (parent != nullptr) + { + _linkTree[parent].push_back(child); + } + } - if (rootLink == nullptr && !findRootLink(_sdfModel)) + // Recursively build tree from nested models + for (std::size_t i = 0; i < _sdfModel->ModelCount(); ++i) { - // No root link was found in this model - return std::nullopt; + const auto *model = _sdfModel->ModelByIndex(i); + if (!buildTrees(model, _parentOf, _linkTree)) + return false; } + return true; +} - // find root link in nested models if one was not already found - for (std::size_t i = 0; i < _sdfModel.ModelCount(); ++i) +///////////////////////////////////////////////// +/// \brief Find all the root links given a model SDF +/// \param[in] _sdfModel Model SDF +/// \param[in] _parentOf A map of child link to its parent info +/// \param[out] _rootLinks A vector of root links paired with their +/// immediate parent model +void findRootLinks(const ::sdf::Model *_sdfModel, + const std::unordered_map &_parentOf, + std::vector> &_rootLinks) +{ + for (std::size_t i = 0; i < _sdfModel->LinkCount(); ++i) { - if (rootLink != nullptr) + const auto *link = _sdfModel->LinkByIndex(i); + auto parentOfIt = _parentOf.find(link); + if (parentOfIt == _parentOf.end() || !parentOfIt->second.parent) { - break; - } - if (!findRootLink(*_sdfModel.ModelByIndex(i))) - { - return std::nullopt; + // If there is not parent or parent is null (world), + // this link must be a root. + _rootLinks.push_back({link, _sdfModel}); } } - if (!rootLink) + for (std::size_t i = 0; i < _sdfModel->ModelCount(); ++i) { - gzerr << "No root link was found for model [" << _sdfModel.Name() << "\n"; - return std::nullopt; + const auto *model = _sdfModel->ModelByIndex(i); + findRootLinks(model, _parentOf, _rootLinks); + } +} + +///////////////////////////////////////////////// +/// \brief Build a structure for each root link +/// \param[in] _rootLink Root link in a model tree +/// \param[in] _parentOf A map of child link to its parent +/// \param[in] _linkTree A map of parent link to its child links +std::optional buildStructure( + const ::sdf::Link * _rootLink, + const ::sdf::Model * _model, + const std::unordered_map &_parentOf, + std::unordered_map> + &_linkTree) +{ + bool fixed = false; + const ::sdf::Joint *rootJoint = nullptr; + // rootJoint only exists if root link is connected to world by a joint + auto linkIt = _parentOf.find(_rootLink); + if (linkIt != _parentOf.end()) + { + const auto &parentInfo = linkIt->second; + rootJoint = parentInfo.joint; + fixed = true; } // The documentation for bullet does not mention whether parent links must @@ -317,63 +362,215 @@ std::optional ValidateModel(const ::sdf::Model &_sdfModel) std::function flattenLinkTree = [&](const ::sdf::Link *link) { - if (link != rootLink) + if (link != _rootLink) { flatLinks.push_back(link); } - if (auto it = linkTree.find(link); it != linkTree.end()) + if (auto it = _linkTree.find(link); it != _linkTree.end()) { - for (const auto &child_link : it->second) + for (const auto &childLink : it->second) { - flattenLinkTree(child_link); + flattenLinkTree(childLink); } } }; - flattenLinkTree(rootLink); + flattenLinkTree(_rootLink); btScalar mass; btVector3 inertia; math::Pose3d linkToPrincipalAxesPose; - extractInertial(rootLink->Inertial(), mass, inertia, linkToPrincipalAxesPose); + extractInertial(_rootLink->Inertial(), mass, inertia, + linkToPrincipalAxesPose); + + // Uncomment to debug structure + // std::cout << "Structure: " << std::endl; + // std::cout << " model: " << _model->Name() << std::endl; + // std::cout << " root link: " << _rootLink->Name() << std::endl; + // std::cout << " root joint: " << ((rootJoint) ? rootJoint->Name() : "N/A") + // << std::endl; + // std::cout << " mass: " << mass << std::endl; + // std::cout << " fixed: " << fixed << std::endl; + // std::cout << " flatLinks size: " << flatLinks.size() << std::endl; return Structure{ - rootLink, rootJoint, mass, inertia, linkToPrincipalAxesPose, fixed, - parentOf, flatLinks}; + _rootLink, _model, rootJoint, mass, inertia, linkToPrincipalAxesPose, fixed, + _parentOf, flatLinks}; } ///////////////////////////////////////////////// -Identity SDFFeatures::ConstructSdfModel( - const Identity &_worldID, +/// \brief Validate input model SDF and build a vector of structures. +/// Each structure represents a single kinematic tree in the model +/// \param[in] _sdfModel Input model SDF +/// \return A vector of structures +std::vector validateModel(const ::sdf::Model &_sdfModel) +{ + // A map of child link and its parent info + std::unordered_map parentOf; + + // A map of parent link to a vector of its child links + std::unordered_map> + linkTree; + + // A vector of root link of its parent model + // Use a vector to preseve order of entities defined in sdf + std::vector> rootLinks; + + buildTrees(&_sdfModel, parentOf, linkTree); + findRootLinks(&_sdfModel, parentOf, rootLinks); + + std::vector structures; + if (rootLinks.empty()) + { + // No root link was found in this model + gzerr << "No root links are found in this model" << std::endl; + return structures; + } + + // Build subtrees + for (const auto &rootLinkIt : rootLinks) + { + auto structure = buildStructure(rootLinkIt.first, rootLinkIt.second, + parentOf, linkTree); + if (structure.has_value()) + { + structures.push_back(*structure); + } + } + + return structures; +} + +///////////////////////////////////////////////// +Identity SDFFeatures::ConstructSdfNestedModel(const Identity &_parentID, + const ::sdf::Model &_sdfModel) +{ + return this->ConstructSdfModelImpl(_parentID, _sdfModel); +} + +///////////////////////////////////////////////// +Identity SDFFeatures::ConstructSdfModelImpl( + std::size_t _parentID, const ::sdf::Model &_sdfModel) { - const auto validation = ValidateModel(_sdfModel); - if (!validation.has_value()) - return this->GenerateInvalidId(); + // The ConstructSdfModelImpl function constructs the entire sdf model tree, + // including nested models So return the nested model identity if it is + // constructed already + auto wIt = this->worlds.find(_parentID); + if (wIt == this->worlds.end()) + { + auto mIt = this->models.find(_parentID); + std::size_t nestedModelID = mIt->second->nestedModelNameToEntityId.at( + _sdfModel.Name()); + return this->GenerateIdentity(nestedModelID, + this->models.at(nestedModelID)); + } - const auto &structure = *validation; - const bool isStatic = _sdfModel.Static(); + auto structures = validateModel(_sdfModel); + if (structures.empty()) + return this->GenerateInvalidId(); - const auto *world = this->ReferenceInterface(_worldID); + if (structures.size() > 1) + { + // multiple sub-trees detected in model + // \todo(iche033) support multiple kinematic trees and + // multiple floating links in a single model + // https://github.com/gazebosim/gz-physics/issues/550 + gzerr << "Multiple sub-trees / floating links detected in a model. " + << "This is not supported in bullet-featherstone implementation yet." + << std::endl; + } + // Create model for the first structure. + auto &structure = structures[0]; + const bool isStatic = _sdfModel.Static(); + WorldInfo *world = nullptr; const auto rootInertialToLink = gz::math::eigen3::convert(structure.linkToPrincipalAxesPose).inverse(); - const auto modelID = this->AddModel( - _sdfModel.Name(), _worldID, rootInertialToLink, - std::make_unique( - static_cast(structure.flatLinks.size()), - structure.mass, - structure.inertia, - structure.fixedBase || isStatic, - true)); + // A map of link sdf to parent model identity + std::unordered_map linkParentModelIds; + + std::unordered_map modelIDs; + std::size_t rootModelID = 0u; + std::shared_ptr rootMultiBody; + // Add all models, including nested models + auto addModels = [&](std::size_t _modelOrWorldID, const ::sdf::Model *_model, + auto &&_addModels) + { + if (!_model) + return false; + + auto worldIt = this->worlds.find(_modelOrWorldID); + const bool isNested = worldIt == this->worlds.end(); + auto modelID = [&] { + if (isNested) + { + auto modelIt = this->models.find(_modelOrWorldID); + auto worldIdentity = modelIt->second->world; + auto modelIdentity = + this->GenerateIdentity(_modelOrWorldID, modelIt->second); + return this->AddNestedModel( + _model->Name(), modelIdentity, worldIdentity, rootInertialToLink, + rootMultiBody); + } + else + { + auto worldIdentity = this->GenerateIdentity( + _modelOrWorldID, worldIt->second); + rootMultiBody = std::make_shared( + static_cast(structure.flatLinks.size()), + structure.mass, + structure.inertia, + structure.fixedBase || isStatic, + true); + world = this->ReferenceInterface(worldIdentity); + auto id = this->AddModel( + _model->Name(), worldIdentity, rootInertialToLink, + rootMultiBody); + rootModelID = id; + return id; + } + }(); + + // build link to model map for use later when adding links + for (std::size_t i = 0; i < _model->LinkCount(); ++i) + { + const ::sdf::Link *link = _model->LinkByIndex(i); + linkParentModelIds[link] = modelID; + } + modelIDs.insert(std::make_pair(_model, modelID)); + + // recursively add nested models + for (std::size_t i = 0; i < _model->ModelCount(); ++i) + { + if (!_addModels(modelID, _model->ModelByIndex(i), _addModels)) + return false; + } + return true; + }; + if (!addModels(_parentID, &_sdfModel, addModels)) + { + return this->GenerateInvalidId(); + } + + // Add base link + std::size_t baseLinkModelID = linkParentModelIds.at(structure.rootLink); const auto rootID = this->AddLink(LinkInfo{ - structure.rootLink->Name(), std::nullopt, modelID, rootInertialToLink + structure.rootLink->Name(), std::nullopt, + this->GenerateIdentity(baseLinkModelID, this->models.at(baseLinkModelID)), + rootInertialToLink }); + rootMultiBody->setUserIndex(std::size_t(rootID)); + + auto modelID = + this->GenerateIdentity(rootModelID, this->models[rootModelID]); const auto *model = this->ReferenceInterface(modelID); + // Add world joint if (structure.rootJoint) { + const auto &parentInfo = structure.parentOf.at(structure.rootLink); this->AddJoint( JointInfo{ structure.rootJoint->Name(), @@ -382,7 +579,7 @@ Identity SDFFeatures::ConstructSdfModel( rootID, Eigen::Isometry3d::Identity(), Eigen::Isometry3d::Identity(), - modelID + modelIDs.find(parentInfo.model)->second }); } @@ -392,6 +589,7 @@ Identity SDFFeatures::ConstructSdfModel( std::unordered_map linkIDs; linkIDs.insert(std::make_pair(structure.rootLink, rootID)); + // Add all links and joints for (int i = 0; i < static_cast(structure.flatLinks.size()); ++i) { const auto *link = structure.flatLinks[static_cast(i)]; @@ -404,17 +602,26 @@ Identity SDFFeatures::ConstructSdfModel( if (linkIDs.find(link) == linkIDs.end()) { + std::size_t parentModelID = linkParentModelIds[link]; const auto linkID = this->AddLink( - LinkInfo{link->Name(), i, modelID, linkToComTf.inverse()}); + LinkInfo{link->Name(), i, + this->GenerateIdentity(parentModelID, this->models.at(parentModelID)), + linkToComTf.inverse()}); linkIDs.insert(std::make_pair(link, linkID)); } - if (structure.parentOf.size()) + if (!structure.parentOf.empty()) { const auto &parentInfo = structure.parentOf.at(link); const auto *joint = parentInfo.joint; + std::string parentLinkName; + ::sdf::Errors errors = joint->ResolveParentLink(parentLinkName); + if (!errors.empty()) + { + parentLinkName = joint->ParentName(); + } const auto &parentLinkID = linkIDs.at( - parentInfo.model->LinkByName(joint->ParentName())); + parentInfo.model->LinkByName(parentLinkName)); const auto *parentLinkInfo = this->ReferenceInterface( parentLinkID); @@ -426,12 +633,14 @@ Identity SDFFeatures::ConstructSdfModel( Eigen::Isometry3d poseParentComToJoint; { gz::math::Pose3d gzPoseParentToJoint; - const auto errors = joint->SemanticPose().Resolve( - gzPoseParentToJoint, joint->ParentName()); + errors = resolveJointPoseRelToLink(gzPoseParentToJoint, + parentInfo.model, joint->Name(), parentLinkName); + if (!errors.empty()) { gzerr << "An error occurred while resolving the transform of Joint [" - << joint->Name() << "] in Model [" << model->name << "]:\n"; + << joint->Name() << "] in Model [" << parentInfo.model->Name() + << "]:\n"; for (const auto &error : errors) { gzerr << error << "\n"; @@ -440,6 +649,7 @@ Identity SDFFeatures::ConstructSdfModel( return this->GenerateInvalidId(); } + poseParentLinkToJoint = gz::math::eigen3::convert(gzPoseParentToJoint); poseParentComToJoint = poseParentLinkToJoint * parentLinkInfo->inertiaToLinkFrame; @@ -447,9 +657,17 @@ Identity SDFFeatures::ConstructSdfModel( Eigen::Isometry3d poseJointToChild; { - gz::math::Pose3d gzPoseJointToChild; - const auto errors = - link->SemanticPose().Resolve(gzPoseJointToChild, joint->Name()); + gz::math::Pose3d gzPoseChildToJoint; + // this retrieves the joint pose relative to link + std::string childLinkName; + errors = joint->ResolveChildLink(childLinkName); + if (!errors.empty()) + { + childLinkName = joint->ChildName(); + } + errors = resolveJointPoseRelToLink(gzPoseChildToJoint, + parentInfo.model, joint->Name(), childLinkName); + if (!errors.empty()) { gzerr << "An error occured while resolving the transform of Link [" @@ -461,7 +679,8 @@ Identity SDFFeatures::ConstructSdfModel( return this->GenerateInvalidId(); } - + gz::math::Pose3d gzPoseJointToChild; + gzPoseJointToChild = gzPoseChildToJoint.Inverse(); poseJointToChild = gz::math::eigen3::convert(gzPoseJointToChild); } @@ -469,45 +688,36 @@ Identity SDFFeatures::ConstructSdfModel( convertMat(poseParentComToJoint.linear()) .getRotation(btRotParentComToJoint); - auto linkParent = _sdfModel.LinkByName(joint->ParentName()); - gz::math::Pose3d parentTransformInWorldSpace; - const auto errors = linkParent->SemanticPose().Resolve( - parentTransformInWorldSpace); - - gz::math::Pose3d parent2joint; - const auto errors2 = linkParent->SemanticPose().Resolve( - parent2joint, joint->Name()); // X_JP - - btTransform parentLocalInertialFrame = convertTf( - parentLinkInfo->inertiaToLinkFrame); - btTransform parent2jointBt = convertTf( - gz::math::eigen3::convert(parent2joint.Inverse())); // X_PJ - - // offsetInABt = parent COM to pivot (X_IpJ) - // offsetInBBt = current COM to pivot (X_IcJ) - // - // X_PIp - // X_PJ - // X_IpJ = X_PIp^-1 * X_PJ - // - // X_IcJ = X_CIc^-1 * X_CJ - btTransform offsetInABt, offsetInBBt; - offsetInABt = parentLocalInertialFrame * parent2jointBt; - offsetInBBt = - convertTf(linkToComTf.inverse() * poseJointToChild.inverse()); - // R_IcJ * R_IpJ ^ -1 = R_IcIp; - btQuaternion parentRotToThis = - offsetInBBt.getRotation() * offsetInABt.inverse().getRotation(); + btTransform parentLocalInertialFrame = convertTf( + parentLinkInfo->inertiaToLinkFrame); + btTransform parent2jointBt = convertTf( + poseParentLinkToJoint); // X_PJ + + // offsetInABt = parent COM to pivot (X_IpJ) + // offsetInBBt = current COM to pivot (X_IcJ) + // + // X_PIp + // X_PJ + // X_IpJ = X_PIp^-1 * X_PJ + // + // X_IcJ = X_CIc^-1 * X_CJ + btTransform offsetInABt, offsetInBBt; + offsetInABt = parentLocalInertialFrame * parent2jointBt; + offsetInBBt = + convertTf(linkToComTf.inverse() * poseJointToChild.inverse()); + // R_IcJ * R_IpJ ^ -1 = R_IcIp; + btQuaternion parentRotToThis = + offsetInBBt.getRotation() * offsetInABt.inverse().getRotation(); auto jointID = this->AddJoint( JointInfo{ joint->Name(), InternalJoint{i}, - model->linkEntityIds[static_cast(parentIndex+1)], + linkIDs.find(parentInfo.parent)->second, linkIDs.find(link)->second, poseParentLinkToJoint, poseJointToChild, - modelID + modelIDs.find(parentInfo.model)->second }); auto jointInfo = this->ReferenceInterface(jointID); @@ -586,7 +796,6 @@ Identity SDFFeatures::ConstructSdfModel( } } - model->body->setHasSelfCollision(_sdfModel.SelfCollide()); model->body->finalizeMultiDof(); @@ -594,13 +803,56 @@ Identity SDFFeatures::ConstructSdfModel( if (!worldToModel) return this->GenerateInvalidId(); + auto modelToNestedModel = Eigen::Isometry3d::Identity(); + // if the model of the root link is nested in a top level model, compute + // its pose relative to top level model + if (structure.model != &_sdfModel) + { + // get the scoped name of the model + // This is used to resolve the model pose + std::string modelScopedName; + auto getModelScopedName = [&](const ::sdf::Model *_targetModel, + const ::sdf::Model *_parentModel, const std::string &_prefix, + auto &&_getModelScopedName) + { + for (std::size_t i = 0u; i < _parentModel->ModelCount(); ++i) + { + auto childModel = _parentModel->ModelByIndex(i); + if (childModel == _targetModel) + { + modelScopedName = _prefix + childModel->Name(); + return true; + } + else + { + if (_getModelScopedName(_targetModel, childModel, + _prefix + childModel->Name() + "::", _getModelScopedName)) + return true; + } + } + return false; + }; + + math::Pose3d modelPose; + if (!getModelScopedName(structure.model, &_sdfModel, + _sdfModel.Name() + "::", getModelScopedName)) + return this->GenerateInvalidId(); + + auto errors = _sdfModel.SemanticPose().Resolve(modelPose, + modelScopedName); + if (!errors.empty()) + return this->GenerateInvalidId(); + modelToNestedModel = math::eigen3::convert(modelPose).inverse(); + } + const auto modelToRootLink = ResolveSdfPose(structure.rootLink->SemanticPose()); if (!modelToRootLink) return this->GenerateInvalidId(); const auto worldToRootCom = - *worldToModel * *modelToRootLink * rootInertialToLink.inverse(); + *worldToModel * modelToNestedModel * *modelToRootLink * + rootInertialToLink.inverse(); model->body->setBaseWorldTransform(convertTf(worldToRootCom)); model->body->setBaseVel(btVector3(0, 0, 0)); @@ -629,9 +881,58 @@ Identity SDFFeatures::ConstructSdfModel( } } + // Add the remaining links in the model without constructing the bullet + // objects. These are dummy links for book-keeping purposes + // \todo(iche033) The code will need to be updated when multiple subtrees in + // a single model is supported. + for (std::size_t i = 1u; i < structures.size(); ++i) + { + auto otherStructure = structures[i]; + // Add base link + std::size_t rootLinkModelID = linkParentModelIds[otherStructure.rootLink]; + auto rootLinkModelInfo = this->models.at(rootLinkModelID); + this->AddLink(LinkInfo{ + otherStructure.rootLink->Name(), + std::nullopt, + this->GenerateIdentity(rootLinkModelID, rootLinkModelInfo), + gz::math::eigen3::convert( + otherStructure.linkToPrincipalAxesPose).inverse() + }); + gzwarn << "Floating body / sub-tree detected. Disabling link: '" + << otherStructure.rootLink->Name() << "' " + << "from model '" << rootLinkModelInfo->name << "'." << std::endl; + // other links + for (int j = 0; j < static_cast(otherStructure.flatLinks.size()); ++j) + { + const auto *link = otherStructure.flatLinks[static_cast(j)]; + std::size_t parentModelID = linkParentModelIds[link]; + btScalar mass; + btVector3 inertia; + math::Pose3d linkToPrincipalAxesPose; + extractInertial(link->Inertial(), mass, inertia, linkToPrincipalAxesPose); + auto parentModelInfo = this->models.at(parentModelID); + const auto linkID = this->AddLink( + LinkInfo{link->Name(), std::nullopt, + this->GenerateIdentity(parentModelID, parentModelInfo), + gz::math::eigen3::convert(linkToPrincipalAxesPose).inverse()}); + gzwarn << "Floating body / sub-tree detected. Disabling link: '" + << link->Name() << "' " << "from model '" << parentModelInfo->name + << "'." << std::endl; + + } + } + return modelID; } +///////////////////////////////////////////////// +Identity SDFFeatures::ConstructSdfModel( + const Identity &_worldID, + const ::sdf::Model &_sdfModel) +{ + return this->ConstructSdfModelImpl(_worldID, _sdfModel); +} + ///////////////////////////////////////////////// bool SDFFeatures::AddSdfCollision( const Identity &_linkID, @@ -958,6 +1259,88 @@ Identity SDFFeatures::ConstructSdfCollision( return this->GenerateInvalidId(); } +///////////////////////////////////////////////// +Identity SDFFeatures::ConstructSdfJoint( + const Identity &_modelID, + const ::sdf::Joint &_sdfJoint) +{ +#if BT_BULLET_VERSION < 289 + // The btMultiBody::setFixedBase function is only available in versions + // >= 2.89. This is needed for dynamically creating world joints, + // i.e. setting the btMultiBody to be fixed. So output an error letting + // users know the joint will not be created. + // \todo(iche033) A workaround for this is to loop through all the joints + // in the world first in ConstructSdfWorld, keep track of the models who are + // a child of the world joint, then when creating the btMultiBody + // in ConstructSdfModelImpl, pass fixedBase as true in its constructor. + (void)_modelID; + gzerr << "ConstructSdfJoint feature is not supported for bullet version " + << "less than 2.89. Joint '" << _sdfJoint.Name() << "' will not " + << "be created." << std::endl; +#else + auto modelInfo = this->ReferenceInterface(_modelID); + if (_sdfJoint.ChildName() == "world") + { + gzerr << "Asked to create a joint with the world as the child in model " + << "[" << modelInfo->name << "]. This is currently not " + << "supported\n"; + + return this->GenerateInvalidId(); + } + + std::string parentLinkName; + const auto resolveParentErrors = _sdfJoint.ResolveParentLink(parentLinkName); + if (!resolveParentErrors.empty()) { + parentLinkName = _sdfJoint.ParentName(); + } + std::string childLinkName; + const auto childResolveErrors = _sdfJoint.ResolveChildLink(childLinkName); + if (!childResolveErrors.empty()) { + childLinkName = _sdfJoint.ChildName(); + } + + // Currently only supports constructing fixed joint with world as parent + if (parentLinkName == "world" && _sdfJoint.Type() == ::sdf::JointType::FIXED) + { + auto worldModelIt = this->models.find(_modelID); + if (worldModelIt == this->models.end()) + return this->GenerateInvalidId(); + const auto worldModel = worldModelIt->second; + std::size_t idx = childLinkName.find("::"); + if (idx == std::string::npos) + return this->GenerateInvalidId(); + + const std::string modelName = childLinkName.substr(0, idx); + std::size_t modelID = worldModel->nestedModelNameToEntityId.at(modelName); + auto model = this->models.at(modelID); + + model->body->setFixedBase(true); + std::size_t linkID = model->body->getUserIndex(); + auto rootID = this->GenerateIdentity(linkID, this->links.at(linkID)); + return this->AddJoint( + JointInfo{ + _sdfJoint.Name(), + RootJoint{}, + std::nullopt, + rootID, + Eigen::Isometry3d::Identity(), + Eigen::Isometry3d::Identity(), + _modelID + }); + } + + // \todo(iche033) Support fixed joint between 2 different models + gzerr << "Unable to create joint between parent: " << parentLinkName << " " + << "and child: " << childLinkName << ". " + << "ConstructSdfJoint in bullet-featherstone implementation currently " + << "only supports creating a fixed joint with the world as parent link." + << std::endl; +#endif + + return this->GenerateInvalidId(); +} + + } // namespace bullet_featherstone } // namespace physics } // namespace gz diff --git a/bullet-featherstone/src/SDFFeatures.hh b/bullet-featherstone/src/SDFFeatures.hh index 2285c1a4b..bea5faa70 100644 --- a/bullet-featherstone/src/SDFFeatures.hh +++ b/bullet-featherstone/src/SDFFeatures.hh @@ -21,7 +21,9 @@ #include #include +#include #include +#include #include #include @@ -34,7 +36,9 @@ namespace physics { namespace bullet_featherstone { struct SDFFeatureList : gz::physics::FeatureList< + sdf::ConstructSdfJoint, sdf::ConstructSdfModel, + sdf::ConstructSdfNestedModel, sdf::ConstructSdfWorld, sdf::ConstructSdfCollision > { }; @@ -51,14 +55,29 @@ class SDFFeatures : const Identity &_worldID, const ::sdf::Model &_sdfModel) override; + public: Identity ConstructSdfNestedModel( + const Identity &_parentID, + const ::sdf::Model &_sdfModel) override; + public: bool AddSdfCollision( const Identity &_linkID, const ::sdf::Collision &_collision, bool isStatic); + public: Identity ConstructSdfJoint( + const Identity &_modelID, + const ::sdf::Joint &_sdfJoint) override; + private: Identity ConstructSdfCollision( const Identity &_linkID, const ::sdf::Collision &_collision) override; + + /// \brief Construct a bullet entity from a sdf::Model + /// \param[in] _parentID Parent identity + /// \param[in] _sdfModel sdf::Model to construct entity from + /// \return The entity identity if constructed otherwise an invalid identity + private: Identity ConstructSdfModelImpl(std::size_t _parentID, + const ::sdf::Model &_sdfModel); }; } // namespace bullet_featherstone diff --git a/test/common_test/CMakeLists.txt b/test/common_test/CMakeLists.txt index a35e2127a..de887560b 100644 --- a/test/common_test/CMakeLists.txt +++ b/test/common_test/CMakeLists.txt @@ -65,6 +65,11 @@ foreach(test ${tests}) target_include_directories(${test_executable} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + if (bullet_version_check_VERSION VERSION_GREATER_EQUAL 2.89) + target_compile_definitions(${test_executable} PRIVATE + "BT_BULLET_VERSION_GE_289" + ) + endif() if (bullet_version_check_VERSION VERSION_LESS_EQUAL 3.25) target_compile_definitions(${test_executable} PRIVATE "BT_BULLET_VERSION_LE_325" diff --git a/test/common_test/Worlds.hh b/test/common_test/Worlds.hh index d57aa32d3..6ad145f8f 100644 --- a/test/common_test/Worlds.hh +++ b/test/common_test/Worlds.hh @@ -48,6 +48,8 @@ const auto kStringPendulumSdf = CommonTestWorld("string_pendulum.sdf"); const auto kTestWorld = CommonTestWorld("test.world"); const auto kWorldJointTestSdf = CommonTestWorld("world_joint_test.sdf"); const auto kWorldUnsortedLinksSdf = CommonTestWorld("world_unsorted_links.sdf"); +const auto kWorldSingleNestedModelSdf = + CommonTestWorld("world_single_nested_model.sdf"); const auto kWorldWithNestedModelSdf = CommonTestWorld("world_with_nested_model.sdf"); } // namespace common_test::worlds diff --git a/test/common_test/free_joint_features.cc b/test/common_test/free_joint_features.cc index d7bd4903c..2cea27766 100644 --- a/test/common_test/free_joint_features.cc +++ b/test/common_test/free_joint_features.cc @@ -105,6 +105,11 @@ TEST_F(FreeGroupFeaturesTest, NestedFreeGroup) { for (const std::string &name : pluginNames) { + // https://github.com/gazebosim/gz-physics/issues/550 + // bullet-feathersone does not support multiple kinematic trees in + // a model so nested_model2 and nested_model3 are not constructed. + CHECK_UNSUPPORTED_ENGINE(name, "bullet-featherstone") + std::cout << "Testing plugin: " << name << std::endl; gz::plugin::PluginPtr plugin = loader.Instantiate(name); @@ -148,6 +153,7 @@ TEST_F(FreeGroupFeaturesTest, NestedFreeGroup) // Expect false because the link in nested_model is referenced by a joint. EXPECT_FALSE(checkFreeGroupForModel("parent_model::nested_model")); } + EXPECT_TRUE(checkFreeGroupForModel("parent_model::nested_model2")); EXPECT_TRUE(checkFreeGroupForModel("parent_model::nested_model3")); } @@ -157,6 +163,11 @@ TEST_F(FreeGroupFeaturesTest, NestedFreeGroupSetWorldPose) { for (const std::string &name : pluginNames) { + // https://github.com/gazebosim/gz-physics/issues/550 + // bullet-feathersone does not support multiple kinematic trees in + // a model so nested_model2 and nested_model3 are not constructed. + CHECK_UNSUPPORTED_ENGINE(name, "bullet-featherstone") + std::cout << "Testing plugin: " << name << std::endl; gz::plugin::PluginPtr plugin = loader.Instantiate(name); diff --git a/test/common_test/joint_features.cc b/test/common_test/joint_features.cc index 5f0a695b6..2e0d5b47d 100644 --- a/test/common_test/joint_features.cc +++ b/test/common_test/joint_features.cc @@ -1526,6 +1526,9 @@ TEST_F(WorldModelTest, JointSetCommand) { for (const std::string &name : this->pluginNames) { + // bullet-feathersone does not support joints between models yet + CHECK_UNSUPPORTED_ENGINE(name, "bullet-featherstone") + auto world = this->LoadWorld(name); ASSERT_NE(nullptr, world); diff --git a/test/common_test/world_features.cc b/test/common_test/world_features.cc index ad28f8bbb..e1e68c0f0 100644 --- a/test/common_test/world_features.cc +++ b/test/common_test/world_features.cc @@ -357,6 +357,106 @@ TEST_F(WorldModelTest, WorldModelAPI) } } +struct WorldNestedModelFeatureList : gz::physics::FeatureList< + GravityFeatures, + gz::physics::ForwardStep, + gz::physics::GetNestedModelFromModel, + gz::physics::sdf::ConstructSdfJoint, + gz::physics::sdf::ConstructSdfModel, + gz::physics::sdf::ConstructSdfNestedModel, + gz::physics::RemoveEntities, + gz::physics::WorldModelFeature +> { }; + + +class WorldNestedModelTest : public WorldFeaturesTest +{ + public: gz::physics::World3dPtr LoadWorld( + const std::string &_pluginName) + { + gz::plugin::PluginPtr plugin = this->loader.Instantiate(_pluginName); + + auto engine = + gz::physics::RequestEngine3d::From(plugin); + + sdf::Root root; + const sdf::Errors errors = root.Load( + common_test::worlds::kWorldSingleNestedModelSdf); + EXPECT_TRUE(errors.empty()) << errors; + if (errors.empty()) + { + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + return world; + } + return nullptr; + } +}; + +TEST_F(WorldNestedModelTest, WorldConstructNestedModel) +{ + for (const std::string &name : this->pluginNames) + { + auto world = this->LoadWorld(name); + ASSERT_NE(nullptr, world); + + auto worldModel = world->GetWorldModel(); + ASSERT_NE(nullptr, worldModel); + EXPECT_EQ(world, worldModel->GetWorld()); + EXPECT_EQ("nested_model_world", worldModel->GetName()); + EXPECT_EQ(0, worldModel->GetLinkCount()); + EXPECT_EQ(0, worldModel->GetIndex()); + EXPECT_EQ(1u, world->GetModelCount()); + EXPECT_EQ(world->GetModelCount(), worldModel->GetNestedModelCount()); + const auto nestedModel = worldModel->GetNestedModel(0); + ASSERT_NE(nullptr, nestedModel); + EXPECT_EQ("parent_model", nestedModel->GetName()); + + // Test joint creation + sdf::Joint joint; + joint.SetName("world_joint"); + joint.SetType(sdf::JointType::FIXED); + joint.SetParentName("world"); + joint.SetChildName("invalid_link"); + EXPECT_FALSE(worldModel->ConstructJoint(joint)); + joint.SetChildName("parent_model::link1"); + if (PhysicsEngineName(name) != "bullet-featherstone") + { + EXPECT_TRUE(worldModel->ConstructJoint(joint)); + } + else + { +#ifdef BT_BULLET_VERSION_GE_289 + EXPECT_TRUE(worldModel->ConstructJoint(joint)); +#endif + } + + gz::physics::ForwardStep::Input input; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Output output; + + // Check invalid input to RemoveNestedModel method + EXPECT_FALSE(worldModel->RemoveNestedModel(1)); + EXPECT_FALSE(worldModel->RemoveNestedModel("invalid")); + + // Check that we can remove models via RemoveNestedModel + EXPECT_TRUE(worldModel->RemoveNestedModel(0)); + EXPECT_TRUE(nestedModel->Removed()); + EXPECT_EQ(0u, world->GetModelCount()); + EXPECT_EQ(0u, worldModel->GetNestedModelCount()); + EXPECT_EQ(nullptr, worldModel->GetNestedModel(0)); + EXPECT_EQ(nullptr, worldModel->GetNestedModel("parent_model")); + + // verify we can step the world after model removal + const size_t numSteps = 1000; + for (size_t i = 0; i < numSteps; ++i) + { + world->Step(output, state, input); + } + EXPECT_EQ(0u, world->GetModelCount()); + EXPECT_EQ(0u, worldModel->GetNestedModelCount()); + } +} + int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); diff --git a/test/common_test/worlds/world_single_nested_model.sdf b/test/common_test/worlds/world_single_nested_model.sdf new file mode 100644 index 000000000..14f724f2b --- /dev/null +++ b/test/common_test/worlds/world_single_nested_model.sdf @@ -0,0 +1,61 @@ + + + + + + 1 2 2 0 0 0 + + 3 1 1 0 0 1.5707 + + + + 2 + + + + + + 0 1 0 1.5707 0 0 + + + + 2 + + + + + + nested_link1 + nested_link2 + + 1 0 0 + + + + + + + + 1 2 3 + + + + + + + 1 2 3 + + + + + + + link1 + nested_model::nested_link1 + + 0 0 1 + + + + +