From bee690e2ab480cad8fbfe0053a9416e2e53157d8 Mon Sep 17 00:00:00 2001
From: Bartek <bartlomiejk@vp.pl>
Date: Mon, 16 Sep 2024 20:19:47 +0200
Subject: [PATCH] Cleaned up code, added new macros and properties

---
 .../actuator/actuator_macros.urdf.xacro       | 16 -------
 .../joints/joint_macros.urdf.xacro            |  9 ++++
 .../joints/joint_properties.urdf.xacro        | 20 +++++++++
 .../feet/feet.urdf.xacro                      |  0
 .../hip/hip.urdf.xacro                        |  0
 .../shank/shank.urdf.xacro                    |  0
 .../thigh/thigh.urdf.xacro                    |  0
 .../trunk/trunk.urdf.xacro                    |  0
 .../description/meldog_core.urdf.xacro        | 45 ++++++++++---------
 .../joint_hardware_macros.urdf.xacro}         | 14 +++---
 .../joint_hardware_properties.urdf.xacro      | 14 ++++++
 .../ros2_control/ros2_control_core.urdf.xacro | 31 +++++++++++++
 .../ros2_control_gazebo.urdf.xacro            | 27 ++---------
 .../transmission_macros.urdf.xacro            | 20 ++++-----
 .../joint_trajectory_example.txt              | 11 +++++
 15 files changed, 127 insertions(+), 80 deletions(-)
 delete mode 100644 src/meldog_simple_description/description/actuator/actuator_macros.urdf.xacro
 create mode 100644 src/meldog_simple_description/description/joints/joint_macros.urdf.xacro
 create mode 100644 src/meldog_simple_description/description/joints/joint_properties.urdf.xacro
 rename src/meldog_simple_description/description/{meldog_links => links}/feet/feet.urdf.xacro (100%)
 rename src/meldog_simple_description/description/{meldog_links => links}/hip/hip.urdf.xacro (100%)
 rename src/meldog_simple_description/description/{meldog_links => links}/shank/shank.urdf.xacro (100%)
 rename src/meldog_simple_description/description/{meldog_links => links}/thigh/thigh.urdf.xacro (100%)
 rename src/meldog_simple_description/description/{meldog_links => links}/trunk/trunk.urdf.xacro (100%)
 rename src/meldog_simple_description/description/ros2_control/{ros2_control_joint_macros.urdf.xacro => joints_hardware/joint_hardware_macros.urdf.xacro} (54%)
 create mode 100644 src/meldog_simple_description/description/ros2_control/joints_hardware/joint_hardware_properties.urdf.xacro
 create mode 100644 src/meldog_simple_description/description/ros2_control/ros2_control_core.urdf.xacro
 rename src/meldog_simple_description/description/ros2_control/{ => transmission}/transmission_macros.urdf.xacro (71%)

