From 70cdc84fccc592da05e113ab7fe31a40ff111f70 Mon Sep 17 00:00:00 2001
From: Adarsh Karan Kesavadas Prasanth <adarshkaran01@gmail.com>
Date: Thu, 28 Nov 2024 13:36:52 +0530
Subject: [PATCH 1/3] Added the necessary srdf elements to disable collisions
 with base_link

---
 neo_ur_moveit_config/srdf/mpo_500.srdf.xacro | 19 +++++----
 neo_ur_moveit_config/srdf/mpo_700.srdf.xacro | 45 ++++++++++++++------
 2 files changed, 42 insertions(+), 22 deletions(-)

diff --git a/neo_ur_moveit_config/srdf/mpo_500.srdf.xacro b/neo_ur_moveit_config/srdf/mpo_500.srdf.xacro
index 315f28e..c362f67 100644
--- a/neo_ur_moveit_config/srdf/mpo_500.srdf.xacro
+++ b/neo_ur_moveit_config/srdf/mpo_500.srdf.xacro
@@ -41,15 +41,16 @@
     </group_state>
 
     <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
-    <disable_collisions link1="base_footprint" link2="cabinet_link" reason="Adjacent"/>
-    <disable_collisions link1="base_footprint" link2="lidar_1_link" reason="Adjacent"/>
-    <disable_collisions link1="base_footprint" link2="lidar_2_link" reason="Adjacent"/>
-    <disable_collisions link1="base_footprint" link2="mpo_500_omni_wheel_back_left_link" reason="Adjacent"/>
-    <disable_collisions link1="base_footprint" link2="mpo_500_omni_wheel_back_right_link" reason="Adjacent"/>
-    <disable_collisions link1="base_footprint" link2="mpo_500_omni_wheel_front_left_link" reason="Adjacent"/>
-    <disable_collisions link1="base_footprint" link2="mpo_500_omni_wheel_front_right_link" reason="Adjacent"/>
-    <disable_collisions link1="base_footprint" link2="${prefix_value}base_link_inertia" reason="Never"/>
-    <disable_collisions link1="base_footprint" link2="${prefix_value}shoulder_link" reason="Never"/>
+    <disable_collisions link1="base_link" link2="cabinet_link" reason="Adjacent"/>
+    <disable_collisions link1="base_link" link2="lidar_1_link" reason="Adjacent"/>
+    <disable_collisions link1="base_link" link2="lidar_2_link" reason="Adjacent"/>
+    <disable_collisions link1="base_link" link2="mpo_500_omni_wheel_back_left_link" reason="Adjacent"/>
+    <disable_collisions link1="base_link" link2="mpo_500_omni_wheel_back_right_link" reason="Adjacent"/>
+    <disable_collisions link1="base_link" link2="mpo_500_omni_wheel_front_left_link" reason="Adjacent"/>
+    <disable_collisions link1="base_link" link2="mpo_500_omni_wheel_front_right_link" reason="Adjacent"/>
+    <disable_collisions link1="base_link" link2="${prefix_value}base_link_inertia" reason="Never"/>
+    <disable_collisions link1="base_link" link2="${prefix_value}shoulder_link" reason="Never"/>
+
     <disable_collisions link1="cabinet_link" link2="lidar_1_link" reason="Never"/>
     <disable_collisions link1="cabinet_link" link2="lidar_2_link" reason="Never"/>
     <disable_collisions link1="cabinet_link" link2="mpo_500_omni_wheel_back_left_link" reason="Never"/>
diff --git a/neo_ur_moveit_config/srdf/mpo_700.srdf.xacro b/neo_ur_moveit_config/srdf/mpo_700.srdf.xacro
index f97b9b1..72f1ca5 100644
--- a/neo_ur_moveit_config/srdf/mpo_700.srdf.xacro
+++ b/neo_ur_moveit_config/srdf/mpo_700.srdf.xacro
@@ -38,20 +38,24 @@
       <joint name="${prefix_value}wrist_2_joint" value="-1.6" />
       <joint name="${prefix_value}wrist_3_joint" value="-0.11" />
     </group_state>
+    
     <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
