From de39a5169e565343f7a519485ee1f2d5fc1738f8 Mon Sep 17 00:00:00 2001
From: gaudelbijay <gaudelbijay10@gmail.com>
Date: Fri, 31 Oct 2025 12:17:43 -0400
Subject: [PATCH 1/4] added ros2 (jazzy) stack for gazebo simulation on
 clearpath_manipulator_description/universal_robots.urdf.xacro

---
 .../urdf/arm/universal_robots.urdf.xacro      | 263 +++++++++++++-----
 1 file changed, 193 insertions(+), 70 deletions(-)

diff --git a/clearpath_manipulators_description/urdf/arm/universal_robots.urdf.xacro b/clearpath_manipulators_description/urdf/arm/universal_robots.urdf.xacro
index 483c0d1e..3a09420a 100644
--- a/clearpath_manipulators_description/urdf/arm/universal_robots.urdf.xacro
+++ b/clearpath_manipulators_description/urdf/arm/universal_robots.urdf.xacro
@@ -48,6 +48,7 @@
     sim_ignition:=''
     ">
     <xacro:property name="use_manipulation_controllers" value="$(arg use_manipulation_controllers)"/>
+
     <!-- Default Description Parameters -->
     <xacro:property name="_tf_prefix" value="${name}_" lazy_eval="false"/>
     <xacro:property name="_initial_positions" value="${dict(shoulder_pan_joint=0.0,shoulder_lift_joint=-1.57,elbow_joint=0.0,wrist_1_joint=-1.57,wrist_2_joint=0.0,wrist_3_joint=0.0)}" lazy_eval="false"/>
@@ -58,28 +59,56 @@
     <xacro:property name="_safety_limits" value="false" lazy_eval="false"/>
     <xacro:property name="_safety_pos_margin" value="0.15" lazy_eval="false"/>
     <xacro:property name="_safety_k_position" value="20" lazy_eval="false"/>
+
     <!-- Default Description Parameters Overwrites -->
-    <xacro:if value="${initial_positions != ''}"> <xacro:property name="_initial_positions" value="${initial_positions}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${initial_positions != ''}">
+      <xacro:property name="_initial_positions" value="${initial_positions}" lazy_eval="false"/>
+    </xacro:if>
     <!-- if initial positions file is passed, overwrite initial positions parameter -->
-    <xacro:if value="${initial_positions_file != ''}"> <xacro:property name="_initial_positions" value="${xacro.load_yaml(initial_positions_file)}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${joint_limits_parameters_file != ''}"> <xacro:property name="_joint_limits_parameters_file" value="${joint_limits_parameters_file}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${kinematics_parameters_file != ''}"> <xacro:property name="_kinematics_parameters_file" value="${kinematics_parameters_file}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${physical_parameters_file != ''}"> <xacro:property name="_physical_parameters_file" value="${physical_parameters_file}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${visual_parameters_file != ''}"> <xacro:property name="$_visual_parameters_file" value="${visual_parameters_file}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${safety_limits != ''}"> <xacro:property name="_safety_limits" value="${safety_limits}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${safety_pos_margin != ''}"> <xacro:property name="_safety_pos_margin" value="${safety_pos_margin}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${safety_k_position != ''}"> <xacro:property name="_safety_k_position" value="${safety_k_position}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${initial_positions_file != ''}">
+      <xacro:property name="_initial_positions" value="${xacro.load_yaml(initial_positions_file)}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${joint_limits_parameters_file != ''}">
+      <xacro:property name="_joint_limits_parameters_file" value="${joint_limits_parameters_file}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${kinematics_parameters_file != ''}">
+      <xacro:property name="_kinematics_parameters_file" value="${kinematics_parameters_file}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${physical_parameters_file != ''}">
+      <xacro:property name="_physical_parameters_file" value="${physical_parameters_file}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${visual_parameters_file != ''}">
+      <xacro:property name="_visual_parameters_file" value="${visual_parameters_file}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${safety_limits != ''}">
+      <xacro:property name="_safety_limits" value="${safety_limits}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${safety_pos_margin != ''}">
+      <xacro:property name="_safety_pos_margin" value="${safety_pos_margin}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${safety_k_position != ''}">
+      <xacro:property name="_safety_k_position" value="${safety_k_position}" lazy_eval="false"/>
+    </xacro:if>
 
     <!-- Default Simulation Parameters -->
     <xacro:property name="_use_mock_hardware" value="false" />
     <xacro:property name="_mock_sensor_commands" value="false" />
     <xacro:property name="_sim_gazebo" value="false" />
     <xacro:property name="_sim_ignition" value="$(arg is_sim)" />
+
     <!-- Default Simulation Parameters Overwrites -->
-    <xacro:if value="${use_mock_hardware != ''}"> <xacro:property name="_use_mock_hardware" value="${use_mock_hardware}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${mock_sensor_commands != ''}"> <xacro:property name="_mock_sensor_commands" value="${mock_sensor_commands}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${sim_gazebo != ''}"> <xacro:property name="_sim_gazebo" value="${sim_gazebo}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${sim_ignition != ''}"> <xacro:property name="_sim_ignition" value="${sim_ignition}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${use_mock_hardware != ''}">
+      <xacro:property name="_use_mock_hardware" value="${use_mock_hardware}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${mock_sensor_commands != ''}">
+      <xacro:property name="_mock_sensor_commands" value="${mock_sensor_commands}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${sim_gazebo != ''}">
+      <xacro:property name="_sim_gazebo" value="${sim_gazebo}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${sim_ignition != ''}">
+      <xacro:property name="_sim_ignition" value="${sim_ignition}" lazy_eval="false"/>
+    </xacro:if>
 
     <!-- Default ros2_control Parameters -->
     <xacro:property name="_headless_mode" value="false" lazy_eval="false"/>
@@ -92,17 +121,39 @@
     <xacro:property name="_transmission_hw_interface" value="hardware_interface/PositionJointInterface" lazy_eval="false"/>
     <xacro:property name="_non_blocking_read" value="true" lazy_eval="false"/>
     <xacro:property name="_keep_alive_count" value="2.0" lazy_eval="false"/>
+
     <!-- Default ros2_control Parameters Overwrites -->
