Merge pull request #7 from marip8/updates

Minor Updates for Compatibility with SNP

config/workcell.srdf

@@ -14,24 +14,29 @@
     <!-- Contact manager plugins -->
     <contact_managers_plugin_config filename="package://snp_automate_2022/config/workcell_plugins.yaml"/>
 
-    <disable_collisions link1="tool0" link2="ee" reason="Adjacent"/>
-    <disable_collisions link1="link_1_s" link2="link_6_t" reason="Never"/>
-    <disable_collisions link1="link_2_l" link2="link_6_t" reason="Never"/>
-    <disable_collisions link1="base_link" link2="table" reason="Never"/>
-    <disable_collisions link1="link_5_b" link2="link_4_r" reason="Adjacent"/>
-    <disable_collisions link1="link_1_s" link2="table" reason="Never"/>
-    <disable_collisions link1="link_4_r" link2="link_1_s" reason="Never"/>
+    <disable_collisions link1="floor" link2="table" reason="Never"/>
+    <disable_collisions link1="floor" link2="base_link" reason="Never"/>
+    <disable_collisions link1="floor" link2="link_1_s" reason="Never"/>
+    <disable_collisions link1="floor" link2="link_2_l" reason="Never"/>
+    <disable_collisions link1="floor" link2="link_3_u" reason="Never"/>
+    <disable_collisions link1="table" link2="base_link" reason="Never"/>
+    <disable_collisions link1="table" link2="link_1_s" reason="Never"/>
     <disable_collisions link1="base_link" link2="link_1_s" reason="Adjacent"/>
-    <disable_collisions link1="link_6_t" link2="link_3_u" reason="Never"/>
-    <disable_collisions link1="link_5_b" link2="link_6_t" reason="Adjacent"/>
-    <disable_collisions link1="link_2_l" link2="link_1_s" reason="Adjacent"/>
-    <disable_collisions link1="link_2_l" link2="table" reason="Never"/>
-    <disable_collisions link1="link_6_t" link2="ee" reason="Never"/>
-    <disable_collisions link1="link_4_r" link2="link_3_u" reason="Adjacent"/>
+    <disable_collisions link1="base_link" link2="link_3_u" reason="Never"/>
+    <disable_collisions link1="link_1_s" link2="link_2_l" reason="Adjacent"/>
+    <disable_collisions link1="link_1_s" link2="link_3_u" reason="Never"/>
     <disable_collisions link1="link_2_l" link2="link_3_u" reason="Adjacent"/>
-    <disable_collisions link1="link_5_b" link2="link_2_l" reason="Never"/>
-    <disable_collisions link1="link_5_b" link2="link_3_u" reason="Never"/>
-    <disable_collisions link1="base_link" link2="link_2_l" reason="Never"/>
     <disable_collisions link1="link_2_l" link2="link_4_r" reason="Never"/>
+    <disable_collisions link1="link_3_u" link2="link_4_r" reason="Adjacent"/>
+    <disable_collisions link1="link_3_u" link2="link_5_b" reason="Never"/>
+    <disable_collisions link1="link_3_u" link2="link_6_t" reason="Never"/>
+    <disable_collisions link1="link_3_u" link2="ee" reason="Never"/>
+    <disable_collisions link1="link_4_r" link2="link_5_b" reason="Adjacent"/>
     <disable_collisions link1="link_4_r" link2="link_6_t" reason="Never"/>
+    <disable_collisions link1="link_5_b" link2="link_6_t" reason="Adjacent"/>
+    <disable_collisions link1="link_5_b" link2="ee" reason="Never"/>
+    <disable_collisions link1="link_5_b" link2="sand_tcp" reason="Never"/>
+    <disable_collisions link1="link_6_t" link2="ee" reason="Always"/>
+    <disable_collisions link1="link_6_t" link2="sand_tcp" reason="Never"/>
+    <disable_collisions link1="ee" link2="sand_tcp" reason="Always"/>
 </robot>

launch/start.launch.xml

@@ -6,7 +6,8 @@
   <arg name="mesh_file" default="$(find-pkg-share snp_automate_2022)/meshes/part_scan.ply"/>
   <arg name="scan_trajectory_file" default="$(find-pkg-share snp_automate_2022)/config/scan_traj.yaml"/>
   <arg name="verbose" default="True"/>
-  <arg name="touch_links" default="[table, base_link, floor]"/>
+  <arg name="scan_disabled_contact_links" default="[table, base_link, floor]"/>
+  <arg name="scan_reduced_contact_links" default="[sand_tcp]"/>
   <arg name="robot_description_file" default="$(find-pkg-share snp_automate_2022)/urdf/workcell.xacro"/>
   <arg name="robot_description" default="$(command 'xacro $(var robot_description_file) ros_distro:=$(env ROS_DISTRO)')"/>
   <arg name="robot_description_semantic" default="$(command 'cat $(find-pkg-share snp_automate_2022)/config/workcell.srdf')"/>
@@ -41,6 +42,7 @@
     <param name="camera_frame" value="camera_color_optical_frame"/>
     <param name="robot_description" value="$(var robot_description)"/>
     <param name="robot_description_semantic" value="$(var robot_description_semantic)"/>
+    <param name="follow_joint_trajectory_action" value="$(var follow_joint_trajectory_action)"/>
     <!-- TSDF parameters -->
     <param name="tsdf.min.x" value="0.21"/>
     <param name="tsdf.min.y" value="-0.45"/>
@@ -83,7 +85,8 @@
     <arg name="robot_description" value="$(var robot_description)"/>
     <arg name="robot_description_semantic" value="$(var robot_description_semantic)"/>
     <arg name="verbose" value="$(var verbose)"/>
-    <arg name="touch_links" value="$(var touch_links)"/>
+    <arg name="scan_disabled_contact_links" value="$(var scan_disabled_contact_links)"/>
+    <arg name="scan_reduced_contact_links" value="$(var scan_reduced_contact_links)"/>
   </include>
 
   <node pkg="snp_scanning" exec="scan_motion_plan_from_file_node">

urdf/workcell.xacro

@@ -167,7 +167,23 @@
   </joint>
 
   <!-- Add TCP -->
-  <link name="sand_tcp"/>
+  <xacro:property name="media_radius" value="0.075"/>
+  <xacro:property name="media_length" value="0.05"/>
+  <xacro:property name="media_collision_offset" value="0.01"/>
+  <link name="sand_tcp">
+    <visual>
+      <origin xyz="0.0 0.0 ${-(media_length / 2.0)}"/>
+      <geometry>
+        <cylinder radius="${media_radius}" length="${media_length}"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 ${-(media_length / 2.0) - media_collision_offset}"/>
+      <geometry>
+        <cylinder radius="${media_radius}" length="${media_length - media_collision_offset}"/>
+      </geometry>
+    </collision>
+  </link>
   <joint name="sand_tcp_joint" type="fixed">
     <parent link="tool_flange"/>
     <child link="sand_tcp"/>