Merge pull request #92 from PickNikRobotics/update-pinch-mock

Update Mock Hardware Config to use new Pinch Geometries

src/picknik_ur_mock_hw_config/config/moveit/picknik_ur_machine_tending.srdf

@@ -56,9 +56,27 @@
     <disable_collisions link1="forearm_link" link2="upper_arm_link" reason="Adjacent"/>
     <disable_collisions link1="forearm_link" link2="wrist_1_link" reason="Adjacent"/>
     <disable_collisions link1="forearm_link" link2="wrist_2_link" reason="Never"/>
-    <disable_collisions link1="pinch_link" link2="forearm_link" reason="Adjacent"/>
-    <disable_collisions link1="pinch_link" link2="upper_arm_link" reason="Never"/>
-    <disable_collisions link1="pinch_link" link2="wrist_1_link" reason="Never"/>
+    <disable_collisions link1="forearm_pinch_link" link2="forearm_link" reason="Adjacent"/>
+    <disable_collisions link1="forearm_pinch_link" link2="tool_changer_link" reason="Never"/>
+    <disable_collisions link1="forearm_pinch_link" link2="upper_arm_link" reason="Never"/>
+    <disable_collisions link1="forearm_pinch_link" link2="wrist_1_link" reason="Never"/>
+    <disable_collisions link1="forearm_pinch_link" link2="wrist_2_link" reason="Never"/>
+    <disable_collisions link1="wrist_3_pinch_link" link2="realsense_camera_adapter_link" reason="Adjacent"/>
+    <disable_collisions link1="wrist_3_pinch_link" link2="robotiq_85_base_link" reason="Adjacent"/>
+    <disable_collisions link1="wrist_3_pinch_link" link2="robotiq_85_left_finger_link" reason="Never"/>
+    <disable_collisions link1="wrist_3_pinch_link" link2="robotiq_85_left_finger_tip_link" reason="Never"/>
+    <disable_collisions link1="wrist_3_pinch_link" link2="robotiq_85_left_inner_knuckle_link" reason="Never"/>
+    <disable_collisions link1="wrist_3_pinch_link" link2="robotiq_85_left_knuckle_link" reason="Never"/>
+    <disable_collisions link1="wrist_3_pinch_link" link2="robotiq_85_right_finger_link" reason="Never"/>
+    <disable_collisions link1="wrist_3_pinch_link" link2="robotiq_85_right_finger_tip_link" reason="Never"/>
+    <disable_collisions link1="wrist_3_pinch_link" link2="robotiq_85_right_inner_knuckle_link" reason="Never"/>
+    <disable_collisions link1="wrist_3_pinch_link" link2="robotiq_85_right_knuckle_link" reason="Never"/>
+    <disable_collisions link1="wrist_3_pinch_link" link2="tool_changer_link" reason="Adjacent"/>
+    <disable_collisions link1="wrist_3_pinch_link" link2="ur_to_robotiq_link" reason="Adjacent"/>
+    <disable_collisions link1="wrist_3_pinch_link" link2="wrist_1_link" reason="Adjacent"/>
+    <disable_collisions link1="wrist_3_pinch_link" link2="wrist_2_link" reason="Adjacent"/>
+    <disable_collisions link1="wrist_3_pinch_link" link2="wrist_3_link" reason="Adjacent"/>
+    <disable_collisions link1="wrist_3_pinch_link" link2="wrist_mounted_camera_link" reason="Never"/>
     <disable_collisions link1="realsense_camera_adapter_link" link2="robotiq_85_base_link" reason="Never"/>
     <disable_collisions link1="realsense_camera_adapter_link" link2="robotiq_85_left_finger_link" reason="Never"/>
     <disable_collisions link1="realsense_camera_adapter_link" link2="robotiq_85_left_finger_tip_link" reason="Never"/>

src/picknik_ur_mock_hw_config/config/moveit/ur_servo.yaml

@@ -59,5 +59,5 @@ command_out_topic: /streaming_controller/commands  # Publish outgoing commands h
 ## Collision checking for the entire robot body
 check_collisions: true  # Check collisions?
 collision_check_rate: 20.0  # [Hz] Collision-checking can easily bog down a CPU if done too often.
-self_collision_proximity_threshold: 0.015  # Start decelerating when a self-collision is this far [m]
+self_collision_proximity_threshold: 0.006  # Start decelerating when a self-collision is this far [m]
 scene_collision_proximity_threshold: 0.02  # Start decelerating when a scene collision is this far [m]

src/picknik_ur_mock_hw_config/description/picknik_ur_machine_tending.xacro

@@ -4,7 +4,7 @@
   <xacro:arg name="use_fake_hardware" default="false" />
   <xacro:arg name="external_camera" default="false" />
   <xacro:arg name="has_tool_changer" default="true" />
-  <xacro:arg name="use_pinch_link" default="true" />
+  <xacro:arg name="use_pinch_links" default="true" />
   <xacro:arg name="name" default="" />
   <xacro:arg name="joint_limits_parameters_file" default="" />
   <xacro:arg name="kinematics_parameters_file" default="" />
@@ -28,7 +28,7 @@
   <xacro:include filename="$(find picknik_accessories)/descriptions/brackets/mtc_ur_tool_changer/mtc_ur_tool_changer.urdf.xacro" />
   <xacro:include filename="$(find picknik_accessories)/descriptions/brackets/ur_realsense_camera_adapter/picknik_ur_camera_adapter.urdf.xacro" />
   <xacro:include filename="$(find picknik_accessories)/descriptions/sensors/realsense_d415.urdf.xacro" />
-  <xacro:include filename="$(find picknik_ur_base_config)/description/pinch_link.xacro" />
+  <xacro:include filename="$(find picknik_ur_base_config)/description/pinch_links.xacro" />
 
   <!-- Initial positions for simulations (Mock Hardware and Gazebo) -->
   <xacro:arg name="initial_positions_file" default="$(find picknik_ur_mock_hw_config)/config/initial_positions.yaml"/>
@@ -60,8 +60,9 @@
   </xacro:if>
 
   <!-- pinch geometry for moveit motion planning. see https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/611 -->
-  <xacro:if value="$(arg use_pinch_link)">
-    <xacro:pinch_link prefix="" />
+  <xacro:if value="$(arg use_pinch_links)">
+    <xacro:forearm_pinch_link />
+    <xacro:wrist_3_pinch_link />
   </xacro:if>
 
   <!-- wrist camera adapter and camera-->