diff --git a/src/systems/buoyancy/Buoyancy.cc b/src/systems/buoyancy/Buoyancy.cc index ff91c83471..fc544941b3 100644 --- a/src/systems/buoyancy/Buoyancy.cc +++ b/src/systems/buoyancy/Buoyancy.cc @@ -441,11 +441,6 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info, { auto newPose = enableComponent(_ecm, _entity); newPose |= enableComponent(_ecm, _entity); - if (newPose) - { - // Skip entity if WorldPose and inertial are not yet ready - return true; - } // World pose of the link. math::Pose3d linkWorldPose = worldPose(_entity, _ecm); @@ -477,6 +472,13 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info, else if (this->dataPtr->buoyancyType == BuoyancyPrivate::BuoyancyType::GRADED_BUOYANCY) { + if (newPose) + { + // Skip entity if WorldPose and inertial are not yet ready + // TODO(arjo): Find a way of disabling gravity effects for + // this first iteration. + return true; + } std::vector collisions = _ecm.ChildrenByComponents( _entity, components::Collision()); this->dataPtr->buoyancyForces.clear(); @@ -521,12 +523,13 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info, } } } - } - auto [force, torque] = this->dataPtr->ResolveForces( + auto [force, torque] = this->dataPtr->ResolveForces( link.WorldInertialPose(_ecm).value()); - // Apply the wrench to the link. This wrench is applied in the - // Physics System. - link.AddWorldWrench(_ecm, force, torque); + // Apply the wrench to the link. This wrench is applied in the + // Physics System. + link.AddWorldWrench(_ecm, force, torque); + } + return true; }); } diff --git a/test/integration/buoyancy.cc b/test/integration/buoyancy.cc index 5ab6ba4ca2..481c7d29ee 100644 --- a/test/integration/buoyancy.cc +++ b/test/integration/buoyancy.cc @@ -57,7 +57,7 @@ TEST_F(BuoyancyTest, UniformWorldMovement) using namespace std::chrono_literals; server.SetUpdatePeriod(1ns); - std::size_t iterations = 1001; + std::size_t iterations = 1000; bool finished = false; test::Relay testSystem;