Merge branch 'gz-sim7' into scpeters/merge_7_8

Peppermint-Robots · May 22, 2024 · 86db789 · 86db789
2 parents 7c95f5a + a8f99a9
commit 86db789
Show file tree

Hide file tree

Showing 11 changed files with 1,246 additions and 6 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="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>
diff --git a/include/gz/sim/Link.hh b/include/gz/sim/Link.hh
@@ -24,6 +24,7 @@
 
 #include <gz/math/Inertial.hh>
 #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>
@@ -277,6 +278,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
@@ -356,6 +356,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
@@ -131,6 +131,7 @@ add_subdirectory(kinetic_energy_monitor)
 add_subdirectory(label)
 add_subdirectory(lens_flare)
 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/force_torque/ForceTorque.cc b/src/systems/force_torque/ForceTorque.cc
@@ -181,10 +181,10 @@ void ForceTorque::PostUpdate(const UpdateInfo &_info,
     // note: gz-sensors does its own throttling. Here the check is mainly
     // to avoid doing work in the ForceTorquePrivate::Update function
     bool needsUpdate = false;
-    for (auto &it : this->dataPtr->entitySensorMap)
+    for (const auto &[sensorEntity, sensor] : this->dataPtr->entitySensorMap)
     {
-      if (it.second->NextDataUpdateTime() <= _info.simTime &&
-          it.second->HasConnections())
+      if (sensor->NextDataUpdateTime() <= _info.simTime &&
+          sensor->HasConnections())
       {
         needsUpdate = true;
         break;
@@ -193,11 +193,16 @@ void ForceTorque::PostUpdate(const UpdateInfo &_info,
     if (!needsUpdate)
       return;
 
+    // Transform joint wrench to sensor wrench and write to sensor
     this->dataPtr->Update(_ecm);
 
-    for (auto &it : this->dataPtr->entitySensorMap)
+    for (auto &[sensorEntity, sensor] : this->dataPtr->entitySensorMap)
     {
-      it.second->Update(_info.simTime, false);
+      // Call gz::sensors::ForceTorqueSensor::Update
+      // * Convert to user-specified frame
+      // * Apply noise
+      // * Publish to gz-transport topic
+      sensor->Update(_info.simTime, false);
     }
   }
 
@@ -269,7 +274,8 @@ void ForceTorquePrivate::Update(const EntityComponentManager &_ecm)
             return true;
           }
 
-          // Appropriate components haven't been populated by physics yet
+          // Return early if JointTransmittedWrench component has not yet been
+          // populated by the Physics system
           auto jointWrench = _ecm.Component<components::JointTransmittedWrench>(
               jointLinkIt->second.joint);
           if (nullptr == jointWrench)

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
+)