From 213444b6258322df110e7512ae87fb5eda62140c Mon Sep 17 00:00:00 2001
From: Zach Anderson <zach.inbox@gmail.com>
Date: Wed, 6 Sep 2017 23:12:58 -0500
Subject: [PATCH] Cleaned up pincher gripper description

Slightly restructured the TF tree of the pincher gripper to make more
sense compared to the physical construction of the gripper.
---
 .../urdf/arm_hardware.xacro                   | 45 +++---------------
 .../urdf/pincher_gripper.xacro                | 47 +++++++++----------
 .../urdf/turtlebot_arm.xacro                  |  7 ++-
 3 files changed, 33 insertions(+), 66 deletions(-)

diff --git a/turtlebot_arm_description/urdf/arm_hardware.xacro b/turtlebot_arm_description/urdf/arm_hardware.xacro
index f15a8f1..b33a0a5 100644
--- a/turtlebot_arm_description/urdf/arm_hardware.xacro
+++ b/turtlebot_arm_description/urdf/arm_hardware.xacro
@@ -10,6 +10,7 @@
   <xacro:property name="AX12_WIDTH" value="0.038"/>
   <xacro:property name="AX12_LENGTH" value="0.050"/>
   <xacro:property name="F2_HEIGHT" value="0.0265"/>
+  <xacro:property name="FINGER_HEIGHT" value="0.038"/>
 
   <!-- Calculates the inertial properties of a bounding box. From
        https://en.wikipedia.org/wiki/List_of_moments_of_inertia#List_of_3D_inertia_tensors-->
@@ -56,39 +57,6 @@
     </gazebo>
   </xacro:macro>
 
-  <xacro:macro name="finger_fixed" params="parent name color *origin">
-    <joint name="${name}_joint" type="fixed">
-      <xacro:insert_block name="origin"/>
-      <parent link="${parent}"/>
-      <child link="${name}_link"/>
-    </joint>
-
-    <link name="${name}_link">
-      <xacro:box_inertia length="0.0783" width="0.03801" height="0.0193" mass="0.015"/>
-
-      <visual>
-        <origin xyz=" 0 0 0 " rpy="0 0 0"/>
-        <geometry>
-          <mesh filename="package://turtlebot_arm_description/meshes/turtlebot_finger.stl" scale="${M_SCALE} ${M_SCALE} ${M_SCALE}"/>
-        </geometry>
-        <material name="${color}"/>
-      </visual>
-
-      <collision>
-        <origin xyz="0.02645 0.0 -0.00655" rpy="0 0 0"/>
-        <geometry>
-          <box size="0.0783 0.03801 0.0193"/>
-        </geometry>
-      </collision>
-    </link>
-    
-    <gazebo reference="${name}_link">
-      <material>Gazebo/${color}</material>
-      <selfCollide>true</selfCollide>
-      <gravity>true</gravity>
-    </gazebo>
-  </xacro:macro>
-
   <xacro:macro name="dynamixel_AX12_fixed" params="parent name *origin">
     <joint name="${name}_joint" type="fixed">
       <xacro:insert_block name="origin"/>
@@ -310,22 +278,21 @@
     </joint>
     
     <link name="${name}_link">
-      <xacro:box_inertia length="0.002" width="0.040" height="0.075" mass="0.00001"/>
+      <xacro:box_inertia length="0.002" width="0.075" height="0.040" mass="0.00001"/>
       <visual>
-        <origin xyz="0.016 0 -.015 " rpy="${M_PI} ${-M_PI/2} ${M_PI/2}"/>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
         <geometry>
-          <mesh filename="package://turtlebot_arm_description/meshes/pincher_finger_base.stl" scale=".01 .01 .01"/>
+          <box size="0.002 0.075 0.040"/>
         </geometry>
         <material name="${color}"/>
       </visual>
       
       <collision>
-        <origin xyz="0.016 0.0 -0.015" rpy="0 0 0"/>
         <geometry>
-          <box size="0.002 0.040 0.075"/>
+          <box size="0.002 0.075 0.040"/>
         </geometry>
       </collision>
-    </link >
+    </link>
     
     <gazebo reference="${name}_link">
       <material>Gazebo/${color}</material>
diff --git a/turtlebot_arm_description/urdf/pincher_gripper.xacro b/turtlebot_arm_description/urdf/pincher_gripper.xacro
index 96d601a..7d7bb39 100644
--- a/turtlebot_arm_description/urdf/pincher_gripper.xacro
+++ b/turtlebot_arm_description/urdf/pincher_gripper.xacro
@@ -1,6 +1,8 @@
 <?xml version="1.0"?>
 <!-- PhantomX Pincher Arm gripper URDF-->
 <robot xmlns:xacro="http://ros.org/wiki/xacro">
