Updating collision operations including base and making things pretty again

ur5_base/launch/load_pretty.launch

+<launch>
+  <param name="robot_description" command="$(find xacro)/xacro.py '$(find ur5_base)/urdf/base.urdf.pretty.xacro'" />
+</launch>

ur5_base/urdf/base.urdf.pretty.xacro

+<?xml version="1.0"?>
+
+<robot name="ur5_with_base">
+  <include filename="$(find ur5_description)/urdf/model.urdf.pretty.xacro" />
+  
+  <link name="world_link" />
+  <joint name="base_1_joint" type="fixed" >
+    <parent link="world_link" />
+    <child link= "base_1_link" />
+  </joint>
+  <joint name="base_2_joint" type="fixed" >
+    <parent link="base_1_link" />
+    <child link= "base_2_link" />
+  </joint>
+  <joint name="base_3_joint" type="fixed" >
+    <parent link="base_2_link" />
+    <child link= "base_3_link" />
+  </joint>
+  <joint name="base_4_joint" type="fixed" >
+    <parent link="base_3_link" />
+    <child link= "base_4_link" />
+  </joint>
+  <joint name="base_5_joint" type="fixed" >
+    <parent link="base_4_link" />
+    <child link= "base_5_link" />
+  </joint>
+  <joint name="robot_joint" type="fixed" >
+    <parent link="base_5_link" />
+    <child link= "base_link" />
+    <origin xyz="0.0 0.50 0.55" rpy="0.0 0.0 0.0" />
+  </joint>
+
+  <link name="base_1_link">
+    <visual>
+      <geometry>
+        <box size="0.25 0.50 0.10" />
+      </geometry>
+      <origin xyz="0.0 0.25 0.05" rpy="0.0 0.0 0.0" />
+    </visual>
+  </link>
+
+  <link name="base_2_link">
+    <visual>
+      <geometry>
+        <box size="0.25 0.45 0.80" />
+      </geometry>
+      <origin xyz="0.0 -0.225 0.40" rpy="0.0 0.0 0.0" />
+    </visual>
+  </link>
+
+  <link name="base_3_link">
+    <visual>
+      <geometry>
+        <box size="0.25 0.60 0.05" />
+      </geometry>
+      <origin xyz="0.0 0.25 0.525" rpy="0.0 0.0 0.0" />
+    </visual>
+  </link>
+
+  <link name="base_4_link">
+    <visual>
+      <geometry>
+        <box size="0.25 0.05 0.65" />
+      </geometry>
+      <origin xyz="0.0 0.075 0.695" rpy="0.0 0.0 0.0" />
+    </visual>
+  </link>
+
+  <link name="base_5_link">
+    <visual>
+      <geometry>
+        <box size="0.45 0.25 0.30" />
+      </geometry>
+      <origin xyz="0.0 -0.325 1.00" rpy="0.0 0.0 0.0" />
+    </visual>
+  </link>
+
+</robot>

ur5_moveit/config/collision_matrix.yaml

 default_collision_operations:
+  - object1: world_link
+    object2: base_1_link
+    operation: disable  # Adjacent in collision
+  - object1: base_1_link
+    object2: base_2_link
+    operation: disable  # Adjacent in collision
+  - object1: base_2_link
+    object2: base_3_link
+    operation: disable  # Adjacent in collision
+  - object1: base_3_link
+    object2: base_4_link
+    operation: disable  # Adjacent in collision
+  - object1: base_4_link
+    object2: base_5_link
+    operation: disable  # Adjacent in collision
+  - object1: base_5_link
+    object2: base_link
+    operation: disable  # Adjacent in collision
   - object1: base_link
     object2: shoulder_link
     operation: disable  # Adjacent in collision
@@ -26,6 +44,81 @@ default_collision_operations:
   - object1: upper_arm_link
     object2: shoulder_collision_link
     operation: disable  # Adjacent in collision
