From 671f8c7983182a62d786efb8b99bd4f8676ca32b Mon Sep 17 00:00:00 2001
From: Witty-Wizard <shashankmarch27@gmail.com>
Date: Mon, 26 Aug 2024 12:21:42 +0530
Subject: [PATCH] [Added]: World Link for the arm to be fixed

---
 .../ur_description/urdf/inc/ur5_macro.xacro   |   8 +-
 .../ur_description/urdf/inc/ur_macro.xacro    | 195 +++++++++++-------
 .../config/initial_positions.yaml             |   4 +-
 .../config/initial_positions.yaml             |   4 +-
 .../config/initial_positions.yaml             |   4 +-
 .../config/initial_positions.yaml             |   4 +-
 .../config/initial_positions.yaml             |   4 +-
 .../config/initial_positions.yaml             |   4 +-
 .../config/initial_positions.yaml             |   4 +-
 .../config/initial_positions.yaml             |   4 +-
 .../config/initial_positions.yaml             |   4 +-
 11 files changed, 138 insertions(+), 101 deletions(-)

diff --git a/workspace/src/ur_description/urdf/inc/ur5_macro.xacro b/workspace/src/ur_description/urdf/inc/ur5_macro.xacro
index 645b240..40c363d 100644
--- a/workspace/src/ur_description/urdf/inc/ur5_macro.xacro
+++ b/workspace/src/ur_description/urdf/inc/ur5_macro.xacro
@@ -20,7 +20,8 @@
           model will be used, which will almost certainly not correspond to any
           real robot.
   -->
-  <xacro:macro name="ur5_robot" params="
+  <xacro:macro name="ur5_robot"
+    params="
    prefix
    joint_limits_parameters_file:='$(find ur_description)/config/ur5/joint_limits.yaml'
    kinematics_parameters_file:='$(find ur_description)/config/ur5/default_kinematics.yaml'
@@ -31,7 +32,7 @@
    safety_pos_margin:=0.15
    safety_k_position:=20"
   >
-    <xacro:include filename="$(find ur_description)/urdf/inc/ur_macro.xacro"/>
+    <xacro:include filename="$(find ur_description)/urdf/inc/ur_macro.xacro" />
     <xacro:ur_robot
       prefix="${prefix}"
       joint_limits_parameters_file="${joint_limits_parameters_file}"
@@ -43,5 +44,6 @@
       safety_pos_margin="${safety_pos_margin}"
       safety_k_position="${safety_k_position}"
     />
+
   </xacro:macro>
-</robot>
+</robot>
\ No newline at end of file
diff --git a/workspace/src/ur_description/urdf/inc/ur_macro.xacro b/workspace/src/ur_description/urdf/inc/ur_macro.xacro
index d5eaca8..634056d 100644
--- a/workspace/src/ur_description/urdf/inc/ur_macro.xacro
+++ b/workspace/src/ur_description/urdf/inc/ur_macro.xacro
@@ -54,7 +54,8 @@
   <xacro:include filename="$(find ur_description)/urdf/inc/ur_transmissions.xacro" />
   <xacro:include filename="$(find ur_description)/urdf/inc/ur_common.xacro" />
 
-  <xacro:macro name="ur_robot" params="
+  <xacro:macro name="ur_robot"
+    params="
     prefix
     joint_limits_parameters_file
     kinematics_parameters_file
@@ -67,154 +68,162 @@
   >
     <!-- Load configuration data from the provided .yaml files -->
     <xacro:read_model_data
-      joint_limits_parameters_file="${joint_limits_parameters_file}" 
+      joint_limits_parameters_file="${joint_limits_parameters_file}"
       kinematics_parameters_file="${kinematics_parameters_file}"
       physical_parameters_file="${physical_parameters_file}"
-      visual_parameters_file="${visual_parameters_file}"/>
+      visual_parameters_file="${visual_parameters_file}" />
 
     <!-- Add URDF transmission elements (for ros_control) -->
     <xacro:ur_arm_transmission prefix="${prefix}" hw_interface="${transmission_hw_interface}" />
 
     <!-- links: main serial chain -->
-    <link name="${prefix}base_link"/>
+    <link name="${prefix}base_link" />
     <link name="${prefix}base_link_inertia">
       <visual>