-    <disable_collisions link1="base_footprint" link2="cabinet_link" reason="Adjacent"/>
-    <disable_collisions link1="base_footprint" link2="lidar_1_link" reason="Adjacent"/>
-    <disable_collisions link1="base_footprint" link2="lidar_2_link" reason="Adjacent"/>
-    <disable_collisions link1="base_footprint" link2="mpo_700_caster_back_left_link" reason="Adjacent"/>
-    <disable_collisions link1="base_footprint" link2="mpo_700_caster_back_right_link" reason="Adjacent"/>
-    <disable_collisions link1="base_footprint" link2="mpo_700_caster_front_left_link" reason="Adjacent"/>
-    <disable_collisions link1="base_footprint" link2="mpo_700_caster_front_right_link" reason="Adjacent"/>
-    <disable_collisions link1="base_footprint" link2="mpo_700_wheel_back_left_link" reason="Never"/>
-    <disable_collisions link1="base_footprint" link2="mpo_700_wheel_back_right_link" reason="Never"/>
-    <disable_collisions link1="base_footprint" link2="mpo_700_wheel_front_left_link" reason="Never"/>
-    <disable_collisions link1="base_footprint" link2="mpo_700_wheel_front_right_link" reason="Never"/>
-    <disable_collisions link1="base_footprint" link2="${prefix_value}base_link_inertia" reason="Never"/>
-    <disable_collisions link1="base_footprint" link2="${prefix_value}shoulder_link" reason="Never"/>
+    <disable_collisions link1="base_link" link2="cabinet_link" reason="Adjacent"/>
+    <disable_collisions link1="base_link" link2="lidar_1_link" reason="Adjacent"/>
+    <disable_collisions link1="base_link" link2="lidar_2_link" reason="Adjacent"/>
+    <disable_collisions link1="base_link" link2="docking_adapter_link" reason="Adjacent"/>
+    <disable_collisions link1="base_link" link2="mpo_700_caster_back_left_link" reason="Adjacent"/>
+    <disable_collisions link1="base_link" link2="mpo_700_caster_back_right_link" reason="Adjacent"/>
+    <disable_collisions link1="base_link" link2="mpo_700_caster_front_left_link" reason="Adjacent"/>
+    <disable_collisions link1="base_link" link2="mpo_700_caster_front_right_link" reason="Adjacent"/>
+    <disable_collisions link1="base_link" link2="mpo_700_wheel_back_left_link" reason="Never"/>
+    <disable_collisions link1="base_link" link2="mpo_700_wheel_back_right_link" reason="Never"/>
+    <disable_collisions link1="base_link" link2="mpo_700_wheel_front_left_link" reason="Never"/>
+    <disable_collisions link1="base_link" link2="mpo_700_wheel_front_right_link" reason="Never"/>
+    <disable_collisions link1="base_link" link2="${prefix_value}base_link_inertia" reason="Never"/>
+    <disable_collisions link1="base_link" link2="${prefix_value}base_link" reason="Never"/>
+    <disable_collisions link1="base_link" link2="${prefix_value}shoulder_link" reason="Never"/>
+    
     <disable_collisions link1="cabinet_link" link2="lidar_1_link" reason="Adjacent"/>
     <disable_collisions link1="cabinet_link" link2="lidar_2_link" reason="Adjacent"/>
     <disable_collisions link1="cabinet_link" link2="mpo_700_caster_back_left_link" reason="Never"/>
@@ -64,6 +68,21 @@
     <disable_collisions link1="cabinet_link" link2="mpo_700_wheel_front_right_link" reason="Never"/>
     <disable_collisions link1="cabinet_link" link2="${prefix_value}base_link_inertia" reason="Adjacent"/>
     <disable_collisions link1="cabinet_link" link2="${prefix_value}shoulder_link" reason="Never"/>