+    <xacro:property name="GRIPPER_MAX" value="0.035"/>
+
     <bioloid_F3_fixed parent="arm_wrist_flex_link" name="arm_wrist_F3_0" color="${color}">
       <origin xyz="0 0 ${F2_HEIGHT}" rpy="0 ${M_PI} ${-M_PI}"/>
     </bioloid_F3_fixed>
@@ -10,43 +12,36 @@
       <origin xyz="0 0 ${-AX12_WIDTH/2}" rpy="${M_PI/2} ${M_PI} ${M_PI/2}"/>
     </dynamixel_AX12_fixed>
     
-     <gripper_finger_base parent="gripper_servo_link" name="gripper_finger_base" color="${color}">
-      <origin xyz="0.01  ${AX12_WIDTH} 0" rpy="${M_PI/2} ${-M_PI} ${M_PI/2}"/>
+    <gripper_finger_base parent="gripper_servo_link" name="gripper_finger_base" color="${color}">
+        <origin xyz="0 ${AX12_WIDTH/2} 0" rpy="${M_PI} 0 ${M_PI/2}"/>
     </gripper_finger_base>
   
-     <!-- Finger 1 --> 
+    <!-- Master finger -->
+    <!-- NOTE: The transmission for gripper_joint MUST specify a mechnaical reduction of two for
+         the gripper seperation to be calculated correctly -->
     <joint name="gripper_joint" type="prismatic">
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <axis xyz="1 0 0"/>
-      <limit effort="30" velocity="0.5" lower="0.002" upper="0.031"/>
-      <parent link="gripper_servo_link"/>
+      <origin xyz="${FINGER_HEIGHT/2} 0 0" rpy="0 0 0"/>
+      <axis xyz="0 1 0"/>
+      <limit effort="30" velocity="0.05" lower="0.001" upper="${GRIPPER_MAX/2}"/>
+      <parent link="gripper_finger_base_link"/>
       <child link="gripper_active_link"/>
     </joint> 
     
     <pincher_gripper name="gripper_active" color="${color}" >
-      <origin xyz="-0.0086 0.0414 0" rpy="${-M_PI/2} 0 0"/>
+      <origin xyz="0 0.01 0" rpy="${M_PI/2} 0 ${M_PI/2}"/>
     </pincher_gripper>
      
-    <!-- Finger 2 --> 
-    <!-- Note: currently static but should be a Mimic of Finger 1 -->
-    <joint name="gripper2_joint" type="fixed">
-      <parent link="gripper_servo_link"/>
+    <!-- Slave finger -->
+    <joint name="gripper2_joint" type="prismatic">
+      <origin xyz="${FINGER_HEIGHT/2} 0 0" rpy="${M_PI} 0 0"/>
+      <axis xyz="0 1 0"/>
+      <limit effort="30" velocity="0.05" lower="0.001" upper="${GRIPPER_MAX/2}"/>
+      <parent link="gripper_finger_base_link"/>
       <child link="gripper_active2_link"/>
+      <mimic joint="gripper_joint"/>
     </joint>  
-    
+
     <pincher_gripper name="gripper_active2" color="${color}" >
-      <origin xyz="-0.0314 0.0414 0" rpy="${-M_PI/2} ${M_PI} 0"/>
+      <origin xyz="0 0.01 0" rpy="${M_PI/2} 0 ${M_PI/2}"/>
     </pincher_gripper>
-      
-    <!-- Using Mimic -->
-    <!-- gripper 2 joint -->
-    <!--  ### Mimic would be better but causes Missing Joint error in Moveit 
-      <joint name="gripper2_joint" type="prismatic">
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <axis xyz="0 0 1"/>
-      <limit effort="30" velocity="0.5" lower="0.002" upper="0.031"/>
-      <parent link="gripper_servo_link"/>
-      <child link="gripper_active2_link"/>
-      <mimic joint="gripper_joint" multiplier="-.5"  offset = "0"/>
-    </joint-->  
 </robot>
diff --git a/turtlebot_arm_description/urdf/turtlebot_arm.xacro b/turtlebot_arm_description/urdf/turtlebot_arm.xacro
index 56fca5d..6ac88ee 100644
--- a/turtlebot_arm_description/urdf/turtlebot_arm.xacro
+++ b/turtlebot_arm_description/urdf/turtlebot_arm.xacro
@@ -29,7 +29,12 @@
          that's a lot of coin! -->
     <link name="gripper_link"/>
     <joint name="gripper_link_joint" type="revolute">
-      <origin xyz="0 0 0.112" rpy="0 -1.57 0"/>
+      <xacro:if value="${turtlebot_gripper}">
+        <origin xyz="0 0 0.112" rpy="0 -1.57 0"/>
+      </xacro:if>
+      <xacro:if value="${pincher_gripper}">
+        <origin xyz="0 0 0.085" rpy="0 -1.57 0"/>
+      </xacro:if>
       <parent link="arm_wrist_flex_link"/>
       <child link="gripper_link"/>
       <limit effort="30" velocity="1" lower="-3.14" upper="3.14"/>