From e5c3ee9a69d5781bddfc5379836c26b2f35b98c0 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 23 Nov 2022 23:46:51 +0800 Subject: [PATCH] Fixes buoyancy flakiness when spawning entities (#1808) * Fixes buoyancy flakiness when spawning entities For reference see https://github.com/osrf/lrauv/pull/272#issuecomment-1323426611 Essentially, if we spawn an entity using the `UserCommand` system, it will create the entity during the `PreUpdate` phase. However, the `Buoyancy` system only checks for new entities during the `PreUpdate` phase (It does this to precompute volume so it does not need to be computed on every run). This means the buoyancy system may or may not catch the newly created entities. Signed-off-by: Arjo Chakravarty --- src/systems/buoyancy/Buoyancy.cc | 333 +++++++++++++++++++------------ src/systems/buoyancy/Buoyancy.hh | 12 +- 2 files changed, 209 insertions(+), 136 deletions(-) diff --git a/src/systems/buoyancy/Buoyancy.cc b/src/systems/buoyancy/Buoyancy.cc index 55f44bfded..a018c1bf36 100644 --- a/src/systems/buoyancy/Buoyancy.cc +++ b/src/systems/buoyancy/Buoyancy.cc @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -88,6 +89,23 @@ class gz::sim::systems::BuoyancyPrivate void GradedFluidDensity( const math::Pose3d &_pose, const T &_shape, const math::Vector3d &_gravity); + /// \brief Check for new links to apply buoyancy forces to. Calculates the + /// volume and center of volume for every new link and stages them to be + /// commited when `CommitNewEntities` is called. + /// \param[in] _ecm The Entity Component Manager. + public: void CheckForNewEntities(const EntityComponentManager &_ecm); + + /// \brief Commits the new entities to the ECM. + /// \param[in] _ecm The Entity Component Manager. + public: void CommitNewEntities(EntityComponentManager &_ecm); + + /// \brief Check if an entity is enabled or not. + /// \param[in] _entity Target entity + /// \param[in] _ecm Entity component manager + /// \return True if buoyancy should be applied. + public: bool IsEnabled(Entity _entity, + const EntityComponentManager &_ecm) const; + /// \brief Model interface public: Entity world{kNullEntity}; @@ -136,6 +154,12 @@ class gz::sim::systems::BuoyancyPrivate /// \brief Scoped names of entities that buoyancy should apply to. If empty, /// all links will receive buoyancy. public: std::unordered_set enabled; + + /// \brief Center of volumes to be added on the next Pre-update + public: std::unordered_map centerOfVolumes; + + /// \brief Volumes to be added on the next. + public: std::unordered_map volumes; }; ////////////////////////////////////////////////// @@ -245,6 +269,165 @@ std::pair BuoyancyPrivate::ResolveForces( return {force, torque}; } +////////////////////////////////////////////////// +void BuoyancyPrivate::CheckForNewEntities(const EntityComponentManager &_ecm) +{ + // Compute the volume and center of volume for each new link + _ecm.EachNew( + [&](const Entity &_entity, + const components::Link *, + const components::Inertial *) -> bool + { + // Skip if the entity already has a volume and center of volume + if (_ecm.EntityHasComponentType(_entity, + components::CenterOfVolume().TypeId()) && + _ecm.EntityHasComponentType(_entity, + components::Volume().TypeId())) + { + return true; + } + + if (!this->IsEnabled(_entity, _ecm)) + { + return true; + } + + Link link(_entity); + + std::vector collisions = _ecm.ChildrenByComponents( + _entity, components::Collision()); + + double volumeSum = 0; + gz::math::Vector3d weightedPosInLinkSum = + gz::math::Vector3d::Zero; + + // Compute the volume of the link by iterating over all the collision + // elements and storing each geometry's volume. + for (const Entity &collision : collisions) + { + double volume = 0; + const components::CollisionElement *coll = + _ecm.Component(collision); + + if (!coll) + { + gzerr << "Invalid collision pointer. This shouldn't happen\n"; + continue; + } + + switch (coll->Data().Geom()->Type()) + { + case sdf::GeometryType::BOX: + volume = coll->Data().Geom()->BoxShape()->Shape().Volume(); + break; + case sdf::GeometryType::SPHERE: + volume = coll->Data().Geom()->SphereShape()->Shape().Volume(); + break; + case sdf::GeometryType::CYLINDER: + volume = coll->Data().Geom()->CylinderShape()->Shape().Volume(); + break; + case sdf::GeometryType::PLANE: + // Ignore plane shapes. They have no volume and are not expected + // to be buoyant. + break; + case sdf::GeometryType::MESH: + { + std::string file = asFullPath( + coll->Data().Geom()->MeshShape()->Uri(), + coll->Data().Geom()->MeshShape()->FilePath()); + if (common::MeshManager::Instance()->IsValidFilename(file)) + { + const common::Mesh *mesh = + common::MeshManager::Instance()->Load(file); + if (mesh) + volume = mesh->Volume(); + else + gzerr << "Unable to load mesh[" << file << "]\n"; + } + else + { + gzerr << "Invalid mesh filename[" << file << "]\n"; + } + break; + } + default: + gzerr << "Unsupported collision geometry[" + << static_cast(coll->Data().Geom()->Type()) << "]\n"; + break; + } + + volumeSum += volume; + auto poseInLink = _ecm.Component(collision)->Data(); + weightedPosInLinkSum += volume * poseInLink.Pos(); + } + + if (volumeSum > 0) + { + // Stage calculation results for future commit. We do this because + // during PostUpdate the ECM is const, so we can't modify it, + this->centerOfVolumes[_entity] = weightedPosInLinkSum / volumeSum; + this->volumes[_entity] = volumeSum; + } + + return true; + }); +} + +////////////////////////////////////////////////// +void BuoyancyPrivate::CommitNewEntities(EntityComponentManager &_ecm) +{ + for (const auto [_entity, _cov] : this->centerOfVolumes) + { + if (_ecm.HasEntity(_entity)) + { + _ecm.CreateComponent(_entity, components::CenterOfVolume(_cov)); + } + } + + for (const auto [_entity, _vol] : this->volumes) + { + if (_ecm.HasEntity(_entity)) + { + _ecm.CreateComponent(_entity, components::Volume(_vol)); + } + } + + this->centerOfVolumes.clear(); + this->volumes.clear(); +} + +////////////////////////////////////////////////// +bool BuoyancyPrivate::IsEnabled(Entity _entity, + const EntityComponentManager &_ecm) const +{ + // If there's nothing enabled, all entities are enabled + if (this->enabled.empty()) + return true; + + auto entity = _entity; + while (entity != kNullEntity) + { + // Fully scoped name + auto name = scopedName(entity, _ecm, "::", false); + + // Remove world name + name = removeParentScope(name, "::"); + + if (this->enabled.find(name) != this->enabled.end()) + return true; + + // Check parent + auto parentComp = _ecm.Component(entity); + + if (nullptr == parentComp) + return false; + + entity = parentComp->Data(); + } + + return false; +} + ////////////////////////////////////////////////// Buoyancy::Buoyancy() : dataPtr(std::make_unique()) @@ -341,6 +524,12 @@ void Buoyancy::PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &_ecm) { GZ_PROFILE("Buoyancy::PreUpdate"); + this->dataPtr->CheckForNewEntities(_ecm); + this->dataPtr->CommitNewEntities(_ecm); + // Only update if not paused. + if (_info.paused) + return; + const components::Gravity *gravity = _ecm.Component( this->dataPtr->world); if (!gravity) @@ -350,112 +539,6 @@ void Buoyancy::PreUpdate(const gz::sim::UpdateInfo &_info, return; } - // Compute the volume and center of volume for each new link - _ecm.EachNew( - [&](const Entity &_entity, - const components::Link *, - const components::Inertial *) -> bool - { - // Skip if the entity already has a volume and center of volume - if (_ecm.EntityHasComponentType(_entity, - components::CenterOfVolume().TypeId()) && - _ecm.EntityHasComponentType(_entity, - components::Volume().TypeId())) - { - return true; - } - - if (!this->IsEnabled(_entity, _ecm)) - { - return true; - } - - Link link(_entity); - - std::vector collisions = _ecm.ChildrenByComponents( - _entity, components::Collision()); - - double volumeSum = 0; - gz::math::Vector3d weightedPosInLinkSum = - gz::math::Vector3d::Zero; - - // Compute the volume of the link by iterating over all the collision - // elements and storing each geometry's volume. - for (const Entity &collision : collisions) - { - double volume = 0; - const components::CollisionElement *coll = - _ecm.Component(collision); - - if (!coll) - { - gzerr << "Invalid collision pointer. This shouldn't happen\n"; - continue; - } - - switch (coll->Data().Geom()->Type()) - { - case sdf::GeometryType::BOX: - volume = coll->Data().Geom()->BoxShape()->Shape().Volume(); - break; - case sdf::GeometryType::SPHERE: - volume = coll->Data().Geom()->SphereShape()->Shape().Volume(); - break; - case sdf::GeometryType::CYLINDER: - volume = coll->Data().Geom()->CylinderShape()->Shape().Volume(); - break; - case sdf::GeometryType::PLANE: - // Ignore plane shapes. They have no volume and are not expected - // to be buoyant. - break; - case sdf::GeometryType::MESH: - { - std::string file = asFullPath( - coll->Data().Geom()->MeshShape()->Uri(), - coll->Data().Geom()->MeshShape()->FilePath()); - if (common::MeshManager::Instance()->IsValidFilename(file)) - { - const common::Mesh *mesh = - common::MeshManager::Instance()->Load(file); - if (mesh) - volume = mesh->Volume(); - else - gzerr << "Unable to load mesh[" << file << "]\n"; - } - else - { - gzerr << "Invalid mesh filename[" << file << "]\n"; - } - break; - } - default: - gzerr << "Unsupported collision geometry[" - << static_cast(coll->Data().Geom()->Type()) << "]\n"; - break; - } - - volumeSum += volume; - auto poseInLink = _ecm.Component(collision)->Data(); - weightedPosInLinkSum += volume * poseInLink.Pos(); - } - - if (volumeSum > 0) - { - // Store the center of volume expressed in the link frame - _ecm.CreateComponent(_entity, components::CenterOfVolume( - weightedPosInLinkSum / volumeSum)); - - // Store the volume - _ecm.CreateComponent(_entity, components::Volume(volumeSum)); - } - - return true; - }); - - // Only update if not paused. - if (_info.paused) - return; - _ecm.Each( @@ -548,42 +631,26 @@ void Buoyancy::PreUpdate(const gz::sim::UpdateInfo &_info, }); } +////////////////////////////////////////////////// +void Buoyancy::PostUpdate( + const UpdateInfo &/*_info*/, + const EntityComponentManager &_ecm) +{ + this->dataPtr->CheckForNewEntities(_ecm); +} + ////////////////////////////////////////////////// bool Buoyancy::IsEnabled(Entity _entity, const EntityComponentManager &_ecm) const { - // If there's nothing enabled, all entities are enabled - if (this->dataPtr->enabled.empty()) - return true; - - auto entity = _entity; - while (entity != kNullEntity) - { - // Fully scoped name - auto name = scopedName(entity, _ecm, "::", false); - - // Remove world name - name = removeParentScope(name, "::"); - - if (this->dataPtr->enabled.find(name) != this->dataPtr->enabled.end()) - return true; - - // Check parent - auto parentComp = _ecm.Component(entity); - - if (nullptr == parentComp) - return false; - - entity = parentComp->Data(); - } - - return false; + return this->IsEnabled(_entity, _ecm); } GZ_ADD_PLUGIN(Buoyancy, gz::sim::System, Buoyancy::ISystemConfigure, - Buoyancy::ISystemPreUpdate) + Buoyancy::ISystemPreUpdate, + Buoyancy::ISystemPostUpdate) GZ_ADD_PLUGIN_ALIAS(Buoyancy, "gz::sim::systems::Buoyancy") diff --git a/src/systems/buoyancy/Buoyancy.hh b/src/systems/buoyancy/Buoyancy.hh index 858b1e7775..5eae0f0864 100644 --- a/src/systems/buoyancy/Buoyancy.hh +++ b/src/systems/buoyancy/Buoyancy.hh @@ -113,7 +113,8 @@ namespace systems class Buoyancy : public System, public ISystemConfigure, - public ISystemPreUpdate + public ISystemPreUpdate, + public ISystemPostUpdate { /// \brief Constructor public: Buoyancy(); @@ -129,8 +130,13 @@ namespace systems // Documentation inherited public: void PreUpdate( - const gz::sim::UpdateInfo &_info, - gz::sim::EntityComponentManager &_ecm) override; + const UpdateInfo &_info, + EntityComponentManager &_ecm) override; + + // Documentation inherited + public: void PostUpdate( + const UpdateInfo &_info, + const EntityComponentManager &_ecm) override; /// \brief Check if an entity is enabled or not. /// \param[in] _entity Target entity