diff --git a/src/meldog_simple_description/description/actuator/actuator_macros.urdf.xacro b/src/meldog_simple_description/description/actuator/actuator_macros.urdf.xacro
deleted file mode 100644
index 0000542..0000000
--- a/src/meldog_simple_description/description/actuator/actuator_macros.urdf.xacro
+++ /dev/null
@@ -1,16 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "meldog_actuator">
-
-
-    <xacro:property name="actuator_max_velocity" value="20.2"/>
-    <xacro:property name="actuator_max_torque" value="30"/>
-    <xacro:property name="actuator_damping" value="0.0"/>
-    <xacro:property name="actuator_friction" value="0.0"/>
-
-
-    <xacro:macro name ="meldog_joint_limits" params="lower upper">
-        <limit lower="${lower}" upper="${upper}" effort="${actuator_max_torque}" velocity="${actuator_max_velocity}"/>
-         <dynamics damping="${actuator_damping}" friction="${actuator_friction}"/>
-    </xacro:macro>
-
-</robot>
\ No newline at end of file
diff --git a/src/meldog_simple_description/description/joints/joint_macros.urdf.xacro b/src/meldog_simple_description/description/joints/joint_macros.urdf.xacro
new file mode 100644
index 0000000..caed7cf
--- /dev/null
+++ b/src/meldog_simple_description/description/joints/joint_macros.urdf.xacro
@@ -0,0 +1,9 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "joint_macros">
+
+    <xacro:macro name ="joint_macro" params="lower upper">
+        <limit lower="${lower}" upper="${upper}" effort="${joint_max_torque}" velocity="${joint_max_velocity}"/>
+        <dynamics damping="${joint_damping}" friction="${joint_friction}"/>
+    </xacro:macro>
+
+</robot>
\ No newline at end of file
diff --git a/src/meldog_simple_description/description/joints/joint_properties.urdf.xacro b/src/meldog_simple_description/description/joints/joint_properties.urdf.xacro
new file mode 100644
index 0000000..8320b56
--- /dev/null
+++ b/src/meldog_simple_description/description/joints/joint_properties.urdf.xacro
@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "joint_properties"> 
+    
+    <!-- Limits for joints (lower and upper position): -->
+    <xacro:property name="H_lower_limit" value="-${PI/2}"/>
+    <xacro:property name="H_upper_limit" value="${PI/2}"/>
+    <xacro:property name="T_lower_limit" value="-${PI}"/>
+    <xacro:property name="T_upper_limit" value="${PI}"/>
+    <xacro:property name="S_lower_limit" value="-${5*PI/6}"/>
+    <xacro:property name="S_upper_limit" value="${5*PI/6}"/>
+
+    <!-- Limits for joints (velocity, torque)-->
+    <xacro:property name="joint_max_velocity" value="20.2"/>
+    <xacro:property name="joint_max_torque" value="30"/>
+
+    <!-- Joint properties -->
+    <xacro:property name="joint_damping" value="0.0"/>
+    <xacro:property name="joint_friction" value="0.0"/>
+
+</robot>
\ No newline at end of file
diff --git a/src/meldog_simple_description/description/meldog_links/feet/feet.urdf.xacro b/src/meldog_simple_description/description/links/feet/feet.urdf.xacro
similarity index 100%
rename from src/meldog_simple_description/description/meldog_links/feet/feet.urdf.xacro
rename to src/meldog_simple_description/description/links/feet/feet.urdf.xacro
diff --git a/src/meldog_simple_description/description/meldog_links/hip/hip.urdf.xacro b/src/meldog_simple_description/description/links/hip/hip.urdf.xacro
similarity index 100%
rename from src/meldog_simple_description/description/meldog_links/hip/hip.urdf.xacro
rename to src/meldog_simple_description/description/links/hip/hip.urdf.xacro
diff --git a/src/meldog_simple_description/description/meldog_links/shank/shank.urdf.xacro b/src/meldog_simple_description/description/links/shank/shank.urdf.xacro
similarity index 100%
rename from src/meldog_simple_description/description/meldog_links/shank/shank.urdf.xacro
rename to src/meldog_simple_description/description/links/shank/shank.urdf.xacro
diff --git a/src/meldog_simple_description/description/meldog_links/thigh/thigh.urdf.xacro b/src/meldog_simple_description/description/links/thigh/thigh.urdf.xacro
similarity index 100%
rename from src/meldog_simple_description/description/meldog_links/thigh/thigh.urdf.xacro
rename to src/meldog_simple_description/description/links/thigh/thigh.urdf.xacro
diff --git a/src/meldog_simple_description/description/meldog_links/trunk/trunk.urdf.xacro b/src/meldog_simple_description/description/links/trunk/trunk.urdf.xacro
similarity index 100%
rename from src/meldog_simple_description/description/meldog_links/trunk/trunk.urdf.xacro
rename to src/meldog_simple_description/description/links/trunk/trunk.urdf.xacro
diff --git a/src/meldog_simple_description/description/meldog_core.urdf.xacro b/src/meldog_simple_description/description/meldog_core.urdf.xacro
index 5bee428..b33e208 100644
--- a/src/meldog_simple_description/description/meldog_core.urdf.xacro
+++ b/src/meldog_simple_description/description/meldog_core.urdf.xacro
@@ -4,25 +4,26 @@
     <!-- PI -->
     <xacro:property name="PI" value="3.1415926535897"/>
 
-    <!-- Actuator macros -->
-    <xacro:include filename="actuator/actuator_macros.urdf.xacro"/>
+    <!-- Joint properties and macros: -->
+    <xacro:include filename="joints/joint_properties.urdf.xacro"/>
+    <xacro:include filename="joints/joint_macros.urdf.xacro"/>
 
     <!-- LINKS: -->
     <!-- Base link spawns in x= 0.0 y = 0.0 z = 0.0 -->
     <link name="base_link"></link>
 
     <!-- Trunk link -->
