From a12993336df5ac7b3105d5539e9a3f4725ec55a3 Mon Sep 17 00:00:00 2001
From: Bartek <bartlomiejk@vp.pl>
Date: Thu, 22 Aug 2024 11:36:03 +0200
Subject: [PATCH 01/23] Created package for meldog urdf

---
 src/meldog_description/CMakeLists.txt |  26 ++++
 src/meldog_description/LICENSE        | 202 ++++++++++++++++++++++++++
 src/meldog_description/package.xml    |  18 +++
 3 files changed, 246 insertions(+)
 create mode 100644 src/meldog_description/CMakeLists.txt
 create mode 100644 src/meldog_description/LICENSE
 create mode 100644 src/meldog_description/package.xml

diff --git a/src/meldog_description/CMakeLists.txt b/src/meldog_description/CMakeLists.txt
new file mode 100644
index 00000000..1586b629
--- /dev/null
+++ b/src/meldog_description/CMakeLists.txt
@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.8)
+project(meldog_description)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
diff --git a/src/meldog_description/LICENSE b/src/meldog_description/LICENSE
new file mode 100644
index 00000000..d6456956
--- /dev/null
+++ b/src/meldog_description/LICENSE
@@ -0,0 +1,202 @@
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
diff --git a/src/meldog_description/package.xml b/src/meldog_description/package.xml
new file mode 100644
index 00000000..8c87aa77
--- /dev/null
+++ b/src/meldog_description/package.xml
@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>meldog_description</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="bartlomiejk@vp.pl">bartek</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

From be2c708e0d654cf70329b45bbfe00695652c63d4 Mon Sep 17 00:00:00 2001
From: Bartek <bartlomiejk@vp.pl>
Date: Sat, 31 Aug 2024 19:21:15 +0200
Subject: [PATCH 02/23] Added first base and trunk links for meldog

---
 src/meldog_description/CMakeLists.txt         | 13 +++
 .../description/meldog_core.urdf.xacro        |  9 ++
 .../description/trunk/trunk.urdf.xacro        | 66 +++++++++++++
 .../launch/robot_visualize.launch.py          | 93 +++++++++++++++++++
 src/meldog_description/package.xml            |  7 ++
 5 files changed, 188 insertions(+)
 create mode 100644 src/meldog_description/description/meldog_core.urdf.xacro
 create mode 100644 src/meldog_description/description/trunk/trunk.urdf.xacro
 create mode 100644 src/meldog_description/launch/robot_visualize.launch.py

diff --git a/src/meldog_description/CMakeLists.txt b/src/meldog_description/CMakeLists.txt
index 1586b629..29752e11 100644
--- a/src/meldog_description/CMakeLists.txt
+++ b/src/meldog_description/CMakeLists.txt
@@ -7,6 +7,10 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(xacro REQUIRED)
+find_package(urdf REQUIRED)
+find_package(rviz2 REQUIRED)
+find_package(robot_state_publisher REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
@@ -23,4 +27,13 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
+
+install(DIRECTORY 
+  test_controllers
+  description 
+  launch 
+  worlds 
+  DESTINATION share/${PROJECT_NAME}
+)
+
 ament_package()
diff --git a/src/meldog_description/description/meldog_core.urdf.xacro b/src/meldog_description/description/meldog_core.urdf.xacro
new file mode 100644
index 00000000..377bd404
--- /dev/null
+++ b/src/meldog_description/description/meldog_core.urdf.xacro
@@ -0,0 +1,9 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "meldog_core">
+
+
+    <!-- BASE LINK spawns in x= 0.0 y = 0.0 z = 0.0-->
+    <link name="base_link"></link>
+    <xacro:include filename="trunk/trunk.urdf.xacro"/>
+
+</robot>
\ No newline at end of file
diff --git a/src/meldog_description/description/trunk/trunk.urdf.xacro b/src/meldog_description/description/trunk/trunk.urdf.xacro
new file mode 100644
index 00000000..3723aef3
--- /dev/null
+++ b/src/meldog_description/description/trunk/trunk.urdf.xacro
@@ -0,0 +1,66 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" >
+
+    <!-- Trunk joint to base -->
+    <joint name="trunk_joint" type="fixed">
+        <parent link="base_link"/>
+        <child link="trunk"/>
+        <!--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-->
+    </joint>
+
+
+    <xacro:property name="trunk_base_length" value="0.8" />
+    <xacro:property name="trunk_base_width" value="0.2" />
+    <xacro:property name="trunk_base_height" value="0.1" />
+
+    <xacro:property name="trunk_actuators_length" value="0.3" />
+    <xacro:property name="trunk_actuators_width" value="0.2" />
+    <xacro:property name="trunk_actuators_height" value="0.1" />
+
+    <link name="trunk">
+        <inertial>
+            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+            <mass value="5.0"/> <!--testing-->
+            <inertia ixx="1.0" ixy="1.0" ixz="1.0" iyy="1.0" iyz="1.0" izz="1.0"/> <!--testing-->
+        </inertial>
+
+        <visual name="trunk_base">
+            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+            <geometry>
+                <box size="${trunk_base_length} ${trunk_base_width} ${trunk_base_height}"/>
+            </geometry>
+            <material name="trunk_base_color">
+                <color rgba="0.0 0.0 1.0 1.0"/>
+                <texture filename=""/>
+            </material>
+        </visual>
+        <visual name="trunk_actuators_left">
+            <origin xyz="0.0 ${trunk_base_width/2 + trunk_actuators_width/2} 0.0" rpy="0.0 0.0 0.0"/>
+            <geometry>
+                <box size="${trunk_actuators_length} ${trunk_actuators_width} ${trunk_actuators_height}"/>
+            </geometry>
+            <material name="">
+                <color rgba="0.0 1.0 0.0 1.0"/>
+                <texture filename="trunk_actuators_left_color"/>
+            </material>
+        </visual>
+        <visual name="trunk_actuators_right">
+            <origin xyz="0.0 ${-trunk_base_width/2 - trunk_actuators_width/2} 0.0" rpy="0.0 0.0 0.0"/>
+            <geometry>
+                <box size="${trunk_actuators_length} ${trunk_actuators_width} ${trunk_actuators_height}"/>
+            </geometry>
+            <material name="">
+                <color rgba="1.0 0.0 0.0 1.0"/>
+                <texture filename="trunk_actuators_right_color"/>
+            </material>
+        </visual>
+
+        <collision> <!--testing-->
+            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+            <geometry>
+                <box size="1.0 1.0 1.0"/>
+            </geometry>
+        </collision>
+    </link>
+</robot>
diff --git a/src/meldog_description/launch/robot_visualize.launch.py b/src/meldog_description/launch/robot_visualize.launch.py
new file mode 100644
index 00000000..0aceadfe
--- /dev/null
+++ b/src/meldog_description/launch/robot_visualize.launch.py
@@ -0,0 +1,93 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, RegisterEventHandler
+from launch.conditions import IfCondition
+from launch.event_handlers import OnProcessExit
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # Declare arguments
+    declared_arguments = []
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "gui",
+            default_value="true",
+            description="Start RViz2 automatically with this launch file.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "package",
+            default_value="meldog_description",
+        )
+    )
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "urdf_file",
+            default_value="meldog_core.urdf.xacro",
+        )
+    )
+
+
+    # Initialize Arguments
+    package_name = LaunchConfiguration("package")
+    urdf_file = LaunchConfiguration("urdf_file")
+
+    # Get URDF via xacro
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [
+                    FindPackageShare(package_name),
+                    "description",
+                    urdf_file,
+                ]
+            ),
+        ]
+    )
+    robot_description = {"robot_description": robot_description_content}
+
+    # rviz_config_file = PathJoinSubstitution(
+    #     [FindPackageShare("ros2_control_demo_description"), "rrbot/rviz", "rrbot.rviz"]
+    # )
+
+    robot_state_pub_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="both",
+        parameters=[robot_description],
+    )
+
+    joint_state_publisher_gui = Node(
+        package = "joint_state_publisher_gui",
+        executable = "joint_state_publisher_gui",
+    )
+
+    # Initialize Arguments
+    gui = LaunchConfiguration("gui")
+
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="log",
+        # arguments=["-d", rviz_config_file],
+        condition=IfCondition(gui),
+    )
+
+
+
+    nodes = [
+        robot_state_pub_node,
+        joint_state_publisher_gui, 
+        rviz_node 
+    ]
+
+    return LaunchDescription(declared_arguments + nodes)
\ No newline at end of file
diff --git a/src/meldog_description/package.xml b/src/meldog_description/package.xml
index 8c87aa77..1c26018a 100644
--- a/src/meldog_description/package.xml
+++ b/src/meldog_description/package.xml
@@ -9,6 +9,13 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <exec_depend>ros2launch</exec_depend>
+  <exec_depend>rviz2</exec_depend>
+  <exec_depend>urdf</exec_depend>
+  <exec_depend>xacro</exec_depend>
+  <exec_depend>joint_state_publisher_gui</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

From f58ac72451d06c6607ba65c48dc24061e8472961 Mon Sep 17 00:00:00 2001
From: Bartek <bartlomiejk@vp.pl>
Date: Sun, 1 Sep 2024 15:52:00 +0200
Subject: [PATCH 03/23] Hip links and joints added

---
 .../description/hip/hip.urdf.xacro            | 35 +++++++++++
 .../description/meldog_core.urdf.xacro        | 59 ++++++++++++++++++-
 .../description/trunk/trunk.urdf.xacro        | 13 +---
 3 files changed, 95 insertions(+), 12 deletions(-)
 create mode 100644 src/meldog_description/description/hip/hip.urdf.xacro

diff --git a/src/meldog_description/description/hip/hip.urdf.xacro b/src/meldog_description/description/hip/hip.urdf.xacro
new file mode 100644
index 00000000..46deb5ca
--- /dev/null
+++ b/src/meldog_description/description/hip/hip.urdf.xacro
@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "meldog_hip">
+
+
+
+    <xacro:property name="hip_length" value="0.2"/>
+    <xacro:property name="hip_width" value="0.1"/>
+    <xacro:property name="hip_height" value="0.05"/>
+
+    <xacro:macro name="hip_link" params="position">
+        <link name="hip_${position}_link">
+            <inertial>
+                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+                <mass value="3.0"/>
+                <inertia ixx="1.0" ixy="1.0" ixz="1.0" iyy="1.0" iyz="1.0" izz="1.0"/>
+            </inertial>
+            <visual name="">
+                <origin xyz="${hip_length/2} 0.0 0.0" rpy="0.0 0.0 0.0"/>
+                <geometry>
+                    <box size="${hip_length} ${hip_width} ${hip_height}"/>
+                </geometry>
+                <material name="hip_${position}_link">
+                    <color rgba="1.0 0.0 0.0 1.0"/>
+                    <texture filename=""/>
+                </material>
+            </visual>
+            <!-- <collision>
+                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+                <geometry>
+                    <box size="0.0 0.0 0.0"/>
+                </geometry>
+            </collision> -->
+        </link>
+    </xacro:macro>
+</robot>
\ No newline at end of file
diff --git a/src/meldog_description/description/meldog_core.urdf.xacro b/src/meldog_description/description/meldog_core.urdf.xacro
index 377bd404..5a9de98b 100644
--- a/src/meldog_description/description/meldog_core.urdf.xacro
+++ b/src/meldog_description/description/meldog_core.urdf.xacro
@@ -1,9 +1,66 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "meldog_core">
 
+    <!-- PI -->
+    <xacro:property name="PI" value="3.1415926535897"/>
 
-    <!-- BASE LINK spawns in x= 0.0 y = 0.0 z = 0.0-->
+
+    <!-- Base link spawns in x= 0.0 y = 0.0 z = 0.0 -->
     <link name="base_link"></link>
+
+    <!-- Trunk link -->
     <xacro:include filename="trunk/trunk.urdf.xacro"/>
 
+    <!-- Hip links -->
+    <xacro:include filename="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"/>
+
+    <!-- Base to Trunk joint -->
+    <joint name="base_trunk_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-->
+    </joint>
+
+    <!-- Trunk to Hip front left joint -->
+    <joint name="trunk_hip_front_left_joint" type="revolute">
+        <origin xyz="${trunk_actuators_length/2} ${trunk_base_width/2 + trunk_actuators_width/2} 0.0" rpy="0.0 0.0 0.0"/>
+        <parent link="trunk_link"/>
+        <child link="hip_front_left_link"/>
+        <axis xyz="1.0 0.0 0.0"/>
+        <limit lower="-1.0" upper="1" effort="10.0" velocity="100.0"/>
+    </joint>
+
+    <!-- Trunk to Hip front right joint -->
+    <joint name="trunk_hip_front_right_joint" type="revolute">
+        <origin xyz="${trunk_actuators_length/2} ${-(trunk_base_width/2 + trunk_actuators_width/2)} 0.0" rpy="0.0 0.0 0.0"/>
+        <parent link="trunk_link"/>
+        <child link="hip_front_right_link"/>
+        <axis xyz="1.0 0.0 0.0"/>
+        <limit lower="-1.0" upper="1" effort="10.0" velocity="100.0"/>
+    </joint>
+
+    <!-- Trunk to Hip back left joint -->
+    <joint name="trunk_hip_back_left_joint" type="revolute">
+        <origin xyz="${-trunk_actuators_length/2} ${trunk_base_width/2 + trunk_actuators_width/2} 0.0" rpy="0.0 0.0 ${PI}"/>
+        <parent link="trunk_link"/>
+        <child link="hip_back_left_link"/>
+        <axis xyz="1.0 0.0 0.0"/>
+        <limit lower="-1.0" upper="1" effort="10.0" velocity="100.0"/>
+    </joint>
+
+    <!-- Trunk to Hip back right joint -->
+    <joint name="trunk_hip_back_right_joint" type="revolute">
+        <origin xyz="${-trunk_actuators_length/2} ${-(trunk_base_width/2 + trunk_actuators_width/2)} 0.0" rpy="0.0 0.0 ${PI}"/>
+        <parent link="trunk_link"/>
+        <child link="hip_back_right_link"/>
+        <axis xyz="1.0 0.0 0.0"/>
+        <limit lower="-1.0" upper="1" effort="10.0" velocity="100.0"/>
+    </joint>
+
+
 </robot>
\ No newline at end of file
diff --git a/src/meldog_description/description/trunk/trunk.urdf.xacro b/src/meldog_description/description/trunk/trunk.urdf.xacro
index 3723aef3..7174b266 100644
--- a/src/meldog_description/description/trunk/trunk.urdf.xacro
+++ b/src/meldog_description/description/trunk/trunk.urdf.xacro
@@ -1,14 +1,5 @@
 <?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" >
-
-    <!-- Trunk joint to base -->
-    <joint name="trunk_joint" type="fixed">
-        <parent link="base_link"/>
-        <child link="trunk"/>
-        <!--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-->
-    </joint>
-
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "meldog_trunk">
 
     <xacro:property name="trunk_base_length" value="0.8" />
     <xacro:property name="trunk_base_width" value="0.2" />
@@ -18,7 +9,7 @@
     <xacro:property name="trunk_actuators_width" value="0.2" />
     <xacro:property name="trunk_actuators_height" value="0.1" />
 
-    <link name="trunk">
+    <link name="trunk_link">
         <inertial>
             <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
             <mass value="5.0"/> <!--testing-->

From 37335dce40754d1e8f13fb2b5648f65a69832139 Mon Sep 17 00:00:00 2001
From: Bartek <bartlomiejk@vp.pl>
Date: Sun, 1 Sep 2024 16:18:17 +0200
Subject: [PATCH 04/23] Lower leg links and joints added

---
 .../lower_leg/lower_leg.urdf.xacro            | 33 ++++++++++++
 .../description/meldog_core.urdf.xacro        | 52 +++++++++++++++++++
 .../launch/robot_visualize.launch.py          |  7 ++-
 3 files changed, 90 insertions(+), 2 deletions(-)
 create mode 100644 src/meldog_description/description/lower_leg/lower_leg.urdf.xacro

diff --git a/src/meldog_description/description/lower_leg/lower_leg.urdf.xacro b/src/meldog_description/description/lower_leg/lower_leg.urdf.xacro
new file mode 100644
index 00000000..9d31d0ac
--- /dev/null
+++ b/src/meldog_description/description/lower_leg/lower_leg.urdf.xacro
@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "meldog_lower_leg">
+
+    <xacro:property name="lower_leg_length" value="0.2"/>
+    <xacro:property name="lower_leg_radius" value="0.05"/>
+
+
+    <xacro:macro name="lower_leg_link" params="position">
+        <link name="lower_leg_${position}_link">
+            <inertial>
+                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+                <mass value="1.0"/>
+                <inertia ixx="1.0" ixy="1.0" ixz="1.0" iyy="1.0" iyz="1.0" izz="1.0"/>
+            </inertial>
+            <visual name="">
+                <origin xyz="0.0 0.0 ${-lower_leg_length/2}" rpy="0.0 0.0 0.0"/>
+                <geometry>
+                    <cylinder radius="${lower_leg_radius}" length="${lower_leg_length}"/>
+                </geometry>
+                <material name="lower_leg_${position}_link">
+                    <color rgba="0.0 1.0 0.0 1.0"/>
+                    <texture filename=""/>
+                </material>
+            </visual>
+            <!-- <collision>
+                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+                <geometry>
+                    <box size="0.0 0.0 0.0"/>
+                </geometry>
+            </collision> -->
+        </link>
+    </xacro:macro>
+</robot>
diff --git a/src/meldog_description/description/meldog_core.urdf.xacro b/src/meldog_description/description/meldog_core.urdf.xacro
index 5a9de98b..cf2764f5 100644
--- a/src/meldog_description/description/meldog_core.urdf.xacro
+++ b/src/meldog_description/description/meldog_core.urdf.xacro
@@ -4,6 +4,8 @@
     <!-- PI -->
     <xacro:property name="PI" value="3.1415926535897"/>
 
+    <!-- LINKS: -->
+
 
     <!-- Base link spawns in x= 0.0 y = 0.0 z = 0.0 -->
     <link name="base_link"></link>
@@ -18,6 +20,17 @@
     <xacro:hip_link position="back_left"/>
     <xacro:hip_link position="back_right"/>
 
+    <!-- Lower leg links -->
+    <xacro:include filename="lower_leg/lower_leg.urdf.xacro"/>
+    <xacro:lower_leg_link position="front_left"/>
+    <xacro:lower_leg_link position="front_right"/>
+    <xacro:lower_leg_link position="back_left"/>
+    <xacro:lower_leg_link position="back_right"/>
+
+
+    <!-- JOINTS: -->
+
+
     <!-- Base to Trunk joint -->
     <joint name="base_trunk_joint" type="fixed">
         <parent link="base_link"/>
@@ -26,6 +39,7 @@
         <origin xyz="0 0 1.0" rpy="0 0 0"/>  <!--testing-->
     </joint>
 
+
     <!-- Trunk to Hip front left joint -->
     <joint name="trunk_hip_front_left_joint" type="revolute">
         <origin xyz="${trunk_actuators_length/2} ${trunk_base_width/2 + trunk_actuators_width/2} 0.0" rpy="0.0 0.0 0.0"/>
@@ -63,4 +77,42 @@
     </joint>
 
 
+    <!-- Hip front left to Lower leg front left -->
+    <joint name="hip_lower_leg_front_left" type="revolute">
+        <origin xyz="${hip_length/2} ${hip_width/2 + lower_leg_radius} 0.0" rpy="0.0 0.0 0.0"/>
+        <parent link="hip_front_left_link"/>
+        <child link="lower_leg_front_left_link"/>
+        <axis xyz="0.0 1.0 0.0"/>
+        <limit lower="-1.0" upper="1.0" effort="10.0" velocity="100.0"/>
+    </joint>
+
+    <!-- Hip front right to Lower leg front right -->
+    <joint name="hip_lower_leg_front_right" type="revolute">
+        <origin xyz="${hip_length/2} ${-(hip_width/2 + lower_leg_radius)} 0.0" rpy="0.0 0.0 0.0"/>
+        <parent link="hip_front_right_link"/>
+        <child link="lower_leg_front_right_link"/>
+        <axis xyz="0.0 1.0 0.0"/>
+        <limit lower="-1.0" upper="1.0" effort="10.0" velocity="100.0"/>
+    </joint>
+
+    <!-- Hip back left to Lower leg back left -->
+    <joint name="hip_lower_leg_back_left" type="revolute">
+        <origin xyz="${hip_length/2} ${-(hip_width/2 + lower_leg_radius)} 0.0" rpy="0.0 0.0 0.0"/>
+        <parent link="hip_back_left_link"/>
+        <child link="lower_leg_back_left_link"/>
+        <axis xyz="0.0 1.0 0.0"/>
+        <limit lower="-1.0" upper="1.0" effort="10.0" velocity="100.0"/>
+    </joint>
+
+    <!-- Hip back right to Lower leg back right -->
+    <joint name="hip_lower_leg_back_right" type="revolute">
+        <origin xyz="${hip_length/2} ${hip_width/2 + lower_leg_radius} 0.0" rpy="0.0 0.0 0.0"/>
+        <parent link="hip_back_right_link"/>
+        <child link="lower_leg_back_right_link"/>
+        <axis xyz="0.0 1.0 0.0"/>
+        <limit lower="-1.0" upper="1.0" effort="10.0" velocity="100.0"/>
+    </joint>
+    
+
+
 </robot>
\ No newline at end of file
diff --git a/src/meldog_description/launch/robot_visualize.launch.py b/src/meldog_description/launch/robot_visualize.launch.py
index 0aceadfe..e32d3ec9 100644
--- a/src/meldog_description/launch/robot_visualize.launch.py
+++ b/src/meldog_description/launch/robot_visualize.launch.py
@@ -7,6 +7,8 @@
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 
+from launch_ros.parameter_descriptions import ParameterValue
+
 
 def generate_launch_description():
     # Declare arguments
@@ -39,7 +41,8 @@ def generate_launch_description():
     urdf_file = LaunchConfiguration("urdf_file")
 
     # Get URDF via xacro
-    robot_description_content = Command(
+    robot_description_content = ParameterValue(
+        Command(
         [
             PathJoinSubstitution([FindExecutable(name="xacro")]),
             " ",
@@ -51,7 +54,7 @@ def generate_launch_description():
                 ]
             ),
         ]
-    )
+    ), value_type=str)
     robot_description = {"robot_description": robot_description_content}
 
     # rviz_config_file = PathJoinSubstitution(

