From ea4c01d9e0f7b7d7c6f8de25bae6097038f5ced1 Mon Sep 17 00:00:00 2001
From: Alexandre Vannobel <41082816+alexvannobel@users.noreply.github.com>
Date: Tue, 9 Jun 2020 08:47:28 -0400
Subject: [PATCH] Tool frame (#97)

* Added tool_frame's for every gripper

* Add tool_frame to MoveIt arm planning group and a small description of the frame
---
 .../gen3_lite_2f/urdf/gen3_lite_2f_macro.xacro         | 10 ++++++++++
 .../urdf/robotiq_arg2f_140_model_macro.xacro           |  9 +++++++++
 .../urdf/robotiq_arg2f_85_model_macro.xacro            |  9 +++++++++
 kortex_description/readme.md                           |  6 +++++-
 kortex_description/robots/kortex_robot.xacro           | 10 ++++++++++
 .../config/gen3_lite_gen3_lite_2f.srdf                 |  3 ++-
 .../gen3_move_it_config/config/6dof/gen3.srdf          |  1 +
 .../gen3_move_it_config/config/7dof/gen3.srdf          |  1 +
 .../config/6dof/gen3_robotiq_2f_140.srdf               |  1 +
 .../config/7dof/gen3_robotiq_2f_140.srdf               |  1 +
 .../config/6dof/gen3_robotiq_2f_85.srdf                |  1 +
 .../config/7dof/gen3_robotiq_2f_85.srdf                |  1 +
 12 files changed, 51 insertions(+), 2 deletions(-)

diff --git a/kortex_description/grippers/gen3_lite_2f/urdf/gen3_lite_2f_macro.xacro b/kortex_description/grippers/gen3_lite_2f/urdf/gen3_lite_2f_macro.xacro
index ff8b3453..69891566 100644
--- a/kortex_description/grippers/gen3_lite_2f/urdf/gen3_lite_2f_macro.xacro
+++ b/kortex_description/grippers/gen3_lite_2f/urdf/gen3_lite_2f_macro.xacro
@@ -3,6 +3,16 @@
 <robot name="gen3_lite_2f_gripper" xmlns:xacro="http://ros.org/wiki/xacro">
   <xacro:macro name="load_gripper" params="parent prefix"> 
 
+    <!-- Tool frame used by the arm -->
+    <link name="${prefix}tool_frame"/>
+    <joint name="${prefix}tool_frame_joint" type="fixed">
+        <origin xyz="0 0 0.130" rpy="0 0 1.57079632679" />
+        <parent link="${parent}" />
+        <child link="${prefix}tool_frame" />
+        <axis xyz="0 0 0" />
+    </joint>
+
+    <!-- Gripper description -->
     <joint name="${prefix}gripper_base_joint" type="fixed">
       <origin xyz="0 0 0" rpy="0 0 0" />
       <parent link="${parent}" />
diff --git a/kortex_description/grippers/robotiq_2f_140/urdf/robotiq_arg2f_140_model_macro.xacro b/kortex_description/grippers/robotiq_2f_140/urdf/robotiq_arg2f_140_model_macro.xacro
index a383edea..484121fe 100644
--- a/kortex_description/grippers/robotiq_2f_140/urdf/robotiq_arg2f_140_model_macro.xacro
+++ b/kortex_description/grippers/robotiq_2f_140/urdf/robotiq_arg2f_140_model_macro.xacro
@@ -232,6 +232,15 @@
   </xacro:macro>
 
   <xacro:macro name="robotiq_arg2f_140" params="parent prefix">
+    <!-- Tool frame used by the arm -->
+    <link name="${prefix}tool_frame"/>
+    <joint name="${prefix}tool_frame_joint" type="fixed">
+        <origin xyz="0 0 0.20" rpy="0 0 0" />
+        <parent link="${parent}" />
+        <child link="${prefix}tool_frame" />
+        <axis xyz="0 0 0" />
+    </joint>
+    <!-- Gripper description -->
     <xacro:robotiq_arg2f_base_link prefix="${prefix}" parent="${parent}"/>
     <xacro:finger_links prefix="${prefix}" fingerprefix="left" stroke="140"/>
     <xacro:finger_links prefix="${prefix}" fingerprefix="right" stroke="140"/>
diff --git a/kortex_description/grippers/robotiq_2f_85/urdf/robotiq_arg2f_85_model_macro.xacro b/kortex_description/grippers/robotiq_2f_85/urdf/robotiq_arg2f_85_model_macro.xacro
index 6d17507a..b8247e74 100644
--- a/kortex_description/grippers/robotiq_2f_85/urdf/robotiq_arg2f_85_model_macro.xacro
+++ b/kortex_description/grippers/robotiq_2f_85/urdf/robotiq_arg2f_85_model_macro.xacro
@@ -195,6 +195,15 @@
 
   <!-- Initialization of all joints and links -->
   <xacro:macro name="robotiq_arg2f_85" params="prefix parent">
+    <!-- Tool frame used by the arm -->
+    <link name="${prefix}tool_frame"/>
+    <joint name="${prefix}tool_frame_joint" type="fixed">
+        <origin xyz="0 0 0.120" rpy="0 0 0" />
+        <parent link="${parent}" />
+        <child link="${prefix}tool_frame" />
+        <axis xyz="0 0 0" />
+    </joint>
+    <!-- Gripper description -->
     <xacro:robotiq_arg2f_base_link prefix="${prefix}" parent="${parent}"/>
     <xacro:finger_links prefix="${prefix}" fingerprefix="left" stroke="85"/>
     <xacro:finger_links prefix="${prefix}" fingerprefix="right" stroke="85"/>
diff --git a/kortex_description/readme.md b/kortex_description/readme.md
index 4a52a139..2ecf830b 100644
--- a/kortex_description/readme.md
+++ b/kortex_description/readme.md
@@ -26,4 +26,8 @@ For example:
 <code><param name="robot_description" command="$(find xacro)/xacro --inorder $(find kortex_description)/robots/gen3_robotiq_2f_85.xacro sim:=true"\/></code>
 
 - To load the Gen3 lite description, you would put in your launch file : 
-<code><param name="robot_description" command="$(find xacro)/xacro --inorder $(find kortex_description)/robots/gen3_lite_gen3_lite_2f.xacro sim:=false"\/></code>
\ No newline at end of file
+<code><param name="robot_description" command="$(find xacro)/xacro --inorder $(find kortex_description)/robots/gen3_lite_gen3_lite_2f.xacro sim:=false"\/></code>
+
+## Tool frame
+
+The `tool_frame` link refers to the tool frame used by the arm when it reports end effector position feedback.
\ No newline at end of file
diff --git a/kortex_description/robots/kortex_robot.xacro b/kortex_description/robots/kortex_robot.xacro
index 807ef1b5..684a644c 100644
--- a/kortex_description/robots/kortex_robot.xacro
+++ b/kortex_description/robots/kortex_robot.xacro
@@ -41,6 +41,16 @@
             <!-- last_arm_link is defined in "$(find kortex_description)/arms/${arm}/urdf/${arm}_macro.xacro" -->
             <xacro:load_gripper parent="${prefix}${last_arm_link}"  prefix="${prefix}"/> 
         </xacro:unless>
+        <!-- If no gripper, define tool frame here -->
+        <xacro:if value="${not gripper}">
+            <link name="${prefix}tool_frame"/>
+            <joint name="${prefix}tool_frame_joint" type="fixed">
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <parent link="${prefix}${last_arm_link}" />
+                <child link="${prefix}tool_frame" />
+                <axis xyz="0 0 0" />
+            </joint>
+        </xacro:if>
 
         <!-- Load the Gazebo transmissions and gazebo_ros_control plugin -->
         <xacro:if value="${sim}">
diff --git a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/gen3_lite_gen3_lite_2f.srdf b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/gen3_lite_gen3_lite_2f.srdf
index c4315247..178ab9bc 100644
--- a/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/gen3_lite_gen3_lite_2f.srdf
+++ b/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/gen3_lite_gen3_lite_2f.srdf
@@ -17,6 +17,7 @@
         <joint name="joint_5" />
         <joint name="joint_6" />
         <joint name="end_effector" />
+        <joint name="tool_frame_joint" />
     </group>
     <group name="gripper">
         <link name="gripper_base_link" />
@@ -63,7 +64,7 @@
         <joint name="right_finger_tip_joint" value="0" />
     </group_state>
     <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
-    <end_effector name="end_effector" parent_link="dummy_link" group="arm" />
+    <end_effector name="end_effector" parent_link="tool_frame" group="arm" />
     <!--PASSIVE JOINT: Purpose: this element is used to mark joints that are not actuated-->
     <passive_joint name="right_finger_tip_joint" />
     <passive_joint name="left_finger_tip_joint" />
diff --git a/kortex_move_it_config/gen3_move_it_config/config/6dof/gen3.srdf b/kortex_move_it_config/gen3_move_it_config/config/6dof/gen3.srdf
index fda606b1..586f19ba 100644
--- a/kortex_move_it_config/gen3_move_it_config/config/6dof/gen3.srdf
+++ b/kortex_move_it_config/gen3_move_it_config/config/6dof/gen3.srdf
@@ -17,6 +17,7 @@
         <joint name="joint_5" />
         <joint name="joint_6" />
         <joint name="end_effector" />
+        <joint name="tool_frame_joint" />
     </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="home" group="arm">
diff --git a/kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf b/kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf
index 7fb21b66..f7b254af 100644
--- a/kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf
+++ b/kortex_move_it_config/gen3_move_it_config/config/7dof/gen3.srdf
@@ -18,6 +18,7 @@
         <joint name="joint_6" />
         <joint name="joint_7" />
         <joint name="end_effector" />
+        <joint name="tool_frame_joint" />
     </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="home" group="arm">
diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/gen3_robotiq_2f_140.srdf b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/gen3_robotiq_2f_140.srdf
index d723b13c..e2cd635b 100644
--- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/gen3_robotiq_2f_140.srdf
+++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/gen3_robotiq_2f_140.srdf
@@ -17,6 +17,7 @@
         <joint name="joint_5" />
         <joint name="joint_6" />
         <joint name="end_effector" />
+        <joint name="tool_frame_joint" />
     </group>
     <group name="gripper">
         <joint name="gripper_base_joint" />
diff --git a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/gen3_robotiq_2f_140.srdf b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/gen3_robotiq_2f_140.srdf
index 8884d19c..fd4a5cfd 100644
--- a/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/gen3_robotiq_2f_140.srdf
+++ b/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/gen3_robotiq_2f_140.srdf
@@ -18,6 +18,7 @@
         <joint name="joint_6" />
         <joint name="joint_7" />
         <joint name="end_effector" />
+        <joint name="tool_frame_joint" />
     </group>
     <group name="gripper">
         <joint name="gripper_base_joint" />
diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/gen3_robotiq_2f_85.srdf b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/gen3_robotiq_2f_85.srdf
index 318de95c..8ec5af66 100644
--- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/gen3_robotiq_2f_85.srdf
+++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/6dof/gen3_robotiq_2f_85.srdf
@@ -17,6 +17,7 @@
         <joint name="joint_5" />
         <joint name="joint_6" />
         <joint name="end_effector" />
+        <joint name="tool_frame_joint" />
     </group>
     <group name="gripper">
         <joint name="gripper_base_joint" />
diff --git a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/gen3_robotiq_2f_85.srdf b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/gen3_robotiq_2f_85.srdf
index 3ae9e136..a15f8f84 100644
--- a/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/gen3_robotiq_2f_85.srdf
+++ b/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/gen3_robotiq_2f_85.srdf
@@ -18,6 +18,7 @@
         <joint name="joint_6" />
         <joint name="joint_7" />
         <joint name="end_effector" />
+        <joint name="tool_frame_joint" />
     </group>
     <group name="gripper">
         <joint name="gripper_base_joint" />