Add payload to the end of the suspended cable

ctu-mrs · May 28, 2024 · c22df4c · c22df4c
1 parent b0383d0
commit c22df4c
Show file tree

Hide file tree

Showing 2 changed files with 64 additions and 1 deletion.
diff --git a/.../mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/component_snippets.sdf.jinja b/.../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/.../mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/generic_components.sdf.jinja b/.../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 {--> #}