From 553a783f8dbc2c37b68635589872e399045ee3dc Mon Sep 17 00:00:00 2001
From: Bartek <bartlomiejk@vp.pl>
Date: Mon, 2 Sep 2024 17:29:51 +0200
Subject: [PATCH 05/23] Added shank links and joints, changed lower_leg to
 thigh

---
 .../description/meldog_core.urdf.xacro        | 99 ++++++++++++++-----
 .../description/shank/shank.urdf.xacro        | 33 +++++++
 .../thigh.urdf.xacro}                         | 16 +--
 3 files changed, 113 insertions(+), 35 deletions(-)
 create mode 100644 src/meldog_description/description/shank/shank.urdf.xacro
 rename src/meldog_description/description/{lower_leg/lower_leg.urdf.xacro => thigh/thigh.urdf.xacro} (63%)

diff --git a/src/meldog_description/description/meldog_core.urdf.xacro b/src/meldog_description/description/meldog_core.urdf.xacro
index cf2764f5..e7a6b7c5 100644
--- a/src/meldog_description/description/meldog_core.urdf.xacro
+++ b/src/meldog_description/description/meldog_core.urdf.xacro
@@ -20,19 +20,27 @@
     <xacro:hip_link position="back_left"/>
     <xacro:hip_link position="back_right"/>
 
-    <!-- Lower leg links -->
-    <xacro:include filename="lower_leg/lower_leg.urdf.xacro"/>
-    <xacro:lower_leg_link position="front_left"/>
-    <xacro:lower_leg_link position="front_right"/>
-    <xacro:lower_leg_link position="back_left"/>
-    <xacro:lower_leg_link position="back_right"/>
+    <!-- Thigh links -->
+    <xacro:include filename="thigh/thigh.urdf.xacro"/>
+    <xacro:thigh_link position="front_left"/>
+    <xacro:thigh_link position="front_right"/>
+    <xacro:thigh_link position="back_left"/>
+    <xacro:thigh_link position="back_right"/>
+
+
+    <!-- Shank links -->
+    <xacro:include filename="shank/shank.urdf.xacro"/>
+    <xacro:shank_link position="front_left"/>
+    <xacro:shank_link position="front_right"/>
+    <xacro:shank_link position="back_left"/>
+    <xacro:shank_link position="back_right"/>
 
 
     <!-- JOINTS: -->
 
 
     <!-- Base to Trunk joint -->
-    <joint name="base_trunk_joint" type="fixed">
+    <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.-->
@@ -41,7 +49,7 @@
 
 
     <!-- Trunk to Hip front left joint -->
-    <joint name="trunk_hip_front_left_joint" type="revolute">
+    <joint name="FLH_joint" type="revolute">
         <origin xyz="${trunk_actuators_length/2} ${trunk_base_width/2 + trunk_actuators_width/2} 0.0" rpy="0.0 0.0 0.0"/>
         <parent link="trunk_link"/>
         <child link="hip_front_left_link"/>
@@ -50,7 +58,7 @@
     </joint>
 
     <!-- Trunk to Hip front right joint -->
-    <joint name="trunk_hip_front_right_joint" type="revolute">
+    <joint name="FRH_joint" type="revolute">
         <origin xyz="${trunk_actuators_length/2} ${-(trunk_base_width/2 + trunk_actuators_width/2)} 0.0" rpy="0.0 0.0 0.0"/>
         <parent link="trunk_link"/>
         <child link="hip_front_right_link"/>
@@ -59,7 +67,7 @@
     </joint>
 
     <!-- Trunk to Hip back left joint -->
-    <joint name="trunk_hip_back_left_joint" type="revolute">
+    <joint name="BLH_joint" type="revolute">
         <origin xyz="${-trunk_actuators_length/2} ${trunk_base_width/2 + trunk_actuators_width/2} 0.0" rpy="0.0 0.0 ${PI}"/>
         <parent link="trunk_link"/>
         <child link="hip_back_left_link"/>
@@ -68,7 +76,7 @@
     </joint>
 
     <!-- Trunk to Hip back right joint -->
-    <joint name="trunk_hip_back_right_joint" type="revolute">
+    <joint name="BRH_joint" type="revolute">
         <origin xyz="${-trunk_actuators_length/2} ${-(trunk_base_width/2 + trunk_actuators_width/2)} 0.0" rpy="0.0 0.0 ${PI}"/>
         <parent link="trunk_link"/>
         <child link="hip_back_right_link"/>
@@ -77,38 +85,75 @@
     </joint>
 
 
-    <!-- Hip front left to Lower leg front left -->
-    <joint name="hip_lower_leg_front_left" type="revolute">
-        <origin xyz="${hip_length/2} ${hip_width/2 + lower_leg_radius} 0.0" rpy="0.0 0.0 0.0"/>
+    <!-- Hip front left to Thigh front left -->
+    <joint name="FLT_joint" type="revolute">
+        <origin xyz="${hip_length/2} ${hip_width/2 + thigh_radius} 0.0" rpy="0.0 0.0 0.0"/>
         <parent link="hip_front_left_link"/>
-        <child link="lower_leg_front_left_link"/>
+        <child link="thigh_front_left_link"/>
         <axis xyz="0.0 1.0 0.0"/>
         <limit lower="-1.0" upper="1.0" effort="10.0" velocity="100.0"/>
     </joint>
 
-    <!-- Hip front right to Lower leg front right -->
-    <joint name="hip_lower_leg_front_right" type="revolute">
-        <origin xyz="${hip_length/2} ${-(hip_width/2 + lower_leg_radius)} 0.0" rpy="0.0 0.0 0.0"/>
+    <!-- Hip front right to Thigh front right -->
+    <joint name="FRT_joint" type="revolute">
+        <origin xyz="${hip_length/2} ${-(hip_width/2 + thigh_radius)} 0.0" rpy="0.0 0.0 0.0"/>
         <parent link="hip_front_right_link"/>
-        <child link="lower_leg_front_right_link"/>
+        <child link="thigh_front_right_link"/>
         <axis xyz="0.0 1.0 0.0"/>
         <limit lower="-1.0" upper="1.0" effort="10.0" velocity="100.0"/>
     </joint>
 
-    <!-- Hip back left to Lower leg back left -->
-    <joint name="hip_lower_leg_back_left" type="revolute">
-        <origin xyz="${hip_length/2} ${-(hip_width/2 + lower_leg_radius)} 0.0" rpy="0.0 0.0 0.0"/>
+    <!-- Hip back left to Thigh back left -->
+    <joint name="BLT_joint" type="revolute">
+        <origin xyz="${hip_length/2} ${-(hip_width/2 + thigh_radius)} 0.0" rpy="0.0 0.0 0.0"/>
         <parent link="hip_back_left_link"/>
-        <child link="lower_leg_back_left_link"/>
+        <child link="thigh_back_left_link"/>
         <axis xyz="0.0 1.0 0.0"/>
         <limit lower="-1.0" upper="1.0" effort="10.0" velocity="100.0"/>
     </joint>
 
-    <!-- Hip back right to Lower leg back right -->
-    <joint name="hip_lower_leg_back_right" type="revolute">
-        <origin xyz="${hip_length/2} ${hip_width/2 + lower_leg_radius} 0.0" rpy="0.0 0.0 0.0"/>
+    <!-- Hip back right to Thigh back right -->
+    <joint name="BRT_joint" type="revolute">
+        <origin xyz="${hip_length/2} ${hip_width/2 + thigh_radius} 0.0" rpy="0.0 0.0 0.0"/>
         <parent link="hip_back_right_link"/>
-        <child link="lower_leg_back_right_link"/>
+        <child link="thigh_back_right_link"/>
+        <axis xyz="0.0 1.0 0.0"/>
+        <limit lower="-1.0" upper="1.0" effort="10.0" velocity="100.0"/>
+    </joint>
+
+
+    <!-- Thigh front left to Shank front left -->
+    <joint name="FLS_joint" type="revolute">
+        <origin xyz="0.0 0.0 -${thigh_length}" rpy="0.0 0.0 0.0"/>
+        <parent link="thigh_front_left_link"/>
+        <child link="shank_front_left_link"/>
+        <axis xyz="0.0 1.0 0.0"/>
+        <limit lower="-1.0" upper="1.0" effort="10.0" velocity="100.0"/>
+    </joint>
+
+    <!-- Thigh front right to Shank front right -->
+    <joint name="FRS_joint" type="revolute">
+        <origin xyz="0.0 0.0 -${thigh_length}" rpy="0.0 0.0 0.0"/>
+        <parent link="thigh_front_right_link"/>
+        <child link="shank_front_right_link"/>
+        <axis xyz="0.0 1.0 0.0"/>
+        <limit lower="-1.0" upper="1.0" effort="10.0" velocity="100.0"/>
+    </joint>
+
+    <!-- Thigh back left to Shank back left -->
+    <joint name="BLS_joint" type="revolute">
+        <origin xyz="0.0 0.0 -${thigh_length}" rpy="0.0 0.0 0.0"/>
+        <parent link="thigh_back_left_link"/>
+        <child link="shank_back_left_link"/>
+        <axis xyz="0.0 1.0 0.0"/>
+        <limit lower="-1.0" upper="1.0" effort="10.0" velocity="100.0"/>
+    </joint>
+
+    <!-- Thigh back right to Shank back right -->
+    <joint name="BRS_joint" type="revolute">
+        <origin xyz="0.0 0.0 -${thigh_length}" rpy="0.0 0.0 0.0"/>
+        <parent link="thigh_back_right_link"/>
+        <child link="shank_back_right_link"/>
         <axis xyz="0.0 1.0 0.0"/>
         <limit lower="-1.0" upper="1.0" effort="10.0" velocity="100.0"/>
     </joint>
diff --git a/src/meldog_description/description/shank/shank.urdf.xacro b/src/meldog_description/description/shank/shank.urdf.xacro
new file mode 100644
index 00000000..f5e739f8
--- /dev/null
+++ b/src/meldog_description/description/shank/shank.urdf.xacro
@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "meldog_shank">
+
+    <xacro:property name="shank_length" value="0.5"/>
+    <xacro:property name="shank_radius" value="0.05"/>
+
+
+    <xacro:macro name="shank_link" params="position">
+        <link name="shank_${position}_link">
+            <inertial>
+                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+                <mass value="1.0"/>
+                <inertia ixx="1.0" ixy="1.0" ixz="1.0" iyy="1.0" iyz="1.0" izz="1.0"/>
+            </inertial>
+            <visual name="">
+                <origin xyz="0.0 0.0 ${-shank_length/2}" rpy="0.0 0.0 0.0"/>
+                <geometry>
+                    <cylinder radius="${shank_radius}" length="${shank_length}"/>
+                </geometry>
+                <material name="shank_${position}_link">
+                    <color rgba="1.0 1.0 0.0 1.0"/>
+                    <texture filename=""/>
+                </material>
+            </visual>
+            <!-- <collision>
+                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+                <geometry>
+                    <box size="0.0 0.0 0.0"/>
+                </geometry>
+            </collision> -->
+        </link>
+    </xacro:macro>
+</robot>
\ No newline at end of file
diff --git a/src/meldog_description/description/lower_leg/lower_leg.urdf.xacro b/src/meldog_description/description/thigh/thigh.urdf.xacro
similarity index 63%
rename from src/meldog_description/description/lower_leg/lower_leg.urdf.xacro
rename to src/meldog_description/description/thigh/thigh.urdf.xacro
index 9d31d0ac..22197a98 100644
--- a/src/meldog_description/description/lower_leg/lower_leg.urdf.xacro
+++ b/src/meldog_description/description/thigh/thigh.urdf.xacro
@@ -1,23 +1,23 @@
 <?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "meldog_lower_leg">
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "meldog_thigh">
 
-    <xacro:property name="lower_leg_length" value="0.2"/>
-    <xacro:property name="lower_leg_radius" value="0.05"/>
+    <xacro:property name="thigh_length" value="0.5"/>
+    <xacro:property name="thigh_radius" value="0.05"/>
 
 
-    <xacro:macro name="lower_leg_link" params="position">
-        <link name="lower_leg_${position}_link">
+    <xacro:macro name="thigh_link" params="position">
+        <link name="thigh_${position}_link">
             <inertial>
                 <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                 <mass value="1.0"/>
                 <inertia ixx="1.0" ixy="1.0" ixz="1.0" iyy="1.0" iyz="1.0" izz="1.0"/>
             </inertial>
             <visual name="">
-                <origin xyz="0.0 0.0 ${-lower_leg_length/2}" rpy="0.0 0.0 0.0"/>
+                <origin xyz="0.0 0.0 ${-thigh_length/2}" rpy="0.0 0.0 0.0"/>
                 <geometry>
-                    <cylinder radius="${lower_leg_radius}" length="${lower_leg_length}"/>
+                    <cylinder radius="${thigh_radius}" length="${thigh_length}"/>
                 </geometry>
-                <material name="lower_leg_${position}_link">
+                <material name="thigh_${position}_link">
                     <color rgba="0.0 1.0 0.0 1.0"/>
                     <texture filename=""/>
                 </material>

From e4b030469d7c4ca2838ce7f11668a68acf8f0bc2 Mon Sep 17 00:00:00 2001
From: Bartek <bartlomiejk@vp.pl>
Date: Tue, 3 Sep 2024 20:09:24 +0200
Subject: [PATCH 06/23] Changed shank joints and added feet links and joints

---
 .../description/feet/feet.urdf.xacro          | 52 +++++++++++++++++++
 .../description/meldog_core.urdf.xacro        | 46 +++++++++++++---
 2 files changed, 92 insertions(+), 6 deletions(-)
 create mode 100644 src/meldog_description/description/feet/feet.urdf.xacro

diff --git a/src/meldog_description/description/feet/feet.urdf.xacro b/src/meldog_description/description/feet/feet.urdf.xacro
new file mode 100644
index 00000000..a930d017
--- /dev/null
+++ b/src/meldog_description/description/feet/feet.urdf.xacro
@@ -0,0 +1,52 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "meldog_feet">
+
+
+    <xacro:property name="feet_radius" value="0.08"/>
+
+    <xacro:macro name="feet_link" params="position">
+        <link name="feet_${position}_link">
+            <inertial>
+                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+                <mass value="3.0"/>
+                <inertia ixx="1.0" ixy="1.0" ixz="1.0" iyy="1.0" iyz="1.0" izz="1.0"/>
+            </inertial>
+            <visual name="">
+                <origin xyz="0.0 0.0 ${-feet_radius}" rpy="0.0 0.0 0.0"/>
+                <geometry>
+                    <sphere radius="${feet_radius}"/>
+                </geometry>
+                <material name="feet_${position}_link">
+                    <color rgba="1.0 0.0 1.0 1.0"/>
+                    <texture filename=""/>
+                </material>
+            </visual>
+            <!-- <collision>
+                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+                <geometry>
+                    <box size="0.0 0.0 0.0"/>
+                </geometry>
+            </collision> -->
+        </link>
+    </xacro:macro>
+    <!-- <xacro:property name="feet_radius" value="0.01"/>
+    
+    <xacro:macro name="feet_link" params="position">
+        <link name="feet_${position}_link">
+            <inertial>
+                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+                <mass value="3.0"/>
+                <inertia ixx="1.0" ixy="1.0" ixz="1.0" iyy="1.0" iyz="1.0" izz="1.0"/>
+            </inertial>
+            <visual name="">
+                <origin xyz="$0.0 0.0 ${-feet_radius}" rpy="0.0 0.0 0.0"/>
+                <geometry>
+                    <sphere radius="${feet_radius}"/>
+                </geometry>
+                <material name="">
+                    <color rgba="1.0 0.0 1.0 1.0"/>
+                </material>
+            </visual>
+        </link>
+    </xacro:macro> -->
+</robot>
\ No newline at end of file
diff --git a/src/meldog_description/description/meldog_core.urdf.xacro b/src/meldog_description/description/meldog_core.urdf.xacro
index e7a6b7c5..5ed44800 100644
--- a/src/meldog_description/description/meldog_core.urdf.xacro
+++ b/src/meldog_description/description/meldog_core.urdf.xacro
@@ -36,6 +36,13 @@
     <xacro:shank_link position="back_right"/>
 
 
+    <!-- Feet links -->
+    <xacro:include filename="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: -->
 
 
@@ -62,7 +69,7 @@
         <origin xyz="${trunk_actuators_length/2} ${-(trunk_base_width/2 + trunk_actuators_width/2)} 0.0" rpy="0.0 0.0 0.0"/>
         <parent link="trunk_link"/>
         <child link="hip_front_right_link"/>
-        <axis xyz="1.0 0.0 0.0"/>
+        <axis xyz="-1.0 0.0 0.0"/>
         <limit lower="-1.0" upper="1" effort="10.0" velocity="100.0"/>
     </joint>
 
@@ -71,7 +78,7 @@
         <origin xyz="${-trunk_actuators_length/2} ${trunk_base_width/2 + trunk_actuators_width/2} 0.0" rpy="0.0 0.0 ${PI}"/>
         <parent link="trunk_link"/>
         <child link="hip_back_left_link"/>
-        <axis xyz="1.0 0.0 0.0"/>
+        <axis xyz="-1.0 0.0 0.0"/>
         <limit lower="-1.0" upper="1" effort="10.0" velocity="100.0"/>
     </joint>
 
@@ -90,7 +97,7 @@
         <origin xyz="${hip_length/2} ${hip_width/2 + thigh_radius} 0.0" rpy="0.0 0.0 0.0"/>
         <parent link="hip_front_left_link"/>
         <child link="thigh_front_left_link"/>
-        <axis xyz="0.0 1.0 0.0"/>
+        <axis xyz="0.0 -1.0 0.0"/>
         <limit lower="-1.0" upper="1.0" effort="10.0" velocity="100.0"/>
     </joint>
 
@@ -99,7 +106,7 @@
         <origin xyz="${hip_length/2} ${-(hip_width/2 + thigh_radius)} 0.0" rpy="0.0 0.0 0.0"/>
         <parent link="hip_front_right_link"/>
         <child link="thigh_front_right_link"/>
-        <axis xyz="0.0 1.0 0.0"/>
+        <axis xyz="0.0 -1.0 0.0"/>
         <limit lower="-1.0" upper="1.0" effort="10.0" velocity="100.0"/>
     </joint>
 
@@ -127,7 +134,7 @@
         <origin xyz="0.0 0.0 -${thigh_length}" rpy="0.0 0.0 0.0"/>
         <parent link="thigh_front_left_link"/>
         <child link="shank_front_left_link"/>
-        <axis xyz="0.0 1.0 0.0"/>
+        <axis xyz="0.0 -1.0 0.0"/>
         <limit lower="-1.0" upper="1.0" effort="10.0" velocity="100.0"/>
     </joint>
 
@@ -136,7 +143,7 @@
         <origin xyz="0.0 0.0 -${thigh_length}" rpy="0.0 0.0 0.0"/>
         <parent link="thigh_front_right_link"/>
         <child link="shank_front_right_link"/>
-        <axis xyz="0.0 1.0 0.0"/>
+        <axis xyz="0.0 -1.0 0.0"/>
         <limit lower="-1.0" upper="1.0" effort="10.0" velocity="100.0"/>
     </joint>
 
@@ -159,5 +166,32 @@
     </joint>
     
 
+    <!-- Shank front left to Feet front left -->
+    <joint name="FLF_joint" type="fixed">
+        <origin xyz="0.0 0.0 ${-shank_length}" rpy="0.0 0.0 0.0"/>
+        <parent link="shank_front_left_link"/>
+        <child link="feet_front_left_link"/>
+    </joint>
+
+    <!-- Shank front right to Feet right left -->
+    <joint name="RLF_joint" type="fixed">
+        <origin xyz="0.0 0.0 ${-shank_length}" rpy="0.0 0.0 0.0"/>
+        <parent link="shank_front_right_link"/>
+        <child link="feet_front_right_link"/>
+    </joint>
+
+    <!-- Shank back left to Feet back left -->
+    <joint name="BLF_joint" type="fixed">
+        <origin xyz="0.0 0.0 ${-shank_length}" rpy="0.0 0.0 0.0"/>
+        <parent link="shank_back_left_link"/>
+        <child link="feet_back_left_link"/>
+    </joint>
+
+    <!-- Shank back right to Feet back right-->
+    <joint name="BRF_joint" type="fixed">
+        <origin xyz="0.0 0.0 ${-shank_length}" rpy="0.0 0.0 0.0"/>
+        <parent link="shank_back_right_link"/>
+        <child link="feet_back_right_link"/>
+    </joint>
 
 </robot>
\ No newline at end of file

From 993d96e5e4be50c97221858acda333caa4504858 Mon Sep 17 00:00:00 2001
From: Bartek <bartlomiejk@vp.pl>
Date: Wed, 4 Sep 2024 18:14:14 +0200
Subject: [PATCH 07/23] Deleted feed link, added only frames

---
 .../description/feet/feet.urdf.xacro          | 52 -------------------
 .../description/meldog_core.urdf.xacro        | 15 +++---
 .../description/shank/shank.urdf.xacro        | 13 ++++-
 3 files changed, 19 insertions(+), 61 deletions(-)
 delete mode 100644 src/meldog_description/description/feet/feet.urdf.xacro

