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
Kinovarobotics · Jun 9, 2020 · ea4c01d · ea4c01d
1 parent 193654f
commit ea4c01d
Show file tree

Hide file tree

Showing 12 changed files with 51 additions and 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
@@ -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
@@ -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
@@ -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
@@ -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>
+<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.
diff --git 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/...x_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/gen3_lite_gen3_lite_2f.srdf b/...x_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
@@ -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
@@ -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/...ex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/6dof/gen3_robotiq_2f_140.srdf b/...ex_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/...ex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/gen3_robotiq_2f_140.srdf b/...ex_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
@@ -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
@@ -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" />