diff --git a/scenario/src/plugins/Physics/Physics.cc b/scenario/src/plugins/Physics/Physics.cc index 720a85ea8..dee160a1a 100644 --- a/scenario/src/plugins/Physics/Physics.cc +++ b/scenario/src/plugins/Physics/Physics.cc @@ -32,6 +32,8 @@ #include #include +#include +#include #include #include #include @@ -41,6 +43,7 @@ #include #include #include +#include #include #include @@ -71,6 +74,7 @@ // SDF #include +#include #include #include #include @@ -79,6 +83,7 @@ #include #include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/Model.hh" #include "ignition/gazebo/Util.hh" // Components @@ -154,7 +159,7 @@ class ignition::gazebo::systems::PhysicsPrivate ignition::physics::FreeGroupFrameSemantics, ignition::physics::LinkFrameSemantics, ignition::physics::ForwardStep, - ignition::physics::RemoveEntities, + ignition::physics::RemoveModelFromWorld, ignition::physics::sdf::ConstructSdfLink, ignition::physics::sdf::ConstructSdfModel, ignition::physics::sdf::ConstructSdfWorld @@ -184,6 +189,30 @@ class ignition::gazebo::systems::PhysicsPrivate /// \param[in] _ecm Constant reference to ECM. public: void CreatePhysicsEntities(const EntityComponentManager &_ecm); + /// \brief Create world entities + /// \param[in] _ecm Constant reference to ECM. + public: void CreateWorldEntities(const EntityComponentManager &_ecm); + + /// \brief Create model entities + /// \param[in] _ecm Constant reference to ECM. + public: void CreateModelEntities(const EntityComponentManager &_ecm); + + /// \brief Create link entities + /// \param[in] _ecm Constant reference to ECM. + public: void CreateLinkEntities(const EntityComponentManager &_ecm); + + /// \brief Create collision entities + /// \param[in] _ecm Constant reference to ECM. + public: void CreateCollisionEntities(const EntityComponentManager &_ecm); + + /// \brief Create joint entities + /// \param[in] _ecm Constant reference to ECM. + public: void CreateJointEntities(const EntityComponentManager &_ecm); + + /// \brief Create Battery entities + /// \param[in] _ecm Constant reference to ECM. + public: void CreateBatteryEntities(const EntityComponentManager &_ecm); + /// \brief Remove physics entities if they are removed from the ECM /// \param[in] _ecm Constant reference to ECM. public: void RemovePhysicsEntities(const EntityComponentManager &_ecm); @@ -214,14 +243,36 @@ class ignition::gazebo::systems::PhysicsPrivate EntityComponentManager &_ecm, const ignition::physics::ForwardStep::Output &_updatedLinks); + /// \brief Helper function to update the pose of a model. + /// \param[in] _model The model to update. + /// \param[in] _canonicalLink The canonical link of _model. + /// \param[in] _ecm The entity component manager. + /// \param[in, out] _linkFrameData Links that experienced a pose change in the + /// most recent physics step. The key is the entity of the link, and the + /// value is the updated frame data corresponding to that entity. The + /// canonical links of _model's nested models are added to _linkFrameData to + /// ensure that all of _model's nested models are marked as models to be + /// updated (if a parent model's pose changes, all nested model poses must be + /// updated since nested model poses are saved w.r.t. the parent model). + public: void UpdateModelPose(const Entity _model, + const Entity _canonicalLink, EntityComponentManager &_ecm, + std::map &_linkFrameData); + + /// \brief Get an entity's frame data relative to world from physics. + /// \param[in] _entity The entity. + /// \param[in, out] _data The frame data to populate. + /// \return True if _data was populated with frame data for _entity, false + /// otherwise. + public: bool GetFrameDataRelativeToWorld(const Entity _entity, + physics::FrameData3d &_data); + /// \brief Update components from physics simulation /// \param[in] _ecm Mutable reference to ECM. - /// \param[in] _linkFrameData Links that experienced a pose change in the + /// \param[in, out] _linkFrameData Links that experienced a pose change in the /// most recent physics step. The key is the entity of the link, and the /// value is the updated frame data corresponding to that entity. public: void UpdateSim(EntityComponentManager &_ecm, - const std::map< - Entity, physics::FrameData3d> &_linkFrameData, + std::map &_linkFrameData, const ignition::gazebo::UpdateInfo &_info); /// \brief Update collision components from physics simulation @@ -440,6 +491,16 @@ class ignition::gazebo::systems::PhysicsPrivate CollisionFeatureList, physics::mesh::AttachMeshShapeFeature>{}; + ////////////////////////////////////////////////// + // Heightmap + + /// \brief Feature list for heightmaps. + /// Include MinimumFeatureList so created collision can be automatically + /// up-cast. + public: struct HeightmapFeatureList : ignition::physics::FeatureList< + CollisionFeatureList, + physics::heightmap::AttachHeightmapShapeFeature>{}; + ////////////////////////////////////////////////// // Collision detector /// \brief Feature list for setting and getting the collision detector @@ -492,6 +553,7 @@ class ignition::gazebo::systems::PhysicsPrivate MinimumFeatureList, DetachableJointFeatureList, CollisionFeatureList, + HeightmapFeatureList, LinkForceFeatureList, MeshFeatureList>; @@ -707,9 +769,28 @@ void Physics::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) ////////////////////////////////////////////////// void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) +{ + this->CreateWorldEntities(_ecm); + this->CreateModelEntities(_ecm); + this->CreateLinkEntities(_ecm); + // We don't need to add visuals to the physics engine. + this->CreateCollisionEntities(_ecm); + this->CreateJointEntities(_ecm); + this->CreateBatteryEntities(_ecm); + + // Make sure that entities are processed by the plugin + // in the first iteration. This is necessary because + // the physics plugin can be loaded programmatically + // after the simulation has started and stepped. + if (this->firstRun) + this->firstRun = false; +} + +////////////////////////////////////////////////// +void PhysicsPrivate::CreateWorldEntities(const EntityComponentManager &_ecm) { // Get all the new worlds - auto processWorld = + auto processEntities = [&](const Entity &_entity, const components::World * /* _world */, const components::Name *_name, @@ -785,7 +866,22 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) return true; }; - auto processModel = + if (this->firstRun) + { + _ecm.Each( + processEntities); + } + else + { + _ecm.EachNew( + processEntities); + } +} + +////////////////////////////////////////////////// +void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm) +{ + auto processEntities = [&](const Entity &_entity, const components::Model *, const components::Name *_name, @@ -912,7 +1008,26 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) return true; }; - auto processLink = + if (this->firstRun) + { + _ecm.Each(processEntities); + } + else + { + _ecm.EachNew(processEntities); + } +} + +////////////////////////////////////////////////// +void PhysicsPrivate::CreateLinkEntities(const EntityComponentManager &_ecm) +{ + auto processEntities = [&](const Entity &_entity, const components::Link * /* _link */, const components::Name *_name, @@ -965,10 +1080,26 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) return true; }; - // We don't need to add visuals to the physics engine. + if (this->firstRun) + { + _ecm.Each(processEntities); + } + else + { + _ecm.EachNew(processEntities); + } +} - // collisions - auto processCollision = +////////////////////////////////////////////////// +void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm) +{ + auto processEntities = [&](const Entity &_entity, const components::Collision *, const components::Name *_name, @@ -1044,6 +1175,56 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) math::eigen3::convert(_pose->Data()), math::eigen3::convert(meshSdf->Scale())); } + else if (_geom->Data().Type() == sdf::GeometryType::HEIGHTMAP) + { + auto linkHeightmapFeature = + this->entityLinkMap.EntityCast( + _parent->Data()); + if (!linkHeightmapFeature) + { + static bool informed{false}; + if (!informed) + { + igndbg << "Attempting to process heightmap geometries, but the " + << "physics engine doesn't support feature " + << "[AttachHeightmapShapeFeature]. Heightmaps will be " + << "ignored." << std::endl; + informed = true; + } + return true; + } + + auto heightmapSdf = _geom->Data().HeightmapShape(); + if (nullptr == heightmapSdf) + { + ignwarn << "Heightmap geometry for collision [" << _name->Data() + << "] missing heightmap shape." << std::endl; + return true; + } + + auto fullPath = asFullPath(heightmapSdf->Uri(), + heightmapSdf->FilePath()); + if (fullPath.empty()) + { + ignerr << "Heightmap geometry missing URI" << std::endl; + return true; + } + + common::ImageHeightmap data; + if (data.Load(fullPath) < 0) + { + ignerr << "Failed to load heightmap image data from [" << fullPath + << "]" << std::endl; + return true; + } + + collisionPtrPhys = linkHeightmapFeature->AttachHeightmapShape( + _name->Data(), + data, + math::eigen3::convert(_pose->Data()), + math::eigen3::convert(heightmapSdf->Size()), + heightmapSdf->Sampling()); + } else { auto linkCollisionFeature = @@ -1067,6 +1248,14 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) linkCollisionFeature->ConstructCollision(collision); } + if (nullptr == collisionPtrPhys) + { + igndbg << "Failed to create collision [" << _name->Data() + << "]. Does the physics engine support geometries of type [" + << static_cast(_geom->Data().Type()) << "]?" << std::endl; + return true; + } + this->entityCollisionMap.AddEntity(_entity, collisionPtrPhys); // Check that the physics engine has a filter mask feature @@ -1095,8 +1284,30 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) return true; }; - // joints - auto processJoint = + if (this->firstRun) + { + _ecm.Each(processEntities); + } + else + { + _ecm.EachNew(processEntities); + } +} + +////////////////////////////////////////////////// +void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm) +{ + auto processJointEntities = [&](const Entity &_entity, const components::Joint * /* _joint */, const components::Name *_name, @@ -1177,17 +1388,8 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) return true; }; - auto processBatterySoC = - [&](const Entity & _entity, const components::BatterySoC *)->bool - { - // Parent entity of battery is model entity - this->entityOffMap.insert(std::make_pair( - _ecm.ParentEntity(_entity), false)); - return true; - }; - // Detachable joints - auto processDetachableJoint = + auto processDetachableJointEntities = [&](const Entity &_entity, const components::DetachableJoint *_jointInfo) -> bool { @@ -1276,28 +1478,6 @@ void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) if (this->firstRun) { - this->firstRun = false; - - _ecm.Each( - processWorld); - - _ecm.Each(processModel); - - _ecm.Each(processLink); - - _ecm.Each(processCollision); - _ecm.Each(processJoint); - - _ecm.Each(processBatterySoC); - - _ecm.Each(processDetachableJoint); + components::ChildLinkName>(processJointEntities); + _ecm.Each(processDetachableJointEntities); } else { - _ecm.EachNew( - processWorld); - - _ecm.EachNew(processModel); - - _ecm.EachNew(processLink); - - _ecm.EachNew(processCollision); - _ecm.EachNew(processJoint); + components::ChildLinkName>(processJointEntities); + _ecm.EachNew(processDetachableJointEntities); + } +} - _ecm.EachNew(processBatterySoC); +////////////////////////////////////////////////// +void PhysicsPrivate::CreateBatteryEntities(const EntityComponentManager &_ecm) +{ + auto processEntities = + [&](const Entity & _entity, const components::BatterySoC *)->bool + { + // Parent entity of battery is model entity + this->entityOffMap.insert(std::make_pair( + _ecm.ParentEntity(_entity), false)); + return true; + }; - _ecm.EachNew(processDetachableJoint); + if (this->firstRun) + { + _ecm.Each(processEntities); + } + else + { + _ecm.EachNew(processEntities); } } @@ -2159,15 +2335,15 @@ std::map PhysicsPrivate::ChangedLinks( // (if the link pose hasn't changed, there's no need for a pose update) const auto worldPoseMath3d = ignition::math::eigen3::convert( frameData.pose); - // if ((this->linkWorldPoses.find(_entity) == this->linkWorldPoses.end()) - // || !this->pose3Eql(this->linkWorldPoses[_entity], worldPoseMath3d)) - // { + if ((this->linkWorldPoses.find(_entity) == this->linkWorldPoses.end()) + || !this->pose3Eql(this->linkWorldPoses[_entity], worldPoseMath3d)) + { // cache the updated link pose to check if the link pose has changed // during the next iteration this->linkWorldPoses[_entity] = worldPoseMath3d; linkFrameData[_entity] = frameData; - // } + } return true; }); @@ -2176,9 +2352,138 @@ std::map PhysicsPrivate::ChangedLinks( return linkFrameData; } +////////////////////////////////////////////////// +void PhysicsPrivate::UpdateModelPose(const Entity _model, + const Entity _canonicalLink, EntityComponentManager &_ecm, + std::map &_linkFrameData) +{ + std::optional parentWorldPose; + + // If this model is nested, the pose of the parent model has already + // been updated since we iterate through the modified links in + // topological order. We expect to find the updated pose in + // this->modelWorldPoses. If not found, this must not be nested, so this + // model's pose component would reflect it's absolute pose. + auto parentModelPoseIt = + this->modelWorldPoses.find( + _ecm.Component(_model)->Data()); + if (parentModelPoseIt != this->modelWorldPoses.end()) + { + parentWorldPose = parentModelPoseIt->second; + } + + // Given the following frame names: + // W: World/inertial frame + // P: Parent frame (this could be a parent model or the World frame) + // M: This model's frame + // L: The frame of this model's canonical link + // + // And the following quantities: + // (See http://sdformat.org/tutorials?tut=specify_pose for pose + // convention) + // parentWorldPose (X_WP): Pose of the parent frame w.r.t the world + // linkPoseFromModel (X_ML): Pose of the canonical link frame w.r.t the + // model frame + // linkWorldPose (X_WL): Pose of the canonical link w.r.t the world + // modelWorldPose (X_WM): Pose of this model w.r.t the world + // + // The Pose component of this model entity stores the pose of M w.r.t P + // (X_PM) and is calculated as + // X_PM = (X_WP)^-1 * X_WM + // + // And X_WM is calculated from X_WL, which is obtained from physics as: + // X_WM = X_WL * (X_ML)^-1 + auto linkPoseFromModel = this->RelativePose(_model, _canonicalLink, _ecm); + const auto &linkWorldPose = _linkFrameData[_canonicalLink].pose; + const auto &modelWorldPose = + math::eigen3::convert(linkWorldPose) * linkPoseFromModel.Inverse(); + + this->modelWorldPoses[_model] = modelWorldPose; + + // update model's pose + auto modelPose = _ecm.Component(_model); + if (parentWorldPose) + { + *modelPose = + components::Pose(parentWorldPose->Inverse() * modelWorldPose); + } + else + { + // This is a non-nested model and parentWorldPose would be identity + // because it would be the pose of the parent (world) w.r.t the world. + *modelPose = components::Pose(modelWorldPose); + } + + _ecm.SetChanged(_model, components::Pose::typeId, + ComponentState::PeriodicChange); + + // once the model pose has been updated, all descendant link poses of this + // model must be updated (whether the link actually changed pose or not) + // since link poses are saved w.r.t. their parent model + auto model = gazebo::Model(_model); + for (const auto &childLink : model.Links(_ecm)) + { + // skip links that are already marked as a link to be updated + if (_linkFrameData.find(childLink) != _linkFrameData.end()) + continue; + + physics::FrameData3d childLinkFrameData; + if (!this->GetFrameDataRelativeToWorld(childLink, childLinkFrameData)) + continue; + + _linkFrameData[childLink] = childLinkFrameData; + } + + // since nested model poses are saved w.r.t. the nested model's parent + // pose, we must also update any nested models that have a different + // canonical link + for (const auto &nestedModel : model.Models(_ecm)) + { + auto nestedModelCanonicalLinkComp = + _ecm.Component(nestedModel); + if (!nestedModelCanonicalLinkComp) + { + ignerr << "Model [" << nestedModel << "] has no canonical link\n"; + continue; + } + + auto nestedCanonicalLink = nestedModelCanonicalLinkComp->Data(); + + // skip links that are already marked as a link to be updated + if (nestedCanonicalLink == _canonicalLink || + _linkFrameData.find(nestedCanonicalLink) != _linkFrameData.end()) + continue; + + // mark this canonical link as one that needs to be updated so that all of + // the models that have this canonical link are updated + physics::FrameData3d canonicalLinkFrameData; + if (!this->GetFrameDataRelativeToWorld(nestedCanonicalLink, + canonicalLinkFrameData)) + continue; + + _linkFrameData[nestedCanonicalLink] = canonicalLinkFrameData; + } +} + +////////////////////////////////////////////////// +bool PhysicsPrivate::GetFrameDataRelativeToWorld(const Entity _entity, + physics::FrameData3d &_data) +{ + auto entityPhys = this->entityLinkMap.Get(_entity); + if (nullptr == entityPhys) + { + ignerr << "Internal error: entity [" << _entity + << "] not in entity map" << std::endl; + return false; + } + + _data = entityPhys->FrameDataRelativeToWorld(); + return true; +} + ////////////////////////////////////////////////// void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, - const std::map &_linkFrameData, + std::map &_linkFrameData, const ignition::gazebo::UpdateInfo &_info) { IGN_PROFILE("PhysicsPrivate::UpdateSim"); @@ -2236,69 +2541,15 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, // (linkEntity). Since we have the models in topological order and // _linkFrameData stores links in topological order thanks to the ordering // of std::map (entity IDs are created in ascending order), this should - // properly handle pose updates for nested models - for (auto &model : canonicalLinkModels) - { - std::optional parentWorldPose; - - // If this model is nested, the pose of the parent model has already - // been updated since we iterate through the modified links in - // topological order. We expect to find the updated pose in - // this->modelWorldPoses. If not found, this must not be nested, so this - // model's pose component would reflect it's absolute pose. - auto parentModelPoseIt = - this->modelWorldPoses.find( - _ecm.Component(model)->Data()); - if (parentModelPoseIt != this->modelWorldPoses.end()) - { - parentWorldPose = parentModelPoseIt->second; - } - - // Given the following frame names: - // W: World/inertial frame - // P: Parent frame (this could be a parent model or the World frame) - // M: This model's frame - // L: The frame of this model's canonical link - // - // And the following quantities: - // (See http://sdformat.org/tutorials?tut=specify_pose for pose - // convention) - // parentWorldPose (X_WP): Pose of the parent frame w.r.t the world - // linkPoseFromModel (X_ML): Pose of the canonical link frame w.r.t the - // model frame - // linkWorldPose (X_WL): Pose of the canonical link w.r.t the world - // modelWorldPose (X_WM): Pose of this model w.r.t the world - // - // The Pose component of this model entity stores the pose of M w.r.t P - // (X_PM) and is calculated as - // X_PM = (X_WP)^-1 * X_WM - // - // And X_WM is calculated from X_WL, which is obtained from physics as: - // X_WM = X_WL * (X_ML)^-1 - auto linkPoseFromModel = this->RelativePose(model, linkEntity, _ecm); - const auto &linkWorldPose = frameData.pose; - const auto &modelWorldPose = - math::eigen3::convert(linkWorldPose) * linkPoseFromModel.Inverse(); - - this->modelWorldPoses[model] = modelWorldPose; - - // update model's pose - auto modelPose = _ecm.Component(model); - if (parentWorldPose) - { - *modelPose = - components::Pose(parentWorldPose->Inverse() * modelWorldPose); - } - else - { - // This is a non-nested model and parentWorldPose would be identity - // because it would be the pose of the parent (world) w.r.t the world. - *modelPose = components::Pose(modelWorldPose); - } - - _ecm.SetChanged(model, components::Pose::typeId, - ComponentState::PeriodicChange); - } + // properly handle pose updates for nested models that share the same + // canonical link. + // + // Nested models that don't share the same canonical link will also need to + // be updated since these nested models have their pose saved w.r.t. their + // parent model, which just experienced a pose update. The UpdateModelPose + // method also handles this case. + for (auto &modelEnt : canonicalLinkModels) + this->UpdateModelPose(modelEnt, linkEntity, _ecm, _linkFrameData); } IGN_PROFILE_END();