-    <xacro:if value="${headless_mode != ''}"> <xacro:property name="_headless_mode" value="${headless_mode}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${robot_ip != ''}"> <xacro:property name="_robot_ip" value="${robot_ip}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${reverse_ip != ''}"> <xacro:property name="_reverse_ip" value="${reverse_ip}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${script_command_port != ''}"> <xacro:property name="_script_command_port" value="${script_command_port}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${reverse_port != ''}"> <xacro:property name="_reverse_port" value="${reverse_port}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${script_sender_port != ''}"> <xacro:property name="_script_sender_port" value="${script_sender_port}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${trajectory_port != ''}"> <xacro:property name="_trajectory_port" value="${trajectory_port}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${transmission_hw_interface != ''}"> <xacro:property name="_transmission_hw_interface" value="${transmission_hw_interface}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${non_blocking_read != ''}"> <xacro:property name="_non_blocking_read" value="${non_blocking_read}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${keep_alive_count != ''}"> <xacro:property name="_keep_alive_count" value="${keep_alive_count}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${headless_mode != ''}">
+      <xacro:property name="_headless_mode" value="${headless_mode}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${robot_ip != ''}">
+      <xacro:property name="_robot_ip" value="${robot_ip}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${reverse_ip != ''}">
+      <xacro:property name="_reverse_ip" value="${reverse_ip}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${script_command_port != ''}">
+      <xacro:property name="_script_command_port" value="${script_command_port}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${reverse_port != ''}">
+      <xacro:property name="_reverse_port" value="${reverse_port}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${script_sender_port != ''}">
+      <xacro:property name="_script_sender_port" value="${script_sender_port}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${trajectory_port != ''}">
+      <xacro:property name="_trajectory_port" value="${trajectory_port}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${transmission_hw_interface != ''}">
+      <xacro:property name="_transmission_hw_interface" value="${transmission_hw_interface}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${non_blocking_read != ''}">
+      <xacro:property name="_non_blocking_read" value="${non_blocking_read}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${keep_alive_count != ''}">
+      <xacro:property name="_keep_alive_count" value="${keep_alive_count}" lazy_eval="false"/>
+    </xacro:if>
+
     <!-- Driver Dependent Parameters -->
     <xacro:if value="${use_manipulation_controllers and not _use_mock_hardware and not _sim_ignition}">
       <xacro:property name="_script_filename" value="$(find ur_client_library)/resources/external_control.urscript" lazy_eval="false"/>
@@ -114,10 +165,17 @@
       <xacro:property name="_output_recipe_filename" value="" lazy_eval="false"/>
       <xacro:property name="_input_recipe_filename" value="" lazy_eval="false"/>
     </xacro:unless>
+
     <!-- Driver Dependent Parameters Overwrites -->
-    <xacro:if value="${script_filename != ''}"> <xacro:property name="_script_filename" value="${script_filename}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${output_recipe_filename != ''}"> <xacro:property name="_output_recipe_filename" value="${output_recipe_filename}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${input_recipe_filename != ''}"> <xacro:property name="_input_recipe_filename" value="${input_recipe_filename}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${script_filename != ''}">
+      <xacro:property name="_script_filename" value="${script_filename}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${output_recipe_filename != ''}">
+      <xacro:property name="_output_recipe_filename" value="${output_recipe_filename}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${input_recipe_filename != ''}">
+      <xacro:property name="_input_recipe_filename" value="${input_recipe_filename}" lazy_eval="false"/>
+    </xacro:if>
 
     <!-- Default Tool Communication Parameters -->
     <xacro:property name="_use_tool_communication" value="false" lazy_eval="false"/>
@@ -129,16 +187,35 @@
     <xacro:property name="_tool_tx_idle_chars" value="3.5" lazy_eval="false"/>
     <xacro:property name="_tool_device_name" value="/tmp/ttyUR" lazy_eval="false"/>
     <xacro:property name="_tool_tcp_port" value="54321" lazy_eval="false"/>
+
     <!-- Tool Communication Parameter Overwrites -->
-    <xacro:if value="${use_tool_communication != ''}"> <xacro:property name="_use_tool_communication" value="${use_tool_communication}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${tool_voltage != ''}"> <xacro:property name="_tool_voltage" value="${tool_voltage}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${tool_parity != ''}"> <xacro:property name="_tool_parity" value="${tool_parity}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${tool_baud_rate != ''}"> <xacro:property name="_tool_baud_rate" value="${tool_baud_rate}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${tool_stop_bits != ''}"> <xacro:property name="_tool_stop_bits" value="${tool_stop_bits}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${tool_rx_idle_chars != ''}"> <xacro:property name="_tool_rx_idle_chars" value="${tool_rx_idle_chars}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${tool_tx_idle_chars != ''}"> <xacro:property name="_tool_tx_idle_chars" value="${tool_tx_idle_chars}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${tool_device_name != ''}"> <xacro:property name="_tool_device_name" value="${tool_device_name}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${tool_tcp_port != ''}"> <xacro:property name="_tool_tcp_port" value="${tool_tcp_port}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${use_tool_communication != ''}">
+      <xacro:property name="_use_tool_communication" value="${use_tool_communication}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${tool_voltage != ''}">
+      <xacro:property name="_tool_voltage" value="${tool_voltage}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${tool_parity != ''}">
+      <xacro:property name="_tool_parity" value="${tool_parity}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${tool_baud_rate != ''}">
+      <xacro:property name="_tool_baud_rate" value="${tool_baud_rate}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${tool_stop_bits != ''}">
+      <xacro:property name="_tool_stop_bits" value="${tool_stop_bits}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${tool_rx_idle_chars != ''}">
+      <xacro:property name="_tool_rx_idle_chars" value="${tool_rx_idle_chars}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${tool_tx_idle_chars != ''}">
+      <xacro:property name="_tool_tx_idle_chars" value="${tool_tx_idle_chars}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${tool_device_name != ''}">
+      <xacro:property name="_tool_device_name" value="${tool_device_name}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${tool_tcp_port != ''}">
+      <xacro:property name="_tool_tcp_port" value="${tool_tcp_port}" lazy_eval="false"/>
+    </xacro:if>
 
     <!-- Universal Robots ROS2 Description -->
     <xacro:ur_robot
@@ -151,43 +228,89 @@
       visual_parameters_file="${_visual_parameters_file}"
       safety_limits="${_safety_limits}"
       safety_pos_margin="${_safety_pos_margin}"
-      safety_k_position="${_safety_k_position}"
-      >
+      safety_k_position="${_safety_k_position}">
       <xacro:insert_block name="origin"/>
     </xacro:ur_robot>
 