diff --git a/src/meldog_description/description/feet/feet.urdf.xacro b/src/meldog_description/description/feet/feet.urdf.xacro
deleted file mode 100644
index a930d017..00000000
--- a/src/meldog_description/description/feet/feet.urdf.xacro
+++ /dev/null
@@ -1,52 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "meldog_feet">
-
-
-    <xacro:property name="feet_radius" value="0.08"/>
-
-    <xacro:macro name="feet_link" params="position">
-        <link name="feet_${position}_link">
-            <inertial>
-                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
-                <mass value="3.0"/>
-                <inertia ixx="1.0" ixy="1.0" ixz="1.0" iyy="1.0" iyz="1.0" izz="1.0"/>
-            </inertial>
-            <visual name="">
-                <origin xyz="0.0 0.0 ${-feet_radius}" rpy="0.0 0.0 0.0"/>
-                <geometry>
-                    <sphere radius="${feet_radius}"/>
-                </geometry>
-                <material name="feet_${position}_link">
-                    <color rgba="1.0 0.0 1.0 1.0"/>
-                    <texture filename=""/>
-                </material>
-            </visual>
-            <!-- <collision>
-                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
-                <geometry>
-                    <box size="0.0 0.0 0.0"/>
-                </geometry>
-            </collision> -->
-        </link>
-    </xacro:macro>
-    <!-- <xacro:property name="feet_radius" value="0.01"/>
-    
-    <xacro:macro name="feet_link" params="position">
-        <link name="feet_${position}_link">
-            <inertial>
-                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
-                <mass value="3.0"/>
-                <inertia ixx="1.0" ixy="1.0" ixz="1.0" iyy="1.0" iyz="1.0" izz="1.0"/>
-            </inertial>
-            <visual name="">
-                <origin xyz="$0.0 0.0 ${-feet_radius}" rpy="0.0 0.0 0.0"/>
-                <geometry>
-                    <sphere radius="${feet_radius}"/>
-                </geometry>
-                <material name="">
-                    <color rgba="1.0 0.0 1.0 1.0"/>
-                </material>
-            </visual>
-        </link>
-    </xacro:macro> -->
-</robot>
\ No newline at end of file
diff --git a/src/meldog_description/description/meldog_core.urdf.xacro b/src/meldog_description/description/meldog_core.urdf.xacro
index 5ed44800..7f144376 100644
--- a/src/meldog_description/description/meldog_core.urdf.xacro
+++ b/src/meldog_description/description/meldog_core.urdf.xacro
@@ -36,14 +36,13 @@
     <xacro:shank_link position="back_right"/>
 
 
-    <!-- Feet links -->
-    <xacro:include filename="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: -->
+    <!-- Feet links (frames in the end of shank) -->
+    <link name="feet_front_left_link"></link>
+    <link name="feet_front_right_link"></link>
+    <link name="feet_back_left_link"></link>
+    <link name="feet_back_right_link"></link>
+
+    <!-- JOINTS: -->    
 
 
     <!-- Base to Trunk joint -->
diff --git a/src/meldog_description/description/shank/shank.urdf.xacro b/src/meldog_description/description/shank/shank.urdf.xacro
index f5e739f8..1bb9ef35 100644
--- a/src/meldog_description/description/shank/shank.urdf.xacro
+++ b/src/meldog_description/description/shank/shank.urdf.xacro
@@ -3,6 +3,7 @@
 
     <xacro:property name="shank_length" value="0.5"/>
     <xacro:property name="shank_radius" value="0.05"/>
+    <xacro:property name="feet_radius"  value ="0.08"/>
 
 
     <xacro:macro name="shank_link" params="position">
@@ -17,11 +18,21 @@
                 <geometry>
                     <cylinder radius="${shank_radius}" length="${shank_length}"/>
                 </geometry>
-                <material name="shank_${position}_link">
+                <material name="yellow">
                     <color rgba="1.0 1.0 0.0 1.0"/>
                     <texture filename=""/>
                 </material>
             </visual>
+            <visual name="">
+                <origin xyz="0.0 0.0 ${-shank_length}" rpy="0.0 0.0 0.0"/>
+                <geometry>
+                    <sphere radius="${feet_radius}"/>
+                </geometry>
+                <material name="purple">
+                    <color rgba="1.0 0.0 1.0 1.0"/>
+                    <texture filename=""/>
+                </material>
+            </visual>
             <!-- <collision>
                 <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                 <geometry>

From 9db93f951155df2a486a7b7bc30362d34c659dd2 Mon Sep 17 00:00:00 2001
From: Bartek <bartlomiejk@vp.pl>
Date: Wed, 4 Sep 2024 18:55:15 +0200
Subject: [PATCH 08/23] Added inertial properties and collision to all links

---
 .../description/hip/hip.urdf.xacro            | 29 +++++++++------
 .../description/shank/shank.urdf.xacro        | 37 ++++++++++++-------
 .../description/thigh/thigh.urdf.xacro        | 23 +++++++-----
 .../description/trunk/trunk.urdf.xacro        | 34 +++++++++++------
 4 files changed, 76 insertions(+), 47 deletions(-)

diff --git a/src/meldog_description/description/hip/hip.urdf.xacro b/src/meldog_description/description/hip/hip.urdf.xacro
index 46deb5ca..487a65de 100644
--- a/src/meldog_description/description/hip/hip.urdf.xacro
+++ b/src/meldog_description/description/hip/hip.urdf.xacro
@@ -4,32 +4,37 @@
 
 
     <xacro:property name="hip_length" value="0.2"/>
-    <xacro:property name="hip_width" value="0.1"/>
-    <xacro:property name="hip_height" value="0.05"/>
+    <xacro:property name="hip_width" value="0.15"/>
+    <xacro:property name="hip_height" value="0.075"/>
+
+    <xacro:property name="hip_mass" value="3.0"/>
 
     <xacro:macro name="hip_link" params="position">
         <link name="hip_${position}_link">
             <inertial>
-                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
-                <mass value="3.0"/>
-                <inertia ixx="1.0" ixy="1.0" ixz="1.0" iyy="1.0" iyz="1.0" izz="1.0"/>
+                <origin xyz="${hip_length/2} 0.0 0.0" rpy="0.0 0.0 0.0"/>
+                <mass value="${hip_mass}"/>
+                 <mass value="${trunk_mass}"/> 
+            <inertia ixx="${1/12*trunk_mass*(hip_width*hip_width + hip_height*hip_height)}" ixy="0.0" ixz="0.0" iyy="${1/12*trunk_mass*(hip_length*hip_length + hip_height*hip_height)}" iyz=".0" izz="${1/12*trunk_mass*(hip_length*hip_length + hip_width*hip_width)}"/> 
             </inertial>
-            <visual name="">
+
+            <visual name="hip_${position}_link_visual">
                 <origin xyz="${hip_length/2} 0.0 0.0" rpy="0.0 0.0 0.0"/>
                 <geometry>
                     <box size="${hip_length} ${hip_width} ${hip_height}"/>
                 </geometry>
-                <material name="hip_${position}_link">
+                <material name="">
                     <color rgba="1.0 0.0 0.0 1.0"/>
-                    <texture filename=""/>
                 </material>
             </visual>
-            <!-- <collision>
-                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+
+            <collision name="hip_${position}_link_collision">
+                <origin xyz="${hip_length/2} 0.0 0.0" rpy="0.0 0.0 0.0"/>
                 <geometry>
-                    <box size="0.0 0.0 0.0"/>
+                    <box size="${hip_length} ${hip_width} ${hip_height}"/>
                 </geometry>
-            </collision> -->
+            </collision>
+            
         </link>
     </xacro:macro>
 </robot>
\ No newline at end of file
diff --git a/src/meldog_description/description/shank/shank.urdf.xacro b/src/meldog_description/description/shank/shank.urdf.xacro
index 1bb9ef35..11155115 100644
--- a/src/meldog_description/description/shank/shank.urdf.xacro
+++ b/src/meldog_description/description/shank/shank.urdf.xacro
@@ -3,42 +3,51 @@
 
     <xacro:property name="shank_length" value="0.5"/>
     <xacro:property name="shank_radius" value="0.05"/>
-    <xacro:property name="feet_radius"  value ="0.08"/>
+    <xacro:property name="feet_radius" value ="0.08"/>
 
+    <xacro:property name="shank_mass" value = "1.0"/>
 
     <xacro:macro name="shank_link" params="position">
         <link name="shank_${position}_link">
             <inertial>
-                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
-                <mass value="1.0"/>
-                <inertia ixx="1.0" ixy="1.0" ixz="1.0" iyy="1.0" iyz="1.0" izz="1.0"/>
+                <origin xyz="0.0 0.0 ${-shank_length/2}" rpy="0.0 0.0 0.0"/>
+                <mass value="${shank_mass}"/>
+                <inertia ixx="${1/12*shank_mass*(3*shank_radius*shank_radius+shank_length*shank_length)}" ixy="0.0" ixz="0.0" iyy="${1/12*shank_mass*(3*shank_radius*shank_radius+shank_length*shank_length)}" iyz="0.0" izz="${1/2*shank_mass*shank_radius*shank_radius}"/>
             </inertial>
-            <visual name="">
+
+            <visual name="shank_${position}_link_visual">
                 <origin xyz="0.0 0.0 ${-shank_length/2}" rpy="0.0 0.0 0.0"/>
                 <geometry>
                     <cylinder radius="${shank_radius}" length="${shank_length}"/>
                 </geometry>
-                <material name="yellow">
+                <material name="">
                     <color rgba="1.0 1.0 0.0 1.0"/>
-                    <texture filename=""/>
                 </material>
             </visual>
-            <visual name="">
+            <visual name="feet_visual">
                 <origin xyz="0.0 0.0 ${-shank_length}" rpy="0.0 0.0 0.0"/>
                 <geometry>
                     <sphere radius="${feet_radius}"/>
                 </geometry>
-                <material name="purple">
+                <material name="">
                     <color rgba="1.0 0.0 1.0 1.0"/>
-                    <texture filename=""/>
                 </material>
             </visual>
-            <!-- <collision>
-                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+
+
+            <collision name="shank_${position}_link_collision">
+                <origin xyz="0.0 0.0 ${-shank_length/2}" rpy="0.0 0.0 0.0"/>
                 <geometry>
-                    <box size="0.0 0.0 0.0"/>
+                    <cylinder radius="${shank_radius}" length="${shank_length}"/>
+                </geometry>
+            </collision>
+            <collision name="feet_collision">
+                <origin xyz="0.0 0.0 ${-shank_length}" rpy="0.0 0.0 0.0"/>
+                <geometry>
+                    <sphere radius="${feet_radius}"/>
                 </geometry>
-            </collision> -->
+            </collision>
+           
         </link>
     </xacro:macro>
 </robot>
\ No newline at end of file
diff --git a/src/meldog_description/description/thigh/thigh.urdf.xacro b/src/meldog_description/description/thigh/thigh.urdf.xacro
index 22197a98..21e0b5e2 100644
--- a/src/meldog_description/description/thigh/thigh.urdf.xacro
+++ b/src/meldog_description/description/thigh/thigh.urdf.xacro
@@ -4,30 +4,33 @@
     <xacro:property name="thigh_length" value="0.5"/>
     <xacro:property name="thigh_radius" value="0.05"/>
 
+    <xacro:property name="thigh_mass" value="1.0"/>
 
     <xacro:macro name="thigh_link" params="position">
         <link name="thigh_${position}_link">
             <inertial>
-                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
-                <mass value="1.0"/>
-                <inertia ixx="1.0" ixy="1.0" ixz="1.0" iyy="1.0" iyz="1.0" izz="1.0"/>
+                <origin xyz="0.0 0.0 ${-thigh_length/2}" rpy="0.0 0.0 0.0"/>
+                <mass value="${thigh_mass}"/>
+                <inertia ixx="${1/12*thigh_mass*(3*thigh_radius*thigh_radius+thigh_length*thigh_length)}" ixy="0.0" ixz="0.0" iyy="${1/12*thigh_mass*(3*thigh_radius*thigh_radius+thigh_length*thigh_length)}" iyz="0.0" izz="${1/2*thigh_mass*thigh_radius*thigh_radius}"/>
             </inertial>
-            <visual name="">
+
+            <visual name="thigh_${position}_link_visual">
                 <origin xyz="0.0 0.0 ${-thigh_length/2}" rpy="0.0 0.0 0.0"/>
                 <geometry>
                     <cylinder radius="${thigh_radius}" length="${thigh_length}"/>
                 </geometry>
-                <material name="thigh_${position}_link">
+                <material name="">
                     <color rgba="0.0 1.0 0.0 1.0"/>
-                    <texture filename=""/>
                 </material>
             </visual>
-            <!-- <collision>
-                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+
+            <collision name="thigh_${position}_link_collision">
+                <origin xyz="0.0 0.0 ${-thigh_length/2}" rpy="0.0 0.0 0.0"/>
                 <geometry>
-                    <box size="0.0 0.0 0.0"/>
+                    <cylinder radius="${thigh_radius}" length="${thigh_length}"/>
                 </geometry>
-            </collision> -->
+            </collision>
+            
         </link>
     </xacro:macro>
 </robot>
diff --git a/src/meldog_description/description/trunk/trunk.urdf.xacro b/src/meldog_description/description/trunk/trunk.urdf.xacro
index 7174b266..34b2dfba 100644
--- a/src/meldog_description/description/trunk/trunk.urdf.xacro
+++ b/src/meldog_description/description/trunk/trunk.urdf.xacro
@@ -9,48 +9,60 @@
     <xacro:property name="trunk_actuators_width" value="0.2" />
     <xacro:property name="trunk_actuators_height" value="0.1" />
 
+    <xacro:property name="trunk_mass" value="8.0"/>
+
     <link name="trunk_link">
         <inertial>
             <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
-            <mass value="5.0"/> <!--testing-->
-            <inertia ixx="1.0" ixy="1.0" ixz="1.0" iyy="1.0" iyz="1.0" izz="1.0"/> <!--testing-->
+            <mass value="${trunk_mass}"/> 
+            <inertia ixx="${1/12*trunk_mass*(trunk_base_width*trunk_base_width + trunk_base_height*trunk_base_height)}" ixy="0.0" ixz="0.0" iyy="${1/12*trunk_mass*(trunk_base_length*trunk_base_length + trunk_base_height*trunk_base_height)}" iyz=".0" izz="${1/12*trunk_mass*(trunk_base_length*trunk_base_length + trunk_base_width*trunk_base_width)}"/> 
         </inertial>
 
-        <visual name="trunk_base">
+        <visual name="trunk_base_visual">
             <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
             <geometry>
                 <box size="${trunk_base_length} ${trunk_base_width} ${trunk_base_height}"/>
             </geometry>
-            <material name="trunk_base_color">
+            <material name="">
                 <color rgba="0.0 0.0 1.0 1.0"/>
-                <texture filename=""/>
             </material>
         </visual>
-        <visual name="trunk_actuators_left">
+        <visual name="trunk_actuators_left_visual">
             <origin xyz="0.0 ${trunk_base_width/2 + trunk_actuators_width/2} 0.0" rpy="0.0 0.0 0.0"/>
             <geometry>
                 <box size="${trunk_actuators_length} ${trunk_actuators_width} ${trunk_actuators_height}"/>
             </geometry>
             <material name="">
                 <color rgba="0.0 1.0 0.0 1.0"/>
-                <texture filename="trunk_actuators_left_color"/>
             </material>
         </visual>
-        <visual name="trunk_actuators_right">
+        <visual name="trunk_actuators_right_visual">
             <origin xyz="0.0 ${-trunk_base_width/2 - trunk_actuators_width/2} 0.0" rpy="0.0 0.0 0.0"/>
             <geometry>
                 <box size="${trunk_actuators_length} ${trunk_actuators_width} ${trunk_actuators_height}"/>
             </geometry>
             <material name="">
                 <color rgba="1.0 0.0 0.0 1.0"/>
-                <texture filename="trunk_actuators_right_color"/>
             </material>
         </visual>
 
-        <collision> <!--testing-->
+        
+         <collision name="trunk_base_collision">
             <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
             <geometry>
-                <box size="1.0 1.0 1.0"/>
+                <box size="${trunk_base_length} ${trunk_base_width} ${trunk_base_height}"/>
+            </geometry>
+        </collision>
+        <collision name="trunk_actuators_left_collision">
+            <origin xyz="0.0 ${trunk_base_width/2 + trunk_actuators_width/2} 0.0" rpy="0.0 0.0 0.0"/>
+            <geometry>
+                <box size="${trunk_actuators_length} ${trunk_actuators_width} ${trunk_actuators_height}"/>
+            </geometry>
+        </collision>
+        <collision name="trunk_actuators_right_collision">
+            <origin xyz="0.0 ${-trunk_base_width/2 - trunk_actuators_width/2} 0.0" rpy="0.0 0.0 0.0"/>
+            <geometry>
+                <box size="${trunk_actuators_length} ${trunk_actuators_width} ${trunk_actuators_height}"/>
             </geometry>
         </collision>
     </link>

From e689457da2f999aa13ee5c9254c9aca01d6d744c Mon Sep 17 00:00:00 2001
From: Bartek <bartlomiejk@vp.pl>
Date: Thu, 5 Sep 2024 18:43:24 +0200
Subject: [PATCH 09/23] Added xacro files for actuator (joints) and gazebo
 links and joints parameters

---
 .../description/actuator/actuator.urdf.xacro  | 16 +++++++
 .../gazebo/gazebo_macros.urdf.xacro           | 46 +++++++++++++++++++
 .../description/meldog_core.urdf.xacro        |  1 -
 3 files changed, 62 insertions(+), 1 deletion(-)
 create mode 100644 src/meldog_description/description/actuator/actuator.urdf.xacro
 create mode 100644 src/meldog_description/description/gazebo/gazebo_macros.urdf.xacro

diff --git a/src/meldog_description/description/actuator/actuator.urdf.xacro b/src/meldog_description/description/actuator/actuator.urdf.xacro
new file mode 100644
index 00000000..217c827b
--- /dev/null
+++ b/src/meldog_description/description/actuator/actuator.urdf.xacro
@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "meldog_actuator">
+
+
+    <xacro:property name="actuator_max_velocity" value="0.5"/>
+    <xacro:property name="actuator_max_torque" value="0.5"/>
+    <xacro:property name="actuator_damping" value="0.5"/>
+    <xacro:property name="actuator_friction" value="1.788"/>
+
+
+    <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_description/description/gazebo/gazebo_macros.urdf.xacro b/src/meldog_description/description/gazebo/gazebo_macros.urdf.xacro
new file mode 100644
index 00000000..60e8a276
--- /dev/null
+++ b/src/meldog_description/description/gazebo/gazebo_macros.urdf.xacro
@@ -0,0 +1,46 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "gazebo_macros">
+
+
+    <!-- Gazebo macro for links -->
+    <xacro:property name="kp_value" value="25000.0"/>
+    <xacro:property name="kd_value" value="0.05"/>
+    <xacro:property name="mu2_value" value="50"/>
+    <xacro:property name="mu2_value" value="25"/>
+    <xacro:property name="maxVel_value" value="0.1"/>
+    <xacro:property name="dampingFactor_value" value="0.004"/>
+    <xacro:property name="selfCollide_value" value="false"/>  
+
+    <xacro:macro name="gazebo_link" params="link_name color">
+        <gazebo reference="${link_name}">
+            <material>Gazebo/${color}</material>
+              <mu1 value="${mu2_value}"/>
+              <mu2 value="${mu2_value}"/>
+              <kp value="${kp_value}" />
+              <kd value="${kd_value}" />
+              <maxVel value="${maxVel_value}"/>
+              <dampingFactor value="${dampingFactor_value}"/>
+              <selfCollide>${selfCollide_value}</selfCollide>  
+        </gazebo>
+    </xacro:macro>
+
+    
+    <!-- Gazebo macro for joints -->
+    <xacro:property name="damper_value" value="true"/>
+    <xacro:property name="springStiffness_value" value="0.5"/>
+    <xacro:property name="springReference_value" value="0.061"/>
+    <xacro:property name="stopErp_value" value="0.2"/>
+    <xacro:property name="stopCfm_value" value="0.45"/> 
+
+    <xacro:macro name="gazebo_joint" params="joint_name">
+        <gazebo reference="${joint_name}">
+            <implicitSpringDamper>${Damper_value}</implicitSpringDamper>
+            <springStiffness>${springstiffness_value}</springStiffness>
+            <springReference>${springReference_value}</springReference>
+            <stopErp>${stopErp_value}</stopErp>
+            <stopCfm>${stopCfm_value}</stopCfm>
+        </gazebo> 
+    </xacro:macro>  
+    
+
+</robot>
\ No newline at end of file
diff --git a/src/meldog_description/description/meldog_core.urdf.xacro b/src/meldog_description/description/meldog_core.urdf.xacro
index 7f144376..1ff1ff58 100644
--- a/src/meldog_description/description/meldog_core.urdf.xacro
+++ b/src/meldog_description/description/meldog_core.urdf.xacro
@@ -44,7 +44,6 @@
 
     <!-- JOINTS: -->    
 
-
     <!-- Base to Trunk joint -->
     <joint name="BT_joint" type="fixed">
         <parent link="base_link"/>

From b0d2ba7a959fc328f4d3585a7495ebeb98bdcee0 Mon Sep 17 00:00:00 2001
From: Bartek <bartlomiejk@vp.pl>
Date: Fri, 6 Sep 2024 13:54:21 +0200
Subject: [PATCH 10/23] Added feet links and joints, added realistic joint
 limits for actuators

---
 .../actuator_macros.urdf.xacro}               |  8 ++--
 .../gazebo_macros.urdf.xacro                  |  3 +-
 .../description/feet/feet.urdf.xacro          | 34 ++++++++++++++++
 .../description/meldog_core.urdf.xacro        | 39 +++++++++++--------
 .../description/meldog_gazebo.urdf.xacro      |  7 ++++
 .../description/shank/shank.urdf.xacro        | 17 --------
 6 files changed, 68 insertions(+), 40 deletions(-)
 rename src/meldog_description/description/{actuator/actuator.urdf.xacro => common/actuator_macros.urdf.xacro} (62%)
 rename src/meldog_description/description/{gazebo => common}/gazebo_macros.urdf.xacro (93%)
 create mode 100644 src/meldog_description/description/feet/feet.urdf.xacro
 create mode 100644 src/meldog_description/description/meldog_gazebo.urdf.xacro

diff --git a/src/meldog_description/description/actuator/actuator.urdf.xacro b/src/meldog_description/description/common/actuator_macros.urdf.xacro
similarity index 62%
rename from src/meldog_description/description/actuator/actuator.urdf.xacro
rename to src/meldog_description/description/common/actuator_macros.urdf.xacro
index 217c827b..0000542b 100644
--- a/src/meldog_description/description/actuator/actuator.urdf.xacro
+++ b/src/meldog_description/description/common/actuator_macros.urdf.xacro
@@ -2,10 +2,10 @@
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "meldog_actuator">
 
 
-    <xacro:property name="actuator_max_velocity" value="0.5"/>
-    <xacro:property name="actuator_max_torque" value="0.5"/>
-    <xacro:property name="actuator_damping" value="0.5"/>
-    <xacro:property name="actuator_friction" value="1.788"/>
+    <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">
diff --git a/src/meldog_description/description/gazebo/gazebo_macros.urdf.xacro b/src/meldog_description/description/common/gazebo_macros.urdf.xacro
similarity index 93%
rename from src/meldog_description/description/gazebo/gazebo_macros.urdf.xacro
rename to src/meldog_description/description/common/gazebo_macros.urdf.xacro
index 60e8a276..21d95a3f 100644
--- a/src/meldog_description/description/gazebo/gazebo_macros.urdf.xacro
+++ b/src/meldog_description/description/common/gazebo_macros.urdf.xacro
@@ -9,9 +9,8 @@
     <xacro:property name="mu2_value" value="25"/>
     <xacro:property name="maxVel_value" value="0.1"/>
     <xacro:property name="dampingFactor_value" value="0.004"/>