-        <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
+        <origin xyz="0 0 0" rpy="0 0 ${pi}" />
         <geometry>
-          <mesh filename="${base_visual_mesh}"/>
+          <mesh filename="${base_visual_mesh}" />
         </geometry>
         <material name="${base_visual_material_name}">
-          <color rgba="${base_visual_material_color}"/>
+          <color rgba="${base_visual_material_color}" />
         </material>
       </visual>
       <collision>
-        <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
+        <origin xyz="0 0 0" rpy="0 0 ${pi}" />
         <geometry>
-          <mesh filename="${base_collision_mesh}"/>
+          <mesh filename="${base_collision_mesh}" />
         </geometry>
       </collision>
-      <xacro:cylinder_inertial radius="${base_inertia_radius}" length="${base_inertia_length}" mass="${base_mass}">
+      <xacro:cylinder_inertial radius="${base_inertia_radius}" length="${base_inertia_length}"
+        mass="${base_mass}">
         <origin xyz="0 0 0" rpy="0 0 0" />
       </xacro:cylinder_inertial>
     </link>
     <link name="${prefix}shoulder_link">
       <visual>
-        <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
+        <origin xyz="0 0 0" rpy="0 0 ${pi}" />
         <geometry>
-          <mesh filename="${shoulder_visual_mesh}"/>
+          <mesh filename="${shoulder_visual_mesh}" />
         </geometry>
         <material name="${shoulder_visual_material_name}">
-          <color rgba="${shoulder_visual_material_color}"/>
+          <color rgba="${shoulder_visual_material_color}" />
         </material>
       </visual>
       <collision>
-        <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
+        <origin xyz="0 0 0" rpy="0 0 ${pi}" />
         <geometry>
-          <mesh filename="${shoulder_collision_mesh}"/>
+          <mesh filename="${shoulder_collision_mesh}" />
         </geometry>
       </collision>
-      <xacro:cylinder_inertial radius="${shoulder_inertia_radius}" length="${shoulder_inertia_length}" mass="${shoulder_mass}">
+      <xacro:cylinder_inertial radius="${shoulder_inertia_radius}"
+        length="${shoulder_inertia_length}" mass="${shoulder_mass}">
         <origin xyz="0 0 0" rpy="0 0 0" />
       </xacro:cylinder_inertial>
     </link>
     <link name="${prefix}upper_arm_link">
       <visual>
-        <origin xyz="0 0 ${shoulder_offset}" rpy="${pi/2} 0 ${-pi/2}"/>
+        <origin xyz="0 0 ${shoulder_offset}" rpy="${pi/2} 0 ${-pi/2}" />
         <geometry>
-          <mesh filename="${upper_arm_visual_mesh}"/>
+          <mesh filename="${upper_arm_visual_mesh}" />
         </geometry>
         <material name="${upper_arm_visual_material_name}">
-          <color rgba="${upper_arm_visual_material_color}"/>
+          <color rgba="${upper_arm_visual_material_color}" />
         </material>
       </visual>
       <collision>
-        <origin xyz="0 0 ${shoulder_offset}" rpy="${pi/2} 0 ${-pi/2}"/>
+        <origin xyz="0 0 ${shoulder_offset}" rpy="${pi/2} 0 ${-pi/2}" />
         <geometry>
-          <mesh filename="${upper_arm_collision_mesh}"/>
+          <mesh filename="${upper_arm_collision_mesh}" />
         </geometry>
       </collision>
-      <xacro:cylinder_inertial radius="${upperarm_inertia_radius}" length="${upperarm_inertia_length}" mass="${upper_arm_mass}">
-        <origin xyz="${-0.5 * upperarm_inertia_length} 0.0 ${upper_arm_inertia_offset}" rpy="0 ${pi/2} 0" />
+      <xacro:cylinder_inertial radius="${upperarm_inertia_radius}"
+        length="${upperarm_inertia_length}" mass="${upper_arm_mass}">
+        <origin xyz="${-0.5 * upperarm_inertia_length} 0.0 ${upper_arm_inertia_offset}"
+          rpy="0 ${pi/2} 0" />
       </xacro:cylinder_inertial>
     </link>
     <link name="${prefix}forearm_link">
       <visual>
