Skip to content

Commit

Permalink
Fixes buoyancy flakiness when spawning entities (gazebosim#1808)
Browse files Browse the repository at this point in the history 
* Fixes buoyancy flakiness when spawning entities

For reference see osrf/lrauv#272 (comment)

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 <[email protected]>
  • Loading branch information
@arjo129
arjo129 authored Nov 23, 2022
1 parent 3d94c0b commit e5c3ee9
Show file tree
Hide file tree
Showing 2 changed files with 209 additions and 136 deletions.
333 changes: 200 additions & 133 deletions src/systems/buoyancy/Buoyancy.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
#include <map>
#include <mutex>
#include <string>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>
Expand Down Expand Up @@ -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};

Expand Down Expand Up @@ -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<std::string> enabled;

/// \brief Center of volumes to be added on the next Pre-update
public: std::unordered_map<Entity, math::Vector3d> centerOfVolumes;

/// \brief Volumes to be added on the next.
public: std::unordered_map<Entity, double> volumes;
};

//////////////////////////////////////////////////
Expand Down Expand Up @@ -245,6 +269,165 @@ std::pair<math::Vector3d, math::Vector3d> BuoyancyPrivate::ResolveForces(
return {force, torque};
}

//////////////////////////////////////////////////
void BuoyancyPrivate::CheckForNewEntities(const EntityComponentManager &_ecm)
{
// Compute the volume and center of volume for each new link
_ecm.EachNew<components::Link, components::Inertial>(
[&](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<Entity> 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<components::CollisionElement>(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<int>(coll->Data().Geom()->Type()) << "]\n";
break;
}

volumeSum += volume;
auto poseInLink = _ecm.Component<components::Pose>(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<components::ParentEntity>(entity);

if (nullptr == parentComp)
return false;

entity = parentComp->Data();
}

return false;
}

//////////////////////////////////////////////////
Buoyancy::Buoyancy()
: dataPtr(std::make_unique<BuoyancyPrivate>())
Expand Down Expand Up @@ -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<components::Gravity>(
this->dataPtr->world);
if (!gravity)
Expand All @@ -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<components::Link, components::Inertial>(
[&](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<Entity> 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<components::CollisionElement>(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<int>(coll->Data().Geom()->Type()) << "]\n";
break;
}

volumeSum += volume;
auto poseInLink = _ecm.Component<components::Pose>(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<components::Link,
components::Volume,
components::CenterOfVolume>(
Expand Down Expand Up @@ -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<components::ParentEntity>(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")
Expand Down
Loading

0 comments on commit e5c3ee9

Please sign in to comment.