-    <xacro:property name="selfCollide_value" value="false"/>  
 
-    <xacro:macro name="gazebo_link" params="link_name color">
+    <xacro:macro name="gazebo_link" params="link_name color selfCollide_value">
         <gazebo reference="${link_name}">
             <material>Gazebo/${color}</material>
               <mu1 value="${mu2_value}"/>
diff --git a/src/meldog_description/description/feet/feet.urdf.xacro b/src/meldog_description/description/feet/feet.urdf.xacro
new file mode 100644
index 00000000..a7512e53
--- /dev/null
+++ b/src/meldog_description/description/feet/feet.urdf.xacro
@@ -0,0 +1,34 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name ="meldog_feet">
+
+     <xacro:property name="feet_radius" value="0.06"/>
+     <xacro:property name="feet_mass" value="0.1"/>
+
+
+    <xacro:macro name="feet_link" params="position">
+        <link name="feet_${position}_link">
+             <inertial>
+                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+                <mass value="${feet_mass}"/>
+                <inertia ixx="${2/5*feet_mass*feet_radius*feet_radius}" ixy="0.0" ixz="0.0" iyy="${2/5*feet_mass*feet_radius*feet_radius}" iyz="0.0" izz="${2/5*feet_mass*feet_radius*feet_radius}"/>
+            </inertial>
+
+            <visual name="feet_visual">
+                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+                <geometry>
+                    <sphere radius="${feet_radius}"/>
+                </geometry>
+                <material name="">
+                    <color rgba="1.0 0.0 1.0 1.0"/>
+                </material>
+            </visual>
+
+            <collision name="feet_collision">
+                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
+                <geometry>
+                    <sphere radius="${feet_radius}"/>
+                </geometry>
+            </collision>
+        </link>
+    </xacro:macro>
+</robot>
\ No newline at end of file
diff --git a/src/meldog_description/description/meldog_core.urdf.xacro b/src/meldog_description/description/meldog_core.urdf.xacro
index 1ff1ff58..fee23c71 100644
--- a/src/meldog_description/description/meldog_core.urdf.xacro
+++ b/src/meldog_description/description/meldog_core.urdf.xacro
@@ -1,9 +1,12 @@
 <?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "meldog_core">
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="meldog_core">
 
     <!-- PI -->
     <xacro:property name="PI" value="3.1415926535897"/>
 
+    <!-- Actuator macros -->
+    <xacro:include filename="common/actuator_macros.urdf.xacro"/>
+
     <!-- LINKS: -->
 
 
@@ -37,10 +40,11 @@
 
 
     <!-- Feet links (frames in the end of shank) -->
-    <link name="feet_front_left_link"></link>
-    <link name="feet_front_right_link"></link>
-    <link name="feet_back_left_link"></link>
-    <link name="feet_back_right_link"></link>
+    <xacro:include filename="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: -->    
 
@@ -59,7 +63,7 @@
         <parent link="trunk_link"/>
         <child link="hip_front_left_link"/>
         <axis xyz="1.0 0.0 0.0"/>
-        <limit lower="-1.0" upper="1" effort="10.0" velocity="100.0"/>
+        <xacro:meldog_joint_limits lower ="-${PI/2}" upper = "${PI/2}"/>
     </joint>
 
     <!-- Trunk to Hip front right joint -->
@@ -68,7 +72,7 @@
         <parent link="trunk_link"/>
         <child link="hip_front_right_link"/>
         <axis xyz="-1.0 0.0 0.0"/>
-        <limit lower="-1.0" upper="1" effort="10.0" velocity="100.0"/>
+        <xacro:meldog_joint_limits lower ="-${PI/2}" upper = "${PI/2}"/>
     </joint>
 
     <!-- Trunk to Hip back left joint -->
@@ -77,7 +81,7 @@
         <parent link="trunk_link"/>
         <child link="hip_back_left_link"/>
         <axis xyz="-1.0 0.0 0.0"/>
-        <limit lower="-1.0" upper="1" effort="10.0" velocity="100.0"/>
+        <xacro:meldog_joint_limits lower ="-${PI/2}" upper = "${PI/2}"/>
     </joint>
 
     <!-- Trunk to Hip back right joint -->
@@ -86,7 +90,7 @@
         <parent link="trunk_link"/>
         <child link="hip_back_right_link"/>
         <axis xyz="1.0 0.0 0.0"/>
-        <limit lower="-1.0" upper="1" effort="10.0" velocity="100.0"/>
+        <xacro:meldog_joint_limits lower ="-${PI/2}" upper = "${PI/2}"/>
     </joint>
 
 
@@ -96,7 +100,7 @@
         <parent link="hip_front_left_link"/>
         <child link="thigh_front_left_link"/>
         <axis xyz="0.0 -1.0 0.0"/>
-        <limit lower="-1.0" upper="1.0" effort="10.0" velocity="100.0"/>
+        <xacro:meldog_joint_limits lower ="-${PI}" upper = "${PI}"/>
     </joint>
 
     <!-- Hip front right to Thigh front right -->
@@ -105,7 +109,7 @@
         <parent link="hip_front_right_link"/>
         <child link="thigh_front_right_link"/>
         <axis xyz="0.0 -1.0 0.0"/>
-        <limit lower="-1.0" upper="1.0" effort="10.0" velocity="100.0"/>
+        <xacro:meldog_joint_limits lower ="-${PI}" upper = "${PI}"/>
     </joint>
 
     <!-- Hip back left to Thigh back left -->
@@ -114,7 +118,7 @@
         <parent link="hip_back_left_link"/>
         <child link="thigh_back_left_link"/>
         <axis xyz="0.0 1.0 0.0"/>
-        <limit lower="-1.0" upper="1.0" effort="10.0" velocity="100.0"/>
+        <xacro:meldog_joint_limits lower ="-${PI}" upper = "${PI}"/>
     </joint>
 
     <!-- Hip back right to Thigh back right -->
@@ -123,7 +127,7 @@
         <parent link="hip_back_right_link"/>
         <child link="thigh_back_right_link"/>
         <axis xyz="0.0 1.0 0.0"/>
-        <limit lower="-1.0" upper="1.0" effort="10.0" velocity="100.0"/>
+        <xacro:meldog_joint_limits lower ="-${PI}" upper = "${PI}"/>
     </joint>
 
 
@@ -133,7 +137,7 @@
         <parent link="thigh_front_left_link"/>
         <child link="shank_front_left_link"/>
         <axis xyz="0.0 -1.0 0.0"/>
-        <limit lower="-1.0" upper="1.0" effort="10.0" velocity="100.0"/>
+        <xacro:meldog_joint_limits lower ="-${PI/2}" upper = "${PI/2}"/>
     </joint>
 
     <!-- Thigh front right to Shank front right -->
@@ -142,7 +146,7 @@
         <parent link="thigh_front_right_link"/>
         <child link="shank_front_right_link"/>
         <axis xyz="0.0 -1.0 0.0"/>
-        <limit lower="-1.0" upper="1.0" effort="10.0" velocity="100.0"/>
+        <xacro:meldog_joint_limits lower ="-${PI/2}" upper = "${PI/2}"/>
     </joint>
 
     <!-- Thigh back left to Shank back left -->
@@ -151,7 +155,7 @@
         <parent link="thigh_back_left_link"/>
         <child link="shank_back_left_link"/>
         <axis xyz="0.0 1.0 0.0"/>
-        <limit lower="-1.0" upper="1.0" effort="10.0" velocity="100.0"/>
+        <xacro:meldog_joint_limits lower ="-${PI/2}" upper = "${PI/2}"/>
     </joint>
 
     <!-- Thigh back right to Shank back right -->
@@ -160,7 +164,7 @@
         <parent link="thigh_back_right_link"/>
         <child link="shank_back_right_link"/>
         <axis xyz="0.0 1.0 0.0"/>
-        <limit lower="-1.0" upper="1.0" effort="10.0" velocity="100.0"/>
+        <xacro:meldog_joint_limits lower ="-${PI/2}" upper = "${PI/2}"/>
     </joint>
     
 
@@ -192,4 +196,5 @@
         <child link="feet_back_right_link"/>
     </joint>
 
+
 </robot>
\ No newline at end of file
diff --git a/src/meldog_description/description/meldog_gazebo.urdf.xacro b/src/meldog_description/description/meldog_gazebo.urdf.xacro
new file mode 100644
index 00000000..7d6b4c78
--- /dev/null
+++ b/src/meldog_description/description/meldog_gazebo.urdf.xacro
@@ -0,0 +1,7 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "meldog_gazebo">
+
+    <!-- Gazebo macros -->
+    <xacro:include filename="common/gazebo_macros.urdf.xacro"/>
+
+</robot>
\ No newline at end of file
diff --git a/src/meldog_description/description/shank/shank.urdf.xacro b/src/meldog_description/description/shank/shank.urdf.xacro
index 11155115..370e5eeb 100644
--- a/src/meldog_description/description/shank/shank.urdf.xacro
+++ b/src/meldog_description/description/shank/shank.urdf.xacro
@@ -3,7 +3,6 @@
 
     <xacro:property name="shank_length" value="0.5"/>
     <xacro:property name="shank_radius" value="0.05"/>
-    <xacro:property name="feet_radius" value ="0.08"/>
 
     <xacro:property name="shank_mass" value = "1.0"/>
 
@@ -24,16 +23,6 @@
                     <color rgba="1.0 1.0 0.0 1.0"/>
                 </material>
             </visual>
-            <visual name="feet_visual">
-                <origin xyz="0.0 0.0 ${-shank_length}" rpy="0.0 0.0 0.0"/>
-                <geometry>
-                    <sphere radius="${feet_radius}"/>
-                </geometry>
-                <material name="">
-                    <color rgba="1.0 0.0 1.0 1.0"/>
-                </material>
-            </visual>
-
 
             <collision name="shank_${position}_link_collision">
                 <origin xyz="0.0 0.0 ${-shank_length/2}" rpy="0.0 0.0 0.0"/>
@@ -41,12 +30,6 @@
                     <cylinder radius="${shank_radius}" length="${shank_length}"/>
                 </geometry>
             </collision>
-            <collision name="feet_collision">
-                <origin xyz="0.0 0.0 ${-shank_length}" rpy="0.0 0.0 0.0"/>
-                <geometry>
-                    <sphere radius="${feet_radius}"/>
-                </geometry>
-            </collision>
            
         </link>
     </xacro:macro>

From 1029fc8c88b13d76f05aac6449c1b549e7f09516 Mon Sep 17 00:00:00 2001
From: Bartek <bartlomiejk@vp.pl>
Date: Fri, 6 Sep 2024 14:32:25 +0200
Subject: [PATCH 11/23] Added transmission macros

---
 .../common/gazebo_macros.urdf.xacro           |  4 +-
 .../common/transmission_macros.urdf.xacro     | 51 +++++++++++++++++
 .../description/meldog_core.urdf.xacro        |  3 -
 .../description/meldog_gazebo.urdf.xacro      | 56 +++++++++++++++++++
 4 files changed, 109 insertions(+), 5 deletions(-)
 create mode 100644 src/meldog_description/description/common/transmission_macros.urdf.xacro

diff --git a/src/meldog_description/description/common/gazebo_macros.urdf.xacro b/src/meldog_description/description/common/gazebo_macros.urdf.xacro
index 21d95a3f..485b476a 100644
--- a/src/meldog_description/description/common/gazebo_macros.urdf.xacro
+++ b/src/meldog_description/description/common/gazebo_macros.urdf.xacro
@@ -11,7 +11,7 @@
     <xacro:property name="dampingFactor_value" value="0.004"/>
 
     <xacro:macro name="gazebo_link" params="link_name color selfCollide_value">
-        <gazebo reference="${link_name}">
+        <gazebo reference="${link_name}_link">
             <material>Gazebo/${color}</material>
               <mu1 value="${mu2_value}"/>
               <mu2 value="${mu2_value}"/>
@@ -32,7 +32,7 @@
     <xacro:property name="stopCfm_value" value="0.45"/> 
 
     <xacro:macro name="gazebo_joint" params="joint_name">
-        <gazebo reference="${joint_name}">
+        <gazebo reference="${joint_name_joint}">
             <implicitSpringDamper>${Damper_value}</implicitSpringDamper>
             <springStiffness>${springstiffness_value}</springStiffness>
             <springReference>${springReference_value}</springReference>
diff --git a/src/meldog_description/description/common/transmission_macros.urdf.xacro b/src/meldog_description/description/common/transmission_macros.urdf.xacro
new file mode 100644
index 00000000..6e9a2104
--- /dev/null
+++ b/src/meldog_description/description/common/transmission_macros.urdf.xacro
@@ -0,0 +1,51 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name ="meldog_transmissions">
+
+    <!-- SimpleTransmission macro -->
+    <xacro:macro name="SimpleTransmission" params="joint_name reduction offset"/>
+        <transmission name="SimpleTransmission_${joint_name}">
+                <plugin>transmission_interface/SimpleTransmission</plugin>
+                <actuator name="${joint_name}_actuator" role="actuator1"/>
+                <joint name="${joint_name}_joint" role="joint1">
+                    <mechanical_reduction>${reduction}</mechanical_reduction>
+                    <offset>${offset}</offset>
+                </joint>
+        </transmission>
+    </xacro:macro>
+
+    <!-- FourBarLinkageTransmission macro -->
+    <xacro:macro name="FourBarLinkageTransmission" params="first_joint_name second__joint_name reduction first_offset second_offset"/>
+        <transmission name="FourBarLinkageTransmission_${first_joint_name}_${second__joint_name}">
+            <plugin>transmission_interface/FourBarLinkageTransmission</plugin>
+            <actuator name="${first_joint_name}_actuator" role="actuator1">
+                <mechanical_reduction>${reduction}</mechanical_reduction>
+            </actuator>
+            <actuator name="${second_joint_name}_actuator" role="actuator2">
+                <mechanical_reduction>${reduction}</mechanical_reduction>
+            </actuator>
+            <joint name="${first_joint_name}_joint" role="joint1">
+                <mechanical_reduction>1.0</mechanical_reduction>
+                <offset>${first_offset}</offset>
+            </joint>
+            <joint name="${second_joint_name}_joint" role="joint2">
+                <mechanical_reduction>1.0</mechanical_reduction>
+                <offset>${second_offset}</offset>
+            </joint>
+        </transmission>
+    </xacro:macro>
+
+    <!-- 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"/>
+
+    <!-- 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"/>
+
+</robot>
\ No newline at end of file
diff --git a/src/meldog_description/description/meldog_core.urdf.xacro b/src/meldog_description/description/meldog_core.urdf.xacro
index fee23c71..196852f3 100644
--- a/src/meldog_description/description/meldog_core.urdf.xacro
+++ b/src/meldog_description/description/meldog_core.urdf.xacro
@@ -8,8 +8,6 @@
     <xacro:include filename="common/actuator_macros.urdf.xacro"/>
 
     <!-- LINKS: -->
-
-
     <!-- Base link spawns in x= 0.0 y = 0.0 z = 0.0 -->
     <link name="base_link"></link>
 
@@ -47,7 +45,6 @@
     <xacro:feet_link position="back_right"/>
 
     <!-- JOINTS: -->    
-
     <!-- Base to Trunk joint -->
     <joint name="BT_joint" type="fixed">
         <parent link="base_link"/>
diff --git a/src/meldog_description/description/meldog_gazebo.urdf.xacro b/src/meldog_description/description/meldog_gazebo.urdf.xacro
index 7d6b4c78..7912a27d 100644
--- a/src/meldog_description/description/meldog_gazebo.urdf.xacro
+++ b/src/meldog_description/description/meldog_gazebo.urdf.xacro
@@ -4,4 +4,60 @@
     <!-- Gazebo macros -->
     <xacro:include filename="common/gazebo_macros.urdf.xacro"/>
 
+    <!-- LINKS: -->
+    <!-- Trunk link -->
+    <xacro:gazebo_link link_name="trunk" color="Blue" selfCollide_value ="false"/>
+
+    <!-- Hip links -->
+    <xacro:gazebo_link link_name="hip_front_left" color="Red" selfCollide_value ="false"/>
+    <xacro:gazebo_link link_name="hip_front_right" color="Red" selfCollide_value ="false"/>
+    <xacro:gazebo_link link_name="hip_back_left" color="Red" selfCollide_value ="false"/>
+    <xacro:gazebo_link link_name="hip_back_right" color="Red" selfCollide_value ="false"/>
+
+    <!-- Thigh links -->
+    <xacro:gazebo_link link_name="thigh_front_left" color="Green" selfCollide_value ="false"/>
+    <xacro:gazebo_link link_name="thigh_front_right" color="Green" selfCollide_value ="false"/>
+    <xacro:gazebo_link link_name="thigh_back_left" color="Green" selfCollide_value ="false"/>
+    <xacro:gazebo_link link_name="thigh_back_right" color="Green" selfCollide_value ="false"/>
+
+    <!-- Shank links -->
+    <xacro:gazebo_link link_name="shank_front_left_" color="Yellow" selfCollide_value ="false"/>
+    <xacro:gazebo_link link_name="shank_front_right" color="Yellow" selfCollide_value ="false"/>
+    <xacro:gazebo_link link_name="shank_back_left" color="Yellow" selfCollide_value ="false"/>
+    <xacro:gazebo_link link_name="shank_back_right" color="Yellow" selfCollide_value ="false"/>
+
+    <!-- Feet links -->
+    <xacro:gazebo_link link_name="feet_front_left" color="Pink" selfCollide_value ="false"/>
+    <xacro:gazebo_link link_name="feet_front_right" color="Pink" selfCollide_value ="false"/>
+    <xacro:gazebo_link link_name="feet_back_left" color="Pink" selfCollide_value ="false"/>
+    <xacro:gazebo_link link_name="feet_back_right" color="Pink" selfCollide_value ="false"/>
+
+
+    <!-- JOINTS (only revolute): -->
+    <!-- Trunk to Hips joints-->
+    <xacro:gazebo_joint joint_name="FLH"/>
+    <xacro:gazebo_joint joint_name="FRH"/>
+    <xacro:gazebo_joint joint_name="BLH"/>
+    <xacro:gazebo_joint joint_name="BRH"/>
+
+    <!-- Hips to Thighs joints -->
+    <xacro:gazebo_joint joint_name="FLT"/>
+    <xacro:gazebo_joint joint_name="FRT"/>
+    <xacro:gazebo_joint joint_name="BLT"/>
+    <xacro:gazebo_joint joint_name="BRT"/>
+
+    <!-- Thighs to Shanks joints -->
+    <xacro:gazebo_joint joint_name="FLS"/>
+    <xacro:gazebo_joint joint_name="FRS"/>
+    <xacro:gazebo_joint joint_name="BLS"/>
+    <xacro:gazebo_joint joint_name="BRS"/>
+
+
+    <!-- ROS2 Control -->
+    <ros2_control name="meldog_gazebo" type="system">
+        <hardware>  
+            <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+        </hardware>
+        <xacro:include filename="common/transmission_macros.urdf.xacro"/>
+
 </robot>
\ No newline at end of file

From b68564688cffdcadef88738765d0e90427974e91 Mon Sep 17 00:00:00 2001
From: Bartek <bartlomiejk@vp.pl>
Date: Fri, 6 Sep 2024 14:41:56 +0200
Subject: [PATCH 12/23] Added ros2_control_gazebo urdf and changed locations of
 other xacro files

---
 .../{common => actuator}/actuator_macros.urdf.xacro  |  0
 .../{common => gazebo}/gazebo_macros.urdf.xacro      |  0
 .../description/meldog_core.urdf.xacro               | 12 ++++++------
 .../description/meldog_gazebo.urdf.xacro             | 10 +---------
 .../{ => meldog_links}/feet/feet.urdf.xacro          |  0
 .../{ => meldog_links}/hip/hip.urdf.xacro            |  0
 .../{ => meldog_links}/shank/shank.urdf.xacro        |  0
 .../{ => meldog_links}/thigh/thigh.urdf.xacro        |  0
 .../{ => meldog_links}/trunk/trunk.urdf.xacro        |  0
 .../ros2_control/ros2_control_gazebo.urdf.xacro      | 12 ++++++++++++
 .../transmission_macros.urdf.xacro                   |  0
 11 files changed, 19 insertions(+), 15 deletions(-)
 rename src/meldog_description/description/{common => actuator}/actuator_macros.urdf.xacro (100%)
 rename src/meldog_description/description/{common => gazebo}/gazebo_macros.urdf.xacro (100%)
 rename src/meldog_description/description/{ => meldog_links}/feet/feet.urdf.xacro (100%)
 rename src/meldog_description/description/{ => meldog_links}/hip/hip.urdf.xacro (100%)
 rename src/meldog_description/description/{ => meldog_links}/shank/shank.urdf.xacro (100%)
 rename src/meldog_description/description/{ => meldog_links}/thigh/thigh.urdf.xacro (100%)
 rename src/meldog_description/description/{ => meldog_links}/trunk/trunk.urdf.xacro (100%)
 create mode 100644 src/meldog_description/description/ros2_control/ros2_control_gazebo.urdf.xacro
 rename src/meldog_description/description/{common => ros2_control}/transmission_macros.urdf.xacro (100%)