+
+    <disable_collisions link1="docking_adapter_link" link2="lidar_1_link" reason="Never"/>
+    <disable_collisions link1="docking_adapter_link" link2="lidar_2_link" reason="Never"/>
+    <disable_collisions link1="docking_adapter_link" link2="mpo_700_caster_back_left_link" reason="Never"/>
+    <disable_collisions link1="docking_adapter_link" link2="mpo_700_caster_back_right_link" reason="Never"/>
+    <disable_collisions link1="docking_adapter_link" link2="mpo_700_caster_front_left_link" reason="Never"/>
+    <disable_collisions link1="docking_adapter_link" link2="mpo_700_caster_front_right_link" reason="Never"/>
+    <disable_collisions link1="docking_adapter_link" link2="mpo_700_wheel_back_left_link" reason="Never"/>
+    <disable_collisions link1="docking_adapter_link" link2="mpo_700_wheel_back_right_link" reason="Never"/>
+    <disable_collisions link1="docking_adapter_link" link2="mpo_700_wheel_front_left_link" reason="Never"/>
+    <disable_collisions link1="docking_adapter_link" link2="mpo_700_wheel_front_right_link" reason="Never"/>
+    <disable_collisions link1="docking_adapter_link" link2="ur10base_link_inertia" reason="Never"/>
+    <disable_collisions link1="docking_adapter_link" link2="ur10shoulder_link" reason="Never"/>
+    <disable_collisions link1="docking_adapter_link" link2="ur10upper_arm_link" reason="Never"/>
+
     <disable_collisions link1="lidar_1_link" link2="lidar_2_link" reason="Adjacent"/>
     <disable_collisions link1="lidar_1_link" link2="mpo_700_caster_back_left_link" reason="Never"/>
     <disable_collisions link1="lidar_1_link" link2="mpo_700_caster_back_right_link" reason="Never"/>

From 9a3613f3cf6c0c7e400eaa35b23fcb482690a321 Mon Sep 17 00:00:00 2001
From: Adarsh Karan Kesavadas Prasanth <adarshkaran01@gmail.com>
Date: Thu, 28 Nov 2024 22:19:15 +0530
Subject: [PATCH 2/3] Added use_docking_adapter condition in srdf

---
 neo_ur_moveit_config/srdf/mpo_700.srdf.xacro | 56 +++++++++++---------
 1 file changed, 30 insertions(+), 26 deletions(-)

diff --git a/neo_ur_moveit_config/srdf/mpo_700.srdf.xacro b/neo_ur_moveit_config/srdf/mpo_700.srdf.xacro
index 72f1ca5..30fc028 100644
--- a/neo_ur_moveit_config/srdf/mpo_700.srdf.xacro
+++ b/neo_ur_moveit_config/srdf/mpo_700.srdf.xacro
@@ -6,6 +6,7 @@
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="mpo_700">
      <xacro:arg name="prefix" default=""/>
      <xacro:property name="prefix_value" value="$(arg prefix)"/>
+     <xacro:arg name="use_docking_adapter" default="false"/>
 
     <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
     <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
@@ -23,20 +24,20 @@
     </group>
     <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
     <group_state name="up" group="ur_manipulator">
-      <joint name="${prefix_value}elbow_joint" value="0" />
-      <joint name="${prefix_value}shoulder_lift_joint" value="-1.5707" />
-      <joint name="${prefix_value}shoulder_pan_joint" value="0" />
-      <joint name="${prefix_value}wrist_1_joint" value="-1.5707" />
-      <joint name="${prefix_value}wrist_2_joint" value="0" />
-      <joint name="${prefix_value}wrist_3_joint" value="0" />
+        <joint name="${prefix_value}elbow_joint" value="0" />
+        <joint name="${prefix_value}shoulder_lift_joint" value="-1.5707" />
+        <joint name="${prefix_value}shoulder_pan_joint" value="0" />
+        <joint name="${prefix_value}wrist_1_joint" value="-1.5707" />
+        <joint name="${prefix_value}wrist_2_joint" value="0" />
+        <joint name="${prefix_value}wrist_3_joint" value="0" />
     </group_state>
     <group_state name="test_configuration" group="ur_manipulator">
-      <joint name="${prefix_value}elbow_joint" value="1.4" />
-      <joint name="${prefix_value}shoulder_lift_joint" value="-1.62" />
-      <joint name="${prefix_value}shoulder_pan_joint" value="1.54" />
-      <joint name="${prefix_value}wrist_1_joint" value="-1.2" />
-      <joint name="${prefix_value}wrist_2_joint" value="-1.6" />
-      <joint name="${prefix_value}wrist_3_joint" value="-0.11" />
+        <joint name="${prefix_value}elbow_joint" value="1.4" />
+        <joint name="${prefix_value}shoulder_lift_joint" value="-1.62" />
+        <joint name="${prefix_value}shoulder_pan_joint" value="1.54" />
+        <joint name="${prefix_value}wrist_1_joint" value="-1.2" />
+        <joint name="${prefix_value}wrist_2_joint" value="-1.6" />
+        <joint name="${prefix_value}wrist_3_joint" value="-0.11" />
     </group_state>
     
     <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
