From c22df4c779686347caa2a7f46f04b7d15a5a4c3c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jirou=C5=A1ek=2C=20Martin?= <jirouma5@fel.cvut.cz>
Date: Tue, 28 May 2024 16:43:04 +0200
Subject: [PATCH] Add payload to the end of the suspended cable

---
 .../sdf/component_snippets.sdf.jinja          | 14 ++++-
 .../sdf/generic_components.sdf.jinja          | 51 +++++++++++++++++++
 2 files changed, 64 insertions(+), 1 deletion(-)

diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/component_snippets.sdf.jinja b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/component_snippets.sdf.jinja
index 8d1ee50..595f5bd 100644
--- a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/component_snippets.sdf.jinja
+++ b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/component_snippets.sdf.jinja
@@ -2645,12 +2645,14 @@ limitations under the License.
 {%- endmacro -%}
 {# <!--}--> #}
 
+
+
 {# pendulum_macro {--> #}
 {%- macro pendulum_macro(parent_link, x, y, z, spawner_args) -%}
 
   {%- set spawner_keyword = 'enable-pendulum' -%}
   {%- set spawner_description = 'Add a pendulum to the vehicle. Specify the length and mass of the whole chain. Length and mass is uniformly distributed between the links' -%}
-  {%- set spawner_default_args = {'chain_length': 3.0, 'chain_mass': 0.5, 'num_links': 30, 'radius': 0.01} -%}
+  {%- set spawner_default_args = {'chain_length': 2.0, 'chain_mass': 0.1, 'load_mass': 1.0, 'load_radius': 0.1, 'num_links': 15, 'radius': 0.01} -%}
 
   {%- if spawner_keyword in spawner_args.keys() -%}
     {{ generic.handle_spawner_args(spawner_keyword, spawner_default_args, spawner_args) }}
@@ -2687,6 +2689,16 @@ limitations under the License.
     {%- endfor -%}
     <!--}-->
 
+    <!-- load {-->
+    {{ generic.load_pendulum_macro(
+      parent_link = 'pendulum_chain_' + (spawner_args[spawner_keyword]['num_links'] - 1)| string() + '_link',
+      link_length = spawner_args[spawner_keyword]['chain_length'] / spawner_args[spawner_keyword]['num_links'],
+      load_mass = spawner_args[spawner_keyword]['load_mass'],
+      load_radius = spawner_args[spawner_keyword]['load_radius']
+      )
+    }}
+    <!--}-->
+
   {%- endif -%}
 {%- endmacro -%}
 <!--}-->
diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/generic_components.sdf.jinja b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/generic_components.sdf.jinja
index 63f8814..19f6960 100644
--- a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/generic_components.sdf.jinja
+++ b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/generic_components.sdf.jinja
@@ -164,6 +164,19 @@ limitations under the License.
 {%- endmacro -%}
 {# <!--}--> #}
 
+<!-- Ball inertia {-->
+{%- macro ball_inertia(m, r) -%}
+	<inertia>
+	  <ixx>{{ 2 * m * r * r / 5 }}</ixx>
+	  <ixy>0</ixy>
+	  <ixz>0</ixz>
+	  <iyy>{{ 2 * m * r * r / 5 }}</iyy>
+	  <iyz>0</iyz>
+	  <izz>{{ 2 * m * r * r / 5 }}</izz>
+	</inertia>
+{%- endmacro -%}
+<!--}-->
+
 {# single_chain_pendulum_macro {--> #}
 {%- macro single_chain_pendulum_macro(parent_link, id, link_mass, link_radius, link_length, joint_offset_x, joint_offset_y, joint_offset_z) -%}
   <link name="pendulum_chain_{{ id }}_link">
@@ -242,6 +255,44 @@ limitations under the License.
 {%- endmacro -%}
 {# <!--}--> #}
 
+{# Macro to add pendulums load {--> #}
+{%- macro load_pendulum_macro(parent_link, link_length, load_mass, load_radius) -%}
+<link name="pendulum_load">
+  <pose relative_to = "{{ parent_link }}"> 0 0 {{ -link_length/2 }} 0 0 0</pose>
+  <inertial>
+    <mass>{{ load_mass }}</mass>
+    {{
+      ball_inertia(
+        m = load_mass,
+        r = load_radius,
+      )
+    }}
+  </inertial>
+  <gravity>1</gravity>
+  <visual name="load_visual">
+    <pose>0 0 0 0 0 0</pose>
+    <geometry>
+      <sphere>
+        <radius>{{ load_radius }}</radius>
+      </sphere>
+    </geometry>
+    <material>
+      <script>
+        <name>Gazebo/Red</name>
+        <uri>file://media/materials/scripts/gazebo.material</uri>
+      </script>
+    </material>
+  </visual>
+</link>
+
+<joint name="pendulum_load_joint" type="fixed">
+  <parent>{{ parent_link }}</parent>
+  <child>pendulum_load</child>
+  <pose relative_to = "{{ parent_link }}">0 0 {{ -link_length/2 }} 0 0 0</pose>
+</joint>
+{%- endmacro -%}
+<!--}-->
+
 {# ========================== visual blocks ========================= #}
 
 {# visual_mesh_macro {--> #}