-    <!-- Universal_Robots_ROS2_Driver HardwareInterface -->
-    <xacro:if value="${use_manipulation_controllers}">
-      <xacro:include filename="$(find ur_robot_driver)/urdf/ur.ros2_control.xacro" />
-
-      <xacro:ur_ros2_control
-        name="${name}"
-        tf_prefix="${_tf_prefix}"
-        kinematics_parameters_file="${_kinematics_parameters_file}"
-        transmission_hw_interface="${_transmission_hw_interface}"
-        use_mock_hardware="${_use_mock_hardware}"
-        mock_sensor_commands="${_mock_sensor_commands}"
-        headless_mode="${_headless_mode}"
-        initial_positions="${_initial_positions}"
-        use_tool_communication="${_use_tool_communication}"
-        tool_voltage="${_tool_voltage}"
-        tool_parity="${_tool_parity}"
-        tool_baud_rate="${_tool_baud_rate}"
-        tool_stop_bits="${_tool_stop_bits}"
-        tool_rx_idle_chars="${_tool_rx_idle_chars}"
-        tool_tx_idle_chars="${_tool_tx_idle_chars}"
-        tool_device_name="${_tool_device_name}"
-        tool_tcp_port="${_tool_tcp_port}"
-        robot_ip="${_robot_ip}"
-        script_filename="${_script_filename}"
-        output_recipe_filename="${_output_recipe_filename}"
-        input_recipe_filename="${_input_recipe_filename}"
-        reverse_ip="${_reverse_ip}"
-        script_command_port="${_script_command_port}"
-        reverse_port="${_reverse_port}"
-        script_sender_port="${_script_sender_port}"
-        trajectory_port="${_trajectory_port}"
-      />
+    <!-- Use the right ros2_control depending on sim vs real -->
+
+    <xacro:if value="${_sim_ignition}">
+      <ros2_control name="${name}_hardware" type="system">
+        <hardware>
+          <plugin>gz_ros2_control/GazeboSimSystem</plugin>
+        </hardware>
+
+        <joint name="${name}_shoulder_pan_joint">
+          <command_interface name="position"/>
+          <state_interface name="position"/>
+          <state_interface name="velocity"/>
+        </joint>
+        <joint name="${name}_shoulder_lift_joint">
+          <command_interface name="position"/>
+          <state_interface name="position"/>
+          <state_interface name="velocity"/>
+        </joint>
+        <joint name="${name}_elbow_joint">
+          <command_interface name="position"/>
+          <state_interface name="position"/>
+          <state_interface name="velocity"/>
+        </joint>
+        <joint name="${name}_wrist_1_joint">
+          <command_interface name="position"/>
+          <state_interface name="position"/>
+          <state_interface name="velocity"/>
+        </joint>
+        <joint name="${name}_wrist_2_joint">
+          <command_interface name="position"/>
+          <state_interface name="position"/>
+          <state_interface name="velocity"/>
+        </joint>
+        <joint name="${name}_wrist_3_joint">
+          <command_interface name="position"/>
+          <state_interface name="position"/>
+          <state_interface name="velocity"/>
+        </joint>
+      </ros2_control>
+
+      <gazebo>
+        <plugin filename="gz_ros2_control-system"
+                name="gz_ros2_control::GazeboSimROS2ControlPlugin"/>
+      </gazebo>
     </xacro:if>
+
+    <!-- REAL ROBOT: only include the UR driver when NOT sim -->
+    <xacro:unless value="${_sim_ignition}">
+      <xacro:if value="${use_manipulation_controllers}">
+        <xacro:include filename="$(find ur_robot_driver)/urdf/ur.ros2_control.xacro" />
+        <xacro:ur_ros2_control
+          name="${name}"
+          tf_prefix="${_tf_prefix}"
+          kinematics_parameters_file="${_kinematics_parameters_file}"
+          transmission_hw_interface="${_transmission_hw_interface}"
+          use_mock_hardware="${_use_mock_hardware}"
+          mock_sensor_commands="${_mock_sensor_commands}"
+          headless_mode="${_headless_mode}"
+          initial_positions="${_initial_positions}"
+          use_tool_communication="${_use_tool_communication}"
+          tool_voltage="${_tool_voltage}"
+          tool_parity="${_tool_parity}"
+          tool_baud_rate="${_tool_baud_rate}"
+          tool_stop_bits="${_tool_stop_bits}"
+          tool_rx_idle_chars="${_tool_rx_idle_chars}"
+          tool_tx_idle_chars="${_tool_tx_idle_chars}"
+          tool_device_name="${_tool_device_name}"
+          tool_tcp_port="${_tool_tcp_port}"
+          robot_ip="${_robot_ip}"
+          script_filename="${_script_filename}"
+          output_recipe_filename="${_output_recipe_filename}"
+          input_recipe_filename="${_input_recipe_filename}"
+          reverse_ip="${_reverse_ip}"
+          script_command_port="${_script_command_port}"
+          reverse_port="${_reverse_port}"
+          script_sender_port="${_script_sender_port}"
+          trajectory_port="${_trajectory_port}"/>
+      </xacro:if>
+    </xacro:unless>
+
   </xacro:macro>
-</robot>
+</robot>
\ No newline at end of file

From 13805f28750e89bf8077a0c6dd4e5019ce0e4d64 Mon Sep 17 00:00:00 2001
From: Bijay Gaudel <57655873+gaudelbijay@users.noreply.github.com>
Date: Fri, 31 Oct 2025 12:21:55 -0400
Subject: [PATCH 2/4] Add ROS 2 Jazzy compatibility for Gazebo simulation

This update integrates ROS 2 Jazzy compatibility into the Gazebo simulation stack, enhancing the clearpath_manipulator_description package with necessary configuration files and updates.
---
 README.md | 9 +++++++++
 1 file changed, 9 insertions(+)
 create mode 100644 README.md

diff --git a/README.md b/README.md
new file mode 100644
index 00000000..49be4002
--- /dev/null
+++ b/README.md
@@ -0,0 +1,9 @@
+This update adds ROS 2 Jazzy compatibility for the Gazebo simulation stack within the clearpath_manipulator_description package. Specifically, it integrates the Universal Robots URDF/Xacro file with ROS 2’s control and Gazebo Sim (Harmonic) interfaces.
+
+### 🧩 Key Changes
+
+Added ROS 2 Jazzy–compatible simulation configuration files.
+
+Updated universal_robots.urdf.xacro to support ROS 2 control (ros2_control) and Gazebo plugins.
+
+Ensured compatibility with the Clearpath manipulator stack for ROS 2 (Jazzy).

From 9e1df88fd3656dbf809ba8a599162e3ffaecfbfd Mon Sep 17 00:00:00 2001
From: gaudelbijay <gaudelbijay10@gmail.com>
Date: Mon, 3 Nov 2025 11:46:12 -0500
Subject: [PATCH 3/4] added ros2 (jazzy) stack for gazebo simulation on
 clearpath_manipulator_description/universal_robots.urdf.xacro

---
 .../urdf/arm/universal_robots.urdf.xacro      | 227 ++++++------------
 1 file changed, 76 insertions(+), 151 deletions(-)

diff --git a/clearpath_manipulators_description/urdf/arm/universal_robots.urdf.xacro b/clearpath_manipulators_description/urdf/arm/universal_robots.urdf.xacro
index 3a09420a..1ddbe013 100644
--- a/clearpath_manipulators_description/urdf/arm/universal_robots.urdf.xacro
+++ b/clearpath_manipulators_description/urdf/arm/universal_robots.urdf.xacro
@@ -48,7 +48,6 @@
     sim_ignition:=''
     ">
     <xacro:property name="use_manipulation_controllers" value="$(arg use_manipulation_controllers)"/>
-
     <!-- Default Description Parameters -->
     <xacro:property name="_tf_prefix" value="${name}_" lazy_eval="false"/>
     <xacro:property name="_initial_positions" value="${dict(shoulder_pan_joint=0.0,shoulder_lift_joint=-1.57,elbow_joint=0.0,wrist_1_joint=-1.57,wrist_2_joint=0.0,wrist_3_joint=0.0)}" lazy_eval="false"/>
@@ -59,56 +58,28 @@
     <xacro:property name="_safety_limits" value="false" lazy_eval="false"/>
     <xacro:property name="_safety_pos_margin" value="0.15" lazy_eval="false"/>
     <xacro:property name="_safety_k_position" value="20" lazy_eval="false"/>
