LTA Dynamics System (#2241)

This adds the dynamics to simulate a lighter-than-air vehicle such as blimps/airships. The modelling is  based on [1] and [2]. Previously we added an airship-dynamics-plugin to Gazebo-Classic for [PX4 Gazebo SITL](PX4/PX4-SITL_gazebo-classic#490) , but we suffered simulation instability similar to that of the underwater-vehicles since Gazebo-Classic could not simulate the Added Mass dynamics.

This PR brings that airship-dynamics-plugin to Gazebo which supports the Added Mass dynamics.

Furthermore, I have extended, the Link class to provide access to the AddedMassMatrix, to allow the lighter-than-air system to easily access this matrix. 

The code is somewhat based of the Hydrodynamics system. 

I have an STL file which I can contribute, which is an envelope from [Wind Reiter](https://www.windreiter.com/shop/sb-324-300/), and the coefficients in the integrations test is for this envelope model.

### Citations
[1] Li, Y., & Nahon, M. (2007). Modeling and simulation of airship dynamics.  Journal of Guidance, Control, and Dynamics, 30(6), 1691–1700.
[2] Li, Y., Nahon, M., & Sharf, I. (2011). Airship dynamics modeling: A literature review. Progress in Aerospace Sciences, 47(3), 217–239.


Signed-off-by: henrykotze <henry@flycloudline.com>

Author: Henry Kotzé

examples/worlds/lighter_than_air_blimp.sdf

@@ -0,0 +1,78 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <world name="blimp">
+
+   <physics name="1ms" type="ode">
+      <max_step_size>0.001</max_step_size>
+      <!-- Zero to run as fast as possible -->
+      <real_time_factor>1</real_time_factor>
+    </physics>
+
+    <gravity>0 0 -9.81</gravity>
+
+    <plugin
+      filename="gz-sim-physics-system"
+      name="gz::sim::systems::Physics">
+    </plugin>
+    <plugin
+      filename="gz-sim-user-commands-system"
+      name="gz::sim::systems::UserCommands">
+    </plugin>
+    <plugin
+      filename="gz-sim-scene-broadcaster-system"
+      name="gz::sim::systems::SceneBroadcaster">
+    </plugin>
+
+    <plugin
+      filename="gz-sim-buoyancy-system"
+      name="gz::sim::systems::Buoyancy">
+      <uniform_fluid_density>1.097</uniform_fluid_density>
+    </plugin>
+
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>1 1 1 1</diffuse>
+      <specular>0.5 0.5 0.5 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+    <include>
+      <uri>
+        https://fuel.gazebosim.org/1.0/hkotze/models/airship
+      </uri>
+    </include>
+  </world>
+</sdf>

include/gz/sim/Link.hh

@@ -23,6 +23,7 @@
 #include <vector>
 
 #include <gz/math/Matrix3.hh>
+#include <gz/math/Matrix6.hh>
 #include <gz/math/Pose3.hh>
 #include <gz/math/Quaternion.hh>
 #include <gz/math/Vector3.hh>
@@ -269,6 +270,14 @@ namespace gz
       public: std::optional<math::Matrix3d> WorldInertiaMatrix(
           const EntityComponentManager &_ecm) const;
 
+      /// \brief Get the fluid added mass matrix in the world frame.
+      /// \param[in] _ecm Entity-component manager.
+      /// \return Fluide added matrix in world frame, returns nullopt if link
+      /// does not have components components::Inertial and
+      /// components::WorldPose.
+      public: std::optional<math::Matrix6d> WorldFluidAddedMassMatrix(
+          const EntityComponentManager &_ecm) const;
+
       /// \brief Get the rotational and translational kinetic energy of the
       /// link with respect to the world frame.
       /// \param[in] _ecm Entity-component manager.

src/Link.cc

@@ -337,6 +337,18 @@ std::optional<math::Matrix3d> Link::WorldInertiaMatrix(
       math::Inertiald(inertial->Data().MassMatrix(), comWorldPose).Moi());
 }
 
+std::optional<math::Matrix6d> Link::WorldFluidAddedMassMatrix(
+    const EntityComponentManager &_ecm) const
+{
+  auto inertial = _ecm.Component<components::Inertial>(this->dataPtr->id);
+  auto worldPose = _ecm.Component<components::WorldPose>(this->dataPtr->id);
+
+  if (!worldPose || !inertial)
+    return std::nullopt;
+
+  return inertial->Data().FluidAddedMass();
+}
+
 //////////////////////////////////////////////////
 std::optional<double> Link::WorldKineticEnergy(
     const EntityComponentManager &_ecm) const

src/systems/CMakeLists.txt

@@ -129,6 +129,7 @@ add_subdirectory(joint_traj_control)
 add_subdirectory(kinetic_energy_monitor)
 add_subdirectory(label)
 add_subdirectory(lift_drag)
+add_subdirectory(lighter_than_air_dynamics)
 add_subdirectory(log)
 add_subdirectory(log_video_recorder)
 add_subdirectory(logical_audio_sensor_plugin)

src/systems/lighter_than_air_dynamics/CMakeLists.txt

@@ -0,0 +1,7 @@
+gz_add_system(lighter_than_air_dynamics
+  SOURCES
+  LighterThanAirDynamics.cc
+  PUBLIC_LINK_LIBS
+    gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER}
+    gz-math${GZ_MATH_VER}::eigen3
+)