Fix graded buoyancy problems

examples/worlds/graded_buoyancy.sdf

@@ -43,6 +43,8 @@
       <!-- enable by top level model name -->
       <enable>balloon_lighter_than_air</enable>
       <enable>box_neutral_buoyancy</enable>
+      <enable>box_neutral_buoyancy_collision_offset</enable>
+      <enable>multi_collision_neutral_buoyancy</enable>
       <enable>box_negative_buoyancy</enable>
     </plugin>
 
@@ -216,6 +218,107 @@
       </link>
     </model>
 
+    <!--
+      This box is neutrally buoyant and therefore should stay still.
+      Its link origin is above water, but its collision is below.
+    -->
+    <model name='box_neutral_buoyancy_collision_offset'>
+      <pose>-3 5 2 0 0 0</pose>
+      <link name='body'>
+        <inertial>
+          <mass>1000</mass>
+          <pose>0 0 -4.9 0 0 0</pose>
+          <inertia>
+            <ixx>86.28907821859966</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>86.28907821859966</iyy>
+            <iyz>0</iyz>
+            <izz>5.026548245743671</izz>
+          </inertia>
+        </inertial>
+
+        <visual name='link_origin'>
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </visual>
+        <visual name='collision_origin'>
+          <pose>0 0 -5 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </visual>
+        <collision name='buoyancy_volume'>
+          <pose>0 0 -5 0 0 0</pose>
+          <geometry>
+             <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </collision>
+      </link>
+    </model>
+
+    <!--
+      This box is neutrally buoyant and therefore should stay still.
+      It has multiple collision shapes.
+    -->
+    <model name='multi_collision_neutral_buoyancy'>
+      <pose>-6 5 -3 0 0 0</pose>
+      <link name='body'>
+        <inertial>
+          <mass>1000</mass>
+          <pose>0 0 0.1 0 0 0</pose>
+          <inertia>
+            <ixx>86.28907821859966</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>86.28907821859966</iyy>
+            <iyz>0</iyz>
+            <izz>5.026548245743671</izz>
+          </inertia>
+        </inertial>
+
+        <visual name='collision_1_vis'>
+          <pose>-0.25 0 0 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.5 1 1</size>
+            </box>
+          </geometry>
+        </visual>
+        <visual name='collision_2_vis'>
+          <pose>0.25 0 0 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.5 1 1</size>
+            </box>
+          </geometry>
+        </visual>
+        <collision name='collision_1'>
+          <pose>-0.25 0 0 0 0 0</pose>
+          <geometry>
+             <box>
+              <size>0.5 1 1</size>
+            </box>
+          </geometry>
+        </collision>
+        <collision name='collision_2'>
+          <pose>0.25 0 0 0 0 0</pose>
+          <geometry>
+             <box>
+              <size>0.5 1 1</size>
+            </box>
+          </geometry>
+        </collision>
+      </link>
+    </model>
+
     <!-- This box is negatively buoyant and therefore should sink -->
     <model name='box_negative_buoyancy'>
       <pose>0 -8 0 0 0 0</pose>

src/systems/buoyancy/Buoyancy.cc

@@ -77,9 +77,10 @@ class ignition::gazebo::systems::BuoyancyPrivate
   public: double UniformFluidDensity(const math::Pose3d &_pose) const;
 
   /// \brief Get the resultant buoyant force on a shape.
-  /// \param[in] _pose The pose of the shape.
+  /// \param[in] _pose World pose of the shape's origin.
   /// \param[in] _shape The collision mesh of a shape. Currently must
-  /// be one of box, cylinder or sphere.
+  /// be box or sphere.
+  /// \param[in] _gravity Gravity acceleration in the world frame.
   /// Updates this->buoyancyForces containing {force, center_of_volume} to be
   /// applied on the link.
   public:
@@ -101,28 +102,36 @@ class ignition::gazebo::systems::BuoyancyPrivate
   /// fluidDensity.
   public: std::map<double, double> layers;
 
-  /// \brief Point from where to apply the force
+  /// \brief Holds information about forces contributed by a single collision
+  /// shape.
   public: struct BuoyancyActionPoint
   {
-    /// \brief The force to be applied
+    /// \brief The force to be applied, expressed in the world frame.
     math::Vector3d force;
 
-    /// \brief The point from which the force will be applied
+    /// \brief The point from which the force will be applied, expressed in
+    /// the collision's frame.
     math::Vector3d point;
 
-    /// \brief The pose of the link in question
+    /// \brief The world pose of the collision.
     math::Pose3d pose;
   };
 
   /// \brief List of points from where the forces act.
+  /// This holds values refent to the current link being processed and must be
+  /// cleared between links.
+  /// \TODO(chapulina) It's dangerous to keep link-specific values in a member
+  /// variable. We should consider reducing the scope of this variable and pass
+  /// it across functions as needed.
   public: std::vector<BuoyancyActionPoint> buoyancyForces;
 
   /// \brief Resolve all forces as if they act as a Wrench from the give pose.