-
     <!-- Default Description Parameters Overwrites -->
-    <xacro:if value="${initial_positions != ''}">
-      <xacro:property name="_initial_positions" value="${initial_positions}" lazy_eval="false"/>
-    </xacro:if>
+    <xacro:if value="${initial_positions != ''}"> <xacro:property name="_initial_positions" value="${initial_positions}" lazy_eval="false"/> </xacro:if>
     <!-- if initial positions file is passed, overwrite initial positions parameter -->
-    <xacro:if value="${initial_positions_file != ''}">
-      <xacro:property name="_initial_positions" value="${xacro.load_yaml(initial_positions_file)}" lazy_eval="false"/>
-    </xacro:if>
-    <xacro:if value="${joint_limits_parameters_file != ''}">
-      <xacro:property name="_joint_limits_parameters_file" value="${joint_limits_parameters_file}" lazy_eval="false"/>
-    </xacro:if>
-    <xacro:if value="${kinematics_parameters_file != ''}">
-      <xacro:property name="_kinematics_parameters_file" value="${kinematics_parameters_file}" lazy_eval="false"/>
-    </xacro:if>
-    <xacro:if value="${physical_parameters_file != ''}">
-      <xacro:property name="_physical_parameters_file" value="${physical_parameters_file}" lazy_eval="false"/>
-    </xacro:if>
-    <xacro:if value="${visual_parameters_file != ''}">
-      <xacro:property name="_visual_parameters_file" value="${visual_parameters_file}" lazy_eval="false"/>
-    </xacro:if>
-    <xacro:if value="${safety_limits != ''}">
-      <xacro:property name="_safety_limits" value="${safety_limits}" lazy_eval="false"/>
-    </xacro:if>
-    <xacro:if value="${safety_pos_margin != ''}">
-      <xacro:property name="_safety_pos_margin" value="${safety_pos_margin}" lazy_eval="false"/>
-    </xacro:if>
-    <xacro:if value="${safety_k_position != ''}">
-      <xacro:property name="_safety_k_position" value="${safety_k_position}" lazy_eval="false"/>
-    </xacro:if>
+    <xacro:if value="${initial_positions_file != ''}"> <xacro:property name="_initial_positions" value="${xacro.load_yaml(initial_positions_file)}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${joint_limits_parameters_file != ''}"> <xacro:property name="_joint_limits_parameters_file" value="${joint_limits_parameters_file}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${kinematics_parameters_file != ''}"> <xacro:property name="_kinematics_parameters_file" value="${kinematics_parameters_file}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${physical_parameters_file != ''}"> <xacro:property name="_physical_parameters_file" value="${physical_parameters_file}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${visual_parameters_file != ''}"> <xacro:property name="$_visual_parameters_file" value="${visual_parameters_file}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${safety_limits != ''}"> <xacro:property name="_safety_limits" value="${safety_limits}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${safety_pos_margin != ''}"> <xacro:property name="_safety_pos_margin" value="${safety_pos_margin}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${safety_k_position != ''}"> <xacro:property name="_safety_k_position" value="${safety_k_position}" lazy_eval="false"/> </xacro:if>
 
     <!-- Default Simulation Parameters -->
     <xacro:property name="_use_mock_hardware" value="false" />
     <xacro:property name="_mock_sensor_commands" value="false" />
     <xacro:property name="_sim_gazebo" value="false" />
     <xacro:property name="_sim_ignition" value="$(arg is_sim)" />
-
     <!-- Default Simulation Parameters Overwrites -->
-    <xacro:if value="${use_mock_hardware != ''}">
-      <xacro:property name="_use_mock_hardware" value="${use_mock_hardware}" lazy_eval="false"/>
-    </xacro:if>
-    <xacro:if value="${mock_sensor_commands != ''}">
-      <xacro:property name="_mock_sensor_commands" value="${mock_sensor_commands}" lazy_eval="false"/>
-    </xacro:if>
-    <xacro:if value="${sim_gazebo != ''}">
-      <xacro:property name="_sim_gazebo" value="${sim_gazebo}" lazy_eval="false"/>
-    </xacro:if>
-    <xacro:if value="${sim_ignition != ''}">
-      <xacro:property name="_sim_ignition" value="${sim_ignition}" lazy_eval="false"/>
-    </xacro:if>
+    <xacro:if value="${use_mock_hardware != ''}"> <xacro:property name="_use_mock_hardware" value="${use_mock_hardware}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${mock_sensor_commands != ''}"> <xacro:property name="_mock_sensor_commands" value="${mock_sensor_commands}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${sim_gazebo != ''}"> <xacro:property name="_sim_gazebo" value="${sim_gazebo}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${sim_ignition != ''}"> <xacro:property name="_sim_ignition" value="${sim_ignition}" lazy_eval="false"/> </xacro:if>
 
     <!-- Default ros2_control Parameters -->
     <xacro:property name="_headless_mode" value="false" lazy_eval="false"/>
@@ -121,39 +92,17 @@
     <xacro:property name="_transmission_hw_interface" value="hardware_interface/PositionJointInterface" lazy_eval="false"/>
     <xacro:property name="_non_blocking_read" value="true" lazy_eval="false"/>
     <xacro:property name="_keep_alive_count" value="2.0" lazy_eval="false"/>
-
     <!-- Default ros2_control Parameters Overwrites -->
-    <xacro:if value="${headless_mode != ''}">
-      <xacro:property name="_headless_mode" value="${headless_mode}" lazy_eval="false"/>
-    </xacro:if>
-    <xacro:if value="${robot_ip != ''}">
-      <xacro:property name="_robot_ip" value="${robot_ip}" lazy_eval="false"/>
-    </xacro:if>
-    <xacro:if value="${reverse_ip != ''}">
-      <xacro:property name="_reverse_ip" value="${reverse_ip}" lazy_eval="false"/>
-    </xacro:if>
-    <xacro:if value="${script_command_port != ''}">
-      <xacro:property name="_script_command_port" value="${script_command_port}" lazy_eval="false"/>
-    </xacro:if>
-    <xacro:if value="${reverse_port != ''}">
-      <xacro:property name="_reverse_port" value="${reverse_port}" lazy_eval="false"/>
-    </xacro:if>
-    <xacro:if value="${script_sender_port != ''}">
-      <xacro:property name="_script_sender_port" value="${script_sender_port}" lazy_eval="false"/>
-    </xacro:if>
-    <xacro:if value="${trajectory_port != ''}">
-      <xacro:property name="_trajectory_port" value="${trajectory_port}" lazy_eval="false"/>
-    </xacro:if>
-    <xacro:if value="${transmission_hw_interface != ''}">
-      <xacro:property name="_transmission_hw_interface" value="${transmission_hw_interface}" lazy_eval="false"/>
-    </xacro:if>
-    <xacro:if value="${non_blocking_read != ''}">
-      <xacro:property name="_non_blocking_read" value="${non_blocking_read}" lazy_eval="false"/>
-    </xacro:if>
-    <xacro:if value="${keep_alive_count != ''}">
-      <xacro:property name="_keep_alive_count" value="${keep_alive_count}" lazy_eval="false"/>
-    </xacro:if>
-
+    <xacro:if value="${headless_mode != ''}"> <xacro:property name="_headless_mode" value="${headless_mode}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${robot_ip != ''}"> <xacro:property name="_robot_ip" value="${robot_ip}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${reverse_ip != ''}"> <xacro:property name="_reverse_ip" value="${reverse_ip}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${script_command_port != ''}"> <xacro:property name="_script_command_port" value="${script_command_port}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${reverse_port != ''}"> <xacro:property name="_reverse_port" value="${reverse_port}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${script_sender_port != ''}"> <xacro:property name="_script_sender_port" value="${script_sender_port}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${trajectory_port != ''}"> <xacro:property name="_trajectory_port" value="${trajectory_port}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${transmission_hw_interface != ''}"> <xacro:property name="_transmission_hw_interface" value="${transmission_hw_interface}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${non_blocking_read != ''}"> <xacro:property name="_non_blocking_read" value="${non_blocking_read}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${keep_alive_count != ''}"> <xacro:property name="_keep_alive_count" value="${keep_alive_count}" lazy_eval="false"/> </xacro:if>
     <!-- Driver Dependent Parameters -->
     <xacro:if value="${use_manipulation_controllers and not _use_mock_hardware and not _sim_ignition}">
       <xacro:property name="_script_filename" value="$(find ur_client_library)/resources/external_control.urscript" lazy_eval="false"/>