-    <xacro:include filename="meldog_links/trunk/trunk.urdf.xacro"/>
+    <xacro:include filename="links/trunk/trunk.urdf.xacro"/>
 
     <!-- Hip links -->
-    <xacro:include filename="meldog_links/hip/hip.urdf.xacro"/>
+    <xacro:include filename="links/hip/hip.urdf.xacro"/>
     <xacro:hip_link position="front_left"/>
     <xacro:hip_link position="front_right"/>
     <xacro:hip_link position="back_left"/>
     <xacro:hip_link position="back_right"/>
 
     <!-- Thigh links -->
-    <xacro:include filename="meldog_links/thigh/thigh.urdf.xacro"/>
+    <xacro:include filename="links/thigh/thigh.urdf.xacro"/>
     <xacro:thigh_link position="front_left"/>
     <xacro:thigh_link position="front_right"/>
     <xacro:thigh_link position="back_left"/>
@@ -30,7 +31,7 @@
 
 
     <!-- Shank links -->
-    <xacro:include filename="meldog_links/shank/shank.urdf.xacro"/>
+    <xacro:include filename="links/shank/shank.urdf.xacro"/>
     <xacro:shank_link position="front_left"/>
     <xacro:shank_link position="front_right"/>
     <xacro:shank_link position="back_left"/>
@@ -38,19 +39,19 @@
 
 
     <!-- Feet links (frames in the end of shank) -->
-    <xacro:include filename="meldog_links/feet/feet.urdf.xacro"/>
+    <xacro:include filename="links/feet/feet.urdf.xacro"/>
     <xacro:feet_link position="front_left"/>
     <xacro:feet_link position="front_right"/>
     <xacro:feet_link position="back_left"/>
     <xacro:feet_link position="back_right"/>
 
-    <!-- JOINTS: -->    
+    <!-- JOINTS: -->   
+
     <!-- Base to Trunk joint -->
     <joint name="BT_joint" type="fixed">
         <parent link="base_link"/>
         <child link="trunk_link"/>
-        <!--Origin defines translation child frame relative to parent frame. Translation is performed in parent frame.-->
-        <origin xyz="0 0 1.0" rpy="0 0 0"/>  <!--testing-->
+        <origin xyz="0 0 1.0" rpy="0 0 0"/>  
     </joint>
 
 
@@ -60,7 +61,7 @@
         <parent link="trunk_link"/>
         <child link="hip_front_left_link"/>
         <axis xyz="1.0 0.0 0.0"/>
-        <xacro:meldog_joint_limits lower ="-${PI/2}" upper = "${PI/2}"/>
+        <xacro:joint_macro lower ="${H_lower_limit}" upper = "${H_upper_limit}"/>
     </joint>
 
     <!-- Trunk to Hip front right joint -->
@@ -69,7 +70,7 @@
         <parent link="trunk_link"/>
         <child link="hip_front_right_link"/>
         <axis xyz="-1.0 0.0 0.0"/>
-        <xacro:meldog_joint_limits lower ="-${PI/2}" upper = "${PI/2}"/>
+        <xacro:joint_macro lower ="${H_lower_limit}" upper = "${H_upper_limit}"/>
     </joint>
 
     <!-- Trunk to Hip back left joint -->
@@ -78,7 +79,7 @@
         <parent link="trunk_link"/>
         <child link="hip_back_left_link"/>
         <axis xyz="-1.0 0.0 0.0"/>
-        <xacro:meldog_joint_limits lower ="-${PI/2}" upper = "${PI/2}"/>
+        <xacro:joint_macro lower ="${H_lower_limit}" upper = "${H_upper_limit}"/>
     </joint>
 
     <!-- Trunk to Hip back right joint -->
@@ -87,7 +88,7 @@
         <parent link="trunk_link"/>
         <child link="hip_back_right_link"/>
         <axis xyz="1.0 0.0 0.0"/>
-        <xacro:meldog_joint_limits lower ="-${PI/2}" upper = "${PI/2}"/>
+        <xacro:joint_macro lower ="${H_lower_limit}" upper = "${H_upper_limit}"/>
     </joint>
 
 