-        <origin xyz="0 0 ${elbow_offset}" rpy="${pi/2} 0 ${-pi/2}"/>
+        <origin xyz="0 0 ${elbow_offset}" rpy="${pi/2} 0 ${-pi/2}" />
         <geometry>
-          <mesh filename="${forearm_visual_mesh}"/>
+          <mesh filename="${forearm_visual_mesh}" />
         </geometry>
         <material name="${forearm_visual_material_name}">
-          <color rgba="${forearm_visual_material_color}"/>
+          <color rgba="${forearm_visual_material_color}" />
         </material>
       </visual>
       <collision>
-        <origin xyz="0 0 ${elbow_offset}" rpy="${pi/2} 0 ${-pi/2}"/>
+        <origin xyz="0 0 ${elbow_offset}" rpy="${pi/2} 0 ${-pi/2}" />
         <geometry>
-          <mesh filename="${forearm_collision_mesh}"/>
+          <mesh filename="${forearm_collision_mesh}" />
         </geometry>
       </collision>
-      <xacro:cylinder_inertial radius="${forearm_inertia_radius}" length="${forearm_inertia_length}"  mass="${forearm_mass}">
+      <xacro:cylinder_inertial radius="${forearm_inertia_radius}" length="${forearm_inertia_length}"
+        mass="${forearm_mass}">
         <origin xyz="${-0.5 * forearm_inertia_length} 0.0 ${elbow_offset}" rpy="0 ${pi/2} 0" />
       </xacro:cylinder_inertial>
     </link>
     <link name="${prefix}wrist_1_link">
       <visual>
         <!-- TODO: Move this to a parameter -->
-        <origin xyz="0 0 ${wrist_1_visual_offset}" rpy="${pi/2} 0 0"/>
+        <origin xyz="0 0 ${wrist_1_visual_offset}" rpy="${pi/2} 0 0" />
         <geometry>
-          <mesh filename="${wrist_1_visual_mesh}"/>
+          <mesh filename="${wrist_1_visual_mesh}" />
         </geometry>
         <material name="${wrist_1_visual_material_name}">
-          <color rgba="${wrist_1_visual_material_color}"/>
+          <color rgba="${wrist_1_visual_material_color}" />
         </material>
       </visual>
       <collision>
-        <origin xyz="0 0 ${wrist_1_visual_offset}" rpy="${pi/2} 0 0"/>
+        <origin xyz="0 0 ${wrist_1_visual_offset}" rpy="${pi/2} 0 0" />
         <geometry>
-          <mesh filename="${wrist_1_collision_mesh}"/>
+          <mesh filename="${wrist_1_collision_mesh}" />
         </geometry>
       </collision>
-      <xacro:cylinder_inertial radius="${wrist_1_inertia_radius}" length="${wrist_1_inertia_length}"  mass="${wrist_1_mass}">
+      <xacro:cylinder_inertial radius="${wrist_1_inertia_radius}" length="${wrist_1_inertia_length}"
+        mass="${wrist_1_mass}">
         <origin xyz="0 0 0" rpy="0 0 0" />
       </xacro:cylinder_inertial>
     </link>
     <link name="${prefix}wrist_2_link">
       <visual>
-        <origin xyz="0 0 ${wrist_2_visual_offset}" rpy="0 0 0"/>
+        <origin xyz="0 0 ${wrist_2_visual_offset}" rpy="0 0 0" />
         <geometry>
-          <mesh filename="${wrist_2_visual_mesh}"/>
+          <mesh filename="${wrist_2_visual_mesh}" />
         </geometry>
         <material name="${wrist_2_visual_material_name}">
-          <color rgba="${wrist_2_visual_material_color}"/>
+          <color rgba="${wrist_2_visual_material_color}" />
         </material>
       </visual>
       <collision>
-        <origin xyz="0 0 ${wrist_2_visual_offset}" rpy="0 0 0"/>
+        <origin xyz="0 0 ${wrist_2_visual_offset}" rpy="0 0 0" />
         <geometry>