@@ -165,17 +114,10 @@
       <xacro:property name="_output_recipe_filename" value="" lazy_eval="false"/>
       <xacro:property name="_input_recipe_filename" value="" lazy_eval="false"/>
     </xacro:unless>
-
     <!-- Driver Dependent Parameters Overwrites -->
-    <xacro:if value="${script_filename != ''}">
-      <xacro:property name="_script_filename" value="${script_filename}" lazy_eval="false"/>
-    </xacro:if>
-    <xacro:if value="${output_recipe_filename != ''}">
-      <xacro:property name="_output_recipe_filename" value="${output_recipe_filename}" lazy_eval="false"/>
-    </xacro:if>
-    <xacro:if value="${input_recipe_filename != ''}">
-      <xacro:property name="_input_recipe_filename" value="${input_recipe_filename}" lazy_eval="false"/>
-    </xacro:if>
+    <xacro:if value="${script_filename != ''}"> <xacro:property name="_script_filename" value="${script_filename}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${output_recipe_filename != ''}"> <xacro:property name="_output_recipe_filename" value="${output_recipe_filename}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${input_recipe_filename != ''}"> <xacro:property name="_input_recipe_filename" value="${input_recipe_filename}" lazy_eval="false"/> </xacro:if>
 
     <!-- Default Tool Communication Parameters -->
     <xacro:property name="_use_tool_communication" value="false" lazy_eval="false"/>
@@ -187,35 +129,16 @@
     <xacro:property name="_tool_tx_idle_chars" value="3.5" lazy_eval="false"/>
     <xacro:property name="_tool_device_name" value="/tmp/ttyUR" lazy_eval="false"/>
     <xacro:property name="_tool_tcp_port" value="54321" lazy_eval="false"/>
-
     <!-- Tool Communication Parameter Overwrites -->
-    <xacro:if value="${use_tool_communication != ''}">
-      <xacro:property name="_use_tool_communication" value="${use_tool_communication}" lazy_eval="false"/>
-    </xacro:if>
-    <xacro:if value="${tool_voltage != ''}">
-      <xacro:property name="_tool_voltage" value="${tool_voltage}" lazy_eval="false"/>
-    </xacro:if>
-    <xacro:if value="${tool_parity != ''}">
-      <xacro:property name="_tool_parity" value="${tool_parity}" lazy_eval="false"/>
-    </xacro:if>
-    <xacro:if value="${tool_baud_rate != ''}">
-      <xacro:property name="_tool_baud_rate" value="${tool_baud_rate}" lazy_eval="false"/>
-    </xacro:if>
-    <xacro:if value="${tool_stop_bits != ''}">
-      <xacro:property name="_tool_stop_bits" value="${tool_stop_bits}" lazy_eval="false"/>
-    </xacro:if>
-    <xacro:if value="${tool_rx_idle_chars != ''}">
-      <xacro:property name="_tool_rx_idle_chars" value="${tool_rx_idle_chars}" lazy_eval="false"/>
-    </xacro:if>
-    <xacro:if value="${tool_tx_idle_chars != ''}">
-      <xacro:property name="_tool_tx_idle_chars" value="${tool_tx_idle_chars}" lazy_eval="false"/>
-    </xacro:if>
-    <xacro:if value="${tool_device_name != ''}">
-      <xacro:property name="_tool_device_name" value="${tool_device_name}" lazy_eval="false"/>
-    </xacro:if>
-    <xacro:if value="${tool_tcp_port != ''}">
-      <xacro:property name="_tool_tcp_port" value="${tool_tcp_port}" lazy_eval="false"/>
-    </xacro:if>
+    <xacro:if value="${use_tool_communication != ''}"> <xacro:property name="_use_tool_communication" value="${use_tool_communication}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${tool_voltage != ''}"> <xacro:property name="_tool_voltage" value="${tool_voltage}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${tool_parity != ''}"> <xacro:property name="_tool_parity" value="${tool_parity}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${tool_baud_rate != ''}"> <xacro:property name="_tool_baud_rate" value="${tool_baud_rate}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${tool_stop_bits != ''}"> <xacro:property name="_tool_stop_bits" value="${tool_stop_bits}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${tool_rx_idle_chars != ''}"> <xacro:property name="_tool_rx_idle_chars" value="${tool_rx_idle_chars}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${tool_tx_idle_chars != ''}"> <xacro:property name="_tool_tx_idle_chars" value="${tool_tx_idle_chars}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${tool_device_name != ''}"> <xacro:property name="_tool_device_name" value="${tool_device_name}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${tool_tcp_port != ''}"> <xacro:property name="_tool_tcp_port" value="${tool_tcp_port}" lazy_eval="false"/> </xacro:if>
 
     <!-- Universal Robots ROS2 Description -->
     <xacro:ur_robot
@@ -228,13 +151,12 @@
       visual_parameters_file="${_visual_parameters_file}"
       safety_limits="${_safety_limits}"
       safety_pos_margin="${_safety_pos_margin}"
-      safety_k_position="${_safety_k_position}">
+      safety_k_position="${_safety_k_position}"
+      >
       <xacro:insert_block name="origin"/>
     </xacro:ur_robot>
 
-    <!-- Use the right ros2_control depending on sim vs real -->
-
-    <xacro:if value="${_sim_ignition}">
+        <xacro:if value="${_sim_ignition}">
       <ros2_control name="${name}_hardware" type="system">
         <hardware>
           <plugin>gz_ros2_control/GazeboSimSystem</plugin>
@@ -274,43 +196,46 @@
 
       <gazebo>
         <plugin filename="gz_ros2_control-system"