+  - object1: base_3_link
+    object2: base_link
+    operation: disable  # Always in collision
+  - object1: shoulder_collision_link
+    object2: shoulder_link
+    operation: disable  # Always in collision
+  - object1: base_1_link
+    object2: base_3_link
+    operation: disable  # Never in collision
+  - object1: base_1_link
+    object2: base_4_link
+    operation: disable  # Never in collision
+  - object1: base_1_link
+    object2: base_5_link
+    operation: disable  # Never in collision
+  - object1: base_1_link
+    object2: base_link
+    operation: disable  # Never in collision
+  - object1: base_1_link
+    object2: elbow_collision_link
+    operation: disable  # Never in collision
+  - object1: base_1_link
+    object2: shoulder_collision_link
+    operation: disable  # Never in collision
+  - object1: base_1_link
+    object2: shoulder_link
+    operation: disable  # Never in collision
+  - object1: base_1_link
+    object2: upper_arm_link
+    operation: disable  # Never in collision
+  - object1: base_2_link
+    object2: base_4_link
+    operation: disable  # Never in collision
+  - object1: base_2_link
+    object2: base_5_link
+    operation: disable  # Never in collision
+  - object1: base_2_link
+    object2: base_link
+    operation: disable  # Never in collision
+  - object1: base_2_link
+    object2: shoulder_collision_link
+    operation: disable  # Never in collision
+  - object1: base_2_link
+    object2: shoulder_link
+    operation: disable  # Never in collision
+  - object1: base_3_link
+    object2: base_5_link
+    operation: disable  # Never in collision
+  - object1: base_3_link
+    object2: shoulder_collision_link
+    operation: disable  # Never in collision
+  - object1: base_3_link
+    object2: shoulder_link
+    operation: disable  # Never in collision
+  - object1: base_4_link
+    object2: base_link
+    operation: disable  # Never in collision
+  - object1: base_4_link
+    object2: shoulder_collision_link
+    operation: disable  # Never in collision
+  - object1: base_4_link
+    object2: shoulder_link
+    operation: disable  # Never in collision
+  - object1: base_5_link
+    object2: elbow_collision_link
+    operation: disable  # Never in collision
+  - object1: base_5_link
+    object2: shoulder_collision_link
+    operation: disable  # Never in collision
+  - object1: base_5_link
+    object2: shoulder_link
+    operation: disable  # Never in collision
+  - object1: base_5_link
+    object2: upper_arm_link
+    operation: disable  # Never in collision
   - object1: base_link
     object2: elbow_collision_link
     operation: disable  # Never in collision
@@ -53,39 +146,15 @@ default_collision_operations:
   - object1: elbow_collision_link
     object2: wrist_3_link
     operation: disable  # Never in collision
-  - object1: forearm_link
-    object2: shoulder_collision_link
-    operation: disable  # Never in collision
-  - object1: forearm_link
-    object2: shoulder_link
-    operation: disable  # Never in collision
   - object1: forearm_link
     object2: wrist_2_link
     operation: disable  # Never in collision
   - object1: forearm_link
     object2: wrist_3_link
     operation: disable  # Never in collision
-  - object1: shoulder_collision_link
-    object2: shoulder_link
-    operation: disable  # Never in collision
-  - object1: shoulder_collision_link
-    object2: wrist_1_link
-    operation: disable  # Never in collision
-  - object1: shoulder_collision_link
-    object2: wrist_2_link
-    operation: disable  # Never in collision
-  - object1: shoulder_collision_link
-    object2: wrist_3_link
-    operation: disable  # Never in collision
   - object1: shoulder_link
     object2: wrist_1_link
     operation: disable  # Never in collision
-  - object1: shoulder_link
-    object2: wrist_2_link
-    operation: disable  # Never in collision
-  - object1: shoulder_link
-    object2: wrist_3_link
-    operation: disable  # Never in collision
   - object1: upper_arm_link
     object2: wrist_1_link
     operation: disable  # Never in collision
@@ -97,4 +166,4 @@ default_collision_operations:
     operation: disable  # Never in collision
   - object1: wrist_1_link
     object2: wrist_3_link
-    operation: disable  # Never in collision
\ No newline at end of file
+    operation: disable  # Never in collision

ur5_moveit/launch/ur5_moveit_visualization.launch

@@ -2,7 +2,7 @@
   <arg name="monitor_robot_state" default="false"/>
   <arg name="allow_trajectory_execution" default="false"/>
 
-  <include file="$(find ur5_description)/launch/load_pretty.launch" />
+  <include file="$(find ur5_base)/launch/load_pretty.launch" />
   <!--<param name="robot_description" textfile="$(find ur5_description)/urdf/universal_or.dae" /-->
   <param name="robot_description_semantic" textfile="$(find ur5_moveit)/config/ur5.srdf" />