-          <mesh filename="${wrist_2_collision_mesh}"/>
+          <mesh filename="${wrist_2_collision_mesh}" />
         </geometry>
       </collision>
-      <xacro:cylinder_inertial radius="${wrist_2_inertia_radius}" length="${wrist_2_inertia_length}"  mass="${wrist_2_mass}">
+      <xacro:cylinder_inertial radius="${wrist_2_inertia_radius}" length="${wrist_2_inertia_length}"
+        mass="${wrist_2_mass}">
         <origin xyz="0 0 0" rpy="0 0 0" />
       </xacro:cylinder_inertial>
     </link>
     <link name="${prefix}wrist_3_link">
       <visual>
-        <origin xyz="0 0 ${wrist_3_visual_offset}" rpy="${pi/2} 0 0"/>
+        <origin xyz="0 0 ${wrist_3_visual_offset}" rpy="${pi/2} 0 0" />
         <geometry>
-          <mesh filename="${wrist_3_visual_mesh}"/>
+          <mesh filename="${wrist_3_visual_mesh}" />
         </geometry>
         <material name="${wrist_3_visual_material_name}">
-          <color rgba="${wrist_3_visual_material_color}"/>
+          <color rgba="${wrist_3_visual_material_color}" />
         </material>
       </visual>
       <collision>
-        <origin xyz="0 0 ${wrist_3_visual_offset}" rpy="${pi/2} 0 0"/>
+        <origin xyz="0 0 ${wrist_3_visual_offset}" rpy="${pi/2} 0 0" />
         <geometry>
-          <mesh filename="${wrist_3_collision_mesh}"/>
+          <mesh filename="${wrist_3_collision_mesh}" />
         </geometry>
       </collision>
-      <xacro:cylinder_inertial radius="${wrist_3_inertia_radius}" length="${wrist_3_inertia_length}"  mass="${wrist_3_mass}">
+      <xacro:cylinder_inertial radius="${wrist_3_inertia_radius}" length="${wrist_3_inertia_length}"
+        mass="${wrist_3_mass}">
         <origin xyz="0.0 0.0 ${-0.5 * wrist_3_inertia_length}" rpy="0 0 0" />
       </xacro:cylinder_inertial>
     </link>
@@ -233,87 +242,105 @@
     <joint name="${prefix}shoulder_pan_joint" type="revolute">
       <parent link="${prefix}base_link_inertia" />
       <child link="${prefix}shoulder_link" />
-      <origin xyz="${shoulder_x} ${shoulder_y} ${shoulder_z}" rpy="${shoulder_roll} ${shoulder_pitch} ${shoulder_yaw}" />
+      <origin xyz="${shoulder_x} ${shoulder_y} ${shoulder_z}"
+        rpy="${shoulder_roll} ${shoulder_pitch} ${shoulder_yaw}" />
       <axis xyz="0 0 1" />
       <limit lower="${shoulder_pan_lower_limit}" upper="${shoulder_pan_upper_limit}"
-        effort="${shoulder_pan_effort_limit}" velocity="${shoulder_pan_velocity_limit}"/>
+        effort="${shoulder_pan_effort_limit}" velocity="${shoulder_pan_velocity_limit}" />
       <xacro:if value="${safety_limits}">
-         <safety_controller soft_lower_limit="${shoulder_pan_lower_limit + safety_pos_margin}" soft_upper_limit="${shoulder_pan_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+        <safety_controller soft_lower_limit="${shoulder_pan_lower_limit + safety_pos_margin}"
+          soft_upper_limit="${shoulder_pan_upper_limit - safety_pos_margin}"
+          k_position="${safety_k_position}" k_velocity="0.0" />
       </xacro:if>
-      <dynamics damping="0" friction="0"/>
+      <dynamics damping="0" friction="0" />
     </joint>
     <joint name="${prefix}shoulder_lift_joint" type="revolute">
       <parent link="${prefix}shoulder_link" />
       <child link="${prefix}upper_arm_link" />
