LTA Dynamics System

copy of Hydrodynamics folder, and rename to lta Renamed to LighterThanAirDynamics - Added some initial private data - Requires still alot of cleanup some initial dynamics added Building sucessfully with some warnings Running test to determine succesfull Spawning successfull ran tests, but still debugging alot Added additional debug messages Protection for zero division - Test running sucessfully with adding forces and torques, but requires more extensive testing and validation Improved Documentation,some more work on tests Improve documentation to include math Silver bullet model withs its coefficients small documentation update Code style updates. wip Style checks completed Small correction to dynamics Added Mass Force and Torques included - Link class now provides the FluidAddedMassMatrix Removed Munk from added mass & correct coeff names Signed-off-by: henrykotze <[email protected]> update year Co-authored-by: Alejandro Hernández Cordero <[email protected]> Signed-off-by: Henry Kotzé <[email protected]> update format suggestion Co-authored-by: Alejandro Hernández Cordero <[email protected]> Signed-off-by: Henry Kotzé <[email protected]> update format suggestion in added_mass_force Co-authored-by: Alejandro Hernández Cordero <[email protected]> Signed-off-by: Henry Kotzé <[email protected]> format fix: function header Co-authored-by: Alejandro Hernández Cordero <[email protected]> Signed-off-by: Henry Kotzé <[email protected]> update formatting Co-authored-by: Alejandro Hernández Cordero <[email protected]> Signed-off-by: Henry Kotzé <[email protected]> Update src/systems/lighter_than_air_dynamics/LighterThanAirDynamics.hh Co-authored-by: Alejandro Hernández Cordero <[email protected]> Signed-off-by: Henry Kotzé <[email protected]> update year Co-authored-by: Alejandro Hernández Cordero <[email protected]> Signed-off-by: Henry Kotzé <[email protected]> Functionality updates and style updates Signed-off-by: henrykotze <[email protected]> Example added for Lighter-than-air envelope Signed-off-by: henrykotze <[email protected]> Use numerical limits to query precision of types Signed-off-by: henrykotze <[email protected]> fix integration path for sdf file Signed-off-by: henrykotze <[email protected]>
gazebosim · Nov 21, 2023 · ea20b0d · ea20b0d
1 parent e88a46f
commit ea20b0d
Show file tree

Hide file tree

Showing 10 changed files with 975 additions and 0 deletions.
diff --git a/examples/worlds/lighter_than_air_blimp.sdf b/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="ignition-gazebo-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>
diff --git a/include/gz/sim/Link.hh b/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.

diff --git a/src/Link.cc b/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

diff --git a/src/systems/CMakeLists.txt b/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)

diff --git a/src/systems/lighter_than_air_dynamics/CMakeLists.txt b/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
+)