-  /// \param[in] _pose The point from which all poses are to be resolved.
+  /// \param[in] _linkInWorld The point from which all poses are to be resolved.
+  /// This is the link's origin in the world frame.
   /// \return A pair of {force, torque} describing the wrench to be applied
-  /// at _pose.
+  /// at _pose, expressed in the world frame.
   public: std::pair<math::Vector3d, math::Vector3d> ResolveForces(
-    const math::Pose3d &_pose);
+    const math::Pose3d &_linkInWorld);
 
   /// \brief Scoped names of entities that buoyancy should apply to. If empty,
   /// all links will receive buoyancy.
@@ -211,18 +220,26 @@ void BuoyancyPrivate::GradedFluidDensity(
 
 //////////////////////////////////////////////////
 std::pair<math::Vector3d, math::Vector3d> BuoyancyPrivate::ResolveForces(
-  const math::Pose3d &_pose)
+  const math::Pose3d &_linkInWorld)
 {
   auto force = math::Vector3d{0, 0, 0};
   auto torque = math::Vector3d{0, 0, 0};
 
   for (const auto &b : this->buoyancyForces)
   {
     force += b.force;
-    math::Pose3d localPoint{b.point, math::Quaterniond{1, 0, 0, 0}};
-    auto globalPoint = b.pose * localPoint;
-    auto offset = globalPoint.Pos() - _pose.Pos();
-    torque += force.Cross(offset);
+
+    // Pose offset from application point (COV) to collision origin, expressed
+    // in the collision frame
+    math::Pose3d pointInCol{b.point, math::Quaterniond::Identity};
+
+    // Application point in the world frame
+    auto pointInWorld = b.pose * pointInCol;
+
+    // Offset between the link origin and the force application point
+    auto offset = _linkInWorld.Pos() - pointInWorld.Pos();
+
+    torque += b.force.Cross(offset);
   }
 
   return {force, torque};
@@ -439,9 +456,6 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
           const components::Volume *_volume,
           const components::CenterOfVolume *_centerOfVolume) -> bool
     {
-      auto newPose = enableComponent<components::Inertial>(_ecm, _entity);
-      newPose |= enableComponent<components::WorldPose>(_ecm, _entity);
-
       // World pose of the link.
       math::Pose3d linkWorldPose = worldPose(_entity, _ecm);
 
@@ -472,13 +486,6 @@ 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<Entity> collisions = _ecm.ChildrenByComponents(
           _entity, components::Collision());
         this->dataPtr->buoyancyForces.clear();
@@ -523,8 +530,7 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
             }
           }
         }
-        auto [force, torque] = this->dataPtr->ResolveForces(
-        link.WorldInertialPose(_ecm).value());
+        auto [force, torque] = this->dataPtr->ResolveForces(linkWorldPose);
         // Apply the wrench to the link. This wrench is applied in the
         // Physics System.
         link.AddWorldWrench(_ecm, force, torque);

src/systems/buoyancy/Buoyancy.hh

@@ -33,8 +33,8 @@ namespace systems
 
   /// \brief A system that simulates buoyancy of objects immersed in fluid.
   /// All SDF parameters are optional. This system must be attached to the
-  /// world and this system will apply buoyancy to all links that have inertia
-  /// and collision shapes.
+  /// world and this system will apply buoyancy to all links that have collision
+  /// shapes.
   ///
   /// The volume and center of volume will be computed for each link, and
   /// stored as components. During each iteration, Archimedes' principle is
@@ -48,9 +48,9 @@ namespace systems
   /// * `<uniform_fluid_density>` sets the density of the fluid that surrounds
   /// the buoyant object. [Units: kgm^-3]
   /// * `<graded_buoyancy>` allows you to define a world where the buoyancy
-  /// changes with height. An example of such a world could be if we are
+  /// changes along the Z axis. An example of such a world could be if we are
   /// simulating an open ocean with its surface and under water behaviour. This
-  /// mode slices the volume of the collision mesh according to where the water
+  /// mode slices the volume of each collision mesh according to where the water
   /// line is set. When defining a `<graded_buoyancy>` tag, one must also define
   /// `<default_density>` and `<density_change>` tags.
   /// * `<default_density>` is the default fluid which the world should be

test/integration/buoyancy.cc

@@ -42,6 +42,121 @@ class BuoyancyTest : public InternalFixture<::testing::Test>
 {
 };
 