-      <origin xyz="${upper_arm_x} ${upper_arm_y} ${upper_arm_z}" rpy="${upper_arm_roll} ${upper_arm_pitch} ${upper_arm_yaw}" />
+      <origin xyz="${upper_arm_x} ${upper_arm_y} ${upper_arm_z}"
+        rpy="${upper_arm_roll} ${upper_arm_pitch} ${upper_arm_yaw}" />
       <axis xyz="0 0 1" />
       <limit lower="${shoulder_lift_lower_limit}" upper="${shoulder_lift_upper_limit}"
-        effort="${shoulder_lift_effort_limit}" velocity="${shoulder_lift_velocity_limit}"/>
+        effort="${shoulder_lift_effort_limit}" velocity="${shoulder_lift_velocity_limit}" />
       <xacro:if value="${safety_limits}">
-         <safety_controller soft_lower_limit="${shoulder_lift_lower_limit + safety_pos_margin}" soft_upper_limit="${shoulder_lift_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+        <safety_controller soft_lower_limit="${shoulder_lift_lower_limit + safety_pos_margin}"
+          soft_upper_limit="${shoulder_lift_upper_limit - safety_pos_margin}"
+          k_position="${safety_k_position}" k_velocity="0.0" />
       </xacro:if>
-      <dynamics damping="0" friction="0"/>
+      <dynamics damping="0" friction="0" />
     </joint>
     <joint name="${prefix}elbow_joint" type="revolute">
       <parent link="${prefix}upper_arm_link" />
       <child link="${prefix}forearm_link" />
-      <origin xyz="${forearm_x} ${forearm_y} ${forearm_z}" rpy="${forearm_roll} ${forearm_pitch} ${forearm_yaw}" />
+      <origin xyz="${forearm_x} ${forearm_y} ${forearm_z}"
+        rpy="${forearm_roll} ${forearm_pitch} ${forearm_yaw}" />
       <axis xyz="0 0 1" />
       <limit lower="${elbow_joint_lower_limit}" upper="${elbow_joint_upper_limit}"
-        effort="${elbow_joint_effort_limit}" velocity="${elbow_joint_velocity_limit}"/>
+        effort="${elbow_joint_effort_limit}" velocity="${elbow_joint_velocity_limit}" />
       <xacro:if value="${safety_limits}">
-         <safety_controller soft_lower_limit="${elbow_joint_lower_limit + safety_pos_margin}" soft_upper_limit="${elbow_joint_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+        <safety_controller soft_lower_limit="${elbow_joint_lower_limit + safety_pos_margin}"
+          soft_upper_limit="${elbow_joint_upper_limit - safety_pos_margin}"
+          k_position="${safety_k_position}" k_velocity="0.0" />
       </xacro:if>
-      <dynamics damping="0" friction="0"/>
+      <dynamics damping="0" friction="0" />
     </joint>
     <joint name="${prefix}wrist_1_joint" type="revolute">
       <parent link="${prefix}forearm_link" />
       <child link="${prefix}wrist_1_link" />
-      <origin xyz="${wrist_1_x} ${wrist_1_y} ${wrist_1_z}" rpy="${wrist_1_roll} ${wrist_1_pitch} ${wrist_1_yaw}" />
+      <origin xyz="${wrist_1_x} ${wrist_1_y} ${wrist_1_z}"
+        rpy="${wrist_1_roll} ${wrist_1_pitch} ${wrist_1_yaw}" />
       <axis xyz="0 0 1" />
       <limit lower="${wrist_1_lower_limit}" upper="${wrist_1_upper_limit}"
-        effort="${wrist_1_effort_limit}" velocity="${wrist_1_velocity_limit}"/>
+        effort="${wrist_1_effort_limit}" velocity="${wrist_1_velocity_limit}" />
       <xacro:if value="${safety_limits}">
-         <safety_controller soft_lower_limit="${wrist_1_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_1_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+        <safety_controller soft_lower_limit="${wrist_1_lower_limit + safety_pos_margin}"
+          soft_upper_limit="${wrist_1_upper_limit - safety_pos_margin}"
+          k_position="${safety_k_position}" k_velocity="0.0" />
       </xacro:if>
-      <dynamics damping="0" friction="0"/>
+      <dynamics damping="0" friction="0" />
     </joint>
     <joint name="${prefix}wrist_2_joint" type="revolute">
       <parent link="${prefix}wrist_1_link" />
       <child link="${prefix}wrist_2_link" />
