Merge pull request #200 from gavanderhoorn/issue95_add_base_frame

Fix for issues #49 and #95: ros-i compatible base and tool0 frames

ur_description/urdf/ur10.urdf.xacro

@@ -42,7 +42,7 @@
 
   <!-- Arbitrary offsets for shoulder/elbow joints -->
   <property name="shoulder_offset" value="0.220941" />  <!-- measured from model -->
-  <property name="elbow_offset" value="-0.1719" /> <!-- measured from model -->       
+  <property name="elbow_offset" value="-0.1719" /> <!-- measured from model -->
 
   <!-- link lengths used in model -->
   <property name="shoulder_height" value="${ur10_d1}" />
@@ -64,7 +64,7 @@
 
 
   <xacro:macro name="ur10_robot" params="prefix joint_limited">
-  
+
     <link name="${prefix}base_link" >
       <visual>
         <geometry>
@@ -97,7 +97,7 @@
       </xacro:if>
       <dynamics damping="0.0" friction="0.0"/>
     </joint>
-    
+
     <link name="${prefix}shoulder_link">
       <visual>
         <geometry>
@@ -120,7 +120,7 @@
     <joint name="${prefix}shoulder_lift_joint" type="revolute">
       <parent link="${prefix}shoulder_link" />
       <child link = "${prefix}upper_arm_link" />
-      <origin xyz="0.0 ${shoulder_offset} 0.0" rpy="0.0 ${pi / 2.0} 0.0" />    
+      <origin xyz="0.0 ${shoulder_offset} 0.0" rpy="0.0 ${pi / 2.0} 0.0" />
       <axis xyz="0 1 0" />
       <xacro:unless value="${joint_limited}">
         <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="330.0" velocity="2.16"/>
@@ -281,7 +281,7 @@
         <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
       </xacro:cylinder_inertial>
     </link>
-    
+
     <joint name="${prefix}ee_fixed_joint" type="fixed">
       <parent link="${prefix}wrist_3_link" />
       <child link = "${prefix}ee_link" />
@@ -299,6 +299,26 @@
 
     <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_fixed_joint" 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_fixed_joint" 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>

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_fixed_joint" 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_fixed_joint" 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"/>

ur_description/urdf/ur10_robot.urdf

@@ -282,6 +282,22 @@
       <mechanicalReduction>1</mechanicalReduction>
     </actuator>
   </transmission>
+  <link name="base"/>
+  <joint name="base_link-base_fixed_joint" 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_fixed_joint" 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"/>

ur_description/urdf/ur5.urdf.xacro

@@ -43,7 +43,7 @@
 
   <!-- Arbitrary offsets for shoulder/elbow joints -->
   <property name="shoulder_offset" value="0.13585" />  <!-- measured from model -->
-  <property name="elbow_offset" value="-0.1197" /> <!-- measured from model -->       
+  <property name="elbow_offset" value="-0.1197" /> <!-- measured from model -->
 
   <!-- link lengths used in model -->
   <property name="shoulder_height" value="${ur5_d1}" />
@@ -52,7 +52,7 @@
   <property name="wrist_1_length" value="${ur5_d4 - elbow_offset - shoulder_offset}" />
   <property name="wrist_2_length" value="${ur5_d5}" />
   <property name="wrist_3_length" value="${ur5_d6}" />
-  <!--property name="shoulder_height" value="0.089159" /-->  
+  <!--property name="shoulder_height" value="0.089159" /-->
   <!--property name="shoulder_offset" value="0.13585" /-->  <!-- shoulder_offset - elbow_offset + wrist_1_length = 0.10915 -->
   <!--property name="upper_arm_length" value="0.42500" /-->
   <!--property name="elbow_offset" value="0.1197" /-->       <!-- CAD measured -->
@@ -112,7 +112,7 @@
       </xacro:if>
       <dynamics damping="0.0" friction="0.0"/>
     </joint>
-    
+
     <link name="${prefix}shoulder_link">
       <visual>
         <geometry>
@@ -135,7 +135,7 @@
     <joint name="${prefix}shoulder_lift_joint" type="revolute">
       <parent link="${prefix}shoulder_link" />
       <child link = "${prefix}upper_arm_link" />
-      <origin xyz="0.0 ${shoulder_offset} 0.0" rpy="0.0 ${pi / 2.0} 0.0" />    
+      <origin xyz="0.0 ${shoulder_offset} 0.0" rpy="0.0 ${pi / 2.0} 0.0" />
       <axis xyz="0 1 0" />
       <xacro:unless value="${joint_limited}">
         <limit lower="${-2.0 * pi}" upper="${2.0 * pi}" effort="150.0" velocity="3.15"/>
@@ -296,7 +296,7 @@
         <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
       </xacro:cylinder_inertial>
     </link>
-    
+
     <joint name="${prefix}ee_fixed_joint" type="fixed">
       <parent link="${prefix}wrist_3_link" />
       <child link = "${prefix}ee_link" />
@@ -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_fixed_joint" 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_fixed_joint" 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>

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_fixed_joint" 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_fixed_joint" 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"/>

ur_description/urdf/ur5_robot.urdf

@@ -282,6 +282,22 @@
       <mechanicalReduction>1</mechanicalReduction>
     </actuator>
   </transmission>
+  <link name="base"/>
+  <joint name="base_link-base_fixed_joint" 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_fixed_joint" 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"/>