diff --git a/CMakeLists.txt b/CMakeLists.txt index b19ff0aa2d..16cce6d5aa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -38,7 +38,7 @@ endif() # Search for project-specific dependencies #============================================================================ -ign_find_package(sdformat9 REQUIRED VERSION 9.2.0) +ign_find_package(sdformat9 REQUIRED VERSION 9.3.0) set(SDF_VER ${sdformat9_VERSION_MAJOR}) #-------------------------------------- diff --git a/examples/worlds/nested_model.sdf b/examples/worlds/nested_model.sdf new file mode 100644 index 0000000000..ca705c7825 --- /dev/null +++ b/examples/worlds/nested_model.sdf @@ -0,0 +1,110 @@ + + + + + 0.001 + 1.0 + + + libignition-physics-tpe-plugin.so + + + + + + + + 1.0 1.0 1.0 + 0.8 0.8 0.8 + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0.5 0 0 0 + + 0 0 0 0 0 0 + + + + 0.5 + + + + + + + 0.5 + + + + + + + 1 0 0.0 0 0 0 + + 0.25 0 0.0 0 0 0.785398 + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + + + diff --git a/include/ignition/gazebo/SdfEntityCreator.hh b/include/ignition/gazebo/SdfEntityCreator.hh index 0a94094609..8ad31f32bb 100644 --- a/include/ignition/gazebo/SdfEntityCreator.hh +++ b/include/ignition/gazebo/SdfEntityCreator.hh @@ -155,6 +155,18 @@ namespace ignition /// \param[in] _parent Entity which should be _child's parent. public: void SetParent(Entity _child, Entity _parent); + /// \brief Overloaded function to recursively create model entities + /// and make sure a) only one canonical link is created per model tree, + /// and b) we override the nested model's static property to true if + /// its parent is static + /// \param[in] _model SDF model object. + /// \param[in] _createCanonicalLink True to create a canonical link + /// component and attach to its child link entity + /// \param[in] _staticParent True if parent is static, false otherwise. + /// \return Model entity. + private: Entity CreateEntities(const sdf::Model *_model, + bool _createCanonicalLink, bool _staticParent); + /// \brief Pointer to private data. private: std::unique_ptr dataPtr; }; diff --git a/include/ignition/gazebo/Util.hh b/include/ignition/gazebo/Util.hh index 3dd37d393f..436cdf4334 100644 --- a/include/ignition/gazebo/Util.hh +++ b/include/ignition/gazebo/Util.hh @@ -131,6 +131,14 @@ namespace ignition void IGNITION_GAZEBO_VISIBLE addResourcePaths( const std::vector &_paths = {}); + /// \brief Get the top level model of an entity + /// \param[in] _entity Input entity + /// \param[in] _ecm Constant reference to ECM. + /// \return Entity of top level model + ignition::gazebo::Entity IGNITION_GAZEBO_VISIBLE topLevelModel( + const Entity &_entity, + const EntityComponentManager &_ecm); + /// \brief Environment variable holding resource paths. const std::string kResourcePathEnv{"IGN_GAZEBO_RESOURCE_PATH"}; diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index cee9d1c21c..3845830acb 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -80,6 +80,10 @@ class ignition::gazebo::SdfEntityCreatorPrivate /// only after we have their scoped name. public: std::map newSensors; + /// \brief Keep track of new models being added, so we load their plugins + /// only after we have their scoped name. + public: std::map newModels; + /// \brief Keep track of new visuals being added, so we load their plugins /// only after we have their scoped name. public: std::map newVisuals; @@ -216,6 +220,41 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Model *_model) { IGN_PROFILE("SdfEntityCreator::CreateEntities(sdf::Model)"); + // todo(anyone) Support multiple canonical links in nested models + // This version of CreateEntties keeps track whether or not to create a + // canonical link in a model tree using the second arg in this recursive + // function. We also override child nested models static property if parent + // model is static + auto ent = this->CreateEntities(_model, true, false); + + // Load all model plugins afterwards, so we get scoped name for nested models. + for (const auto &[entity, element] : this->dataPtr->newModels) + { + this->dataPtr->eventManager->Emit(entity, element); + } + this->dataPtr->newModels.clear(); + + // Load sensor plugins after model, so we get scoped name. + for (const auto &[entity, element] : this->dataPtr->newSensors) + { + this->dataPtr->eventManager->Emit(entity, element); + } + this->dataPtr->newSensors.clear(); + + // Load visual plugins after model, so we get scoped name. + for (const auto &[entity, element] : this->dataPtr->newVisuals) + { + this->dataPtr->eventManager->Emit(entity, element); + } + this->dataPtr->newVisuals.clear(); + + return ent; +} + +////////////////////////////////////////////////// +Entity SdfEntityCreator::CreateEntities(const sdf::Model *_model, + bool _createCanonicalLink, bool _staticParent) +{ // Entity Entity modelEntity = this->dataPtr->ecm->CreateEntity(); @@ -225,8 +264,9 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Model *_model) components::Pose(ResolveSdfPose(_model->SemanticPose()))); this->dataPtr->ecm->CreateComponent(modelEntity, components::Name(_model->Name())); + bool isStatic = _model->Static() || _staticParent; this->dataPtr->ecm->CreateComponent(modelEntity, - components::Static(_model->Static())); + components::Static(isStatic)); this->dataPtr->ecm->CreateComponent( modelEntity, components::WindMode(_model->EnableWind())); this->dataPtr->ecm->CreateComponent( @@ -238,6 +278,7 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Model *_model) // the parent frame until we get frames working. // Links + bool canonicalLinkCreated = false; for (uint64_t linkIndex = 0; linkIndex < _model->LinkCount(); ++linkIndex) { @@ -245,11 +286,14 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Model *_model) auto linkEntity = this->CreateEntities(link); this->SetParent(linkEntity, modelEntity); - if ((_model->CanonicalLinkName().empty() && linkIndex == 0) || - (link == _model->CanonicalLink())) + + if (_createCanonicalLink && + ((_model->CanonicalLinkName().empty() && linkIndex == 0) || + (link == _model->CanonicalLink()))) { this->dataPtr->ecm->CreateComponent(linkEntity, components::CanonicalLink()); + canonicalLinkCreated = true; } // Set wind mode if the link didn't override it @@ -270,27 +314,29 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Model *_model) this->SetParent(jointEntity, modelEntity); } - // Model plugins - this->dataPtr->eventManager->Emit(modelEntity, - _model->Element()); - - // Load sensor plugins after model, so we get scoped name. - for (const auto &[entity, element] : this->dataPtr->newSensors) + // Nested Models + for (uint64_t modelIndex = 0; modelIndex < _model->ModelCount(); + ++modelIndex) { - this->dataPtr->eventManager->Emit(entity, element); + auto nestedModel = _model->ModelByIndex(modelIndex); + + // Create nested model. Make sure to only create canonical link component + // in the nested model if a canonical link has not been created in this + // model yet. Also override static propery of the nested model if this model + // is static + auto nestedModelEntity = this->CreateEntities(nestedModel, + (_createCanonicalLink && !canonicalLinkCreated), isStatic); + + this->SetParent(nestedModelEntity, modelEntity); } - this->dataPtr->newSensors.clear(); // Store the model's SDF DOM to be used when saving the world to file this->dataPtr->ecm->CreateComponent( modelEntity, components::ModelSdf(*_model)); - // Load visual plugins after model, so we get scoped name. - for (const auto &[entity, element] : this->dataPtr->newVisuals) - { - this->dataPtr->eventManager->Emit(entity, element); - } - this->dataPtr->newVisuals.clear(); + // Keep track of models so we can load their plugins after loading the entire + // model and having its full scoped name. + this->dataPtr->newModels[modelEntity] = _model->Element(); return modelEntity; } diff --git a/src/SdfGenerator.cc b/src/SdfGenerator.cc index 7201fd4800..abf62a2cbf 100644 --- a/src/SdfGenerator.cc +++ b/src/SdfGenerator.cc @@ -24,6 +24,7 @@ #include "ignition/gazebo/components/Light.hh" #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/SourceFilePath.hh" #include "ignition/gazebo/components/World.hh" @@ -293,6 +294,12 @@ namespace sdf_generator [&](const Entity &_modelEntity, const components::Model *, const components::ModelSdf *_modelSdf) { + // skip nested models as they are not direct children of world + auto parentComp = _ecm.Component( + _modelEntity); + if (parentComp && parentComp->Data() != _entity) + return true; + auto modelDir = common::parentPath(_modelSdf->Data().Element()->FilePath()); const std::string modelName = diff --git a/src/Util.cc b/src/Util.cc index 1477af29a6..5b5995ae9e 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -392,6 +392,30 @@ void addResourcePaths(const std::vector &_paths) // SDF is evaluated at find call systemPaths->SetFilePathEnv(systemPaths->FilePathEnv()); } + +////////////////////////////////////////////////// +ignition::gazebo::Entity topLevelModel(const Entity &_entity, + const EntityComponentManager &_ecm) +{ + auto entity = _entity; + + // check if parent is a model + auto parentComp = _ecm.Component(entity); + while (parentComp) + { + // check if parent is a model + auto parentEntity = parentComp->Data(); + auto modelComp = _ecm.Component( + parentEntity); + if (!modelComp) + break; + + // set current model entity + entity = parentEntity; + parentComp = _ecm.Component(entity); + } + return entity; +} } } } diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index 525abe539c..501fdb5939 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -406,3 +406,60 @@ TEST(UtilTest, AsFullPath) } #endif } + +///////////////////////////////////////////////// +TEST(UtilTest, TopLevelModel) +{ + EntityComponentManager ecm; + + // world + // - modelA + // - linkA + // - modelB + // - linkB + // - modelC + + // World + auto worldEntity = ecm.CreateEntity(); + ecm.CreateComponent(worldEntity, components::World()); + ecm.CreateComponent(worldEntity, components::Name("world_name")); + + // Model A + auto modelAEntity = ecm.CreateEntity(); + ecm.CreateComponent(modelAEntity, components::Model()); + ecm.CreateComponent(modelAEntity, components::Name("modelA_name")); + ecm.CreateComponent(modelAEntity, components::ParentEntity(worldEntity)); + + // Link A - Child of Model A + auto linkAEntity = ecm.CreateEntity(); + ecm.CreateComponent(linkAEntity, components::Link()); + ecm.CreateComponent(linkAEntity, components::Name("linkA_name")); + ecm.CreateComponent(linkAEntity, components::ParentEntity(modelAEntity)); + + // Model B - nested inside Model A + auto modelBEntity = ecm.CreateEntity(); + ecm.CreateComponent(modelBEntity, components::Model()); + ecm.CreateComponent(modelBEntity, components::Name("modelB_name")); + ecm.CreateComponent(modelBEntity, components::ParentEntity(modelAEntity)); + + // Link B - child of Model B + auto linkBEntity = ecm.CreateEntity(); + ecm.CreateComponent(linkBEntity, components::Link()); + ecm.CreateComponent(linkBEntity, components::Name("linkB_name")); + ecm.CreateComponent(linkBEntity, components::ParentEntity(modelBEntity)); + + // Model C + auto modelCEntity = ecm.CreateEntity(); + ecm.CreateComponent(modelCEntity, components::Model()); + ecm.CreateComponent(modelCEntity, components::Name("modelC_name")); + ecm.CreateComponent(modelCEntity, components::ParentEntity(worldEntity)); + + // model A, link A, model B and link B should have model A as top level entity + EXPECT_EQ(modelAEntity, topLevelModel(modelAEntity, ecm)); + EXPECT_EQ(modelAEntity, topLevelModel(linkAEntity, ecm)); + EXPECT_EQ(modelAEntity, topLevelModel(modelBEntity, ecm)); + EXPECT_EQ(modelAEntity, topLevelModel(linkBEntity, ecm)); + + // model C should have itself as the top level entity + EXPECT_EQ(modelCEntity, topLevelModel(modelCEntity, ecm)); +} diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index 2f66e9903d..b8b3f67519 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -79,6 +79,9 @@ namespace ignition::gazebo /// \brief Entity type, such as 'world' or 'model'. public: QString type; + /// \brief Nested model or not + public: bool nestedModel = false; + /// \brief Whether currently locked on a given entity public: bool locked{false}; @@ -310,6 +313,17 @@ void ComponentInspector::Update(const UpdateInfo &, if (typeId == components::Model::typeId) { this->SetType("model"); + + // check if entity is nested model + auto parentComp = _ecm.Component( + this->dataPtr->entity); + if (parentComp) + { + auto modelComp = _ecm.Component(parentComp->Data()); + this->dataPtr->nestedModel = (modelComp); + } + this->NestedModelChanged(); + continue; } @@ -703,6 +717,12 @@ void ComponentInspector::OnPose(double _x, double _y, double _z, double _roll, this->dataPtr->node.Request(poseCmdService, req, cb); } +///////////////////////////////////////////////// +bool ComponentInspector::NestedModel() const +{ + return this->dataPtr->nestedModel; +} + // Register this plugin IGNITION_ADD_PLUGIN(ignition::gazebo::ComponentInspector, ignition::gui::Plugin) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.hh b/src/gui/plugins/component_inspector/ComponentInspector.hh index 6c71a4fc63..470c7b508f 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.hh +++ b/src/gui/plugins/component_inspector/ComponentInspector.hh @@ -178,6 +178,13 @@ namespace gazebo NOTIFY PausedChanged ) + /// \brief Nested Model + Q_PROPERTY( + bool nestedModel + READ NestedModel + NOTIFY NestedModelChanged + ) + /// \brief Constructor public: ComponentInspector(); @@ -200,6 +207,13 @@ namespace gazebo public: Q_INVOKABLE void OnPose(double _x, double _y, double _z, double _roll, double _pitch, double _yaw); + /// \brief Get whether the entity is a nested model or not + /// \return True if the entity is a nested model, false otherwise + public: Q_INVOKABLE bool NestedModel() const; + + /// \brief Notify that is nested model property has changed + signals: void NestedModelChanged(); + // Documentation inherited protected: bool eventFilter(QObject *_obj, QEvent *_event) override; diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index d5a4bc47fa..aa058cae4f 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -44,6 +44,11 @@ Rectangle { */ property string entityType: ComponentInspector.type + /** + * Get if entity is nested model or not + */ + property bool nestedModel : ComponentInspector.nestedModel + /** * Light grey according to theme */ diff --git a/src/gui/plugins/component_inspector/Pose3d.qml b/src/gui/plugins/component_inspector/Pose3d.qml index 8700857066..4d6852a09a 100644 --- a/src/gui/plugins/component_inspector/Pose3d.qml +++ b/src/gui/plugins/component_inspector/Pose3d.qml @@ -41,7 +41,7 @@ Rectangle { // Read-only / write property bool readOnly: { var isModel = entityType == "model" - return !(isModel) + return !(isModel) || nestedModel } // Loaded item for X diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 4d020ce27f..0bcb730fce 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -59,6 +59,7 @@ #include #include #include +#include #include // SDF @@ -191,6 +192,14 @@ class ignition::gazebo::systems::PhysicsPrivate public: physics::FrameData3d LinkFrameDataAtOffset( const LinkPtrType &_link, const math::Pose3d &_pose) const; + /// \brief Get transform from one ancestor entity to a descendant entity + /// that are in the same model. + /// \param[in] _from An ancestor of the _to entity. + /// \param[in] _to A descendant of the _from entity. + /// \return Pose transform between the two entities + public: ignition::math::Pose3d RelativePose(const Entity &_from, + const Entity &_to, const EntityComponentManager &_ecm) const; + /// \brief A map between world entity ids in the ECM to World Entities in /// ign-physics. public: std::unordered_map entityWorldMap; @@ -485,6 +494,36 @@ class ignition::gazebo::systems::PhysicsPrivate /// that here they've been casted for `MeshFeatureList`. public: std::unordered_map entityLinkMeshMap; + + ////////////////////////////////////////////////// + // Nested Models + + /// \brief Feature list to construct nested models + public: using NestedModelFeatureList = ignition::physics::FeatureList< + MinimumFeatureList, + ignition::physics::sdf::ConstructSdfNestedModel>; + + /// \brief Model type with nested model feature. + public: using ModelNestedModelPtrType = physics::ModelPtr< + physics::FeaturePolicy3d, NestedModelFeatureList>; + + /// \brief World type with nested model feature. + public: using WorldNestedModelPtrType = physics::WorldPtr< + physics::FeaturePolicy3d, NestedModelFeatureList>; + + /// \brief A map between model entity ids in the ECM to Model Entities in + /// ign-physics, with Nested Model feature. + /// All models on this map are also in `entityModelMap`. The difference is + /// that here they've been casted for `ConstructedSdfNestedModel`. + public: std::unordered_map + entityModelNestedModelMap; + + /// \brief A map between model entity ids in the ECM to World Entities in + /// ign-physics, with Nested Model feature. + /// All models on this map are also in `entityWorldMap`. The difference is + /// that here they've been casted for `ConstructedSdfNestedModel`. + public: std::unordered_map + entityWorldNestedModelMap; }; ////////////////////////////////////////////////// @@ -686,36 +725,107 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) // TODO(anyone) Don't load models unless they have collisions - // Check if parent world exists - // TODO(louise): Support nested models, see - // https://github.com/ignitionrobotics/ign-physics/issues/10 - if (this->entityWorldMap.find(_parent->Data()) - == this->entityWorldMap.end()) - { - ignwarn << "Model's parent entity [" << _parent->Data() - << "] not found on world map." << std::endl; - return true; - } - auto worldPtrPhys = this->entityWorldMap.at(_parent->Data()); - + // Check if parent world / model exists sdf::Model model; model.SetName(_name->Data()); model.SetRawPose(_pose->Data()); - auto staticComp = _ecm.Component(_entity); if (staticComp && staticComp->Data()) { model.SetStatic(staticComp->Data()); } - auto selfCollideComp = _ecm.Component(_entity); if (selfCollideComp && selfCollideComp ->Data()) { model.SetSelfCollide(selfCollideComp->Data()); } - auto modelPtrPhys = worldPtrPhys->ConstructModel(model); - this->entityModelMap.insert(std::make_pair(_entity, modelPtrPhys)); + // check if parent is a world + auto worldIt = this->entityWorldMap.find(_parent->Data()); + if (worldIt != this->entityWorldMap.end()) + { + auto worldPtrPhys = worldIt->second; + + // Use the ConstructNestedModel feature for nested models + if (model.ModelCount() > 0) + { + auto nestedModelFeature = entityCast(_parent->Data(), worldPtrPhys, + this->entityWorldNestedModelMap); + if (!nestedModelFeature) + { + static bool informed{false}; + if (!informed) + { + igndbg << "Attempting to construct nested models, but the " + << "phyiscs engine doesn't support feature " + << "[ConstructSdfNestedModelFeature]. " + << "Nested model will be ignored." + << std::endl; + informed = true; + } + return true; + } + auto modelPtrPhys = nestedModelFeature->ConstructNestedModel(model); + this->entityModelMap.insert(std::make_pair(_entity, modelPtrPhys)); + } + else + { + auto modelPtrPhys = worldPtrPhys->ConstructModel(model); + this->entityModelMap.insert(std::make_pair(_entity, modelPtrPhys)); + } + } + // check if parent is a model (nested model) + else + { + auto parentIt = this->entityModelMap.find(_parent->Data()); + if (parentIt != this->entityModelMap.end()) + { + auto parentPtrPhys = parentIt->second; + + auto nestedModelFeature = entityCast(_parent->Data(), parentPtrPhys, + this->entityModelNestedModelMap); + if (!nestedModelFeature) + { + static bool informed{false}; + if (!informed) + { + igndbg << "Attempting to construct nested models, but the " + << "physics engine doesn't support feature " + << "[ConstructSdfNestedModelFeature]. " + << "Nested model will be ignored." + << std::endl; + informed = true; + } + return true; + } + + // override static property only if parent is static. + auto parentStaticComp = + _ecm.Component(_parent->Data()); + if (parentStaticComp && parentStaticComp->Data()) + model.SetStatic(true); + + + auto modelPtrPhys = nestedModelFeature->ConstructNestedModel(model); + if (modelPtrPhys) + { + this->entityModelMap.insert( + std::make_pair(_entity, modelPtrPhys)); + } + else + { + ignerr << "Model: '" << _name->Data() << "' not loaded. " + << "Failed to create nested model." + << std::endl; + } + } + else + { + ignwarn << "Model's parent entity [" << _parent->Data() + << "] not found on world / model map." << std::endl; + return true; + } + } return true; }); @@ -1369,6 +1479,14 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) if (modelIt == this->entityModelMap.end()) return true; + // world pose cmd currently not supported for nested models + if (_entity != topLevelModel(_entity, _ecm)) + { + ignerr << "Unable to set world pose for nested models." + << std::endl; + return true; + } + // The canonical link as specified by sdformat is different from the // canonical link of the FreeGroup object @@ -1384,11 +1502,14 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) if (linkEntityIt == this->linkEntityMap.end()) return true; - auto canonicalPoseComp = - _ecm.Component(linkEntityIt->second); + // set world pose of canonical link in freegroup + // canonical link might be in a nested model so use RelativePose to get + // its pose relative to this model + math::Pose3d linkPose = + this->RelativePose(_entity, linkEntityIt->second, _ecm); freeGroup->SetWorldPose(math::eigen3::convert(_poseCmd->Data() * - canonicalPoseComp->Data())); + linkPose)); // Process pose commands for static models here, as one-time changes const components::Static *staticComp = @@ -1454,6 +1575,14 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) if (modelIt == this->entityModelMap.end()) return true; + // angular vel cmd currently not supported for nested models + if (_entity != topLevelModel(_entity, _ecm)) + { + ignerr << "Unable to set angular velocity for nested models." + << std::endl; + return true; + } + auto freeGroup = modelIt->second->FindFreeGroup(); if (!freeGroup) return true; @@ -1494,6 +1623,14 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) if (modelIt == this->entityModelMap.end()) return true; + // linear vel cmd currently not supported for nested models + if (_entity != topLevelModel(_entity, _ecm)) + { + ignerr << "Unable to set linear velocity for nested models." + << std::endl; + return true; + } + auto freeGroup = modelIt->second->FindFreeGroup(); if (!freeGroup) return true; @@ -1601,6 +1738,42 @@ void PhysicsPrivate::Step(const std::chrono::steady_clock::duration &_dt) } } +////////////////////////////////////////////////// +ignition::math::Pose3d PhysicsPrivate::RelativePose(const Entity &_from, + const Entity &_to, const EntityComponentManager &_ecm) const +{ + math::Pose3d transform; + + if (_from == _to) + return transform; + + auto currentEntity = _to; + auto parentComp = _ecm.Component(_to); + while (parentComp) + { + auto parentEntity = parentComp->Data(); + + // get the entity pose + auto entityPoseComp = + _ecm.Component(currentEntity); + + // update transform + transform = entityPoseComp->Data() * transform; + + if (parentEntity == _from) + break; + + // set current entity to parent + currentEntity = parentEntity; + + // get entity's parent + parentComp = _ecm.Component( + parentEntity); + } + + return transform; +} + ////////////////////////////////////////////////// void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm) { @@ -1622,46 +1795,58 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm) auto linkIt = this->entityLinkMap.find(_entity); if (linkIt != this->entityLinkMap.end()) { + // get top level model of this link + auto topLevelModelEnt = topLevelModel(_parent->Data(), _ecm); + auto canonicalLink = _ecm.Component(_entity); - // get the pose component of the parent model - const components::Pose *parentPose = - _ecm.Component(_parent->Data()); - auto frameData = linkIt->second->FrameDataRelativeToWorld(); const auto &worldPose = frameData.pose; - // if the parentPose is a nullptr, something is wrong with ECS - // creation - if (!parentPose) - { - ignerr << "The pose component of " << _parent->Data() - << " could not be found. This should never happen!\n"; - return true; - } if (canonicalLink) { - // This is the canonical link, update the model - // The Pose component, _pose, of this link is the initial - // transform of the link w.r.t its model. This component never - // changes because it's "fixed" to the model. Instead, we change - // the model's pose here. The physics engine gives us the pose of - // this link relative to world so to set the model's pose, we have - // to post-multiply it by the inverse of the initial transform of - // the link w.r.t to its model. - auto mutableParentPose = - _ecm.Component(_parent->Data()); - *(mutableParentPose) = components::Pose(_pose->Data().Inverse() + - math::eigen3::convert(worldPose)); - _ecm.SetChanged(_parent->Data(), components::Pose::typeId, + // This is the canonical link, update the top level model. + // The pose of this link w.r.t its top level model never changes + // because it's "fixed" to the model. Instead, we change + // the top level model's pose here. The physics engine gives us the + // pose of this link relative to world so to set the top level + // model's pose, we have to post-multiply it by the inverse of the + // transform of the link w.r.t to its top level model. + math::Pose3d linkPoseFromTopLevelModel; + linkPoseFromTopLevelModel = + this->RelativePose(topLevelModelEnt, _entity, _ecm); + + // update top level model's pose + auto mutableModelPose = + _ecm.Component(topLevelModelEnt); + *(mutableModelPose) = components::Pose( + math::eigen3::convert(worldPose) * + linkPoseFromTopLevelModel.Inverse()); + + _ecm.SetChanged(topLevelModelEnt, components::Pose::typeId, ComponentState::PeriodicChange); } else { - // Compute the relative pose of this link from the model + // Compute the relative pose of this link from the top level model + // first get the world pose of the top level model + auto worldComp = + _ecm.Component(topLevelModelEnt); + // if the worldComp is a nullptr, something is wrong with ECS + if (!worldComp) + { + ignerr << "The parent component of " << topLevelModelEnt + << " could not be found. This should never happen!\n"; + return true; + } + math::Pose3d parentWorldPose = + this->RelativePose(worldComp->Data(), _parent->Data(), _ecm); + + // Unlike canonical links, pose of regular links can move relative. + // to the parent. Same for links inside nested models. *_pose = components::Pose(math::eigen3::convert(worldPose) + - parentPose->Data().Inverse()); + parentWorldPose.Inverse()); _ecm.SetChanged(_entity, components::Pose::typeId, ComponentState::PeriodicChange); } @@ -1798,10 +1983,6 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm) state); } } - else - { - ignwarn << "Unknown link with id " << _entity << " found\n"; - } return true; }); diff --git a/test/integration/log_system.cc b/test/integration/log_system.cc index 5f62d47d66..0a0d158011 100644 --- a/test/integration/log_system.cc +++ b/test/integration/log_system.cc @@ -621,7 +621,6 @@ TEST_F(LogSystemTest, LogPaths) // Terminate server to close tlog file, otherwise we get a temporary // tlog-journal file } - EXPECT_TRUE(common::exists(cppPath)); EXPECT_TRUE(common::exists(common::joinPaths(cppPath, "state.tlog"))); #ifndef __APPLE__ @@ -781,7 +780,8 @@ TEST_F(LogSystemTest, RecordAndPlayback) msgs::SerializedStateMap stateMsg; stateMsg.ParseFromString(recordedIter->Data()); - EXPECT_EQ(28, stateMsg.entities_size()); + // entity size = 28 in dbl pendulum + 4 in nested model + EXPECT_EQ(32, stateMsg.entities_size()); EXPECT_EQ(batch.end(), ++recordedIter); // Pass changed SDF to server @@ -849,9 +849,9 @@ TEST_F(LogSystemTest, RecordAndPlayback) entityRecordedPose[recordedMsg.pose(i).name()] = recordedMsg.pose(i); } - // Has 4 dynamic entities - EXPECT_EQ(4, _playedMsg.pose().size()); - EXPECT_EQ(4u, entityRecordedPose.size()); + // Has 6 dynamic entities: 4 in dbl pendulum and 2 in nested model + EXPECT_EQ(6, _playedMsg.pose().size()); + EXPECT_EQ(6u, entityRecordedPose.size()); // Loop through all entities and compare played poses to recorded ones for (int i = 0; i < _playedMsg.pose_size(); ++i) diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc index 1fdf94e72e..a0fa82f856 100644 --- a/test/integration/physics_system.cc +++ b/test/integration/physics_system.cc @@ -875,3 +875,110 @@ TEST_F(PhysicsSystemFixture, GetBoundingBox) ignition::math::Vector3d(-0.25, 2, 1)), bbox.begin()->second); } + + +///////////////////////////////////////////////// +// This tests whether nested models can be loaded correctly +TEST_F(PhysicsSystemFixture, NestedModel) +{ + ignition::gazebo::ServerConfig serverConfig; + + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/nested_model.sdf"; + + sdf::Root root; + root.Load(sdfFile); + const sdf::World *world = root.WorldByIndex(0); + ASSERT_TRUE(nullptr != world); + + serverConfig.SetSdfFile(sdfFile); + + gazebo::Server server(serverConfig); + + server.SetUpdatePeriod(1us); + + // Create a system that records the poses of the links after physics + test::Relay testSystem; + + std::unordered_map postUpModelPoses; + std::unordered_map postUpLinkPoses; + std::unordered_map parents; + testSystem.OnPostUpdate( + [&postUpModelPoses, &postUpLinkPoses, &parents](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &_entity, const components::Model *, + const components::Name *_name, const components::Pose *_pose)->bool + { + // store model pose + postUpModelPoses[_name->Data()] = _pose->Data(); + + // store parent model name, if any + auto parentId = _ecm.Component(_entity); + if (parentId) + { + auto parentName = + _ecm.Component(parentId->Data()); + parents[_name->Data()] = parentName->Data(); + } + return true; + }); + + _ecm.Each( + [&](const ignition::gazebo::Entity &, const components::Link *, + const components::Name *_name, const components::Pose *_pose, + const components::ParentEntity *_parent)->bool + { + // store link pose + postUpLinkPoses[_name->Data()] = _pose->Data(); + + // store parent model name + auto parentName = _ecm.Component(_parent->Data()); + parents[_name->Data()] = parentName->Data(); + return true; + }); + + return true; + }); + + server.AddSystem(testSystem.systemPtr); + server.Run(true, 1, false); + + EXPECT_EQ(2u, postUpModelPoses.size()); + EXPECT_EQ(2u, postUpLinkPoses.size()); + EXPECT_EQ(4u, parents.size()); + + auto modelIt = postUpModelPoses.find("model_00"); + EXPECT_NE(postUpModelPoses.end(), modelIt); + EXPECT_EQ(math::Pose3d(0, 0, 0.5, 0, 0, 0), modelIt->second); + + modelIt = postUpModelPoses.find("model_01"); + EXPECT_NE(postUpModelPoses.end(), modelIt); + EXPECT_EQ(math::Pose3d(1.0, 0, 0.0, 0, 0, 0), modelIt->second); + + auto linkIt = postUpLinkPoses.find("link_00"); + EXPECT_NE(postUpLinkPoses.end(), linkIt); + EXPECT_EQ(math::Pose3d(0, 0, 0.0, 0, 0, 0), linkIt->second); + + linkIt = postUpLinkPoses.find("link_01"); + EXPECT_NE(postUpLinkPoses.end(), linkIt); + EXPECT_EQ(math::Pose3d(0.25, 0, 0.0, 0, 0, 0), linkIt->second); + + auto parentIt = parents.find("model_00"); + EXPECT_NE(parents.end(), parentIt); + EXPECT_EQ("nested_model_world", parentIt->second); + + parentIt = parents.find("model_01"); + EXPECT_NE(parents.end(), parentIt); + EXPECT_EQ("model_00", parentIt->second); + + parentIt = parents.find("link_00"); + EXPECT_NE(parents.end(), parentIt); + EXPECT_EQ("model_00", parentIt->second); + + parentIt = parents.find("link_01"); + EXPECT_NE(parents.end(), parentIt); + EXPECT_EQ("model_01", parentIt->second); +} diff --git a/test/integration/pose_publisher_system.cc b/test/integration/pose_publisher_system.cc index 4ff55fc376..2dd9ab1fe9 100644 --- a/test/integration/pose_publisher_system.cc +++ b/test/integration/pose_publisher_system.cc @@ -696,3 +696,36 @@ TEST_F(PosePublisherTest, StaticPoseUpdateFrequency) EXPECT_NEAR(100.0, freq, 10.0); } } + +///////////////////////////////////////////////// +TEST_F(PosePublisherTest, NestedModelLoadPlugin) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/nested_model.sdf"); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + poseMsgs.clear(); + + // test that the pose publisher plugin can be loaded by a nested model + // by subscribe to the its topic + transport::Node node; + node.Subscribe(std::string("/model/model_00/model/model_01/pose"), &poseCb); + + // Run server + unsigned int iters = 1000u; + server.Run(true, iters, false); + + // Wait for messages to be received + int sleep = 0; + while (poseMsgs.empty() && sleep++ < 300) + { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + EXPECT_TRUE(!poseMsgs.empty()); +} diff --git a/test/integration/save_world.cc b/test/integration/save_world.cc index 03fba54533..2ab31e760f 100644 --- a/test/integration/save_world.cc +++ b/test/integration/save_world.cc @@ -19,7 +19,11 @@ #include #include +#include +#include +#include #include +#include #include #include @@ -55,7 +59,7 @@ class SdfGeneratorFixture : public ::testing::Test this->server = std::make_unique(serverConfig); EXPECT_FALSE(server->Running()); } - public: std::string RequestGeneratedSdf() + public: std::string RequestGeneratedSdf(const std::string &_worldName) { transport::Node node; msgs::SdfGeneratorConfig req; @@ -63,7 +67,7 @@ class SdfGeneratorFixture : public ::testing::Test msgs::StringMsg worldGenSdfRes; bool result; unsigned int timeout = 5000; - std::string service{"/world/save_world/generate_world_sdf"}; + std::string service{"/world/" + _worldName + "/generate_world_sdf"}; EXPECT_TRUE(node.Request(service, req, timeout, worldGenSdfRes, result)); EXPECT_TRUE(result); return worldGenSdfRes.data(); @@ -134,7 +138,7 @@ TEST_F(SdfGeneratorFixture, WorldWithModelsSpawnedAfterLoad) EXPECT_TRUE(this->server->EntityByName("spawned_model").has_value()); EXPECT_NE(kNullEntity, this->server->EntityByName("spawned_model")); - const std::string worldGenSdfRes = this->RequestGeneratedSdf(); + const std::string worldGenSdfRes = this->RequestGeneratedSdf("save_world"); sdf::Root root; sdf::Errors err = root.LoadSdfString(worldGenSdfRes); EXPECT_TRUE(err.empty()); @@ -231,7 +235,7 @@ TEST_F(SdfGeneratorFixture, EXPECT_TRUE(this->server->EntityByName("new_model_name").has_value()); EXPECT_NE(kNullEntity, this->server->EntityByName("new_model_name")); - const std::string worldGenSdfRes = this->RequestGeneratedSdf(); + const std::string worldGenSdfRes = this->RequestGeneratedSdf("save_world"); sdf::Root root; sdf::Errors err = root.LoadSdfString(worldGenSdfRes); EXPECT_TRUE(err.empty()); @@ -240,6 +244,63 @@ TEST_F(SdfGeneratorFixture, EXPECT_TRUE(world->ModelNameExists("new_model_name")); } +///////////////////////////////////////////////// +TEST_F(SdfGeneratorFixture, WorldWithNestedModel) +{ + this->LoadWorld("test/worlds/nested_model.sdf"); + + EXPECT_NE(kNullEntity, this->server->EntityByName("model_00")); + EXPECT_NE(kNullEntity, this->server->EntityByName("link_00")); + EXPECT_NE(kNullEntity, this->server->EntityByName("collision_00")); + EXPECT_NE(kNullEntity, this->server->EntityByName("visual_00")); + EXPECT_NE(kNullEntity, this->server->EntityByName("model_01")); + EXPECT_NE(kNullEntity, this->server->EntityByName("link_01")); + EXPECT_NE(kNullEntity, this->server->EntityByName("collision_01")); + EXPECT_NE(kNullEntity, this->server->EntityByName("visual_01")); + + const std::string worldGenSdfRes = + this->RequestGeneratedSdf("nested_model_world"); + + sdf::Root root; + sdf::Errors err = root.LoadSdfString(worldGenSdfRes); + EXPECT_TRUE(err.empty()); + auto *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + EXPECT_EQ(1u, world->ModelCount()); + + EXPECT_TRUE(world->ModelNameExists("model_00")); + EXPECT_FALSE(world->ModelNameExists("model_01")); + + auto *model00 = world->ModelByName("model_00"); + ASSERT_NE(nullptr, model00); + EXPECT_EQ(1u, model00->LinkCount()); + EXPECT_EQ(1u, model00->ModelCount()); + + auto *link00 = model00->LinkByName("link_00"); + ASSERT_NE(nullptr, link00); + EXPECT_EQ(1u, link00->CollisionCount()); + EXPECT_NE(nullptr, link00->CollisionByName("collision_00")); + EXPECT_EQ(1u, link00->VisualCount()); + EXPECT_NE(nullptr, link00->VisualByName("visual_00")); + + auto *model01 = model00->ModelByName("model_01"); + ASSERT_NE(nullptr, model01); + EXPECT_EQ(1u, model01->LinkCount()); + + auto *link01 = model01->LinkByName("link_01"); + ASSERT_NE(nullptr, link01); + EXPECT_EQ(1u, link01->CollisionCount()); + EXPECT_NE(nullptr, link01->CollisionByName("collision_01")); + EXPECT_EQ(1u, link01->VisualCount()); + EXPECT_NE(nullptr, link01->VisualByName("visual_01")); + + tinyxml2::XMLDocument genSdfDoc; + genSdfDoc.Parse(worldGenSdfRes.c_str()); + ASSERT_NE(nullptr, genSdfDoc.RootElement()); + auto genWorld = genSdfDoc.RootElement()->FirstChildElement("world"); + ASSERT_NE(nullptr, genWorld); +} + ///////////////////////////////////////////////// /// Main int main(int _argc, char **_argv) diff --git a/test/worlds/log_record_dbl_pendulum.sdf b/test/worlds/log_record_dbl_pendulum.sdf index df85f23840..b93844b5fc 100644 --- a/test/worlds/log_record_dbl_pendulum.sdf +++ b/test/worlds/log_record_dbl_pendulum.sdf @@ -268,6 +268,28 @@ + + + 2 0 0.5 0 0 0 + + 0 0 0 0 0 0 + + + + 0.5 + + + + + + + 0.5 + + + + + + diff --git a/test/worlds/nested_model.sdf b/test/worlds/nested_model.sdf new file mode 100644 index 0000000000..b22c0dd17d --- /dev/null +++ b/test/worlds/nested_model.sdf @@ -0,0 +1,77 @@ + + + + + 0.001 + 1.0 + + + libignition-physics-tpe-plugin.so + + + + + + 1.0 1.0 1.0 + 0.8 0.8 0.8 + + + + 0 0 0.5 0 0 0 + + 0 0 0 0 0 0 + + + + 0.5 + + + + + + + 0.5 + + + + + + + 1 0 0.0 0 0 0 + + 0.25 0 0.0 0 0 0 + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + true + false + false + false + false + 100 + + + + + +