-                name="gz_ros2_control::GazeboSimROS2ControlPlugin"/>
+                name="gz_ros2_control::GazeboSimROS2ControlPlugin">
+          <ros2_control>
+            <update_rate>33</update_rate>
+          </ros2_control>
+        </plugin>
       </gazebo>
     </xacro:if>
 
-    <!-- REAL ROBOT: only include the UR driver when NOT sim -->
-    <xacro:unless value="${_sim_ignition}">
-      <xacro:if value="${use_manipulation_controllers}">
-        <xacro:include filename="$(find ur_robot_driver)/urdf/ur.ros2_control.xacro" />
-        <xacro:ur_ros2_control
-          name="${name}"
-          tf_prefix="${_tf_prefix}"
-          kinematics_parameters_file="${_kinematics_parameters_file}"
-          transmission_hw_interface="${_transmission_hw_interface}"
-          use_mock_hardware="${_use_mock_hardware}"
-          mock_sensor_commands="${_mock_sensor_commands}"
-          headless_mode="${_headless_mode}"
-          initial_positions="${_initial_positions}"
-          use_tool_communication="${_use_tool_communication}"
-          tool_voltage="${_tool_voltage}"
-          tool_parity="${_tool_parity}"
-          tool_baud_rate="${_tool_baud_rate}"
-          tool_stop_bits="${_tool_stop_bits}"
-          tool_rx_idle_chars="${_tool_rx_idle_chars}"
-          tool_tx_idle_chars="${_tool_tx_idle_chars}"
-          tool_device_name="${_tool_device_name}"
-          tool_tcp_port="${_tool_tcp_port}"
-          robot_ip="${_robot_ip}"
-          script_filename="${_script_filename}"
-          output_recipe_filename="${_output_recipe_filename}"
-          input_recipe_filename="${_input_recipe_filename}"
-          reverse_ip="${_reverse_ip}"
-          script_command_port="${_script_command_port}"
-          reverse_port="${_reverse_port}"
-          script_sender_port="${_script_sender_port}"
-          trajectory_port="${_trajectory_port}"/>
-      </xacro:if>
-    </xacro:unless>
-
+    <!-- Universal_Robots_ROS2_Driver HardwareInterface -->
+    <xacro:if value="${use_manipulation_controllers}">
+      <xacro:include filename="$(find ur_robot_driver)/urdf/ur.ros2_control.xacro" />
+
+      <xacro:ur_ros2_control
+        name="${name}"
+        tf_prefix="${_tf_prefix}"
+        kinematics_parameters_file="${_kinematics_parameters_file}"
+        transmission_hw_interface="${_transmission_hw_interface}"
+        use_mock_hardware="${_use_mock_hardware}"
+        mock_sensor_commands="${_mock_sensor_commands}"
+        headless_mode="${_headless_mode}"
+        initial_positions="${_initial_positions}"
+        use_tool_communication="${_use_tool_communication}"
+        tool_voltage="${_tool_voltage}"
+        tool_parity="${_tool_parity}"
+        tool_baud_rate="${_tool_baud_rate}"
+        tool_stop_bits="${_tool_stop_bits}"
+        tool_rx_idle_chars="${_tool_rx_idle_chars}"
+        tool_tx_idle_chars="${_tool_tx_idle_chars}"
+        tool_device_name="${_tool_device_name}"
+        tool_tcp_port="${_tool_tcp_port}"
+        robot_ip="${_robot_ip}"
+        script_filename="${_script_filename}"
+        output_recipe_filename="${_output_recipe_filename}"
+        input_recipe_filename="${_input_recipe_filename}"
+        reverse_ip="${_reverse_ip}"
+        script_command_port="${_script_command_port}"
+        reverse_port="${_reverse_port}"
+        script_sender_port="${_script_sender_port}"
+        trajectory_port="${_trajectory_port}"
+      />
+    </xacro:if>
   </xacro:macro>
 </robot>
\ No newline at end of file

From 25cd0ead9899fb8f83824d8c3b20183788dfa6f9 Mon Sep 17 00:00:00 2001
From: gaudelbijay <gaudelbijay10@gmail.com>
Date: Wed, 12 Nov 2025 15:31:00 -0500
Subject: [PATCH 4/4] urdf ur5 arm update

---
 .../urdf/arm/universal_robots.urdf.xacro      | 227 ++++++++++++------
 1 file changed, 151 insertions(+), 76 deletions(-)

diff --git a/clearpath_manipulators_description/urdf/arm/universal_robots.urdf.xacro b/clearpath_manipulators_description/urdf/arm/universal_robots.urdf.xacro
index 1ddbe013..3a09420a 100644
--- a/clearpath_manipulators_description/urdf/arm/universal_robots.urdf.xacro
+++ b/clearpath_manipulators_description/urdf/arm/universal_robots.urdf.xacro
@@ -48,6 +48,7 @@
     sim_ignition:=''
     ">
     <xacro:property name="use_manipulation_controllers" value="$(arg use_manipulation_controllers)"/>
+
     <!-- Default Description Parameters -->
     <xacro:property name="_tf_prefix" value="${name}_" lazy_eval="false"/>
     <xacro:property name="_initial_positions" value="${dict(shoulder_pan_joint=0.0,shoulder_lift_joint=-1.57,elbow_joint=0.0,wrist_1_joint=-1.57,wrist_2_joint=0.0,wrist_3_joint=0.0)}" lazy_eval="false"/>
@@ -58,28 +59,56 @@
     <xacro:property name="_safety_limits" value="false" lazy_eval="false"/>
     <xacro:property name="_safety_pos_margin" value="0.15" lazy_eval="false"/>
     <xacro:property name="_safety_k_position" value="20" lazy_eval="false"/>
+
     <!-- Default Description Parameters Overwrites -->
-    <xacro:if value="${initial_positions != ''}"> <xacro:property name="_initial_positions" value="${initial_positions}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${initial_positions != ''}">
+      <xacro:property name="_initial_positions" value="${initial_positions}" lazy_eval="false"/>
+    </xacro:if>
     <!-- if initial positions file is passed, overwrite initial positions parameter -->