@@ -97,7 +98,7 @@
         <parent link="hip_front_left_link"/>
         <child link="thigh_front_left_link"/>
         <axis xyz="0.0 -1.0 0.0"/>
-        <xacro:meldog_joint_limits lower ="-${PI}" upper = "${PI}"/>
+        <xacro:joint_macro lower ="${T_lower_limit}" upper = "${T_upper_limit}"/>
     </joint>
 
     <!-- Hip front right to Thigh front right -->
@@ -106,7 +107,7 @@
         <parent link="hip_front_right_link"/>
         <child link="thigh_front_right_link"/>
         <axis xyz="0.0 -1.0 0.0"/>
-        <xacro:meldog_joint_limits lower ="-${PI}" upper = "${PI}"/>
+        <xacro:joint_macro lower ="${T_lower_limit}" upper = "${T_upper_limit}"/>
     </joint>
 
     <!-- Hip back left to Thigh back left -->
@@ -115,7 +116,7 @@
         <parent link="hip_back_left_link"/>
         <child link="thigh_back_left_link"/>
         <axis xyz="0.0 1.0 0.0"/>
-        <xacro:meldog_joint_limits lower ="-${PI}" upper = "${PI}"/>
+        <xacro:joint_macro lower ="${T_lower_limit}" upper = "${T_upper_limit}"/>
     </joint>
 
     <!-- Hip back right to Thigh back right -->
@@ -124,7 +125,7 @@
         <parent link="hip_back_right_link"/>
         <child link="thigh_back_right_link"/>
         <axis xyz="0.0 1.0 0.0"/>
-        <xacro:meldog_joint_limits lower ="-${PI}" upper = "${PI}"/>
+        <xacro:joint_macro lower ="${T_lower_limit}" upper = "${T_upper_limit}"/>
     </joint>
 
 
@@ -134,7 +135,7 @@
         <parent link="thigh_front_left_link"/>
         <child link="shank_front_left_link"/>
         <axis xyz="0.0 -1.0 0.0"/>
-        <xacro:meldog_joint_limits lower ="-${5*PI/6}" upper = "${5*PI/6}"/>
+        <xacro:joint_macro lower ="${S_lower_limit}" upper = "${S_upper_limit}"/>
     </joint>
 
     <!-- Thigh front right to Shank front right -->
@@ -143,7 +144,7 @@
         <parent link="thigh_front_right_link"/>
         <child link="shank_front_right_link"/>
         <axis xyz="0.0 -1.0 0.0"/>
-        <xacro:meldog_joint_limits lower ="-${5*PI/6}" upper = "${5*PI/6}"/>
+        <xacro:joint_macro lower ="${S_lower_limit}" upper = "${S_upper_limit}"/>
     </joint>
 
     <!-- Thigh back left to Shank back left -->
@@ -152,7 +153,7 @@
         <parent link="thigh_back_left_link"/>
         <child link="shank_back_left_link"/>
         <axis xyz="0.0 1.0 0.0"/>
-        <xacro:meldog_joint_limits lower ="-${5*PI/6}" upper = "${5*PI/6}"/>
+        <xacro:joint_macro lower ="${S_lower_limit}" upper = "${S_upper_limit}"/>
     </joint>
 
     <!-- Thigh back right to Shank back right -->
@@ -161,7 +162,7 @@
         <parent link="thigh_back_right_link"/>
         <child link="shank_back_right_link"/>
         <axis xyz="0.0 1.0 0.0"/>
-        <xacro:meldog_joint_limits lower ="-${5*PI/6}" upper = "${5*PI/6}"/>
+        <xacro:joint_macro lower ="${S_lower_limit}" upper = "${S_upper_limit}"/>
     </joint>
     
 
diff --git a/src/meldog_simple_description/description/ros2_control/ros2_control_joint_macros.urdf.xacro b/src/meldog_simple_description/description/ros2_control/joints_hardware/joint_hardware_macros.urdf.xacro
similarity index 54%
rename from src/meldog_simple_description/description/ros2_control/ros2_control_joint_macros.urdf.xacro
rename to src/meldog_simple_description/description/ros2_control/joints_hardware/joint_hardware_macros.urdf.xacro
index 734c090..3c2d1d9 100644
--- a/src/meldog_simple_description/description/ros2_control/ros2_control_joint_macros.urdf.xacro
+++ b/src/meldog_simple_description/description/ros2_control/joints_hardware/joint_hardware_macros.urdf.xacro
@@ -1,21 +1,19 @@
 <?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name ="meldog_ros2_control_joint_macros">
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name ="joint_hardware_macros">
 