-      <origin xyz="${wrist_2_x} ${wrist_2_y} ${wrist_2_z}" rpy="${wrist_2_roll} ${wrist_2_pitch} ${wrist_2_yaw}" />
+      <origin xyz="${wrist_2_x} ${wrist_2_y} ${wrist_2_z}"
+        rpy="${wrist_2_roll} ${wrist_2_pitch} ${wrist_2_yaw}" />
       <axis xyz="0 0 1" />
       <limit lower="${wrist_2_lower_limit}" upper="${wrist_2_upper_limit}"
-             effort="${wrist_2_effort_limit}" velocity="${wrist_2_velocity_limit}"/>
+        effort="${wrist_2_effort_limit}" velocity="${wrist_2_velocity_limit}" />
       <xacro:if value="${safety_limits}">
-         <safety_controller soft_lower_limit="${wrist_2_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_2_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+        <safety_controller soft_lower_limit="${wrist_2_lower_limit + safety_pos_margin}"
+          soft_upper_limit="${wrist_2_upper_limit - safety_pos_margin}"
+          k_position="${safety_k_position}" k_velocity="0.0" />
       </xacro:if>
-      <dynamics damping="0" friction="0"/>
+      <dynamics damping="0" friction="0" />
     </joint>
     <joint name="${prefix}wrist_3_joint" type="revolute">
       <parent link="${prefix}wrist_2_link" />
       <child link="${prefix}wrist_3_link" />
-      <origin xyz="${wrist_3_x} ${wrist_3_y} ${wrist_3_z}" rpy="${wrist_3_roll} ${wrist_3_pitch} ${wrist_3_yaw}" />
+      <origin xyz="${wrist_3_x} ${wrist_3_y} ${wrist_3_z}"
+        rpy="${wrist_3_roll} ${wrist_3_pitch} ${wrist_3_yaw}" />
       <axis xyz="0 0 1" />
       <limit lower="${wrist_3_lower_limit}" upper="${wrist_3_upper_limit}"
-             effort="${wrist_3_effort_limit}" velocity="${wrist_3_velocity_limit}"/>
+        effort="${wrist_3_effort_limit}" velocity="${wrist_3_velocity_limit}" />
       <xacro:if value="${safety_limits}">
-         <safety_controller soft_lower_limit="${wrist_3_lower_limit + safety_pos_margin}" soft_upper_limit="${wrist_3_upper_limit - safety_pos_margin}" k_position="${safety_k_position}" k_velocity="0.0"/>
+        <safety_controller soft_lower_limit="${wrist_3_lower_limit + safety_pos_margin}"
+          soft_upper_limit="${wrist_3_upper_limit - safety_pos_margin}"
+          k_position="${safety_k_position}" k_velocity="0.0" />
       </xacro:if>
-      <dynamics damping="0" friction="0"/>
+      <dynamics damping="0" friction="0" />
     </joint>
 
     <!-- ROS-Industrial 'base' frame: base_link to UR 'Base' Coordinates transform -->
-    <link name="${prefix}base"/>
+    <link name="${prefix}base" />
     <joint name="${prefix}base_link-base_fixed_joint" type="fixed">
       <!-- Note the rotation over Z of pi radians: as base_link is REP-103
            aligned (ie: has X+ forward, Y+ left and Z+ up), this is needed
            to correctly align 'base' with the 'Base' coordinate system of
            the UR controller.
       -->
-      <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
-      <parent link="${prefix}base_link"/>
-      <child link="${prefix}base"/>
+      <origin xyz="0 0 0" rpy="0 0 ${pi}" />
+      <parent link="${prefix}base_link" />
+      <child link="${prefix}base" />
     </joint>
 
     <!-- ROS-Industrial 'flange' frame: attachment point for EEF models -->
@@ -325,12 +352,20 @@
     </joint>
 
     <!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
-    <link name="${prefix}tool0"/>
+    <link name="${prefix}tool0" />
     <joint name="${prefix}flange-tool0" type="fixed">
       <!-- default toolframe: X+ left, Y+ up, Z+ front -->