+/////////////////////////////////////////////////
+TEST_F(BuoyancyTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(RestoringMoments))
+{
+  // This test checks if the restoring moments are correctly calculated accross
+  // both uniform and graded modes when the vehicle is fully submerged.
+  std::size_t iterations{10000};
+  std::vector<math::Pose3d> posesUniform, posesGraded;
+  {
+    // Start server
+    ServerConfig serverConfig;
+    const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH),
+      "test", "worlds", "buoyancy_uniform_restoring_moments.sdf");
+    serverConfig.SetSdfFile(sdfFile);
+
+    test::Relay testSystem;
+    testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &/*unused*/,
+                              const gazebo::EntityComponentManager &_ecm)
+    {
+      // Get Submarine
+      Entity submarine = _ecm.EntityByComponents(
+        components::Model(), components::Name("submarine"));
+
+      // Get pose
+      auto submarinePose = _ecm.Component<components::Pose>(submarine);
+      ASSERT_NE(submarinePose, nullptr);
+
+      posesUniform.push_back(submarinePose->Data());
+    });
+
+    Server server(serverConfig);
+    EXPECT_FALSE(server.Running());
+    EXPECT_FALSE(*server.Running(0));
+
+    server.AddSystem(testSystem.systemPtr);
+    server.Run(true, iterations, false);
+  }
+
+  {
+    // Start server
+    ServerConfig serverConfig;
+    const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH),
+      "/test/worlds/buoyancy_graded_restoring_moments.sdf");
+    serverConfig.SetSdfFile(sdfFile);
+
+    test::Relay testSystem;
+    testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &/*unused*/,
+                              const gazebo::EntityComponentManager &_ecm)
+    {
+      // Get Submarine
+      Entity submarine = _ecm.EntityByComponents(
+        components::Model(), components::Name("submarine"));
+
+      // Get pose
+      auto submarinePose = _ecm.Component<components::Pose>(submarine);
+      ASSERT_NE(submarinePose, nullptr);
+
+      posesGraded.push_back(submarinePose->Data());
+    });
+
+    Server server(serverConfig);
+    EXPECT_FALSE(server.Running());
+    EXPECT_FALSE(*server.Running(0));
+
+    server.AddSystem(testSystem.systemPtr);
+    server.Run(true, iterations, false);
+  }
+
+  double maxUniformRoll{0}, maxGradedRoll{0};
+  double minUniformRoll{0}, minGradedRoll{0};
+
+  ASSERT_EQ(posesGraded.size(), posesUniform.size());
+  ASSERT_EQ(posesUniform.size(), iterations);
+  for (std::size_t i = 0; i <  posesGraded.size(); i++)
+  {
+    // The two models should track a similar course when fully submerged
+    // Ignore roll for now as that changes a lot and the two sims are not
+    // perfectly in sync.
+    EXPECT_NEAR(posesGraded[i].Rot().Euler().Y(),
+      posesUniform[i].Rot().Euler().Y(), 1e-1);
+    EXPECT_NEAR(posesGraded[i].Rot().Euler().Z(),
+      posesUniform[i].Rot().Euler().Z(), 1e-1);
+
+    EXPECT_NEAR(posesGraded[i].Pos().X(), posesUniform[i].Pos().X(), 1e-1);
+    EXPECT_NEAR(posesGraded[i].Pos().Y(), posesUniform[i].Pos().Y(), 1e-1);
+    EXPECT_NEAR(posesGraded[i].Pos().Z(), posesUniform[i].Pos().Z(), 1e-1);
+
+    // The box should stay around zero
+    EXPECT_NEAR(posesGraded[i].Pos().X(), 0, 1e-1);
+    EXPECT_NEAR(posesGraded[i].Pos().Y(), 0, 1e-1);
+    EXPECT_NEAR(posesGraded[i].Pos().Z(), 0, 1e-1);
+
+    // The box should not yaw or pitch
+    EXPECT_NEAR(posesGraded[i].Rot().Euler().Y(), 0, 1e-1);
+    EXPECT_NEAR(posesGraded[i].Rot().Euler().Z(), 0, 1e-1);
+
+    // The roll should not exceed its equilibrium position
+    EXPECT_LT(posesGraded[i].Rot().Euler().X(), 0.11 + 0.01);
+    EXPECT_LT(posesUniform[i].Rot().Euler().X(), 0.11 + 0.01);
+
+    // Get the maximum roll
+    maxUniformRoll = std::max(maxUniformRoll, posesUniform[i].Rot().X());
+    maxGradedRoll = std::max(maxGradedRoll, posesGraded[i].Rot().X());
+
+    // And the minimum roll
+    minUniformRoll = std::min(minUniformRoll, posesUniform[i].Rot().X());
+    minGradedRoll = std::min(minGradedRoll, posesGraded[i].Rot().X());
+  }
+  // The max and min roll should be similar
+  EXPECT_NEAR(maxUniformRoll, maxGradedRoll, 1e-1);
+  EXPECT_NEAR(maxUniformRoll, 0.11, 1e-1);
+  EXPECT_NEAR(minUniformRoll, minGradedRoll, 1e-1);
+  // Emperically derived
+  EXPECT_NEAR(minUniformRoll, -0.15, 1e-1);
+}
+
 /////////////////////////////////////////////////
 // See https://github.com/ignitionrobotics/ign-gazebo/issues/1175
 TEST_F(BuoyancyTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(UniformWorldMovement))