From db6b1637b37ac6e4bdbf581e5d68960b0af55b62 Mon Sep 17 00:00:00 2001
From: gavanderhoorn <g.a.vanderhoorn@tudelft.nl>
Date: Wed, 22 Apr 2015 09:00:14 +0200
Subject: [PATCH] description: add ROS-I base and tool0 frames. Fix #49 and
 #95.

Note that 'base' is essentially 'base_link' but rotated by 180
degrees over the Z-axis. This is necessary as the visual and
collision geometries appear to also have their origins rotated
180 degrees wrt the real robot.

'tool0' is similar to 'ee_link', but with its orientation such
that it coincides with an all-zeros TCP setting on the UR
controller. Users are expected to attach their own TCP frames
to this frame, instead of updating it (see also [1]).

[1] http://wiki.ros.org/Industrial/Tutorials/WorkingWithRosIndustrialRobotSupportPackages#Standardised_links_.2BAC8_frames
---
 ur_description/urdf/ur10.urdf.xacro           | 20 +++++++++++++++++++
 .../urdf/ur10_joint_limited_robot.urdf        | 16 +++++++++++++++
 ur_description/urdf/ur10_robot.urdf           | 16 +++++++++++++++
 ur_description/urdf/ur5.urdf.xacro            | 20 +++++++++++++++++++
 .../urdf/ur5_joint_limited_robot.urdf         | 16 +++++++++++++++
 ur_description/urdf/ur5_robot.urdf            | 16 +++++++++++++++
 6 files changed, 104 insertions(+)

diff --git a/ur_description/urdf/ur10.urdf.xacro b/ur_description/urdf/ur10.urdf.xacro
index c6a9e29dc..a4dd2fbdd 100644
--- a/ur_description/urdf/ur10.urdf.xacro
+++ b/ur_description/urdf/ur10.urdf.xacro
@@ -300,5 +300,25 @@
     <xacro:ur10_arm_transmission prefix="${prefix}" />
     <xacro:ur10_arm_gazebo prefix="${prefix}" />
 
+    <!-- ROS base_link to UR 'Base' Coordinates transform -->
+    <link name="${prefix}base"/>
+    <joint name="${prefix}base_link-base" type="fixed">
+      <!-- NOTE: this rotation is only needed as long as base_link itself is
+                 not corrected wrt the real robot (ie: rotated over 180
+                 degrees)
+      -->
+      <origin xyz="0 0 0" rpy="0 0 ${-pi}"/>
+      <parent link="${prefix}base_link"/>
+      <child link="${prefix}base"/>
+    </joint>
+
+    <!-- Frame coincident with all-zeros TCP on UR controller -->
+    <link name="${prefix}tool0"/>
+    <joint name="${prefix}wrist_3_link-tool0" type="fixed">
+      <origin xyz="0 ${wrist_3_length} 0" rpy="${pi/-2.0} 0 0"/>
+      <parent link="${prefix}wrist_3_link"/>
+      <child link="${prefix}tool0"/>
+    </joint>
+
   </xacro:macro>
 </robot>
diff --git a/ur_description/urdf/ur10_joint_limited_robot.urdf b/ur_description/urdf/ur10_joint_limited_robot.urdf
index 6889829ed..3aab4c032 100644
--- a/ur_description/urdf/ur10_joint_limited_robot.urdf
+++ b/ur_description/urdf/ur10_joint_limited_robot.urdf
@@ -282,6 +282,22 @@
       <mechanicalReduction>1</mechanicalReduction>
     </actuator>
   </transmission>
+  <link name="base"/>
+  <joint name="base_link-base" type="fixed">
+    <!-- NOTE: this rotation is only needed as long as base_link itself is
+                 not corrected wrt the real robot (ie: rotated over 180
+                 degrees)
+      -->
+    <origin rpy="0 0 -3.14159265" xyz="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="base"/>
+  </joint>
+  <link name="tool0"/>
+  <joint name="wrist_3_link-tool0" type="fixed">
+    <origin rpy="-1.570796325 0 0" xyz="0 0.0922 0"/>
+    <parent link="wrist_3_link"/>
+    <child link="tool0"/>
+  </joint>
   <link name="world"/>
   <joint name="world_joint" type="fixed">
     <parent link="world"/>
diff --git a/ur_description/urdf/ur10_robot.urdf b/ur_description/urdf/ur10_robot.urdf
index 321c04a3b..a61441331 100644
--- a/ur_description/urdf/ur10_robot.urdf
+++ b/ur_description/urdf/ur10_robot.urdf
@@ -282,6 +282,22 @@
       <mechanicalReduction>1</mechanicalReduction>
     </actuator>
   </transmission>
