From e42e3060f88f24926adf5bbf6f6436c13bbb4eef Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 16 Nov 2021 20:02:26 +0800 Subject: [PATCH 1/3] Don't waite on new inertia Signed-off-by: Arjo Chakravarty --- src/systems/buoyancy/Buoyancy.cc | 24 ++++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/src/systems/buoyancy/Buoyancy.cc b/src/systems/buoyancy/Buoyancy.cc index ff91c83471..de16a13b53 100644 --- a/src/systems/buoyancy/Buoyancy.cc +++ b/src/systems/buoyancy/Buoyancy.cc @@ -441,11 +441,7 @@ 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 +473,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 +524,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; }); } From adfb751542419476f38bcd2217445a53650065dd Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 24 Nov 2021 14:41:50 +0800 Subject: [PATCH 2/3] codecheck Signed-off-by: Arjo Chakravarty --- src/systems/buoyancy/Buoyancy.cc | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/systems/buoyancy/Buoyancy.cc b/src/systems/buoyancy/Buoyancy.cc index de16a13b53..fc544941b3 100644 --- a/src/systems/buoyancy/Buoyancy.cc +++ b/src/systems/buoyancy/Buoyancy.cc @@ -441,7 +441,6 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info, { auto newPose = enableComponent(_ecm, _entity); newPose |= enableComponent(_ecm, _entity); - // World pose of the link. math::Pose3d linkWorldPose = worldPose(_entity, _ecm); @@ -530,7 +529,7 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info, // Physics System. link.AddWorldWrench(_ecm, force, torque); } - + return true; }); } From eb842e00187cdbf945e941ab75c30a56351dcd25 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 24 Nov 2021 16:00:04 +0800 Subject: [PATCH 3/3] fix unit tests Signed-off-by: Arjo Chakravarty --- test/integration/buoyancy.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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;