diff --git a/src/meldog_description/description/common/actuator_macros.urdf.xacro b/src/meldog_description/description/actuator/actuator_macros.urdf.xacro
similarity index 100%
rename from src/meldog_description/description/common/actuator_macros.urdf.xacro
rename to src/meldog_description/description/actuator/actuator_macros.urdf.xacro
diff --git a/src/meldog_description/description/common/gazebo_macros.urdf.xacro b/src/meldog_description/description/gazebo/gazebo_macros.urdf.xacro
similarity index 100%
rename from src/meldog_description/description/common/gazebo_macros.urdf.xacro
rename to src/meldog_description/description/gazebo/gazebo_macros.urdf.xacro
diff --git a/src/meldog_description/description/meldog_core.urdf.xacro b/src/meldog_description/description/meldog_core.urdf.xacro
index 196852f3..c343001d 100644
--- a/src/meldog_description/description/meldog_core.urdf.xacro
+++ b/src/meldog_description/description/meldog_core.urdf.xacro
@@ -5,24 +5,24 @@
     <xacro:property name="PI" value="3.1415926535897"/>
 
     <!-- Actuator macros -->
-    <xacro:include filename="common/actuator_macros.urdf.xacro"/>
+    <xacro:include filename="actuator/actuator_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="trunk/trunk.urdf.xacro"/>
+    <xacro:include filename="meldog_links/trunk/trunk.urdf.xacro"/>
 
     <!-- Hip links -->
-    <xacro:include filename="hip/hip.urdf.xacro"/>
+    <xacro:include filename="meldog_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="thigh/thigh.urdf.xacro"/>
+    <xacro:include filename="meldog_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 +30,7 @@
 
 
     <!-- Shank links -->
-    <xacro:include filename="shank/shank.urdf.xacro"/>
+    <xacro:include filename="meldog_links/shank/shank.urdf.xacro"/>
     <xacro:shank_link position="front_left"/>
     <xacro:shank_link position="front_right"/>
     <xacro:shank_link position="back_left"/>
@@ -38,7 +38,7 @@
 
 
     <!-- Feet links (frames in the end of shank) -->
-    <xacro:include filename="feet/feet.urdf.xacro"/>
+    <xacro:include filename="meldog_links/feet/feet.urdf.xacro"/>
     <xacro:feet_link position="front_left"/>
     <xacro:feet_link position="front_right"/>
     <xacro:feet_link position="back_left"/>
diff --git a/src/meldog_description/description/meldog_gazebo.urdf.xacro b/src/meldog_description/description/meldog_gazebo.urdf.xacro
index 7912a27d..1b108f24 100644
--- a/src/meldog_description/description/meldog_gazebo.urdf.xacro
+++ b/src/meldog_description/description/meldog_gazebo.urdf.xacro
@@ -2,7 +2,7 @@
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "meldog_gazebo">
 
     <!-- Gazebo macros -->
-    <xacro:include filename="common/gazebo_macros.urdf.xacro"/>
+    <xacro:include filename="gazebo/gazebo_macros.urdf.xacro"/>
 
     <!-- LINKS: -->
     <!-- Trunk link -->
@@ -52,12 +52,4 @@
     <xacro:gazebo_joint joint_name="BLS"/>
     <xacro:gazebo_joint joint_name="BRS"/>
 
-
-    <!-- ROS2 Control -->
-    <ros2_control name="meldog_gazebo" type="system">
-        <hardware>  
-            <plugin>gazebo_ros2_control/GazeboSystem</plugin>
-        </hardware>
-        <xacro:include filename="common/transmission_macros.urdf.xacro"/>
-
 </robot>
\ No newline at end of file
diff --git a/src/meldog_description/description/feet/feet.urdf.xacro b/src/meldog_description/description/meldog_links/feet/feet.urdf.xacro
similarity index 100%
rename from src/meldog_description/description/feet/feet.urdf.xacro
rename to src/meldog_description/description/meldog_links/feet/feet.urdf.xacro
diff --git a/src/meldog_description/description/hip/hip.urdf.xacro b/src/meldog_description/description/meldog_links/hip/hip.urdf.xacro
similarity index 100%
rename from src/meldog_description/description/hip/hip.urdf.xacro
rename to src/meldog_description/description/meldog_links/hip/hip.urdf.xacro
diff --git a/src/meldog_description/description/shank/shank.urdf.xacro b/src/meldog_description/description/meldog_links/shank/shank.urdf.xacro
similarity index 100%
rename from src/meldog_description/description/shank/shank.urdf.xacro
rename to src/meldog_description/description/meldog_links/shank/shank.urdf.xacro
diff --git a/src/meldog_description/description/thigh/thigh.urdf.xacro b/src/meldog_description/description/meldog_links/thigh/thigh.urdf.xacro
similarity index 100%
rename from src/meldog_description/description/thigh/thigh.urdf.xacro
rename to src/meldog_description/description/meldog_links/thigh/thigh.urdf.xacro
diff --git a/src/meldog_description/description/trunk/trunk.urdf.xacro b/src/meldog_description/description/meldog_links/trunk/trunk.urdf.xacro
similarity index 100%
rename from src/meldog_description/description/trunk/trunk.urdf.xacro
rename to src/meldog_description/description/meldog_links/trunk/trunk.urdf.xacro
diff --git a/src/meldog_description/description/ros2_control/ros2_control_gazebo.urdf.xacro b/src/meldog_description/description/ros2_control/ros2_control_gazebo.urdf.xacro
new file mode 100644
index 00000000..92e82702
--- /dev/null
+++ b/src/meldog_description/description/ros2_control/ros2_control_gazebo.urdf.xacro
@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "meldog_ros2_control_gazebo">
+
+<!-- ROS2 Control Gazebo -->
+    <ros2_control name="meldog_gazebo" type="system">
+        <hardware>  
+            <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+        </hardware>
+        <xacro:include filename="ros2_control/transmission_macros.urdf.xacro"/>
+        <!-- TUTAJ DODAJ JOINTY -->
+    </ros2_control>
+</robot>
\ No newline at end of file
diff --git a/src/meldog_description/description/common/transmission_macros.urdf.xacro b/src/meldog_description/description/ros2_control/transmission_macros.urdf.xacro
similarity index 100%
rename from src/meldog_description/description/common/transmission_macros.urdf.xacro
rename to src/meldog_description/description/ros2_control/transmission_macros.urdf.xacro

From bc8a7d0b802367e00afa587c7f4fd9a27c600463 Mon Sep 17 00:00:00 2001
From: Bartek <bartlomiejk@vp.pl>
Date: Sat, 7 Sep 2024 20:51:07 +0200
Subject: [PATCH 13/23] Succesfully run meldog urdf in gazebo classic

---
 .../gazebo/gazebo_macros.urdf.xacro           | 40 ++++-----
 .../description/meldog_gazebo.urdf.xacro      | 37 +++++----
 .../meldog_links/shank/shank.urdf.xacro       |  2 +-
 .../meldog_links/thigh/thigh.urdf.xacro       |  2 +-
 .../launch/gazebo.launch.py                   | 33 ++++++++
 .../launch/meldog_gazebo.launch.py            | 83 +++++++++++++++++++
 ...ualize.launch.py => meldog_rviz.launch.py} |  0
 7 files changed, 158 insertions(+), 39 deletions(-)
 create mode 100644 src/meldog_description/launch/gazebo.launch.py
 create mode 100644 src/meldog_description/launch/meldog_gazebo.launch.py
 rename src/meldog_description/launch/{robot_visualize.launch.py => meldog_rviz.launch.py} (100%)

diff --git a/src/meldog_description/description/gazebo/gazebo_macros.urdf.xacro b/src/meldog_description/description/gazebo/gazebo_macros.urdf.xacro
index 485b476a..d3e11bf6 100644
--- a/src/meldog_description/description/gazebo/gazebo_macros.urdf.xacro
+++ b/src/meldog_description/description/gazebo/gazebo_macros.urdf.xacro
@@ -4,37 +4,37 @@
 
     <!-- Gazebo macro for links -->
     <xacro:property name="kp_value" value="25000.0"/>
-    <xacro:property name="kd_value" value="0.05"/>
-    <xacro:property name="mu2_value" value="50"/>
-    <xacro:property name="mu2_value" value="25"/>
-    <xacro:property name="maxVel_value" value="0.1"/>
-    <xacro:property name="dampingFactor_value" value="0.004"/>
+    <xacro:property name="kd_value" value="0.15"/>
+    <xacro:property name="mu2_value" value="0.7"/>
+    <xacro:property name="mu2_value" value="0.7"/>
+    <xacro:property name="maxVel_value" value="0.25"/>
+    <xacro:property name="dampingFactor_value" value="0.00"/>
 
     <xacro:macro name="gazebo_link" params="link_name color selfCollide_value">
         <gazebo reference="${link_name}_link">
             <material>Gazebo/${color}</material>
-              <mu1 value="${mu2_value}"/>
-              <mu2 value="${mu2_value}"/>
-              <kp value="${kp_value}" />
-              <kd value="${kd_value}" />
-              <maxVel value="${maxVel_value}"/>
-              <dampingFactor value="${dampingFactor_value}"/>
-              <selfCollide>${selfCollide_value}</selfCollide>  
+            <mu1 value="${mu2_value}"/>
+            <mu2 value="${mu2_value}"/>
+            <kp value="${kp_value}" />
+            <kd value="${kd_value}" />
+            <maxVel value="${maxVel_value}"/>
+            <dampingFactor value="${dampingFactor_value}"/>
+            <selfCollide value="${selfCollide_value}"/>
         </gazebo>
     </xacro:macro>
 
     
     <!-- Gazebo macro for joints -->
-    <xacro:property name="damper_value" value="true"/>
-    <xacro:property name="springStiffness_value" value="0.5"/>
-    <xacro:property name="springReference_value" value="0.061"/>
-    <xacro:property name="stopErp_value" value="0.2"/>
-    <xacro:property name="stopCfm_value" value="0.45"/> 
+    <xacro:property name="implicitSpringDamper_value" value="true"/>
+    <xacro:property name="springStiffness_value" value="0.0"/>
+    <xacro:property name="springReference_value" value="0.0"/>
+    <xacro:property name="stopErp_value" value="0.0"/>
+    <xacro:property name="stopCfm_value" value="0.0"/> 
 
     <xacro:macro name="gazebo_joint" params="joint_name">
-        <gazebo reference="${joint_name_joint}">
-            <implicitSpringDamper>${Damper_value}</implicitSpringDamper>
-            <springStiffness>${springstiffness_value}</springStiffness>
+        <gazebo reference="${joint_name}_joint">
+            <implicitSpringDamper>${implicitSpringDamper_value}</implicitSpringDamper>
+            <springStiffness>${springStiffness_value}</springStiffness>
             <springReference>${springReference_value}</springReference>
             <stopErp>${stopErp_value}</stopErp>
             <stopCfm>${stopCfm_value}</stopCfm>
diff --git a/src/meldog_description/description/meldog_gazebo.urdf.xacro b/src/meldog_description/description/meldog_gazebo.urdf.xacro
index 1b108f24..6c9d5e6a 100644
--- a/src/meldog_description/description/meldog_gazebo.urdf.xacro
+++ b/src/meldog_description/description/meldog_gazebo.urdf.xacro
@@ -1,36 +1,39 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "meldog_gazebo">
 
+    <!-- Meldog Core file -->
+    <xacro:include filename="meldog_core.urdf.xacro"/>
+
     <!-- Gazebo macros -->
     <xacro:include filename="gazebo/gazebo_macros.urdf.xacro"/>
 
     <!-- LINKS: -->
     <!-- Trunk link -->
-    <xacro:gazebo_link link_name="trunk" color="Blue" selfCollide_value ="false"/>
+    <xacro:gazebo_link link_name="trunk" color="Blue" selfCollide_value ="true"/>
 
     <!-- Hip links -->
-    <xacro:gazebo_link link_name="hip_front_left" color="Red" selfCollide_value ="false"/>
-    <xacro:gazebo_link link_name="hip_front_right" color="Red" selfCollide_value ="false"/>
-    <xacro:gazebo_link link_name="hip_back_left" color="Red" selfCollide_value ="false"/>
-    <xacro:gazebo_link link_name="hip_back_right" color="Red" selfCollide_value ="false"/>
+    <xacro:gazebo_link link_name="hip_front_left" color="Red" selfCollide_value ="true"/>
+    <xacro:gazebo_link link_name="hip_front_right" color="Red" selfCollide_value ="true"/>
+    <xacro:gazebo_link link_name="hip_back_left" color="Red" selfCollide_value ="true"/>
+    <xacro:gazebo_link link_name="hip_back_right" color="Red" selfCollide_value ="true"/>
 
     <!-- Thigh links -->
-    <xacro:gazebo_link link_name="thigh_front_left" color="Green" selfCollide_value ="false"/>
-    <xacro:gazebo_link link_name="thigh_front_right" color="Green" selfCollide_value ="false"/>
-    <xacro:gazebo_link link_name="thigh_back_left" color="Green" selfCollide_value ="false"/>
-    <xacro:gazebo_link link_name="thigh_back_right" color="Green" selfCollide_value ="false"/>
+    <xacro:gazebo_link link_name="thigh_front_left" color="Green" selfCollide_value ="true"/>
+    <xacro:gazebo_link link_name="thigh_front_right" color="Green" selfCollide_value ="true"/>
+    <xacro:gazebo_link link_name="thigh_back_left" color="Green" selfCollide_value ="true"/>
+    <xacro:gazebo_link link_name="thigh_back_right" color="Green" selfCollide_value ="true"/>
 
     <!-- Shank links -->
-    <xacro:gazebo_link link_name="shank_front_left_" color="Yellow" selfCollide_value ="false"/>
-    <xacro:gazebo_link link_name="shank_front_right" color="Yellow" selfCollide_value ="false"/>
-    <xacro:gazebo_link link_name="shank_back_left" color="Yellow" selfCollide_value ="false"/>
-    <xacro:gazebo_link link_name="shank_back_right" color="Yellow" selfCollide_value ="false"/>
+    <xacro:gazebo_link link_name="shank_front_left_" color="Yellow" selfCollide_value ="true"/>
+    <xacro:gazebo_link link_name="shank_front_right" color="Yellow" selfCollide_value ="true"/>
+    <xacro:gazebo_link link_name="shank_back_left" color="Yellow" selfCollide_value ="true"/>
+    <xacro:gazebo_link link_name="shank_back_right" color="Yellow" selfCollide_value ="true"/>
 
     <!-- Feet links -->
-    <xacro:gazebo_link link_name="feet_front_left" color="Pink" selfCollide_value ="false"/>
-    <xacro:gazebo_link link_name="feet_front_right" color="Pink" selfCollide_value ="false"/>
-    <xacro:gazebo_link link_name="feet_back_left" color="Pink" selfCollide_value ="false"/>
-    <xacro:gazebo_link link_name="feet_back_right" color="Pink" selfCollide_value ="false"/>
+    <xacro:gazebo_link link_name="feet_front_left" color="Pink" selfCollide_value ="true"/>
+    <xacro:gazebo_link link_name="feet_front_right" color="Pink" selfCollide_value ="true"/>
+    <xacro:gazebo_link link_name="feet_back_left" color="Pink" selfCollide_value ="true"/>
+    <xacro:gazebo_link link_name="feet_back_right" color="Pink" selfCollide_value ="true"/>
 
 
     <!-- JOINTS (only revolute): -->
diff --git a/src/meldog_description/description/meldog_links/shank/shank.urdf.xacro b/src/meldog_description/description/meldog_links/shank/shank.urdf.xacro
index 370e5eeb..f0b0d301 100644
--- a/src/meldog_description/description/meldog_links/shank/shank.urdf.xacro
+++ b/src/meldog_description/description/meldog_links/shank/shank.urdf.xacro
@@ -27,7 +27,7 @@
             <collision name="shank_${position}_link_collision">
                 <origin xyz="0.0 0.0 ${-shank_length/2}" rpy="0.0 0.0 0.0"/>
                 <geometry>
-                    <cylinder radius="${shank_radius}" length="${shank_length}"/>
+                    <cylinder radius="${shank_radius}" length="${3*shank_length/4}"/>
                 </geometry>
             </collision>
            
diff --git a/src/meldog_description/description/meldog_links/thigh/thigh.urdf.xacro b/src/meldog_description/description/meldog_links/thigh/thigh.urdf.xacro
index 21e0b5e2..5e81f7d6 100644
--- a/src/meldog_description/description/meldog_links/thigh/thigh.urdf.xacro
+++ b/src/meldog_description/description/meldog_links/thigh/thigh.urdf.xacro
@@ -27,7 +27,7 @@
             <collision name="thigh_${position}_link_collision">
                 <origin xyz="0.0 0.0 ${-thigh_length/2}" rpy="0.0 0.0 0.0"/>
                 <geometry>
-                    <cylinder radius="${thigh_radius}" length="${thigh_length}"/>
+                    <cylinder radius="${thigh_radius}" length="${3*thigh_length/4}"/>
                 </geometry>
             </collision>
             
diff --git a/src/meldog_description/launch/gazebo.launch.py b/src/meldog_description/launch/gazebo.launch.py
new file mode 100644
index 00000000..b99c9610
--- /dev/null
+++ b/src/meldog_description/launch/gazebo.launch.py
@@ -0,0 +1,33 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, RegisterEventHandler
+from launch.conditions import IfCondition
+from launch.event_handlers import OnProcessExit
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+from launch_ros.parameter_descriptions import ParameterValue
+
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+
+    gazebo = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([os.path.join(
+            get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']),
+        )
+
+
+
+    # Nodes
+    nodes = [
+        gazebo,
+    ]
+    return LaunchDescription(nodes)
\ No newline at end of file
diff --git a/src/meldog_description/launch/meldog_gazebo.launch.py b/src/meldog_description/launch/meldog_gazebo.launch.py
new file mode 100644
index 00000000..bc589667
--- /dev/null
+++ b/src/meldog_description/launch/meldog_gazebo.launch.py
@@ -0,0 +1,83 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, RegisterEventHandler
+from launch.conditions import IfCondition
+from launch.event_handlers import OnProcessExit
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+from launch_ros.parameter_descriptions import ParameterValue
+
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+
+   # Declare arguments
+    declared_arguments = []
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "package",
+            default_value="meldog_description",
+        )
+    )
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "urdf_file",
+            default_value="meldog_gazebo.urdf.xacro",
+        )
+    )
+     # Initialize Arguments
+    package_name = LaunchConfiguration("package")
+    urdf_file = LaunchConfiguration("urdf_file")
+
+    # Get URDF via xacro
+    robot_description_content = ParameterValue(
+        Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [
+                    FindPackageShare(package_name),
+                    "description",
+                    urdf_file,
+                ]
+            ),
+        ]
+    ), value_type=str)
+    robot_description = {"robot_description": robot_description_content}
+    use_sim_time = {"use_sim_time": True}
+
+    robot_state_pub_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="both",
+        parameters=[robot_description, use_sim_time],
+    )
+
+
+    spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
+                    arguments=['-topic', 'robot_description',
+                                '-entity', 'Meldog'],
+                    output='screen')
+
+
+
+
+
+
+    # Nodes
+    nodes = [
+        robot_state_pub_node,
+        spawn_entity,
+    ]
+    return LaunchDescription(declared_arguments + nodes)
\ No newline at end of file
diff --git a/src/meldog_description/launch/robot_visualize.launch.py b/src/meldog_description/launch/meldog_rviz.launch.py
similarity index 100%
rename from src/meldog_description/launch/robot_visualize.launch.py
rename to src/meldog_description/launch/meldog_rviz.launch.py

From c15ec4c2c5f3e2ea4fff0ed35eb564bf451dc34a Mon Sep 17 00:00:00 2001
From: Bartek <bartlomiejk@vp.pl>
Date: Sun, 8 Sep 2024 20:30:40 +0200
Subject: [PATCH 14/23] Meldog succesfully run on gazebo sim

---
 .../launch/gazebo_sim.launch.py               | 39 +++++++++
 .../launch/meldog_gazebo_sim.launch.py        | 83 +++++++++++++++++++
 src/meldog_description/package.xml            |  3 +
 src/meldog_description/worlds/empty.world     | 73 ++++++++++++++++
 4 files changed, 198 insertions(+)
 create mode 100644 src/meldog_description/launch/gazebo_sim.launch.py
 create mode 100644 src/meldog_description/launch/meldog_gazebo_sim.launch.py
 create mode 100644 src/meldog_description/worlds/empty.world

diff --git a/src/meldog_description/launch/gazebo_sim.launch.py b/src/meldog_description/launch/gazebo_sim.launch.py
new file mode 100644
index 00000000..e9435ffe
--- /dev/null
+++ b/src/meldog_description/launch/gazebo_sim.launch.py
@@ -0,0 +1,39 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, RegisterEventHandler
+from launch.conditions import IfCondition
+from launch.event_handlers import OnProcessExit
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+from launch_ros.parameter_descriptions import ParameterValue
+
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+
+    world = os.path.join(
+        get_package_share_directory('meldog_description'),
+        'worlds',
+        'empty.world'
+    )
+    gazebo_sim = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([os.path.join(
+            get_package_share_directory('ros_gz_sim'), 'launch'), '/gz_sim.launch.py']),
+            launch_arguments={'gz_args': [world], 'on_exit_shutdown': 'true'}.items()
+        )
+
+
+
+    # Nodes
+    nodes = [
+        gazebo_sim,
+    ]
+    return LaunchDescription(nodes)
\ No newline at end of file
diff --git a/src/meldog_description/launch/meldog_gazebo_sim.launch.py b/src/meldog_description/launch/meldog_gazebo_sim.launch.py
new file mode 100644
index 00000000..ec60d7c7
--- /dev/null
+++ b/src/meldog_description/launch/meldog_gazebo_sim.launch.py
@@ -0,0 +1,83 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, RegisterEventHandler
+from launch.conditions import IfCondition
+from launch.event_handlers import OnProcessExit
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+from launch_ros.parameter_descriptions import ParameterValue
+
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+
+   # Declare arguments
+    declared_arguments = []
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "package",
+            default_value="meldog_description",
+        )
+    )
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "urdf_file",
+            default_value="meldog_gazebo.urdf.xacro",
+        )
+    )
+     # Initialize Arguments
+    package_name = LaunchConfiguration("package")
+    urdf_file = LaunchConfiguration("urdf_file")
+
+    # Get URDF via xacro
+    robot_description_content = ParameterValue(
+        Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [
+                    FindPackageShare(package_name),
+                    "description",
+                    urdf_file,
+                ]
+            ),
+        ]
+    ), value_type=str)
+    robot_description = {"robot_description": robot_description_content}
+    use_sim_time = {"use_sim_time": True}
+
+    robot_state_pub_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="both",
+        parameters=[robot_description, use_sim_time],
+    )
+
+
+    spawn_entity = Node(package='ros_gz_sim', executable='create',
+                    arguments=['-topic', 'robot_description',
+                                '-name', 'Meldog'],
+                    output='screen')
+
+
+
+
+
+
+    # Nodes
+    nodes = [
+        robot_state_pub_node,
+        spawn_entity,
+    ]
+    return LaunchDescription(declared_arguments + nodes)
\ No newline at end of file
diff --git a/src/meldog_description/package.xml b/src/meldog_description/package.xml
index 1c26018a..4edd9184 100644
--- a/src/meldog_description/package.xml
+++ b/src/meldog_description/package.xml
@@ -15,6 +15,9 @@
   <exec_depend>xacro</exec_depend>
   <exec_depend>joint_state_publisher_gui</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