-      <origin xyz="0 0 0" rpy="${pi/2.0} 0 ${pi/2.0}"/>
-      <parent link="${prefix}flange"/>
-      <child link="${prefix}tool0"/>
+      <origin xyz="0 0 0" rpy="${pi/2.0} 0 ${pi/2.0}" />
+      <parent link="${prefix}flange" />
+      <child link="${prefix}tool0" />
     </joint>
+
+    <!-- Fixed World link for the arm -->
+    <link name="world" />
+    <joint name="world_joint" type="fixed">
+      <parent link="world" />
+      <child link="base_link" />
+    </joint>
+
   </xacro:macro>
-</robot>
+</robot>
\ No newline at end of file
diff --git a/workspace/src/ur_moveit_bringup/ur10_moveit_config/config/initial_positions.yaml b/workspace/src/ur_moveit_bringup/ur10_moveit_config/config/initial_positions.yaml
index 8f2e90d..c157a1b 100644
--- a/workspace/src/ur_moveit_bringup/ur10_moveit_config/config/initial_positions.yaml
+++ b/workspace/src/ur_moveit_bringup/ur10_moveit_config/config/initial_positions.yaml
@@ -1,8 +1,8 @@
 # Default initial positions for ur10_robot's ros2_control fake system
 
 initial_positions:
-  elbow_joint: 0
-  shoulder_lift_joint: 0
+  elbow_joint: 1.7
+  shoulder_lift_joint: -2.4
   shoulder_pan_joint: 0
   wrist_1_joint: 0
   wrist_2_joint: 0
diff --git a/workspace/src/ur_moveit_bringup/ur10e_moveit_config/config/initial_positions.yaml b/workspace/src/ur_moveit_bringup/ur10e_moveit_config/config/initial_positions.yaml
index 890f639..97c548b 100644
--- a/workspace/src/ur_moveit_bringup/ur10e_moveit_config/config/initial_positions.yaml
+++ b/workspace/src/ur_moveit_bringup/ur10e_moveit_config/config/initial_positions.yaml
@@ -1,8 +1,8 @@
 # Default initial positions for ur10e_robot's ros2_control fake system
 
 initial_positions:
-  elbow_joint: 0
-  shoulder_lift_joint: 0
+  elbow_joint: 1.7
+  shoulder_lift_joint: -2.4
   shoulder_pan_joint: 0
   wrist_1_joint: 0
   wrist_2_joint: 0
diff --git a/workspace/src/ur_moveit_bringup/ur16e_moveit_config/config/initial_positions.yaml b/workspace/src/ur_moveit_bringup/ur16e_moveit_config/config/initial_positions.yaml
index 2e29dc4..3a4337e 100644
--- a/workspace/src/ur_moveit_bringup/ur16e_moveit_config/config/initial_positions.yaml
+++ b/workspace/src/ur_moveit_bringup/ur16e_moveit_config/config/initial_positions.yaml
@@ -1,8 +1,8 @@
 # Default initial positions for ur16e_robot's ros2_control fake system
 
 initial_positions:
-  elbow_joint: 0
-  shoulder_lift_joint: 0
+  elbow_joint: 1.7
+  shoulder_lift_joint: -2.4
   shoulder_pan_joint: 0
   wrist_1_joint: 0
   wrist_2_joint: 0
diff --git a/workspace/src/ur_moveit_bringup/ur20_moveit_config/config/initial_positions.yaml b/workspace/src/ur_moveit_bringup/ur20_moveit_config/config/initial_positions.yaml
index 3d6fbb8..66f7117 100644
--- a/workspace/src/ur_moveit_bringup/ur20_moveit_config/config/initial_positions.yaml
+++ b/workspace/src/ur_moveit_bringup/ur20_moveit_config/config/initial_positions.yaml
@@ -1,8 +1,8 @@
 # Default initial positions for ur20_robot's ros2_control fake system
 
 initial_positions:
-  elbow_joint: 0
-  shoulder_lift_joint: 0
+  elbow_joint: 1.7
+  shoulder_lift_joint: -2.4
   shoulder_pan_joint: 0
   wrist_1_joint: 0
   wrist_2_joint: 0