-    <xacro:if value="${initial_positions_file != ''}"> <xacro:property name="_initial_positions" value="${xacro.load_yaml(initial_positions_file)}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${joint_limits_parameters_file != ''}"> <xacro:property name="_joint_limits_parameters_file" value="${joint_limits_parameters_file}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${kinematics_parameters_file != ''}"> <xacro:property name="_kinematics_parameters_file" value="${kinematics_parameters_file}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${physical_parameters_file != ''}"> <xacro:property name="_physical_parameters_file" value="${physical_parameters_file}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${visual_parameters_file != ''}"> <xacro:property name="$_visual_parameters_file" value="${visual_parameters_file}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${safety_limits != ''}"> <xacro:property name="_safety_limits" value="${safety_limits}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${safety_pos_margin != ''}"> <xacro:property name="_safety_pos_margin" value="${safety_pos_margin}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${safety_k_position != ''}"> <xacro:property name="_safety_k_position" value="${safety_k_position}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${initial_positions_file != ''}">
+      <xacro:property name="_initial_positions" value="${xacro.load_yaml(initial_positions_file)}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${joint_limits_parameters_file != ''}">
+      <xacro:property name="_joint_limits_parameters_file" value="${joint_limits_parameters_file}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${kinematics_parameters_file != ''}">
+      <xacro:property name="_kinematics_parameters_file" value="${kinematics_parameters_file}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${physical_parameters_file != ''}">
+      <xacro:property name="_physical_parameters_file" value="${physical_parameters_file}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${visual_parameters_file != ''}">
+      <xacro:property name="_visual_parameters_file" value="${visual_parameters_file}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${safety_limits != ''}">
+      <xacro:property name="_safety_limits" value="${safety_limits}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${safety_pos_margin != ''}">
+      <xacro:property name="_safety_pos_margin" value="${safety_pos_margin}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${safety_k_position != ''}">
+      <xacro:property name="_safety_k_position" value="${safety_k_position}" lazy_eval="false"/>
+    </xacro:if>
 
     <!-- Default Simulation Parameters -->
     <xacro:property name="_use_mock_hardware" value="false" />
     <xacro:property name="_mock_sensor_commands" value="false" />
     <xacro:property name="_sim_gazebo" value="false" />
     <xacro:property name="_sim_ignition" value="$(arg is_sim)" />
+
     <!-- Default Simulation Parameters Overwrites -->
-    <xacro:if value="${use_mock_hardware != ''}"> <xacro:property name="_use_mock_hardware" value="${use_mock_hardware}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${mock_sensor_commands != ''}"> <xacro:property name="_mock_sensor_commands" value="${mock_sensor_commands}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${sim_gazebo != ''}"> <xacro:property name="_sim_gazebo" value="${sim_gazebo}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${sim_ignition != ''}"> <xacro:property name="_sim_ignition" value="${sim_ignition}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${use_mock_hardware != ''}">
+      <xacro:property name="_use_mock_hardware" value="${use_mock_hardware}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${mock_sensor_commands != ''}">
+      <xacro:property name="_mock_sensor_commands" value="${mock_sensor_commands}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${sim_gazebo != ''}">
+      <xacro:property name="_sim_gazebo" value="${sim_gazebo}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${sim_ignition != ''}">
+      <xacro:property name="_sim_ignition" value="${sim_ignition}" lazy_eval="false"/>
+    </xacro:if>
 
     <!-- Default ros2_control Parameters -->
     <xacro:property name="_headless_mode" value="false" lazy_eval="false"/>
@@ -92,17 +121,39 @@
     <xacro:property name="_transmission_hw_interface" value="hardware_interface/PositionJointInterface" lazy_eval="false"/>
     <xacro:property name="_non_blocking_read" value="true" lazy_eval="false"/>
     <xacro:property name="_keep_alive_count" value="2.0" lazy_eval="false"/>
+
     <!-- Default ros2_control Parameters Overwrites -->
-    <xacro:if value="${headless_mode != ''}"> <xacro:property name="_headless_mode" value="${headless_mode}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${robot_ip != ''}"> <xacro:property name="_robot_ip" value="${robot_ip}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${reverse_ip != ''}"> <xacro:property name="_reverse_ip" value="${reverse_ip}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${script_command_port != ''}"> <xacro:property name="_script_command_port" value="${script_command_port}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${reverse_port != ''}"> <xacro:property name="_reverse_port" value="${reverse_port}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${script_sender_port != ''}"> <xacro:property name="_script_sender_port" value="${script_sender_port}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${trajectory_port != ''}"> <xacro:property name="_trajectory_port" value="${trajectory_port}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${transmission_hw_interface != ''}"> <xacro:property name="_transmission_hw_interface" value="${transmission_hw_interface}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${non_blocking_read != ''}"> <xacro:property name="_non_blocking_read" value="${non_blocking_read}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${keep_alive_count != ''}"> <xacro:property name="_keep_alive_count" value="${keep_alive_count}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${headless_mode != ''}">
+      <xacro:property name="_headless_mode" value="${headless_mode}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${robot_ip != ''}">
+      <xacro:property name="_robot_ip" value="${robot_ip}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${reverse_ip != ''}">
+      <xacro:property name="_reverse_ip" value="${reverse_ip}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${script_command_port != ''}">
+      <xacro:property name="_script_command_port" value="${script_command_port}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${reverse_port != ''}">
+      <xacro:property name="_reverse_port" value="${reverse_port}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${script_sender_port != ''}">
+      <xacro:property name="_script_sender_port" value="${script_sender_port}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${trajectory_port != ''}">
+      <xacro:property name="_trajectory_port" value="${trajectory_port}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${transmission_hw_interface != ''}">
+      <xacro:property name="_transmission_hw_interface" value="${transmission_hw_interface}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${non_blocking_read != ''}">
+      <xacro:property name="_non_blocking_read" value="${non_blocking_read}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${keep_alive_count != ''}">
+      <xacro:property name="_keep_alive_count" value="${keep_alive_count}" lazy_eval="false"/>
+    </xacro:if>
+
     <!-- Driver Dependent Parameters -->
     <xacro:if value="${use_manipulation_controllers and not _use_mock_hardware and not _sim_ignition}">
       <xacro:property name="_script_filename" value="$(find ur_client_library)/resources/external_control.urscript" lazy_eval="false"/>
@@ -114,10 +165,17 @@
       <xacro:property name="_output_recipe_filename" value="" lazy_eval="false"/>
       <xacro:property name="_input_recipe_filename" value="" lazy_eval="false"/>
     </xacro:unless>
+
     <!-- Driver Dependent Parameters Overwrites -->
-    <xacro:if value="${script_filename != ''}"> <xacro:property name="_script_filename" value="${script_filename}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${output_recipe_filename != ''}"> <xacro:property name="_output_recipe_filename" value="${output_recipe_filename}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${input_recipe_filename != ''}"> <xacro:property name="_input_recipe_filename" value="${input_recipe_filename}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${script_filename != ''}">
+      <xacro:property name="_script_filename" value="${script_filename}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${output_recipe_filename != ''}">
+      <xacro:property name="_output_recipe_filename" value="${output_recipe_filename}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${input_recipe_filename != ''}">
+      <xacro:property name="_input_recipe_filename" value="${input_recipe_filename}" lazy_eval="false"/>
+    </xacro:if>
 
     <!-- Default Tool Communication Parameters -->
     <xacro:property name="_use_tool_communication" value="false" lazy_eval="false"/>
@@ -129,16 +187,35 @@
     <xacro:property name="_tool_tx_idle_chars" value="3.5" lazy_eval="false"/>
     <xacro:property name="_tool_device_name" value="/tmp/ttyUR" lazy_eval="false"/>
     <xacro:property name="_tool_tcp_port" value="54321" lazy_eval="false"/>
+
     <!-- Tool Communication Parameter Overwrites -->