+  <depend>ros_gz_bridge</depend>
+  <depend>ros_gz_image</depend>
+  <depend>ros_gz_sim</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
diff --git a/src/meldog_description/worlds/empty.world b/src/meldog_description/worlds/empty.world
new file mode 100644
index 00000000..9794ad0a
--- /dev/null
+++ b/src/meldog_description/worlds/empty.world
@@ -0,0 +1,73 @@
+<?xml version="1.0"?>
+<sdf version="1.6">
+  <world name="default">
+    <!-- <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-sensors-system"
+      name="gz::sim::systems::Sensors">
+      <render_engine>ogre2</render_engine>
+    </plugin>
+    <plugin
+      filename="gz-sim-imu-system"
+      name="gz::sim::systems::Imu">
+    </plugin> -->
+
+    <include>
+      <uri>
+        https://fuel.gazebosim.org/1.0/OpenRobotics/models/Ground Plane
+      </uri>
+    </include>
+
+    <include>
+      <uri>
+        https://fuel.gazebosim.org/1.0/OpenRobotics/models/Sun
+      </uri>
+    </include>
+
+    <scene>
+      <shadows>false</shadows>
+    </scene>
+
+
+    <gui fullscreen='0'>
+      <camera name='user_camera'>
+        <pose frame=''>0.319654 -0.235002 9.29441 0 1.5138 0.009599</pose>
+        <view_controller>orbit</view_controller>
+        <projection_type>perspective</projection_type>
+      </camera>
+    </gui>
+
+    <physics type="ode">
+      <real_time_update_rate>2000.0</real_time_update_rate>
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1</real_time_factor>
+      <ode>
+        <solver>
+          <type>quick</type>
+          <iters>150</iters>
+          <precon_iters>0</precon_iters>
+          <sor>1.400000</sor>
+          <use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
+        </solver>
+        <constraints>
+          <cfm>0.00001</cfm>
+          <erp>0.2</erp>
+          <contact_max_correcting_vel>2000.000000</contact_max_correcting_vel>
+          <contact_surface_layer>0.01000</contact_surface_layer>
+        </constraints>
+      </ode>
+    </physics>
+
+  </world>
+</sdf>
\ No newline at end of file

From 19f6a3e769ecd7c92cbdf722cdbe54116bfbbe6d Mon Sep 17 00:00:00 2001
From: Bartek <bartlomiejk@vp.pl>
Date: Mon, 9 Sep 2024 22:30:33 +0200
Subject: [PATCH 15/23] Renamed 'meldog_description' to
 'meldog_simple_description'

---
 .../CMakeLists.txt                                              | 2 +-
 src/{meldog_description => meldog_simple_description}/LICENSE   | 0
 .../description/actuator/actuator_macros.urdf.xacro             | 0
 .../description/gazebo/gazebo_macros.urdf.xacro                 | 0
 .../description/meldog_core.urdf.xacro                          | 2 +-
 .../description/meldog_gazebo.urdf.xacro                        | 2 +-
 .../description/meldog_links/feet/feet.urdf.xacro               | 0
 .../description/meldog_links/hip/hip.urdf.xacro                 | 0
 .../description/meldog_links/shank/shank.urdf.xacro             | 0
 .../description/meldog_links/thigh/thigh.urdf.xacro             | 0
 .../description/meldog_links/trunk/trunk.urdf.xacro             | 0
 .../description/ros2_control/ros2_control_gazebo.urdf.xacro     | 0
 .../description/ros2_control/transmission_macros.urdf.xacro     | 0
 .../launch/gazebo.launch.py                                     | 0
 .../launch/gazebo_sim.launch.py                                 | 0
 .../launch/meldog_gazebo.launch.py                              | 0
 .../launch/meldog_gazebo_sim.launch.py                          | 0
 .../launch/meldog_rviz.launch.py                                | 0
 .../package.xml                                                 | 2 +-
 .../worlds/empty.world                                          | 0
 20 files changed, 4 insertions(+), 4 deletions(-)
 rename src/{meldog_description => meldog_simple_description}/CMakeLists.txt (96%)
 rename src/{meldog_description => meldog_simple_description}/LICENSE (100%)
 rename src/{meldog_description => meldog_simple_description}/description/actuator/actuator_macros.urdf.xacro (100%)
 rename src/{meldog_description => meldog_simple_description}/description/gazebo/gazebo_macros.urdf.xacro (100%)
 rename src/{meldog_description => meldog_simple_description}/description/meldog_core.urdf.xacro (99%)
 rename src/{meldog_description => meldog_simple_description}/description/meldog_gazebo.urdf.xacro (99%)
 rename src/{meldog_description => meldog_simple_description}/description/meldog_links/feet/feet.urdf.xacro (100%)
 rename src/{meldog_description => meldog_simple_description}/description/meldog_links/hip/hip.urdf.xacro (100%)
 rename src/{meldog_description => meldog_simple_description}/description/meldog_links/shank/shank.urdf.xacro (100%)
 rename src/{meldog_description => meldog_simple_description}/description/meldog_links/thigh/thigh.urdf.xacro (100%)
 rename src/{meldog_description => meldog_simple_description}/description/meldog_links/trunk/trunk.urdf.xacro (100%)
 rename src/{meldog_description => meldog_simple_description}/description/ros2_control/ros2_control_gazebo.urdf.xacro (100%)
 rename src/{meldog_description => meldog_simple_description}/description/ros2_control/transmission_macros.urdf.xacro (100%)
 rename src/{meldog_description => meldog_simple_description}/launch/gazebo.launch.py (100%)
 rename src/{meldog_description => meldog_simple_description}/launch/gazebo_sim.launch.py (100%)
 rename src/{meldog_description => meldog_simple_description}/launch/meldog_gazebo.launch.py (100%)
 rename src/{meldog_description => meldog_simple_description}/launch/meldog_gazebo_sim.launch.py (100%)
 rename src/{meldog_description => meldog_simple_description}/launch/meldog_rviz.launch.py (100%)
 rename src/{meldog_description => meldog_simple_description}/package.xml (95%)
 rename src/{meldog_description => meldog_simple_description}/worlds/empty.world (100%)

diff --git a/src/meldog_description/CMakeLists.txt b/src/meldog_simple_description/CMakeLists.txt
similarity index 96%
rename from src/meldog_description/CMakeLists.txt
rename to src/meldog_simple_description/CMakeLists.txt
index 29752e11..4ed9caf2 100644
--- a/src/meldog_description/CMakeLists.txt
+++ b/src/meldog_simple_description/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.8)
-project(meldog_description)
+project(meldog_simple_description)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
diff --git a/src/meldog_description/LICENSE b/src/meldog_simple_description/LICENSE
similarity index 100%
rename from src/meldog_description/LICENSE
rename to src/meldog_simple_description/LICENSE
diff --git a/src/meldog_description/description/actuator/actuator_macros.urdf.xacro b/src/meldog_simple_description/description/actuator/actuator_macros.urdf.xacro
similarity index 100%
rename from src/meldog_description/description/actuator/actuator_macros.urdf.xacro
rename to src/meldog_simple_description/description/actuator/actuator_macros.urdf.xacro
diff --git a/src/meldog_description/description/gazebo/gazebo_macros.urdf.xacro b/src/meldog_simple_description/description/gazebo/gazebo_macros.urdf.xacro
similarity index 100%
rename from src/meldog_description/description/gazebo/gazebo_macros.urdf.xacro
rename to src/meldog_simple_description/description/gazebo/gazebo_macros.urdf.xacro
diff --git a/src/meldog_description/description/meldog_core.urdf.xacro b/src/meldog_simple_description/description/meldog_core.urdf.xacro
similarity index 99%
rename from src/meldog_description/description/meldog_core.urdf.xacro
rename to src/meldog_simple_description/description/meldog_core.urdf.xacro
index c343001d..4f1c9703 100644
--- a/src/meldog_description/description/meldog_core.urdf.xacro
+++ b/src/meldog_simple_description/description/meldog_core.urdf.xacro
@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="meldog_core">
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="meldog_simple_core">
 
     <!-- PI -->
     <xacro:property name="PI" value="3.1415926535897"/>
diff --git a/src/meldog_description/description/meldog_gazebo.urdf.xacro b/src/meldog_simple_description/description/meldog_gazebo.urdf.xacro
similarity index 99%
rename from src/meldog_description/description/meldog_gazebo.urdf.xacro
rename to src/meldog_simple_description/description/meldog_gazebo.urdf.xacro
index 6c9d5e6a..d5c8ea0c 100644
--- a/src/meldog_description/description/meldog_gazebo.urdf.xacro
+++ b/src/meldog_simple_description/description/meldog_gazebo.urdf.xacro
@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "meldog_gazebo">
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "meldog_simple_gazebo">
 
     <!-- Meldog Core file -->
     <xacro:include filename="meldog_core.urdf.xacro"/>
diff --git a/src/meldog_description/description/meldog_links/feet/feet.urdf.xacro b/src/meldog_simple_description/description/meldog_links/feet/feet.urdf.xacro
similarity index 100%
rename from src/meldog_description/description/meldog_links/feet/feet.urdf.xacro
rename to src/meldog_simple_description/description/meldog_links/feet/feet.urdf.xacro
diff --git a/src/meldog_description/description/meldog_links/hip/hip.urdf.xacro b/src/meldog_simple_description/description/meldog_links/hip/hip.urdf.xacro
similarity index 100%
rename from src/meldog_description/description/meldog_links/hip/hip.urdf.xacro
rename to src/meldog_simple_description/description/meldog_links/hip/hip.urdf.xacro
diff --git a/src/meldog_description/description/meldog_links/shank/shank.urdf.xacro b/src/meldog_simple_description/description/meldog_links/shank/shank.urdf.xacro
similarity index 100%
rename from src/meldog_description/description/meldog_links/shank/shank.urdf.xacro
rename to src/meldog_simple_description/description/meldog_links/shank/shank.urdf.xacro
diff --git a/src/meldog_description/description/meldog_links/thigh/thigh.urdf.xacro b/src/meldog_simple_description/description/meldog_links/thigh/thigh.urdf.xacro
similarity index 100%
rename from src/meldog_description/description/meldog_links/thigh/thigh.urdf.xacro
rename to src/meldog_simple_description/description/meldog_links/thigh/thigh.urdf.xacro
diff --git a/src/meldog_description/description/meldog_links/trunk/trunk.urdf.xacro b/src/meldog_simple_description/description/meldog_links/trunk/trunk.urdf.xacro
similarity index 100%
rename from src/meldog_description/description/meldog_links/trunk/trunk.urdf.xacro
rename to src/meldog_simple_description/description/meldog_links/trunk/trunk.urdf.xacro
diff --git a/src/meldog_description/description/ros2_control/ros2_control_gazebo.urdf.xacro b/src/meldog_simple_description/description/ros2_control/ros2_control_gazebo.urdf.xacro
similarity index 100%
rename from src/meldog_description/description/ros2_control/ros2_control_gazebo.urdf.xacro
rename to src/meldog_simple_description/description/ros2_control/ros2_control_gazebo.urdf.xacro
diff --git a/src/meldog_description/description/ros2_control/transmission_macros.urdf.xacro b/src/meldog_simple_description/description/ros2_control/transmission_macros.urdf.xacro
similarity index 100%
rename from src/meldog_description/description/ros2_control/transmission_macros.urdf.xacro
rename to src/meldog_simple_description/description/ros2_control/transmission_macros.urdf.xacro
diff --git a/src/meldog_description/launch/gazebo.launch.py b/src/meldog_simple_description/launch/gazebo.launch.py
similarity index 100%
rename from src/meldog_description/launch/gazebo.launch.py
rename to src/meldog_simple_description/launch/gazebo.launch.py
diff --git a/src/meldog_description/launch/gazebo_sim.launch.py b/src/meldog_simple_description/launch/gazebo_sim.launch.py
similarity index 100%
rename from src/meldog_description/launch/gazebo_sim.launch.py
rename to src/meldog_simple_description/launch/gazebo_sim.launch.py
diff --git a/src/meldog_description/launch/meldog_gazebo.launch.py b/src/meldog_simple_description/launch/meldog_gazebo.launch.py
similarity index 100%
rename from src/meldog_description/launch/meldog_gazebo.launch.py
rename to src/meldog_simple_description/launch/meldog_gazebo.launch.py
diff --git a/src/meldog_description/launch/meldog_gazebo_sim.launch.py b/src/meldog_simple_description/launch/meldog_gazebo_sim.launch.py
similarity index 100%
rename from src/meldog_description/launch/meldog_gazebo_sim.launch.py
rename to src/meldog_simple_description/launch/meldog_gazebo_sim.launch.py
diff --git a/src/meldog_description/launch/meldog_rviz.launch.py b/src/meldog_simple_description/launch/meldog_rviz.launch.py
similarity index 100%
rename from src/meldog_description/launch/meldog_rviz.launch.py
rename to src/meldog_simple_description/launch/meldog_rviz.launch.py
diff --git a/src/meldog_description/package.xml b/src/meldog_simple_description/package.xml
similarity index 95%
rename from src/meldog_description/package.xml
rename to src/meldog_simple_description/package.xml
index 4edd9184..18acb8bb 100644
--- a/src/meldog_description/package.xml
+++ b/src/meldog_simple_description/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>meldog_description</name>
+  <name>meldog_simple_description</name>
   <version>0.0.0</version>
   <description>TODO: Package description</description>
   <maintainer email="bartlomiejk@vp.pl">bartek</maintainer>
diff --git a/src/meldog_description/worlds/empty.world b/src/meldog_simple_description/worlds/empty.world
similarity index 100%
rename from src/meldog_description/worlds/empty.world
rename to src/meldog_simple_description/worlds/empty.world

From bacf74c77bbf67e9a2800f3974e0a1b4319da6c9 Mon Sep 17 00:00:00 2001
From: Bartek <bartlomiejk@vp.pl>
Date: Tue, 10 Sep 2024 19:49:22 +0200
Subject: [PATCH 16/23] Added ros2_control macro for joints

---
 .../ros2_control_joint_macros.urdf.xacro      | 27 +++++++++++++++++++
 1 file changed, 27 insertions(+)
 create mode 100644 src/meldog_simple_description/description/ros2_control/ros2_control_joint_macros.urdf.xacro

diff --git a/src/meldog_simple_description/description/ros2_control/ros2_control_joint_macros.urdf.xacro b/src/meldog_simple_description/description/ros2_control/ros2_control_joint_macros.urdf.xacro
new file mode 100644
index 00000000..8263dbdc
--- /dev/null
+++ b/src/meldog_simple_description/description/ros2_control/ros2_control_joint_macros.urdf.xacro
@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name ="meldog_ros2_control_joint_macros">
+
+    <xacro:include filename="actuator/actuator_macros.urdf.xacro"/>
+
+    <xacro:macro name="ros2_control_joint" 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>
+            </command_interface>
+            <xacro:if value="$(arg use_gazebo)">
+                <command_interface name="effort"/>
+                    <param name="min">-${actuator_max_torque}</param>
+                    <param name="max">${actuator_max_torque}</param>
+            </xacro:if>                
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+    </xacro:macro>
+
+</robot>
\ No newline at end of file

From 41336a1c045377bef3d3a9decaa32dcf3d05629f Mon Sep 17 00:00:00 2001
From: Bartek <bartlomiejk@vp.pl>
Date: Wed, 11 Sep 2024 20:18:42 +0200
Subject: [PATCH 17/23] Added joint trajectory controller for tests and joint
 hardware interface for gazebo sim

---
 .../ros2_control_gazebo.urdf.xacro            |  33 +++-
 .../joint_trajectory_controller_test.yaml     | 143 ++++++++++++++++++
 2 files changed, 174 insertions(+), 2 deletions(-)
 create mode 100644 src/meldog_simple_description/test_controllers/joint_trajectory_controller_test.yaml

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 92e82702..509198fb 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
@@ -4,9 +4,38 @@
 <!-- ROS2 Control Gazebo -->
     <ros2_control name="meldog_gazebo" type="system">
         <hardware>  
-            <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+            <plugin>ign_ros2_control/IgnitionSystem</plugin>
         </hardware>
+        <!-- Transmissions -->
         <xacro:include filename="ros2_control/transmission_macros.urdf.xacro"/>
-        <!-- TUTAJ DODAJ JOINTY -->
+
+        <!-- Hardware interface for Gazebo -->
+        <xacro:include filename="ros2_control/ros2_control_joint_macros.urdf.xacro"/>
+
+        <!-- Trunk to Hips joints-->
+        <xacro:ros2_control_joint joint_name="FLH_joint" lower="-${PI/2}" upper="${PI/2}"/>
+        <xacro:ros2_control_joint joint_name="FRH_joint" lower="-${PI/2}" upper="${PI/2}"/>
+        <xacro:ros2_control_joint joint_name="BLH_joint" lower="-${PI/2}" upper="${PI/2}"/>
+        <xacro:ros2_control_joint joint_name="BRH_joint" lower="-${PI/2}" upper="${PI/2}"/>
+
+        <!-- Hips to Thighs joints -->
+        <xacro:ros2_control_joint joint_name="FLT_joint" lower="-${PI}" upper="${PI}"/>
+        <xacro:ros2_control_joint joint_name="FRT_joint" lower="-${PI}" upper="${PI}"/>
+        <xacro:ros2_control_joint joint_name="BLT_joint" lower="-${PI}" upper="${PI}"/>
+        <xacro:ros2_control_joint joint_name="BRT_joint" lower="-${PI}" upper="${PI}"/>
+
+
+        <!-- Thighs to Shanks joints -->
+        <xacro:ros2_control_joint joint_name="FLS_joint" lower="-${PI/2}" upper="${PI/2}"/>
+        <xacro:ros2_control_joint joint_name="FRS_joint" lower="-${PI/2}" upper="${PI/2}"/>
+        <xacro:ros2_control_joint joint_name="BLS_joint" lower="-${PI/2}" upper="${PI/2}"/>
+        <xacro:ros2_control_joint joint_name="BRS_joint" lower="-${PI/2}" upper="${PI/2}"/>
+
     </ros2_control>
+
+    <gazebo>
+        <plugin filename="ign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin">
+          <parameters>$(find meldog_simple_description)/test_controllers/joint_trajectory_controller_test.yaml</parameters>
+        </plugin>
+    </gazebo>
 </robot>
\ No newline at end of file
diff --git a/src/meldog_simple_description/test_controllers/joint_trajectory_controller_test.yaml b/src/meldog_simple_description/test_controllers/joint_trajectory_controller_test.yaml
new file mode 100644
index 00000000..ec3c6145
--- /dev/null
+++ b/src/meldog_simple_description/test_controllers/joint_trajectory_controller_test.yaml
@@ -0,0 +1,143 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 200.0  # Hz
+    use_sim_time: true
+
+    joint_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+#     imu_sensor_broadcaster:
+#       type: imu_sensor_broadcaster/IMUSensorBroadcaster # TODO: DODAJ IMU
+
+# imu_sensor_broadcaster:
+#   ros__parameters:
+#     sensor_name: MPU6050sensor
+#     frame_id:  imu
+
+joint_trajectory_controller:
+  ros__parameters:
+    joints:
+      - FLH_joint
+      - FRH_joint
+      - BLH_joint
+      - BRH_joint
+      - FLT_joint
+      - FRT_joint
+      - BLT_joint
+      - BRT_joint
+      - FLS_joint
+      - FRS_joint
+      - BLS_joint
+      - BRS_joint
+
+    command_joints:
+      - FLH_joint
+      - FRH_joint
+      - BLH_joint
+      - BRH_joint
+      - FLT_joint
+      - FRT_joint
+      - BLT_joint
+      - BRT_joint
+      - FLS_joint
+      - FRS_joint
+      - BLS_joint
+      - BRS_joint
+
+    command_interfaces:
+      - effort
+    state_interfaces:
+      - position
+      - velocity
+    action_monitor_rate: 200.0
+    state_publish_rate: 200.0  # Hz
+    allow_nonzero_velocity_at_trajectory_end: false
+
+    gains:
+      FLH_joint:
+        angle_wraparound: false
+        d: 0.1
+        ff_velocity_scale: 0.0
+        i: 0.0
+        i_clamp: 0.0
+        p: 5.0
+      FRH_joint:
+        angle_wraparound: false
+        d: 0.1
+        ff_velocity_scale: 0.0
+        i: 0.0
+        i_clamp: 0.0
+        p: 5.0
+      BLH_joint:
+        angle_wraparound: false
+        d: 0.1
+        ff_velocity_scale: 0.0
+        i: 0.0
+        i_clamp: 0.0
+        p: 5.0
+      BRH_joint:
+        angle_wraparound: false
+        d: 0.1
+        ff_velocity_scale: 0.0
+        i: 0.0
+        i_clamp: 0.0
+        p: 5.0
+      FLT_joint:
+        angle_wraparound: false
+        d: 0.1
+        ff_velocity_scale: 0.0
+        i: 0.0
+        i_clamp: 0.0
+        p: 5.0
+      FRT_joint:
+        angle_wraparound: false
+        d: 0.1
+        ff_velocity_scale: 0.0
+        i: 0.0
+        i_clamp: 0.0
+        p: 5.0
+      BLT_joint:
+        angle_wraparound: false
+        d: 0.1
+        ff_velocity_scale: 0.0
+        i: 0.0
+        i_clamp: 0.0
+        p: 5.0
+      BRT_joint:
+        angle_wraparound: false
+        d: 0.1
+        ff_velocity_scale: 0.0
+        i: 0.0
+        i_clamp: 0.0
+        p: 5.0
+      FLS_joint:
+        angle_wraparound: false
+        d: 0.1
+        ff_velocity_scale: 0.0
+        i: 0.0
+        i_clamp: 0.0
+        p: 5.0
+      FRS_joint:
+        angle_wraparound: false
+        d: 0.1
+        ff_velocity_scale: 0.0
+        i: 0.0
+        i_clamp: 0.0
+        p: 5.0
+      BLS_joint:
+        angle_wraparound: false
+        d: 0.1
+        ff_velocity_scale: 0.0
+        i: 0.0
+        i_clamp: 0.0
+        p: 5.0
+      BRS_joint:
+        angle_wraparound: false
+        d: 0.1
+        ff_velocity_scale: 0.0
+        i: 0.0
+        i_clamp: 0.0
+        p: 5.0
\ No newline at end of file