-    <xacro:include filename="../actuator/actuator_macros.urdf.xacro"/>
-
-    <xacro:macro name="ros2_control_joint" params="joint_name upper lower">
+    <xacro:macro name="joint_hardware_macro" params="joint_name upper lower">
         <joint name="${joint_name}_joint">
             <command_interface name="position">
                 <param name="min">${lower}</param>
                 <param name="max">${upper}</param>
             </command_interface>
             <command_interface name="velocity">
-                <param name="min">-${actuator_max_velocity}</param>
-                <param name="max">${actuator_max_velocity}</param>
+                <param name="min">-${motor_max_velocity}</param>
+                <param name="max">${motor_max_velocity}</param>
             </command_interface>
             <command_interface name="effort">
-                <param name="min">-${actuator_max_torque}</param>
-                <param name="max">${actuator_max_torque}</param>
+                <param name="min">-${motor_max_torque}</param>
+                <param name="max">${motor_max_torque}</param>
             </command_interface>                  
             <state_interface name="position"/>
             <state_interface name="velocity"/>
diff --git a/src/meldog_simple_description/description/ros2_control/joints_hardware/joint_hardware_properties.urdf.xacro b/src/meldog_simple_description/description/ros2_control/joints_hardware/joint_hardware_properties.urdf.xacro
new file mode 100644
index 0000000..05affec
--- /dev/null
+++ b/src/meldog_simple_description/description/ros2_control/joints_hardware/joint_hardware_properties.urdf.xacro
@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name ="joint_hardware_properties">
+
+    <!-- Actuator and motor parameters: -->
+    <xacro:property name="actuator_gear_ratio" value="9.97"/>
+    <xacro:property name="motor_max_velocity" value="${actuator_gear_ratio*20.2}"/>
+    <xacro:property name="motor_max_torque" value="${30.0/actuator_gear_ratio}"/>
+    <xacro:property name="motor_lower_limit" value="-${100}"/>
+    <xacro:property name="motor_upper_limit" value="${100}"/>
+
+
+    
+
+</robot>
\ No newline at end of file
diff --git a/src/meldog_simple_description/description/ros2_control/ros2_control_core.urdf.xacro b/src/meldog_simple_description/description/ros2_control/ros2_control_core.urdf.xacro
new file mode 100644
index 0000000..b5126c8
--- /dev/null
+++ b/src/meldog_simple_description/description/ros2_control/ros2_control_core.urdf.xacro
@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "meldog_ros2_control_core">
+
+    <!-- Joint hardware properties and macros: -->
+    <xacro:include filename="joints_hardware/joint_hardware_properties.urdf.xacro"/>
+    <xacro:include filename="joints_hardware/joint_hardware_macros.urdf.xacro"/>
+
+    <!-- Transmissions: -->
+    <xacro:include filename="transmission/transmission_macros.urdf.xacro"/>
+
+    <!-- Hardware interface for Gazebo: -->
+
+    <!-- Trunk to Hips joints-->
+    <xacro:joint_hardware_macro joint_name="FLH" lower="${motor_lower_limit}" upper="${motor_upper_limit}"/>
+    <xacro:joint_hardware_macro joint_name="FRH" lower="${motor_lower_limit}" upper="${motor_upper_limit}"/>
+    <xacro:joint_hardware_macro joint_name="BLH" lower="${motor_lower_limit}" upper="${motor_upper_limit}"/>
+    <xacro:joint_hardware_macro joint_name="BRH" lower="${motor_lower_limit}" upper="${motor_upper_limit}"/>
+    
+    <!-- Hips to Thighs joints -->
+    <xacro:joint_hardware_macro joint_name="FLT" lower="${motor_lower_limit}" upper="${motor_upper_limit}"/>
+    <xacro:joint_hardware_macro joint_name="FRT" lower="${motor_lower_limit}" upper="${motor_upper_limit}"/>
+    <xacro:joint_hardware_macro joint_name="BLT" lower="${motor_lower_limit}" upper="${motor_upper_limit}"/>
+    <xacro:joint_hardware_macro joint_name="BRT" lower="${motor_lower_limit}" upper="${motor_upper_limit}"/>
+    
+    <!-- Thighs to Shanks joints -->
+    <xacro:joint_hardware_macro joint_name="FLS" lower="${motor_lower_limit}" upper="${motor_upper_limit}"/>
+    <xacro:joint_hardware_macro joint_name="FRS" lower="${motor_lower_limit}" upper="${motor_upper_limit}"/>
+    <xacro:joint_hardware_macro joint_name="BLS" lower="${motor_lower_limit}" upper="${motor_upper_limit}"/>
+    <xacro:joint_hardware_macro joint_name="BRS" lower="${motor_lower_limit}" upper="${motor_upper_limit}"/>
+
+</robot>
\ No newline at end of file
diff --git a/src/meldog_simple_description/description/ros2_control/ros2_control_gazebo.urdf.xacro b/src/meldog_simple_description/description/ros2_control/ros2_control_gazebo.urdf.xacro
index 1cfc992..5507615 100644
--- a/src/meldog_simple_description/description/ros2_control/ros2_control_gazebo.urdf.xacro
+++ b/src/meldog_simple_description/description/ros2_control/ros2_control_gazebo.urdf.xacro
@@ -1,35 +1,14 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "meldog_ros2_control_gazebo">
 