@@ -68,20 +69,23 @@
     <disable_collisions link1="cabinet_link" link2="mpo_700_wheel_front_right_link" reason="Never"/>
     <disable_collisions link1="cabinet_link" link2="${prefix_value}base_link_inertia" reason="Adjacent"/>
     <disable_collisions link1="cabinet_link" link2="${prefix_value}shoulder_link" reason="Never"/>
-
-    <disable_collisions link1="docking_adapter_link" link2="lidar_1_link" reason="Never"/>
-    <disable_collisions link1="docking_adapter_link" link2="lidar_2_link" reason="Never"/>
-    <disable_collisions link1="docking_adapter_link" link2="mpo_700_caster_back_left_link" reason="Never"/>
-    <disable_collisions link1="docking_adapter_link" link2="mpo_700_caster_back_right_link" reason="Never"/>
-    <disable_collisions link1="docking_adapter_link" link2="mpo_700_caster_front_left_link" reason="Never"/>
-    <disable_collisions link1="docking_adapter_link" link2="mpo_700_caster_front_right_link" reason="Never"/>
-    <disable_collisions link1="docking_adapter_link" link2="mpo_700_wheel_back_left_link" reason="Never"/>
-    <disable_collisions link1="docking_adapter_link" link2="mpo_700_wheel_back_right_link" reason="Never"/>
-    <disable_collisions link1="docking_adapter_link" link2="mpo_700_wheel_front_left_link" reason="Never"/>
-    <disable_collisions link1="docking_adapter_link" link2="mpo_700_wheel_front_right_link" reason="Never"/>
-    <disable_collisions link1="docking_adapter_link" link2="ur10base_link_inertia" reason="Never"/>
-    <disable_collisions link1="docking_adapter_link" link2="ur10shoulder_link" reason="Never"/>
-    <disable_collisions link1="docking_adapter_link" link2="ur10upper_arm_link" reason="Never"/>
+    
+    <!--Use docking adapter-->
+    <xacro:if value="$(arg use_docking_adapter)">
+        <disable_collisions link1="docking_adapter_link" link2="lidar_1_link" reason="Never"/>
+        <disable_collisions link1="docking_adapter_link" link2="lidar_2_link" reason="Never"/>
+        <disable_collisions link1="docking_adapter_link" link2="mpo_700_caster_back_left_link" reason="Never"/>
+        <disable_collisions link1="docking_adapter_link" link2="mpo_700_caster_back_right_link" reason="Never"/>
+        <disable_collisions link1="docking_adapter_link" link2="mpo_700_caster_front_left_link" reason="Never"/>
+        <disable_collisions link1="docking_adapter_link" link2="mpo_700_caster_front_right_link" reason="Never"/>
+        <disable_collisions link1="docking_adapter_link" link2="mpo_700_wheel_back_left_link" reason="Never"/>
+        <disable_collisions link1="docking_adapter_link" link2="mpo_700_wheel_back_right_link" reason="Never"/>
+        <disable_collisions link1="docking_adapter_link" link2="mpo_700_wheel_front_left_link" reason="Never"/>
+        <disable_collisions link1="docking_adapter_link" link2="mpo_700_wheel_front_right_link" reason="Never"/>
+        <disable_collisions link1="docking_adapter_link" link2="ur10base_link_inertia" reason="Never"/>
+        <disable_collisions link1="docking_adapter_link" link2="ur10shoulder_link" reason="Never"/>
+        <disable_collisions link1="docking_adapter_link" link2="ur10upper_arm_link" reason="Never"/>
+    </xacro:if>
 
     <disable_collisions link1="lidar_1_link" link2="lidar_2_link" reason="Adjacent"/>
     <disable_collisions link1="lidar_1_link" link2="mpo_700_caster_back_left_link" reason="Never"/>