From f9e3e92beda90746742672dc475ea1595bbbd481 Mon Sep 17 00:00:00 2001
From: Bartek <bartlomiejk@vp.pl>
Date: Sat, 14 Sep 2024 17:25:38 +0200
Subject: [PATCH 18/23] Succesfully runned simulation with ros2 control plugin
 and controllers

---
 .../description/meldog_gazebo.urdf.xacro      |   2 +
 .../ros2_control_gazebo.urdf.xacro            |  28 ++--
 .../ros2_control_joint_macros.urdf.xacro      |  13 +-
 .../transmission_macros.urdf.xacro            |  14 +-
 .../meldog_gazebo_sim_ros2_control.launch.py  | 157 ++++++++++++++++++
 .../joint_trajectory_controller_test.yaml     |  78 +++------
 6 files changed, 213 insertions(+), 79 deletions(-)
 create mode 100644 src/meldog_simple_description/launch/meldog_gazebo_sim_ros2_control.launch.py

diff --git a/src/meldog_simple_description/description/meldog_gazebo.urdf.xacro b/src/meldog_simple_description/description/meldog_gazebo.urdf.xacro
index d5c8ea0c..af40456f 100644
--- a/src/meldog_simple_description/description/meldog_gazebo.urdf.xacro
+++ b/src/meldog_simple_description/description/meldog_gazebo.urdf.xacro
@@ -55,4 +55,6 @@
     <xacro:gazebo_joint joint_name="BLS"/>
     <xacro:gazebo_joint joint_name="BRS"/>
 
+    <!-- ROS2 Control -->
+    <xacro:include filename="ros2_control/ros2_control_gazebo.urdf.xacro"/>
 </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 509198fb..b142bbfa 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
@@ -7,29 +7,29 @@
             <plugin>ign_ros2_control/IgnitionSystem</plugin>
         </hardware>
         <!-- Transmissions -->
-        <xacro:include filename="ros2_control/transmission_macros.urdf.xacro"/>
+        <!-- <xacro:include filename="transmission_macros.urdf.xacro"/> -->
 
         <!-- Hardware interface for Gazebo -->
-        <xacro:include filename="ros2_control/ros2_control_joint_macros.urdf.xacro"/>
+        <xacro:include filename="ros2_control_joint_macros.urdf.xacro"/>
 
         <!-- Trunk to Hips joints-->
-        <xacro:ros2_control_joint joint_name="FLH_joint" lower="-${PI/2}" upper="${PI/2}"/>
-        <xacro:ros2_control_joint joint_name="FRH_joint" lower="-${PI/2}" upper="${PI/2}"/>
-        <xacro:ros2_control_joint joint_name="BLH_joint" lower="-${PI/2}" upper="${PI/2}"/>
-        <xacro:ros2_control_joint joint_name="BRH_joint" lower="-${PI/2}" upper="${PI/2}"/>
+        <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_joint" lower="-${PI}" upper="${PI}"/>
-        <xacro:ros2_control_joint joint_name="FRT_joint" lower="-${PI}" upper="${PI}"/>
-        <xacro:ros2_control_joint joint_name="BLT_joint" lower="-${PI}" upper="${PI}"/>
-        <xacro:ros2_control_joint joint_name="BRT_joint" lower="-${PI}" upper="${PI}"/>
+        <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_joint" lower="-${PI/2}" upper="${PI/2}"/>
-        <xacro:ros2_control_joint joint_name="FRS_joint" lower="-${PI/2}" upper="${PI/2}"/>
-        <xacro:ros2_control_joint joint_name="BLS_joint" lower="-${PI/2}" upper="${PI/2}"/>
-        <xacro:ros2_control_joint joint_name="BRS_joint" lower="-${PI/2}" upper="${PI/2}"/>
+        <xacro:ros2_control_joint joint_name="FLS" lower="-${PI/2}" upper="${PI/2}"/>
+        <xacro:ros2_control_joint joint_name="FRS" lower="-${PI/2}" upper="${PI/2}"/>
+        <xacro:ros2_control_joint joint_name="BLS" lower="-${PI/2}" upper="${PI/2}"/>
+        <xacro:ros2_control_joint joint_name="BRS" lower="-${PI/2}" upper="${PI/2}"/>
 
     </ros2_control>
 
diff --git a/src/meldog_simple_description/description/ros2_control/ros2_control_joint_macros.urdf.xacro b/src/meldog_simple_description/description/ros2_control/ros2_control_joint_macros.urdf.xacro
index 8263dbdc..734c0903 100644
--- a/src/meldog_simple_description/description/ros2_control/ros2_control_joint_macros.urdf.xacro
+++ b/src/meldog_simple_description/description/ros2_control/ros2_control_joint_macros.urdf.xacro
@@ -1,9 +1,9 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name ="meldog_ros2_control_joint_macros">
 
-    <xacro:include filename="actuator/actuator_macros.urdf.xacro"/>
+    <xacro:include filename="../actuator/actuator_macros.urdf.xacro"/>
 
-    <xacro:macro name="ros2_control_joint" params="joint_name upper lower"/>
+    <xacro:macro name="ros2_control_joint" params="joint_name upper lower">
         <joint name="${joint_name}_joint">
             <command_interface name="position">
                 <param name="min">${lower}</param>
@@ -13,11 +13,10 @@
                 <param name="min">-${actuator_max_velocity}</param>
                 <param name="max">${actuator_max_velocity}</param>
             </command_interface>
-            <xacro:if value="$(arg use_gazebo)">
-                <command_interface name="effort"/>
-                    <param name="min">-${actuator_max_torque}</param>
-                    <param name="max">${actuator_max_torque}</param>
-            </xacro:if>                
+            <command_interface name="effort">
+                <param name="min">-${actuator_max_torque}</param>
+                <param name="max">${actuator_max_torque}</param>
+            </command_interface>                  
             <state_interface name="position"/>
             <state_interface name="velocity"/>
             <state_interface name="effort"/>
diff --git a/src/meldog_simple_description/description/ros2_control/transmission_macros.urdf.xacro b/src/meldog_simple_description/description/ros2_control/transmission_macros.urdf.xacro
index 6e9a2104..8ffc223f 100644
--- a/src/meldog_simple_description/description/ros2_control/transmission_macros.urdf.xacro
+++ b/src/meldog_simple_description/description/ros2_control/transmission_macros.urdf.xacro
@@ -2,7 +2,7 @@
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name ="meldog_transmissions">
 
     <!-- SimpleTransmission macro -->
-    <xacro:macro name="SimpleTransmission" params="joint_name reduction offset"/>
+    <xacro:macro name="SimpleTransmission" params="joint_name reduction offset">
         <transmission name="SimpleTransmission_${joint_name}">
                 <plugin>transmission_interface/SimpleTransmission</plugin>
                 <actuator name="${joint_name}_actuator" role="actuator1"/>
@@ -14,8 +14,8 @@
     </xacro:macro>
 
     <!-- FourBarLinkageTransmission macro -->
-    <xacro:macro name="FourBarLinkageTransmission" params="first_joint_name second__joint_name reduction first_offset second_offset"/>
-        <transmission name="FourBarLinkageTransmission_${first_joint_name}_${second__joint_name}">
+    <xacro:macro name="FourBarLinkageTransmission" params="first_joint_name second_joint_name reduction first_offset second_offset">
+        <transmission name="FourBarLinkageTransmission_${first_joint_name}_${second_joint_name}">
             <plugin>transmission_interface/FourBarLinkageTransmission</plugin>
             <actuator name="${first_joint_name}_actuator" role="actuator1">
                 <mechanical_reduction>${reduction}</mechanical_reduction>
@@ -43,9 +43,9 @@
     <xacro:SimpleTransmission joint_name="BRH" reduction="9.97" 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"/>
+    <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"/>
 
 </robot>
\ No newline at end of file
diff --git a/src/meldog_simple_description/launch/meldog_gazebo_sim_ros2_control.launch.py b/src/meldog_simple_description/launch/meldog_gazebo_sim_ros2_control.launch.py
new file mode 100644
index 00000000..070949ce
--- /dev/null
+++ b/src/meldog_simple_description/launch/meldog_gazebo_sim_ros2_control.launch.py
@@ -0,0 +1,157 @@
+# Copyright 2020 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, ExecuteProcess
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, RegisterEventHandler
+from launch.conditions import IfCondition
+from launch.event_handlers import OnProcessExit, OnProcessStart
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+from launch_ros.parameter_descriptions import ParameterValue
+
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+
+    # Declare arguments
+    declared_arguments = []
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "package",
+            default_value="meldog_simple_description",
+        )
+    )
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "urdf_file",
+            default_value="meldog_gazebo.urdf.xacro",
+        )
+    )
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "controller_type_file",
+            default_value="joint_trajectory_controller_test.yaml",
+        )
+    )
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "controller_type",
+            default_value="joint_trajectory_controller",
+        )
+    )
+
+    # Initialize Arguments
+    package_name = LaunchConfiguration("package")
+    urdf_file = LaunchConfiguration("urdf_file")
+    controller_file = LaunchConfiguration("controller_type_file")
+    controller_type = LaunchConfiguration("controller_type")
+
+
+    # Get URDF via xacro
+    robot_description_content = ParameterValue(
+        Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [
+                    FindPackageShare(package_name),
+                    "description",
+                    urdf_file,
+                ]
+            ),
+        ]
+    ), value_type=str)
+
+    # Set up dictionary parameters:
+    robot_description = {"robot_description": robot_description_content}
+    use_sim_time = {"use_sim_time": True}
+
+    # Load world for gazebo sim
+    world = PathJoinSubstitution(
+        [
+            FindPackageShare(package_name),
+            "worlds",
+            'empty.world'
+        ]
+    )
+
+    # robot_state_publisher node:
+    robot_state_pub_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="both",
+        parameters=[robot_description, use_sim_time],
+    )
+
+    # Spawn Meldog:
+    spawn_entity = Node(package='ros_gz_sim', executable='create',
+                    arguments=['-topic', 'robot_description',
+                                '-name', 'Meldog'],
+                    output='screen')
+
+    # Load joint_state_broadcaster
+    load_joint_state_broadcaster = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'joint_state_broadcaster'],
+        output='screen'
+    )
+
+    # Load joint_trajectory_controller 
+    load_joint_effort_controller = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', controller_type],
+        output='screen'
+    )
+
+    # Bridge between ros2 and Ignition Gazebo
+    ros2_gazebo_sim_bridge =  IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                [os.path.join(get_package_share_directory('ros_ign_gazebo'),
+                              'launch', 'ign_gazebo.launch.py')]),
+            launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])])
+    
+    nodes = [
+        ros2_gazebo_sim_bridge,
+
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=spawn_entity,
+                on_exit=[load_joint_state_broadcaster],
+            )
+        ),
+
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action= load_joint_state_broadcaster,
+                on_exit=[load_joint_effort_controller],
+            )
+        ),
+
+        robot_state_pub_node,
+        spawn_entity,
+    ]
+    return LaunchDescription(declared_arguments + nodes)
\ No newline at end of file
diff --git a/src/meldog_simple_description/test_controllers/joint_trajectory_controller_test.yaml b/src/meldog_simple_description/test_controllers/joint_trajectory_controller_test.yaml
index ec3c6145..48b75918 100644
--- a/src/meldog_simple_description/test_controllers/joint_trajectory_controller_test.yaml
+++ b/src/meldog_simple_description/test_controllers/joint_trajectory_controller_test.yaml
@@ -1,6 +1,6 @@
 controller_manager:
   ros__parameters:
-    update_rate: 200.0  # Hz
+    update_rate: 1000  # Hz
     use_sim_time: true
 
     joint_trajectory_controller:
@@ -52,92 +52,68 @@ joint_trajectory_controller:
     state_interfaces:
       - position
       - velocity
-    action_monitor_rate: 200.0
-    state_publish_rate: 200.0  # Hz
+    action_monitor_rate: 1000.0
+    state_publish_rate: 1000.0  # Hz
     allow_nonzero_velocity_at_trajectory_end: false
 
     gains:
       FLH_joint:
-        angle_wraparound: false
-        d: 0.1
-        ff_velocity_scale: 0.0
+        d: 2.5
         i: 0.0
         i_clamp: 0.0
-        p: 5.0
+        p: 350.0
       FRH_joint:
-        angle_wraparound: false
-        d: 0.1
-        ff_velocity_scale: 0.0
+        d: 2.5
         i: 0.0
         i_clamp: 0.0
-        p: 5.0
+        p: 350.0
       BLH_joint:
-        angle_wraparound: false
-        d: 0.1
-        ff_velocity_scale: 0.0
+        d: 2.5
         i: 0.0
         i_clamp: 0.0
-        p: 5.0
+        p: 350.0
       BRH_joint:
-        angle_wraparound: false
-        d: 0.1
-        ff_velocity_scale: 0.0
+        d: 2.5
         i: 0.0
         i_clamp: 0.0
-        p: 5.0
+        p: 350.0
       FLT_joint:
-        angle_wraparound: false
-        d: 0.1
-        ff_velocity_scale: 0.0
+        d: 2.5
         i: 0.0
         i_clamp: 0.0
-        p: 5.0
+        p: 350.0
       FRT_joint:
-        angle_wraparound: false
-        d: 0.1
-        ff_velocity_scale: 0.0
+        d: 2.5
         i: 0.0
         i_clamp: 0.0
-        p: 5.0
+        p: 350.0
       BLT_joint:
-        angle_wraparound: false
-        d: 0.1
-        ff_velocity_scale: 0.0
+        d: 2.5
         i: 0.0
         i_clamp: 0.0
-        p: 5.0
+        p: 350.0
       BRT_joint:
-        angle_wraparound: false
-        d: 0.1
-        ff_velocity_scale: 0.0
+        d: 2.5
         i: 0.0
         i_clamp: 0.0
-        p: 5.0
+        p: 350.0
       FLS_joint:
-        angle_wraparound: false
-        d: 0.1
-        ff_velocity_scale: 0.0
+        d: 2.5
         i: 0.0
         i_clamp: 0.0
-        p: 5.0
+        p: 350.0
       FRS_joint:
-        angle_wraparound: false
-        d: 0.1
-        ff_velocity_scale: 0.0
+        d: 2.5
         i: 0.0
         i_clamp: 0.0
-        p: 5.0
+        p: 350.0
       BLS_joint:
-        angle_wraparound: false
-        d: 0.1
-        ff_velocity_scale: 0.0
+        d: 2.5
         i: 0.0
         i_clamp: 0.0
-        p: 5.0
+        p: 350.0
       BRS_joint:
-        angle_wraparound: false
-        d: 0.1
-        ff_velocity_scale: 0.0
+        d: 2.5
         i: 0.0
         i_clamp: 0.0
-        p: 5.0
\ No newline at end of file
+        p: 350.0
\ No newline at end of file

From bb27e4c0fa98a33b21fd9be3effa03cc6e72e84f Mon Sep 17 00:00:00 2001
From: Bartek <bartlomiejk@vp.pl>
Date: Sun, 15 Sep 2024 20:36:46 +0200
Subject: [PATCH 19/23] Successfully launched meldog in custom world in gazebo
 sim with ros2 control

---
 .../gazebo/gazebo_macros.urdf.xacro           |  4 +-
 .../description/meldog_core.urdf.xacro        |  8 ++--
 .../ros2_control_gazebo.urdf.xacro            |  8 ++--
 .../meldog_gazebo_sim_ros2_control.launch.py  | 17 ++++---
 .../joint_trajectory_controller_test.yaml     | 48 +++++++++----------
 .../worlds/empty.world                        |  6 +--
 6 files changed, 48 insertions(+), 43 deletions(-)

diff --git a/src/meldog_simple_description/description/gazebo/gazebo_macros.urdf.xacro b/src/meldog_simple_description/description/gazebo/gazebo_macros.urdf.xacro
index d3e11bf6..02a7170c 100644
--- a/src/meldog_simple_description/description/gazebo/gazebo_macros.urdf.xacro
+++ b/src/meldog_simple_description/description/gazebo/gazebo_macros.urdf.xacro
@@ -5,8 +5,8 @@
     <!-- Gazebo macro for links -->
     <xacro:property name="kp_value" value="25000.0"/>
     <xacro:property name="kd_value" value="0.15"/>
-    <xacro:property name="mu2_value" value="0.7"/>
-    <xacro:property name="mu2_value" value="0.7"/>
+    <xacro:property name="mu2_value" value="0.9"/>
+    <xacro:property name="mu2_value" value="0.9"/>
     <xacro:property name="maxVel_value" value="0.25"/>
     <xacro:property name="dampingFactor_value" value="0.00"/>
 
diff --git a/src/meldog_simple_description/description/meldog_core.urdf.xacro b/src/meldog_simple_description/description/meldog_core.urdf.xacro
index 4f1c9703..5bee4280 100644
--- a/src/meldog_simple_description/description/meldog_core.urdf.xacro
+++ b/src/meldog_simple_description/description/meldog_core.urdf.xacro
@@ -134,7 +134,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 ="-${PI/2}" upper = "${PI/2}"/>
+        <xacro:meldog_joint_limits lower ="-${5*PI/6}" upper = "${5*PI/6}"/>
     </joint>
 
     <!-- Thigh front right to Shank front right -->
@@ -143,7 +143,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 ="-${PI/2}" upper = "${PI/2}"/>
+        <xacro:meldog_joint_limits lower ="-${5*PI/6}" upper = "${5*PI/6}"/>
     </joint>
 
     <!-- Thigh back left to Shank back left -->
@@ -152,7 +152,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 ="-${PI/2}" upper = "${PI/2}"/>
+        <xacro:meldog_joint_limits lower ="-${5*PI/6}" upper = "${5*PI/6}"/>
     </joint>
 
     <!-- Thigh back right to Shank back right -->
@@ -161,7 +161,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 ="-${PI/2}" upper = "${PI/2}"/>
+        <xacro:meldog_joint_limits lower ="-${5*PI/6}" upper = "${5*PI/6}"/>
     </joint>
     
 
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 b142bbfa..1cfc9926 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
@@ -26,10 +26,10 @@
 
 
         <!-- Thighs to Shanks joints -->
-        <xacro:ros2_control_joint joint_name="FLS" lower="-${PI/2}" upper="${PI/2}"/>
-        <xacro:ros2_control_joint joint_name="FRS" lower="-${PI/2}" upper="${PI/2}"/>
-        <xacro:ros2_control_joint joint_name="BLS" lower="-${PI/2}" upper="${PI/2}"/>
-        <xacro:ros2_control_joint joint_name="BRS" lower="-${PI/2}" upper="${PI/2}"/>
+        <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>
 
diff --git a/src/meldog_simple_description/launch/meldog_gazebo_sim_ros2_control.launch.py b/src/meldog_simple_description/launch/meldog_gazebo_sim_ros2_control.launch.py
index 070949ce..1fde7048 100644
--- a/src/meldog_simple_description/launch/meldog_gazebo_sim_ros2_control.launch.py
+++ b/src/meldog_simple_description/launch/meldog_gazebo_sim_ros2_control.launch.py
@@ -128,14 +128,19 @@ def generate_launch_description():
     )
 
     # Bridge between ros2 and Ignition Gazebo