-<!-- ROS2 Control Gazebo -->
+    <!-- ROS2 Control for Gazebo: -->
     <ros2_control name="meldog_gazebo" type="system">
         <hardware>  
             <plugin>ign_ros2_control/IgnitionSystem</plugin>
         </hardware>
-        <!-- Transmissions -->
-        <!-- <xacro:include filename="transmission_macros.urdf.xacro"/> -->
 
-        <!-- Hardware interface for Gazebo -->
-        <xacro:include filename="ros2_control_joint_macros.urdf.xacro"/>
-
-        <!-- Trunk to Hips joints-->
-        <xacro:ros2_control_joint joint_name="FLH" lower="-${PI/2}" upper="${PI/2}"/>
-        <xacro:ros2_control_joint joint_name="FRH" lower="-${PI/2}" upper="${PI/2}"/>
-        <xacro:ros2_control_joint joint_name="BLH" lower="-${PI/2}" upper="${PI/2}"/>
-        <xacro:ros2_control_joint joint_name="BRH" lower="-${PI/2}" upper="${PI/2}"/>
-
-        <!-- Hips to Thighs joints -->
-        <xacro:ros2_control_joint joint_name="FLT" lower="-${PI}" upper="${PI}"/>
-        <xacro:ros2_control_joint joint_name="FRT" lower="-${PI}" upper="${PI}"/>
-        <xacro:ros2_control_joint joint_name="BLT" lower="-${PI}" upper="${PI}"/>
-        <xacro:ros2_control_joint joint_name="BRT" lower="-${PI}" upper="${PI}"/>
-
-
-        <!-- Thighs to Shanks joints -->
-        <xacro:ros2_control_joint joint_name="FLS" lower="-${5*PI/6}" upper="${5*PI/6}"/>
-        <xacro:ros2_control_joint joint_name="FRS" lower="-${5*PI/6}" upper="${5*PI/6}"/>
-        <xacro:ros2_control_joint joint_name="BLS" lower="-${5*PI/6}" upper="${5*PI/6}"/>
-        <xacro:ros2_control_joint joint_name="BRS" lower="-${5*PI/6}" upper="${5*PI/6}"/>
+        <!-- ROS2 Control core xacro file: -->
+        <xacro:include filename="ros2_control_core.urdf.xacro"/>   
 
     </ros2_control>
 
diff --git a/src/meldog_simple_description/description/ros2_control/transmission_macros.urdf.xacro b/src/meldog_simple_description/description/ros2_control/transmission/transmission_macros.urdf.xacro
similarity index 71%
rename from src/meldog_simple_description/description/ros2_control/transmission_macros.urdf.xacro
rename to src/meldog_simple_description/description/ros2_control/transmission/transmission_macros.urdf.xacro
index 8ffc223..7505125 100644
--- a/src/meldog_simple_description/description/ros2_control/transmission_macros.urdf.xacro
+++ b/src/meldog_simple_description/description/ros2_control/transmission/transmission_macros.urdf.xacro
@@ -36,16 +36,16 @@
 
     <!-- Meldog Transmissions: -->
 