+  <link name="base"/>
+  <joint name="base_link-base" type="fixed">
+    <!-- NOTE: this rotation is only needed as long as base_link itself is
+                 not corrected wrt the real robot (ie: rotated over 180
+                 degrees)
+      -->
+    <origin rpy="0 0 -3.14159265" xyz="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="base"/>
+  </joint>
+  <link name="tool0"/>
+  <joint name="wrist_3_link-tool0" type="fixed">
+    <origin rpy="-1.570796325 0 0" xyz="0 0.0922 0"/>
+    <parent link="wrist_3_link"/>
+    <child link="tool0"/>
+  </joint>
   <link name="world"/>
   <joint name="world_joint" type="fixed">
     <parent link="world"/>
diff --git a/ur_description/urdf/ur5.urdf.xacro b/ur_description/urdf/ur5.urdf.xacro
index 4ccf0b31c..2c5998b0a 100644
--- a/ur_description/urdf/ur5.urdf.xacro
+++ b/ur_description/urdf/ur5.urdf.xacro
@@ -315,5 +315,25 @@
     <xacro:ur5_arm_transmission prefix="${prefix}" />
     <xacro:ur5_arm_gazebo prefix="${prefix}" />
 
+    <!-- ROS base_link to UR 'Base' Coordinates transform -->
+    <link name="${prefix}base"/>
+    <joint name="${prefix}base_link-base" type="fixed">
+      <!-- NOTE: this rotation is only needed as long as base_link itself is
+                 not corrected wrt the real robot (ie: rotated over 180
+                 degrees)
+      -->
+      <origin xyz="0 0 0" rpy="0 0 ${-pi}"/>
+      <parent link="${prefix}base_link"/>
+      <child link="${prefix}base"/>
+    </joint>
+
+    <!-- Frame coincident with all-zeros TCP on UR controller -->
+    <link name="${prefix}tool0"/>
+    <joint name="${prefix}wrist_3_link-tool0" type="fixed">
+      <origin xyz="0 ${wrist_3_length} 0" rpy="${pi/-2.0} 0 0"/>
+      <parent link="${prefix}wrist_3_link"/>
+      <child link="${prefix}tool0"/>
+    </joint>
+
   </xacro:macro>
 </robot>
diff --git a/ur_description/urdf/ur5_joint_limited_robot.urdf b/ur_description/urdf/ur5_joint_limited_robot.urdf
index 983a2c044..eb2000834 100644
--- a/ur_description/urdf/ur5_joint_limited_robot.urdf
+++ b/ur_description/urdf/ur5_joint_limited_robot.urdf
@@ -282,6 +282,22 @@
       <mechanicalReduction>1</mechanicalReduction>
     </actuator>
   </transmission>
+  <link name="base"/>
+  <joint name="base_link-base" type="fixed">
+    <!-- NOTE: this rotation is only needed as long as base_link itself is
+                 not corrected wrt the real robot (ie: rotated over 180
+                 degrees)
+      -->
+    <origin rpy="0 0 -3.14159265" xyz="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="base"/>
+  </joint>
+  <link name="tool0"/>
+  <joint name="wrist_3_link-tool0" type="fixed">
+    <origin rpy="-1.570796325 0 0" xyz="0 0.0823 0"/>
+    <parent link="wrist_3_link"/>
+    <child link="tool0"/>
+  </joint>
   <link name="world"/>
   <joint name="world_joint" type="fixed">
     <parent link="world"/>
diff --git a/ur_description/urdf/ur5_robot.urdf b/ur_description/urdf/ur5_robot.urdf
index 0179fa898..a912fba9e 100644
--- a/ur_description/urdf/ur5_robot.urdf
+++ b/ur_description/urdf/ur5_robot.urdf
@@ -282,6 +282,22 @@
       <mechanicalReduction>1</mechanicalReduction>
     </actuator>
   </transmission>
+  <link name="base"/>
+  <joint name="base_link-base" type="fixed">
+    <!-- NOTE: this rotation is only needed as long as base_link itself is
+                 not corrected wrt the real robot (ie: rotated over 180
+                 degrees)
+      -->
+    <origin rpy="0 0 -3.14159265" xyz="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="base"/>
+  </joint>
+  <link name="tool0"/>
+  <joint name="wrist_3_link-tool0" type="fixed">
+    <origin rpy="-1.570796325 0 0" xyz="0 0.0823 0"/>
+    <parent link="wrist_3_link"/>
+    <child link="tool0"/>
+  </joint>
   <link name="world"/>
   <joint name="world_joint" type="fixed">
     <parent link="world"/>