Add surface to buoyancy engine. (retarget fortress) (#1298)

The buoyancy engine should stop working when it crosses the water surface. This PR adds a <surface> tag which stops the buoyancy engine from exerting an upward force when the vehicle is surfacing. Signed-off-by: Arjo Chakravarty <[email protected]>
gazebosim · Jan 19, 2022 · 05b9232 · 05b9232
1 parent 9cd15fc
commit 05b9232
Show file tree

Hide file tree

Showing 5 changed files with 183 additions and 9 deletions.
diff --git a/examples/worlds/buoyancy_engine.sdf b/examples/worlds/buoyancy_engine.sdf
@@ -30,21 +30,27 @@ listens to the topic `buoyancy_engine` or
 * Publishes a ignition::msgs::Double on `buoyancy_engine` or
  `/model/{namespace}/buoyancy_engine/current_volume` on the current volume
 ## Examples
+
 To get started run:
 ```
 ign gazebo buoyancy_engine.sdf
 ```
+You will see two boxes. The box on the right doesn't have a surface set whereas
+the box on the left does. When commanded to go up, the box on the left will rise
+until it breaches and then start oscillating around the surface. On the other
+hand the box on the right will rise forever as no surface is set.
 Enter the following in a separate terminal:
 ```
 ign topic -t  /model/buoyant_box/buoyancy_engine/ -m ignition.msgs.Double
    -p "data: 0.003"
 ```
-To see the box float up.
+The boxes will float up. Note that the box on the left will start oscillating
+once it breaches the surface.
 ```
 ign topic -t  /model/buoyant_box/buoyancy_engine/ -m ignition.msgs.Double
    -p "data: 0.001"
 ```
-To see the box go down.
+The boxes will go down.
 To see the current volume enter:
 ```
 ign topic -t  /model/buoyant_box/buoyancy_engine/current_volume -e
@@ -136,7 +142,47 @@ ign topic -t  /model/buoyant_box/buoyancy_engine/current_volume -e
         <max_volume>0.003</max_volume>
         <max_inflation_rate>0.0003</max_inflation_rate>
       </plugin>
+    </model>
+
 
+    <model name="buoyant_box_surface">
+      <pose>0 5 -0.8 0 0 0</pose>
+      <link name='body'>
+        <inertial>
+          <mass>1000</mass>
+          <inertia>
+            <ixx>133.3333</ixx>
+            <iyy>133.3333</iyy>
+            <izz>133.3333</izz>
+          </inertia>
+        </inertial>
+        <visual name='body_visual'>
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </visual>
+        <collision name='body_collision'>
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </collision>
+      </link>
+      <plugin
+        filename="libignition-gazebo-buoyancy-engine-system.so"
+        name="ignition::gazebo::systems::BuoyancyEngine">
+        <link_name>body</link_name>
+        <namespace>buoyant_box</namespace>
+        <min_volume>0.0</min_volume>
+        <neutral_volume>0.002</neutral_volume>
+        <default_volume>0.002</default_volume>
+        <max_volume>0.003</max_volume>
+        <max_inflation_rate>0.0003</max_inflation_rate>
+        <surface>0</surface>
+      </plugin>
     </model>
   </world>
 </sdf>
diff --git a/src/systems/buoyancy_engine/BuoyancyEngine.cc b/src/systems/buoyancy_engine/BuoyancyEngine.cc
@@ -24,6 +24,8 @@
 #include <ignition/gazebo/components/Gravity.hh>
 #include <ignition/gazebo/components/World.hh>
 #include <ignition/gazebo/Link.hh>
+#include <ignition/gazebo/Util.hh>
+
 #include <ignition/msgs/double.pb.h>
 #include <ignition/plugin/Register.hh>
 #include <ignition/transport/Node.hh>
@@ -69,6 +71,9 @@ class ignition::gazebo::systems::BuoyancyEnginePrivateData
   /// \brief The neutral volume in m^3
   public: double neutralVolume = 0.0003;
 
+  /// \brief Surface location
+  public: std::optional<double> surface = std::nullopt;
+
   /// \brief Trasport node for control
   public: ignition::transport::Node node;
 
@@ -77,8 +82,29 @@ class ignition::gazebo::systems::BuoyancyEnginePrivateData
 
   /// \brief mutex for protecting bladder volume and set point.
   public: std::mutex mtx;
+
+  /// \brief  Get fluid density based on the link origin's current position.
+  /// \param[in] _ecm - The ecm in question.
+  public: double CurrentFluidDensity(
+    const EntityComponentManager &_ecm) const;
 };
 
+//////////////////////////////////////////////////
+double BuoyancyEnginePrivateData::CurrentFluidDensity(
+  const EntityComponentManager &_ecm) const
+{
+  if (!this->surface.has_value())
+    return fluidDensity;
+
+  auto pose = gazebo::worldPose(this->linkEntity, _ecm);
+
+  if (pose.Pos().Z() < this->surface.value())
+  {
+    return fluidDensity;
+  }
+  return 0;
+}
+
 //////////////////////////////////////////////////
 void BuoyancyEnginePrivateData::OnCmdBuoyancyEngine(
   const ignition::msgs::Double &_volumeSetpoint)
@@ -146,11 +172,16 @@ void BuoyancyEnginePlugin::Configure(
     this->dataPtr->neutralVolume = _sdf->Get<double>("neutral_volume");
   }
 
-  if(_sdf->HasElement("max_inflation_rate"))
+  if (_sdf->HasElement("max_inflation_rate"))
   {
     this->dataPtr->maxInflationRate = _sdf->Get<double>("max_inflation_rate");
   }
 
+  if (_sdf->HasElement("surface"))
+  {
+    this->dataPtr->surface = _sdf->Get<double>("surface");
+  }
+
   this->dataPtr->world = _ecm.EntityByComponents(components::World());
   if (this->dataPtr->world == kNullEntity)
   {
@@ -169,7 +200,7 @@ void BuoyancyEnginePlugin::Configure(
       + "/buoyancy_engine/current_volume");
   }
 
-  if(!this->dataPtr->node.Subscribe(cmdTopic,
+  if (!this->dataPtr->node.Subscribe(cmdTopic,
     &BuoyancyEnginePrivateData::OnCmdBuoyancyEngine, this->dataPtr.get()))
   {
     ignerr << "Failed to subscribe to [" << cmdTopic << "]" << std::endl;
@@ -203,6 +234,7 @@ void BuoyancyEnginePlugin::PreUpdate(
     return;
   }
 
+  ignition::gazebo::Link link(this->dataPtr->linkEntity);
   math::Vector3d zForce;
   {
     std::lock_guard lock(this->dataPtr->mtx);
@@ -229,13 +261,16 @@ void BuoyancyEnginePlugin::PreUpdate(
     msg.set_data(this->dataPtr->bladderVolume);
     this->dataPtr->statusPub.Publish(msg);
 
+    // Get the fluid density of the current layer
+    auto currentFluidDensity = this->dataPtr->CurrentFluidDensity(_ecm);
+
     // Simply use Archimede's principle to apply a force at the desired link
     // position. We take off the neutral buoyancy element in order to simulate
     // the mass of the oil in the bladder.
-    zForce = - gravity->Data() * this->dataPtr->fluidDensity
-      * (this->dataPtr->bladderVolume - this->dataPtr->neutralVolume);
+    zForce = - gravity->Data() *
+      ( currentFluidDensity * this->dataPtr->bladderVolume
+      - this->dataPtr->fluidDensity * this->dataPtr->neutralVolume);
   }
-  ignition::gazebo::Link link(this->dataPtr->linkEntity);
   link.AddWorldWrench(_ecm, zForce, {0, 0, 0});
 }
 

diff --git a/src/systems/buoyancy_engine/BuoyancyEngine.hh b/src/systems/buoyancy_engine/BuoyancyEngine.hh
@@ -55,6 +55,8 @@ namespace systems
   ///   float, default=0.000003m^3/s]
   /// <fluid_density> - The fluid density of the liquid its suspended in kgm^-3.
   ///   [optional, float, default=1000kgm^-3]
+  /// <surface> - The Z height in metres at which the surface of the water is.
+  /// If not defined then there is no surface [optional, float]
   ///
   /// ## Topics
   /// * Subscribes to a ignition::msgs::Double on `buoyancy_engine` or

diff --git a/test/integration/buoyancy_engine.cc b/test/integration/buoyancy_engine.cc
@@ -106,8 +106,8 @@ TEST_F(BuoyancyEngineTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(TestDownward))
 TEST_F(BuoyancyEngineTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(TestUpward))
 {
   ServerConfig serverConfig;
-  const auto sdfFile = std::string(PROJECT_SOURCE_PATH) +
-    "/test/worlds/buoyancy_engine.sdf";
+  const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH),
+    "test", "worlds", "buoyancy_engine.sdf");
   serverConfig.SetSdfFile(sdfFile);
 
   Server server(serverConfig);
@@ -151,3 +151,54 @@ TEST_F(BuoyancyEngineTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(TestUpward))
   EXPECT_NEAR(poses.rbegin()->Pos().Y(), poses.begin()->Pos().Y(), 1e-3);
 }
 
+/////////////////////////////////////////////////
+TEST_F(BuoyancyEngineTest, TestUpwardSurface)
+{
+  ServerConfig serverConfig;
+  const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH),
+    "test", "worlds", "buoyancy_engine.sdf");
+  serverConfig.SetSdfFile(sdfFile);
+
+  Server server(serverConfig);
+  EXPECT_FALSE(server.Running());
+  EXPECT_FALSE(*server.Running(0));
+
+  using namespace std::chrono_literals;
+  server.SetUpdatePeriod(1ns);
+
+  std::size_t iterations = 10000;
+
+  test::Relay testSystem;
+  std::vector<ignition::math::Pose3d> poses;
+
+  testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &/*_info*/,
+                             const gazebo::EntityComponentManager &_ecm)
+  {
+    // Check pose
+    Entity buoyantBox = _ecm.EntityByComponents(
+      components::Model(), components::Name("buoyant_box_w_surface"));
+    EXPECT_NE(kNullEntity, buoyantBox);
+
+    auto submarineBuoyantPose = _ecm.Component<components::Pose>(buoyantBox);
+    EXPECT_NE(submarineBuoyantPose, nullptr);
+    if (submarineBuoyantPose == nullptr)
+    {
+      ignerr << "Unable to get pose" <<std::endl;
+      return;
+    }
+    poses.push_back(submarineBuoyantPose->Data());
+    double tol = 0.1;
+    EXPECT_LT(submarineBuoyantPose->Data().Z(), 0 + tol);
+  });
+
+  server.AddSystem(testSystem.systemPtr);
+  ignition::msgs::Double volume;
+  volume.set_data(10);
+  this->pub.Publish(volume);
+  server.Run(true, iterations, false);
+
+  EXPECT_GT(poses.rbegin()->Pos().Z(), poses.begin()->Pos().Z());
+  EXPECT_NEAR(poses.rbegin()->Pos().X(), poses.begin()->Pos().X(), 1e-3);
+  EXPECT_NEAR(poses.rbegin()->Pos().Y(), poses.begin()->Pos().Y(), 1e-3);
+}
+
diff --git a/test/worlds/buoyancy_engine.sdf b/test/worlds/buoyancy_engine.sdf
@@ -66,7 +66,47 @@
         <max_volume>0.003</max_volume>
         <max_inflation_rate>0.0003</max_inflation_rate>
       </plugin>
+    </model>
+
 
+    <model name="buoyant_box_w_surface">
+      <pose>5 0 -0.1 0 0 0</pose>
+      <link name='body'>
+        <inertial>
+          <mass>1000</mass>
+          <inertia>
+            <ixx>133.3333</ixx>
+            <iyy>133.3333</iyy>
+            <izz>133.3333</izz>
+          </inertia>
+        </inertial>
+        <visual name='body_visual'>
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </visual>
+        <collision name='body_collision'>
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </collision>
+      </link>
+      <plugin
+        filename="libignition-gazebo-buoyancy-engine-system.so"
+        name="ignition::gazebo::systems::BuoyancyEngine">
+        <link_name>body</link_name>
+        <namespace>buoyant_box</namespace>
+        <min_volume>0.001</min_volume>
+        <neutral_volume>0.002</neutral_volume>
+        <default_volume>0.002</default_volume>
+        <max_volume>0.003</max_volume>
+        <max_inflation_rate>0.0003</max_inflation_rate>
+        <surface>0</surface>
+      </plugin>
     </model>
   </world>
 </sdf>