-    <!-- Trunk to Hips transmissions -->
-    <xacro:SimpleTransmission joint_name="FLH" reduction="9.97" offset="0.0"/>
-    <xacro:SimpleTransmission joint_name="FRH" reduction="9.97" offset="0.0"/>
-    <xacro:SimpleTransmission joint_name="BLH" reduction="9.97" offset="0.0"/>
-    <xacro:SimpleTransmission joint_name="BRH" reduction="9.97" offset="0.0"/>
+    <!-- Trunk to Hips transmissions: -->
+    <xacro:SimpleTransmission joint_name="FLH" reduction="${actuator_gear_ratio}" offset="0.0"/>
+    <xacro:SimpleTransmission joint_name="FRH" reduction="${actuator_gear_ratio}" offset="0.0"/>
+    <xacro:SimpleTransmission joint_name="BLH" reduction="${actuator_gear_ratio}" offset="0.0"/>
+    <xacro:SimpleTransmission joint_name="BRH" reduction="${actuator_gear_ratio}" offset="0.0"/>
 
-    <!-- Thighs to Shanks transmissions -->
-    <xacro:FourBarLinkageTransmission first_joint_name="FLT" second_joint_name="FLS" reduction="9.97" first_offset="0.0" second_offset="0.0"/>
-    <xacro:FourBarLinkageTransmission first_joint_name="FRT" second_joint_name="FRS" reduction="9.97" first_offset="0.0" second_offset="0.0"/>
-    <xacro:FourBarLinkageTransmission first_joint_name="BLT" second_joint_name="BLS" reduction="9.97" first_offset="0.0" second_offset="0.0"/>
-    <xacro:FourBarLinkageTransmission first_joint_name="BRT" second_joint_name="BRS" reduction="9.97" first_offset="0.0" second_offset="0.0"/>
+    <!-- Thighs to Shanks transmissions: -->
+    <xacro:FourBarLinkageTransmission first_joint_name="FLT" second_joint_name="FLS" reduction="${actuator_gear_ratio}" first_offset="0.0" second_offset="0.0"/>
+    <xacro:FourBarLinkageTransmission first_joint_name="FRT" second_joint_name="FRS" reduction="${actuator_gear_ratio}" first_offset="0.0" second_offset="0.0"/>
+    <xacro:FourBarLinkageTransmission first_joint_name="BLT" second_joint_name="BLS" reduction="${actuator_gear_ratio}" first_offset="0.0" second_offset="0.0"/>
+    <xacro:FourBarLinkageTransmission first_joint_name="BRT" second_joint_name="BRS" reduction="${actuator_gear_ratio}" first_offset="0.0" second_offset="0.0"/>
 
 </robot>
\ No newline at end of file
diff --git a/src/meldog_simple_description/simple_commands/joint_trajectory_example.txt b/src/meldog_simple_description/simple_commands/joint_trajectory_example.txt
index f66b9e6..1a3431d 100644
--- a/src/meldog_simple_description/simple_commands/joint_trajectory_example.txt
+++ b/src/meldog_simple_description/simple_commands/joint_trajectory_example.txt
@@ -22,3 +22,14 @@ PRZYSIAD:
         { positions: [0.0, 0.0, 0.0, 0.0, -0.78539, -0.78539, -0.78539, -0.78539, 1.5707, 1.5707, 1.5707, 1.5707], time_from_start: { sec: 9 } }    ]
     }
   }" 
+  
+  WYPROSTOWANIE:
+  
+  ros2 action send_goal /joint_trajectory_controller/follow_joint_trajectory control_msgs/action/FollowJointTrajectory -f "{
+    trajectory: {
+      joint_names: ['FLH_joint', 'FRH_joint', 'BLH_joint', 'BRH_joint', 'FLT_joint', 'FRT_joint', 'BLT_joint', 'BRT_joint', 'FLS_joint', 'FRS_joint', 'BLS_joint', 'BRS_joint'],
+      points: [
+        { positions: [0.0, 0.0, 0.0, 0.0, -0.78539, -0.78539, -0.78539, -0.78539, 1.5707, 1.5707, 1.5707, 1.5707], time_from_start: { sec: 1 } },
+        { positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], time_from_start: { sec: 6 } }    ]
+    }
+  }"