-    ros2_gazebo_sim_bridge =  IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                [os.path.join(get_package_share_directory('ros_ign_gazebo'),
-                              'launch', 'ign_gazebo.launch.py')]),
-            launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])])
+    # ros2_gazebo_sim_bridge =  IncludeLaunchDescription(
+    #         PythonLaunchDescriptionSource(
+    #             [os.path.join(get_package_share_directory('ros_ign_gazebo'),
+    #                           'launch', 'ign_gazebo.launch.py')]),
+    #         launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])])
+    gazebo_sim_bridge = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([os.path.join(
+            get_package_share_directory('ros_gz_sim'), 'launch'), '/gz_sim.launch.py']),
+            launch_arguments={'gz_args': ['-r -v -v4 ', world], 'on_exit_shutdown': 'true'}.items()
+        )
     
     nodes = [
-        ros2_gazebo_sim_bridge,
+        gazebo_sim_bridge,
 
         RegisterEventHandler(
             event_handler=OnProcessExit(
diff --git a/src/meldog_simple_description/test_controllers/joint_trajectory_controller_test.yaml b/src/meldog_simple_description/test_controllers/joint_trajectory_controller_test.yaml
index 48b75918..c8e730c1 100644
--- a/src/meldog_simple_description/test_controllers/joint_trajectory_controller_test.yaml
+++ b/src/meldog_simple_description/test_controllers/joint_trajectory_controller_test.yaml
@@ -58,62 +58,62 @@ joint_trajectory_controller:
 
     gains:
       FLH_joint:
-        d: 2.5
+        d: 100.0
         i: 0.0
         i_clamp: 0.0
-        p: 350.0
+        p: 1500.0
       FRH_joint:
-        d: 2.5
+        d: 100.0
         i: 0.0
         i_clamp: 0.0
-        p: 350.0
+        p: 1500.0
       BLH_joint:
-        d: 2.5
+        d: 100.0
         i: 0.0
         i_clamp: 0.0
-        p: 350.0
+        p: 1500.0
       BRH_joint:
-        d: 2.5
+        d: 100.0
         i: 0.0
         i_clamp: 0.0
-        p: 350.0
+        p: 1500.0
       FLT_joint:
-        d: 2.5
+        d: 100.0
         i: 0.0
         i_clamp: 0.0
-        p: 350.0
+        p: 1500.0
       FRT_joint:
-        d: 2.5
+        d: 100.0
         i: 0.0
         i_clamp: 0.0
-        p: 350.0
+        p: 1500.0
       BLT_joint:
-        d: 2.5
+        d: 100.0
         i: 0.0
         i_clamp: 0.0
-        p: 350.0
+        p: 1500.0
       BRT_joint:
-        d: 2.5
+        d: 100.0
         i: 0.0
         i_clamp: 0.0
-        p: 350.0
+        p: 1500.0
       FLS_joint:
-        d: 2.5
+        d: 100.0
         i: 0.0
         i_clamp: 0.0
-        p: 350.0
+        p: 1500.0
       FRS_joint:
-        d: 2.5
+        d: 100.0
         i: 0.0
         i_clamp: 0.0
-        p: 350.0
+        p: 1500.0
       BLS_joint:
-        d: 2.5
+        d: 100.0
         i: 0.0
         i_clamp: 0.0
-        p: 350.0
+        p: 1500.0
       BRS_joint:
-        d: 2.5
+        d: 100.0
         i: 0.0
         i_clamp: 0.0
-        p: 350.0
\ No newline at end of file
+        p: 1500.0
\ No newline at end of file
diff --git a/src/meldog_simple_description/worlds/empty.world b/src/meldog_simple_description/worlds/empty.world
index 9794ad0a..4abcd714 100644
--- a/src/meldog_simple_description/worlds/empty.world
+++ b/src/meldog_simple_description/worlds/empty.world
@@ -49,13 +49,13 @@
     </gui>
 
     <physics type="ode">
-      <real_time_update_rate>2000.0</real_time_update_rate>
-      <max_step_size>0.001</max_step_size>
+      <real_time_update_rate>2500.0</real_time_update_rate>
+      <max_step_size>0.0005</max_step_size>
       <real_time_factor>1</real_time_factor>
       <ode>
         <solver>
           <type>quick</type>
-          <iters>150</iters>
+          <iters>200</iters>
           <precon_iters>0</precon_iters>
           <sor>1.400000</sor>
           <use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>

From ddd5b83ada700944b55ebe5c95036d5623c06958 Mon Sep 17 00:00:00 2001
From: Bartek <bartlomiejk@vp.pl>
Date: Sun, 15 Sep 2024 20:38:04 +0200
Subject: [PATCH 20/23] Added example commands for joint_trajectory_controller

---
 .../joint_trajectory_example.txt              | 24 +++++++++++++++++++
 1 file changed, 24 insertions(+)
 create mode 100644 src/meldog_simple_description/simple_commands/joint_trajectory_example.txt

diff --git a/src/meldog_simple_description/simple_commands/joint_trajectory_example.txt b/src/meldog_simple_description/simple_commands/joint_trajectory_example.txt
new file mode 100644
index 00000000..f66b9e64
--- /dev/null
+++ b/src/meldog_simple_description/simple_commands/joint_trajectory_example.txt
@@ -0,0 +1,24 @@
+ POZYCJA STARTOWA:
+ 
+ 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.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], time_from_start: { sec: 1 } },
+        { 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: 6 } }    ]
+    }
+  }"
+  
+
+
+PRZYSIAD:
+
+   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: 3 } },
+        { positions: [0.0, 0.0, 0.0, 0.0, -1.04719, -1.04719, -1.04719, -1.04719, 2.094, 2.094, 2.094, 2.094], time_from_start: { sec: 6 } },
+        { 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 } }    ]
+    }
+  }" 

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 21/23] 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 0000542b..00000000
--- 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 00000000..caed7cfd
--- /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 00000000..8320b567
--- /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 5bee4280..b33e2089 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 734c0903..3c2d1d91 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 00000000..05affec9
--- /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 00000000..b5126c82
--- /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 1cfc9926..5507615d 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 8ffc223f..7505125c 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 f66b9e64..1a3431d6 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 } }    ]
+    }
+  }"

From 7a62db4560cae6f3072637af8293e4fb76301a78 Mon Sep 17 00:00:00 2001
From: Bartek <bartlomiejk@vp.pl>
Date: Mon, 16 Sep 2024 20:21:20 +0200
Subject: [PATCH 22/23] Removed test launch files

---
 .../launch/gazebo.launch.py                   | 33 --------
 .../launch/gazebo_sim.launch.py               | 39 ---------
 .../launch/meldog_gazebo.launch.py            | 83 -------------------
 .../launch/meldog_gazebo_sim.launch.py        | 83 -------------------
 4 files changed, 238 deletions(-)
 delete mode 100644 src/meldog_simple_description/launch/gazebo.launch.py
 delete mode 100644 src/meldog_simple_description/launch/gazebo_sim.launch.py
 delete mode 100644 src/meldog_simple_description/launch/meldog_gazebo.launch.py
 delete mode 100644 src/meldog_simple_description/launch/meldog_gazebo_sim.launch.py

diff --git a/src/meldog_simple_description/launch/gazebo.launch.py b/src/meldog_simple_description/launch/gazebo.launch.py
deleted file mode 100644
index b99c9610..00000000
--- a/src/meldog_simple_description/launch/gazebo.launch.py
+++ /dev/null
@@ -1,33 +0,0 @@
-import os
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, RegisterEventHandler
-from launch.conditions import IfCondition
-from launch.event_handlers import OnProcessExit
-from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
-
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-
-from launch_ros.parameter_descriptions import ParameterValue
-
-from launch_ros.actions import Node
-
-
-def generate_launch_description():
-
-    gazebo = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource([os.path.join(
-            get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']),
-        )
-
-
-
-    # Nodes
-    nodes = [
-        gazebo,
-    ]
-    return LaunchDescription(nodes)
\ No newline at end of file
diff --git a/src/meldog_simple_description/launch/gazebo_sim.launch.py b/src/meldog_simple_description/launch/gazebo_sim.launch.py
deleted file mode 100644
index e9435ffe..00000000
--- a/src/meldog_simple_description/launch/gazebo_sim.launch.py
+++ /dev/null
@@ -1,39 +0,0 @@
-import os
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, RegisterEventHandler
-from launch.conditions import IfCondition
-from launch.event_handlers import OnProcessExit
-from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
-
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-
-from launch_ros.parameter_descriptions import ParameterValue
-
-from launch_ros.actions import Node
-
-
-def generate_launch_description():
-
-    world = os.path.join(
-        get_package_share_directory('meldog_description'),
-        'worlds',
-        'empty.world'
-    )
-    gazebo_sim = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource([os.path.join(
-            get_package_share_directory('ros_gz_sim'), 'launch'), '/gz_sim.launch.py']),
-            launch_arguments={'gz_args': [world], 'on_exit_shutdown': 'true'}.items()
-        )
-
-
-
-    # Nodes
-    nodes = [
-        gazebo_sim,
-    ]
-    return LaunchDescription(nodes)
\ No newline at end of file
diff --git a/src/meldog_simple_description/launch/meldog_gazebo.launch.py b/src/meldog_simple_description/launch/meldog_gazebo.launch.py
deleted file mode 100644
index bc589667..00000000
--- a/src/meldog_simple_description/launch/meldog_gazebo.launch.py
+++ /dev/null
@@ -1,83 +0,0 @@
-import os
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, RegisterEventHandler
-from launch.conditions import IfCondition
-from launch.event_handlers import OnProcessExit
-from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
-
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-
-from launch_ros.parameter_descriptions import ParameterValue
-
-from launch_ros.actions import Node
-
-
-def generate_launch_description():
-
-   # Declare arguments
-    declared_arguments = []
-
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "package",
-            default_value="meldog_description",
-        )
-    )
-
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "urdf_file",
-            default_value="meldog_gazebo.urdf.xacro",
-        )
-    )
-     # Initialize Arguments
-    package_name = LaunchConfiguration("package")
-    urdf_file = LaunchConfiguration("urdf_file")
-
-    # Get URDF via xacro
-    robot_description_content = ParameterValue(
-        Command(
-        [
-            PathJoinSubstitution([FindExecutable(name="xacro")]),
-            " ",
-            PathJoinSubstitution(
-                [
-                    FindPackageShare(package_name),
-                    "description",
-                    urdf_file,
-                ]
-            ),
-        ]
-    ), value_type=str)
-    robot_description = {"robot_description": robot_description_content}
-    use_sim_time = {"use_sim_time": True}
-
-    robot_state_pub_node = Node(
-        package="robot_state_publisher",
-        executable="robot_state_publisher",
-        output="both",
-        parameters=[robot_description, use_sim_time],
-    )
-
-
-    spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
-                    arguments=['-topic', 'robot_description',
-                                '-entity', 'Meldog'],
-                    output='screen')
-
-
-
-
-
-
-    # Nodes
-    nodes = [
-        robot_state_pub_node,
-        spawn_entity,
-    ]
-    return LaunchDescription(declared_arguments + nodes)
\ No newline at end of file
diff --git a/src/meldog_simple_description/launch/meldog_gazebo_sim.launch.py b/src/meldog_simple_description/launch/meldog_gazebo_sim.launch.py
deleted file mode 100644
index ec60d7c7..00000000
--- a/src/meldog_simple_description/launch/meldog_gazebo_sim.launch.py
+++ /dev/null
@@ -1,83 +0,0 @@
-import os
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, RegisterEventHandler
-from launch.conditions import IfCondition
-from launch.event_handlers import OnProcessExit
-from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
-
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-
-from launch_ros.parameter_descriptions import ParameterValue
-
-from launch_ros.actions import Node
-
-
-def generate_launch_description():
-
-   # Declare arguments
-    declared_arguments = []
-
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "package",
-            default_value="meldog_description",
-        )
-    )
-
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "urdf_file",
-            default_value="meldog_gazebo.urdf.xacro",
-        )
-    )
-     # Initialize Arguments
-    package_name = LaunchConfiguration("package")
-    urdf_file = LaunchConfiguration("urdf_file")
-
-    # Get URDF via xacro
-    robot_description_content = ParameterValue(
-        Command(
-        [
-            PathJoinSubstitution([FindExecutable(name="xacro")]),
-            " ",
-            PathJoinSubstitution(
-                [
-                    FindPackageShare(package_name),
-                    "description",
-                    urdf_file,
-                ]
-            ),
-        ]
-    ), value_type=str)
-    robot_description = {"robot_description": robot_description_content}
-    use_sim_time = {"use_sim_time": True}
-
-    robot_state_pub_node = Node(
-        package="robot_state_publisher",
-        executable="robot_state_publisher",
-        output="both",
-        parameters=[robot_description, use_sim_time],
-    )
-
-
-    spawn_entity = Node(package='ros_gz_sim', executable='create',
-                    arguments=['-topic', 'robot_description',
-                                '-name', 'Meldog'],
-                    output='screen')
-
-
-
-
-
-
-    # Nodes
-    nodes = [
-        robot_state_pub_node,
-        spawn_entity,
-    ]
-    return LaunchDescription(declared_arguments + nodes)
\ No newline at end of file

From 165e3498eb78181941b7a965a11179217da86d71 Mon Sep 17 00:00:00 2001
From: Bartek <bartlomiejk@vp.pl>
Date: Tue, 17 Sep 2024 20:28:41 +0200
Subject: [PATCH 23/23] Added/edited comments

---
 .../description/joints/joint_macros.urdf.xacro              | 1 +
 .../description/joints/joint_properties.urdf.xacro          | 4 ++--
 .../description/links/feet/feet.urdf.xacro                  | 6 +++---
 .../description/links/hip/hip.urdf.xacro                    | 3 +--
 .../description/links/shank/shank.urdf.xacro                | 2 +-
 .../description/links/thigh/thigh.urdf.xacro                | 2 +-
 .../description/links/trunk/trunk.urdf.xacro                | 1 +
 .../description/meldog_core.urdf.xacro                      | 6 +++---
 .../description/meldog_gazebo.urdf.xacro                    | 4 ++--
 .../joints_hardware/joint_hardware_macros.urdf.xacro        | 1 +
 .../joints_hardware/joint_hardware_properties.urdf.xacro    | 2 +-
 .../description/ros2_control/ros2_control_core.urdf.xacro   | 6 +++---
 .../description/ros2_control/ros2_control_gazebo.urdf.xacro | 4 ++--
 .../transmission/transmission_macros.urdf.xacro             | 6 +++---
 14 files changed, 25 insertions(+), 23 deletions(-)

diff --git a/src/meldog_simple_description/description/joints/joint_macros.urdf.xacro b/src/meldog_simple_description/description/joints/joint_macros.urdf.xacro
index caed7cfd..604c8032 100644
--- a/src/meldog_simple_description/description/joints/joint_macros.urdf.xacro
+++ b/src/meldog_simple_description/description/joints/joint_macros.urdf.xacro
@@ -1,6 +1,7 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "joint_macros">
 
+    <!-- Joint macro for limits and dynamics -->
     <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}"/>
diff --git a/src/meldog_simple_description/description/joints/joint_properties.urdf.xacro b/src/meldog_simple_description/description/joints/joint_properties.urdf.xacro
index 8320b567..122e1de5 100644
--- a/src/meldog_simple_description/description/joints/joint_properties.urdf.xacro
+++ b/src/meldog_simple_description/description/joints/joint_properties.urdf.xacro
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "joint_properties"> 
     
-    <!-- Limits for joints (lower and upper position): -->
+    <!-- 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}"/>
@@ -13,7 +13,7 @@
     <xacro:property name="joint_max_velocity" value="20.2"/>
     <xacro:property name="joint_max_torque" value="30"/>
 
-    <!-- Joint properties -->
+    <!-- Joint dynamic properties -->
     <xacro:property name="joint_damping" value="0.0"/>
     <xacro:property name="joint_friction" value="0.0"/>
 
diff --git a/src/meldog_simple_description/description/links/feet/feet.urdf.xacro b/src/meldog_simple_description/description/links/feet/feet.urdf.xacro
index a7512e53..29b4263a 100644
--- a/src/meldog_simple_description/description/links/feet/feet.urdf.xacro
+++ b/src/meldog_simple_description/description/links/feet/feet.urdf.xacro
@@ -1,9 +1,9 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name ="meldog_feet">
 
-     <xacro:property name="feet_radius" value="0.06"/>
-     <xacro:property name="feet_mass" value="0.1"/>
-
+    <!-- Meldog feet (simple spheres) -->
+    <xacro:property name="feet_radius" value="0.06"/>
+    <xacro:property name="feet_mass" value="0.1"/>
 
     <xacro:macro name="feet_link" params="position">
         <link name="feet_${position}_link">
diff --git a/src/meldog_simple_description/description/links/hip/hip.urdf.xacro b/src/meldog_simple_description/description/links/hip/hip.urdf.xacro
index 487a65de..02b47890 100644
--- a/src/meldog_simple_description/description/links/hip/hip.urdf.xacro
+++ b/src/meldog_simple_description/description/links/hip/hip.urdf.xacro
@@ -2,11 +2,10 @@
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "meldog_hip">
 
 
-
+    <!-- Meldog hips (simple boxes)-->
     <xacro:property name="hip_length" value="0.2"/>
     <xacro:property name="hip_width" value="0.15"/>
     <xacro:property name="hip_height" value="0.075"/>
-
     <xacro:property name="hip_mass" value="3.0"/>
 
     <xacro:macro name="hip_link" params="position">
diff --git a/src/meldog_simple_description/description/links/shank/shank.urdf.xacro b/src/meldog_simple_description/description/links/shank/shank.urdf.xacro
index f0b0d301..a430226c 100644
--- a/src/meldog_simple_description/description/links/shank/shank.urdf.xacro
+++ b/src/meldog_simple_description/description/links/shank/shank.urdf.xacro
@@ -1,9 +1,9 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "meldog_shank">
 
+    <!-- Meldog shank (simple cylinders) -->
     <xacro:property name="shank_length" value="0.5"/>
     <xacro:property name="shank_radius" value="0.05"/>
-
     <xacro:property name="shank_mass" value = "1.0"/>
 
     <xacro:macro name="shank_link" params="position">
diff --git a/src/meldog_simple_description/description/links/thigh/thigh.urdf.xacro b/src/meldog_simple_description/description/links/thigh/thigh.urdf.xacro
index 5e81f7d6..f4620406 100644
--- a/src/meldog_simple_description/description/links/thigh/thigh.urdf.xacro
+++ b/src/meldog_simple_description/description/links/thigh/thigh.urdf.xacro
@@ -1,9 +1,9 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "meldog_thigh">
 
+    <!-- Meldog thigh (simple cylinders) -->
     <xacro:property name="thigh_length" value="0.5"/>
     <xacro:property name="thigh_radius" value="0.05"/>
-
     <xacro:property name="thigh_mass" value="1.0"/>
 
     <xacro:macro name="thigh_link" params="position">
diff --git a/src/meldog_simple_description/description/links/trunk/trunk.urdf.xacro b/src/meldog_simple_description/description/links/trunk/trunk.urdf.xacro
index 34b2dfba..3b92d811 100644
--- a/src/meldog_simple_description/description/links/trunk/trunk.urdf.xacro
+++ b/src/meldog_simple_description/description/links/trunk/trunk.urdf.xacro
@@ -1,6 +1,7 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "meldog_trunk">
 
+    <!-- Meldog trunk (3 simple boxes) -->
     <xacro:property name="trunk_base_length" value="0.8" />
     <xacro:property name="trunk_base_width" value="0.2" />
     <xacro:property name="trunk_base_height" value="0.1" />
diff --git a/src/meldog_simple_description/description/meldog_core.urdf.xacro b/src/meldog_simple_description/description/meldog_core.urdf.xacro
index b33e2089..383f566e 100644
--- a/src/meldog_simple_description/description/meldog_core.urdf.xacro
+++ b/src/meldog_simple_description/description/meldog_core.urdf.xacro
@@ -4,11 +4,11 @@
     <!-- PI -->
     <xacro:property name="PI" value="3.1415926535897"/>
 
-    <!-- Joint properties and macros: -->
+    <!-- Joint properties and macros -->
     <xacro:include filename="joints/joint_properties.urdf.xacro"/>
     <xacro:include filename="joints/joint_macros.urdf.xacro"/>
 
-    <!-- LINKS: -->
+    <!-- LINKS -->
     <!-- Base link spawns in x= 0.0 y = 0.0 z = 0.0 -->
     <link name="base_link"></link>
 
@@ -45,7 +45,7 @@
     <xacro:feet_link position="back_left"/>
     <xacro:feet_link position="back_right"/>
 
-    <!-- JOINTS: -->   
+    <!-- JOINTS -->   
 
     <!-- Base to Trunk joint -->
     <joint name="BT_joint" type="fixed">
diff --git a/src/meldog_simple_description/description/meldog_gazebo.urdf.xacro b/src/meldog_simple_description/description/meldog_gazebo.urdf.xacro
index af40456f..9c8b37b7 100644
--- a/src/meldog_simple_description/description/meldog_gazebo.urdf.xacro
+++ b/src/meldog_simple_description/description/meldog_gazebo.urdf.xacro
@@ -7,7 +7,7 @@
     <!-- Gazebo macros -->
     <xacro:include filename="gazebo/gazebo_macros.urdf.xacro"/>
 
-    <!-- LINKS: -->
+    <!-- LINKS -->
     <!-- Trunk link -->
     <xacro:gazebo_link link_name="trunk" color="Blue" selfCollide_value ="true"/>
 
@@ -36,7 +36,7 @@
     <xacro:gazebo_link link_name="feet_back_right" color="Pink" selfCollide_value ="true"/>
 
 
-    <!-- JOINTS (only revolute): -->
+    <!-- JOINTS (only revolute) -->
     <!-- Trunk to Hips joints-->
     <xacro:gazebo_joint joint_name="FLH"/>
     <xacro:gazebo_joint joint_name="FRH"/>
diff --git a/src/meldog_simple_description/description/ros2_control/joints_hardware/joint_hardware_macros.urdf.xacro b/src/meldog_simple_description/description/ros2_control/joints_hardware/joint_hardware_macros.urdf.xacro
index 3c2d1d91..ac32c422 100644
--- a/src/meldog_simple_description/description/ros2_control/joints_hardware/joint_hardware_macros.urdf.xacro
+++ b/src/meldog_simple_description/description/ros2_control/joints_hardware/joint_hardware_macros.urdf.xacro
@@ -1,6 +1,7 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name ="joint_hardware_macros">
 
+    <!-- JMotor and actuator macro for ros2_control hardware interface -->
     <xacro:macro name="joint_hardware_macro" params="joint_name upper lower">
         <joint name="${joint_name}_joint">
             <command_interface name="position">
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
index 05affec9..f2c9b8f6 100644
--- 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
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name ="joint_hardware_properties">
 
-    <!-- Actuator and motor parameters: -->
+    <!-- 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}"/>
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
index b5126c82..0862edaf 100644
--- 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
@@ -1,14 +1,14 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "meldog_ros2_control_core">
 
-    <!-- Joint hardware properties and macros: -->
+    <!-- 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: -->
+    <!-- Transmissions -->
     <xacro:include filename="transmission/transmission_macros.urdf.xacro"/>
 
-    <!-- Hardware interface for Gazebo: -->
+    <!-- Hardware interface for Gazebo -->
 
     <!-- Trunk to Hips joints-->
     <xacro:joint_hardware_macro joint_name="FLH" lower="${motor_lower_limit}" upper="${motor_upper_limit}"/>
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 5507615d..5c9ec2ea 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,13 +1,13 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name = "meldog_ros2_control_gazebo">
 
-    <!-- ROS2 Control for Gazebo: -->
+    <!-- ROS2 Control for Gazebo -->
     <ros2_control name="meldog_gazebo" type="system">
         <hardware>  
             <plugin>ign_ros2_control/IgnitionSystem</plugin>
         </hardware>
 
-        <!-- ROS2 Control core xacro file: -->
+        <!-- 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/transmission_macros.urdf.xacro b/src/meldog_simple_description/description/ros2_control/transmission/transmission_macros.urdf.xacro
index 7505125c..71cab3f9 100644
--- a/src/meldog_simple_description/description/ros2_control/transmission/transmission_macros.urdf.xacro
+++ b/src/meldog_simple_description/description/ros2_control/transmission/transmission_macros.urdf.xacro
@@ -34,15 +34,15 @@
         </transmission>
     </xacro:macro>
 
-    <!-- Meldog Transmissions: -->
+    <!-- Meldog Transmissions -->
 
-    <!-- Trunk to Hips transmissions: -->
+    <!-- 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: -->
+    <!-- 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"/>