From 9c30dfda5cc7ceebbfa9eed0e5a32500753a4fc6 Mon Sep 17 00:00:00 2001
From: Adarsh Karan Kesavadas Prasanth <adarshkaran01@gmail.com>
Date: Thu, 5 Dec 2024 12:09:06 +0530
Subject: [PATCH 3/3] Ensured Consistent indentations in srdf

---
 neo_ur_moveit_config/srdf/mpo_500.srdf.xacro | 28 ++++++++++----------
 neo_ur_moveit_config/srdf/mpo_700.srdf.xacro |  6 ++---
 2 files changed, 17 insertions(+), 17 deletions(-)

diff --git a/neo_ur_moveit_config/srdf/mpo_500.srdf.xacro b/neo_ur_moveit_config/srdf/mpo_500.srdf.xacro
index c362f67..4713f27 100644
--- a/neo_ur_moveit_config/srdf/mpo_500.srdf.xacro
+++ b/neo_ur_moveit_config/srdf/mpo_500.srdf.xacro
@@ -4,8 +4,8 @@
     A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
 -->
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="mpo_500">
-     <xacro:arg name="prefix" default=""/>
-     <xacro:property name="prefix_value" value="$(arg prefix)"/>
+    <xacro:arg name="prefix" default=""/>
+    <xacro:property name="prefix_value" value="$(arg prefix)"/>
 
     <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
     <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
@@ -24,20 +24,20 @@
 
     <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
     <group_state name="up" group="ur_manipulator">
-      <joint name="${prefix_value}elbow_joint" value="0" />
-      <joint name="${prefix_value}shoulder_lift_joint" value="-1.5707" />
-      <joint name="${prefix_value}shoulder_pan_joint" value="0" />
-      <joint name="${prefix_value}wrist_1_joint" value="-1.5707" />
-      <joint name="${prefix_value}wrist_2_joint" value="0" />
-      <joint name="${prefix_value}wrist_3_joint" value="0" />
+        <joint name="${prefix_value}elbow_joint" value="0" />
+        <joint name="${prefix_value}shoulder_lift_joint" value="-1.5707" />
+        <joint name="${prefix_value}shoulder_pan_joint" value="0" />
+        <joint name="${prefix_value}wrist_1_joint" value="-1.5707" />
+        <joint name="${prefix_value}wrist_2_joint" value="0" />
+        <joint name="${prefix_value}wrist_3_joint" value="0" />
     </group_state>
     <group_state name="test_configuration" group="ur_manipulator">
-      <joint name="${prefix_value}elbow_joint" value="1.4" />
-      <joint name="${prefix_value}shoulder_lift_joint" value="-1.62" />
-      <joint name="${prefix_value}shoulder_pan_joint" value="1.54" />
-      <joint name="${prefix_value}wrist_1_joint" value="-1.2" />
-      <joint name="${prefix_value}wrist_2_joint" value="-1.6" />
-      <joint name="${prefix_value}wrist_3_joint" value="-0.11" />
+        <joint name="${prefix_value}elbow_joint" value="1.4" />
+        <joint name="${prefix_value}shoulder_lift_joint" value="-1.62" />
+        <joint name="${prefix_value}shoulder_pan_joint" value="1.54" />
+        <joint name="${prefix_value}wrist_1_joint" value="-1.2" />
+        <joint name="${prefix_value}wrist_2_joint" value="-1.6" />
+        <joint name="${prefix_value}wrist_3_joint" value="-0.11" />
     </group_state>
 
     <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
diff --git a/neo_ur_moveit_config/srdf/mpo_700.srdf.xacro b/neo_ur_moveit_config/srdf/mpo_700.srdf.xacro
index 30fc028..d4fbf38 100644
--- a/neo_ur_moveit_config/srdf/mpo_700.srdf.xacro
+++ b/neo_ur_moveit_config/srdf/mpo_700.srdf.xacro
@@ -4,9 +4,9 @@
     A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
 -->
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="mpo_700">
-     <xacro:arg name="prefix" default=""/>
-     <xacro:property name="prefix_value" value="$(arg prefix)"/>
-     <xacro:arg name="use_docking_adapter" default="false"/>
+    <xacro:arg name="prefix" default=""/>
+    <xacro:property name="prefix_value" value="$(arg prefix)"/>
+    <xacro:arg name="use_docking_adapter" default="false"/>
 
     <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
     <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->