diff --git a/workspace/src/ur_moveit_bringup/ur30_moveit_config/config/initial_positions.yaml b/workspace/src/ur_moveit_bringup/ur30_moveit_config/config/initial_positions.yaml
index 2dbc466..986c8ff 100644
--- a/workspace/src/ur_moveit_bringup/ur30_moveit_config/config/initial_positions.yaml
+++ b/workspace/src/ur_moveit_bringup/ur30_moveit_config/config/initial_positions.yaml
@@ -1,8 +1,8 @@
 # Default initial positions for ur30_robot's ros2_control fake system
 
 initial_positions:
-  elbow_joint: 0
-  shoulder_lift_joint: 0
+  elbow_joint: 1.7
+  shoulder_lift_joint: -2.4
   shoulder_pan_joint: 0
   wrist_1_joint: 0
   wrist_2_joint: 0
diff --git a/workspace/src/ur_moveit_bringup/ur3_moveit_config/config/initial_positions.yaml b/workspace/src/ur_moveit_bringup/ur3_moveit_config/config/initial_positions.yaml
index 00445c4..2ef24e3 100644
--- a/workspace/src/ur_moveit_bringup/ur3_moveit_config/config/initial_positions.yaml
+++ b/workspace/src/ur_moveit_bringup/ur3_moveit_config/config/initial_positions.yaml
@@ -1,8 +1,8 @@
 # Default initial positions for ur3_robot's ros2_control fake system
 
 initial_positions:
-  elbow_joint: 0
-  shoulder_lift_joint: 0
+  elbow_joint: 1.7
+  shoulder_lift_joint: -2.4
   shoulder_pan_joint: 0
   wrist_1_joint: 0
   wrist_2_joint: 0
diff --git a/workspace/src/ur_moveit_bringup/ur3e_moveit_config/config/initial_positions.yaml b/workspace/src/ur_moveit_bringup/ur3e_moveit_config/config/initial_positions.yaml
index e960f37..d33ac12 100644
--- a/workspace/src/ur_moveit_bringup/ur3e_moveit_config/config/initial_positions.yaml
+++ b/workspace/src/ur_moveit_bringup/ur3e_moveit_config/config/initial_positions.yaml
@@ -1,8 +1,8 @@
 # Default initial positions for ur3e_robot's ros2_control fake system
 
 initial_positions:
-  elbow_joint: 0
-  shoulder_lift_joint: 0
+  elbow_joint: 1.7
+  shoulder_lift_joint: -2.4
   shoulder_pan_joint: 0
   wrist_1_joint: 0
   wrist_2_joint: 0
diff --git a/workspace/src/ur_moveit_bringup/ur5_moveit_config/config/initial_positions.yaml b/workspace/src/ur_moveit_bringup/ur5_moveit_config/config/initial_positions.yaml
index 238fcb2..effe23c 100644
--- a/workspace/src/ur_moveit_bringup/ur5_moveit_config/config/initial_positions.yaml
+++ b/workspace/src/ur_moveit_bringup/ur5_moveit_config/config/initial_positions.yaml
@@ -1,8 +1,8 @@
 # Default initial positions for ur5_robot's ros2_control fake system
 
 initial_positions:
-  elbow_joint: 0
-  shoulder_lift_joint: -0.3
+  elbow_joint: 1.7
+  shoulder_lift_joint: -2.4
   shoulder_pan_joint: 0
   wrist_1_joint: 0
   wrist_2_joint: 0
diff --git a/workspace/src/ur_moveit_bringup/ur5e_moveit_config/config/initial_positions.yaml b/workspace/src/ur_moveit_bringup/ur5e_moveit_config/config/initial_positions.yaml
index 4db0c62..1f7af84 100644
--- a/workspace/src/ur_moveit_bringup/ur5e_moveit_config/config/initial_positions.yaml
+++ b/workspace/src/ur_moveit_bringup/ur5e_moveit_config/config/initial_positions.yaml
@@ -1,8 +1,8 @@
 # Default initial positions for ur5e_robot's ros2_control fake system
 
 initial_positions:
-  elbow_joint: 0
-  shoulder_lift_joint: 0
+  elbow_joint: 1.7
+  shoulder_lift_joint: -2.4
   shoulder_pan_joint: 0
   wrist_1_joint: 0
   wrist_2_joint: 0