-    <xacro:if value="${use_tool_communication != ''}"> <xacro:property name="_use_tool_communication" value="${use_tool_communication}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${tool_voltage != ''}"> <xacro:property name="_tool_voltage" value="${tool_voltage}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${tool_parity != ''}"> <xacro:property name="_tool_parity" value="${tool_parity}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${tool_baud_rate != ''}"> <xacro:property name="_tool_baud_rate" value="${tool_baud_rate}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${tool_stop_bits != ''}"> <xacro:property name="_tool_stop_bits" value="${tool_stop_bits}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${tool_rx_idle_chars != ''}"> <xacro:property name="_tool_rx_idle_chars" value="${tool_rx_idle_chars}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${tool_tx_idle_chars != ''}"> <xacro:property name="_tool_tx_idle_chars" value="${tool_tx_idle_chars}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${tool_device_name != ''}"> <xacro:property name="_tool_device_name" value="${tool_device_name}" lazy_eval="false"/> </xacro:if>
-    <xacro:if value="${tool_tcp_port != ''}"> <xacro:property name="_tool_tcp_port" value="${tool_tcp_port}" lazy_eval="false"/> </xacro:if>
+    <xacro:if value="${use_tool_communication != ''}">
+      <xacro:property name="_use_tool_communication" value="${use_tool_communication}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${tool_voltage != ''}">
+      <xacro:property name="_tool_voltage" value="${tool_voltage}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${tool_parity != ''}">
+      <xacro:property name="_tool_parity" value="${tool_parity}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${tool_baud_rate != ''}">
+      <xacro:property name="_tool_baud_rate" value="${tool_baud_rate}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${tool_stop_bits != ''}">
+      <xacro:property name="_tool_stop_bits" value="${tool_stop_bits}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${tool_rx_idle_chars != ''}">
+      <xacro:property name="_tool_rx_idle_chars" value="${tool_rx_idle_chars}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${tool_tx_idle_chars != ''}">
+      <xacro:property name="_tool_tx_idle_chars" value="${tool_tx_idle_chars}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${tool_device_name != ''}">
+      <xacro:property name="_tool_device_name" value="${tool_device_name}" lazy_eval="false"/>
+    </xacro:if>
+    <xacro:if value="${tool_tcp_port != ''}">
+      <xacro:property name="_tool_tcp_port" value="${tool_tcp_port}" lazy_eval="false"/>
+    </xacro:if>
 
     <!-- Universal Robots ROS2 Description -->
     <xacro:ur_robot
@@ -151,12 +228,13 @@
       visual_parameters_file="${_visual_parameters_file}"
       safety_limits="${_safety_limits}"
       safety_pos_margin="${_safety_pos_margin}"
-      safety_k_position="${_safety_k_position}"
-      >
+      safety_k_position="${_safety_k_position}">
       <xacro:insert_block name="origin"/>
     </xacro:ur_robot>
 
-        <xacro:if value="${_sim_ignition}">
+    <!-- Use the right ros2_control depending on sim vs real -->
+
+    <xacro:if value="${_sim_ignition}">
       <ros2_control name="${name}_hardware" type="system">
         <hardware>
           <plugin>gz_ros2_control/GazeboSimSystem</plugin>
@@ -196,46 +274,43 @@
 
       <gazebo>
         <plugin filename="gz_ros2_control-system"
-                name="gz_ros2_control::GazeboSimROS2ControlPlugin">
-          <ros2_control>
-            <update_rate>33</update_rate>
-          </ros2_control>
-        </plugin>
+                name="gz_ros2_control::GazeboSimROS2ControlPlugin"/>
       </gazebo>
     </xacro:if>
 
-    <!-- Universal_Robots_ROS2_Driver HardwareInterface -->
-    <xacro:if value="${use_manipulation_controllers}">
-      <xacro:include filename="$(find ur_robot_driver)/urdf/ur.ros2_control.xacro" />
-
-      <xacro:ur_ros2_control
-        name="${name}"
-        tf_prefix="${_tf_prefix}"
-        kinematics_parameters_file="${_kinematics_parameters_file}"
-        transmission_hw_interface="${_transmission_hw_interface}"
-        use_mock_hardware="${_use_mock_hardware}"
-        mock_sensor_commands="${_mock_sensor_commands}"
-        headless_mode="${_headless_mode}"
-        initial_positions="${_initial_positions}"
-        use_tool_communication="${_use_tool_communication}"
-        tool_voltage="${_tool_voltage}"
-        tool_parity="${_tool_parity}"
-        tool_baud_rate="${_tool_baud_rate}"
-        tool_stop_bits="${_tool_stop_bits}"
-        tool_rx_idle_chars="${_tool_rx_idle_chars}"
-        tool_tx_idle_chars="${_tool_tx_idle_chars}"
-        tool_device_name="${_tool_device_name}"
-        tool_tcp_port="${_tool_tcp_port}"
-        robot_ip="${_robot_ip}"
-        script_filename="${_script_filename}"
-        output_recipe_filename="${_output_recipe_filename}"
-        input_recipe_filename="${_input_recipe_filename}"
-        reverse_ip="${_reverse_ip}"
-        script_command_port="${_script_command_port}"
-        reverse_port="${_reverse_port}"
-        script_sender_port="${_script_sender_port}"
-        trajectory_port="${_trajectory_port}"
-      />
-    </xacro:if>
+    <!-- REAL ROBOT: only include the UR driver when NOT sim -->
+    <xacro:unless value="${_sim_ignition}">
+      <xacro:if value="${use_manipulation_controllers}">
+        <xacro:include filename="$(find ur_robot_driver)/urdf/ur.ros2_control.xacro" />
+        <xacro:ur_ros2_control
+          name="${name}"
+          tf_prefix="${_tf_prefix}"
+          kinematics_parameters_file="${_kinematics_parameters_file}"
+          transmission_hw_interface="${_transmission_hw_interface}"
+          use_mock_hardware="${_use_mock_hardware}"
+          mock_sensor_commands="${_mock_sensor_commands}"
+          headless_mode="${_headless_mode}"
+          initial_positions="${_initial_positions}"
+          use_tool_communication="${_use_tool_communication}"
+          tool_voltage="${_tool_voltage}"
+          tool_parity="${_tool_parity}"
+          tool_baud_rate="${_tool_baud_rate}"
+          tool_stop_bits="${_tool_stop_bits}"
+          tool_rx_idle_chars="${_tool_rx_idle_chars}"
+          tool_tx_idle_chars="${_tool_tx_idle_chars}"
+          tool_device_name="${_tool_device_name}"
+          tool_tcp_port="${_tool_tcp_port}"
+          robot_ip="${_robot_ip}"
+          script_filename="${_script_filename}"
+          output_recipe_filename="${_output_recipe_filename}"
+          input_recipe_filename="${_input_recipe_filename}"
+          reverse_ip="${_reverse_ip}"
+          script_command_port="${_script_command_port}"
+          reverse_port="${_reverse_port}"
+          script_sender_port="${_script_sender_port}"
+          trajectory_port="${_trajectory_port}"/>
+      </xacro:if>
+    </xacro:unless>
+
   </xacro:macro>
 </robot>
\ No newline at end of file