Enable 6D calibration of arm, head and torso joints.

pal-robotics · Sep 23, 2020 · 362fcb8 · 362fcb8
1 parent 64f0d6a
commit 362fcb8
Show file tree

Hide file tree

Showing 7 changed files with 177 additions and 22 deletions.
diff --git a/talos_description/urdf/arm/arm.urdf.xacro b/talos_description/urdf/arm/arm.urdf.xacro
@@ -30,6 +30,7 @@
 
 
   <xacro:macro name="talos_arm" params="name parent side reflect">
+    <xacro:include filename="$(find talos_description_calibration)/urdf/calibration/arm_${side}.xacro" />
 
     <!--************************-->
     <!--        SHOULDER        -->
@@ -56,8 +57,8 @@
     <joint name="${name}_${side}_1_joint" type="revolute">
       <parent link="${parent}" />
       <child link="${name}_${side}_1_link" />
-      <origin xyz="0.00000 ${0.15750*reflect} 0.23200"
-              rpy="${0.0 * deg_to_rad} ${0.0 * deg_to_rad} ${0.0 * deg_to_rad}"/>
+      <origin xyz="${0.00000 + arm_1_joint_x_offset} ${0.15750*reflect + arm_1_joint_y_offset} ${0.23200 + arm_1_joint_z_offset}"
+              rpy="${0.0 * deg_to_rad + arm_1_joint_roll_offset} ${0.0 * deg_to_rad + arm_1_joint_pitch_offset} ${0.0 * deg_to_rad + arm_1_joint_yaw_offset}"/>
       <axis xyz="0 0 1" />
 
       <xacro:arm_1_joint_limits reflect="${reflect}" />
@@ -91,8 +92,8 @@
     <joint name="${name}_${side}_2_joint" type="revolute">
       <parent link="${name}_${side}_1_link" />
       <child link="${name}_${side}_2_link" />
-      <origin xyz="0.00493 ${0.13650*reflect} 0.04673"
-              rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
+      <origin xyz="${0.00493 + arm_2_joint_x_offset} ${0.13650*reflect + arm_2_joint_y_offset} ${0.04673 + arm_2_joint_z_offset}"
+              rpy="${0.00000 * deg_to_rad + arm_2_joint_roll_offset} ${0.00000 * deg_to_rad + arm_2_joint_pitch_offset} ${0.00000 * deg_to_rad + arm_2_joint_yaw_offset}"/>
       <axis xyz="1 0 0" />
       <limit lower="${(-82.0000 + 82.5000*reflect)* deg_to_rad}" upper="${(82.50000*reflect + 82.0000)* deg_to_rad}" effort="${arm_2_max_effort}" velocity="${arm_2_max_vel}" />
       <dynamics friction="${arm_friction}" damping="${arm_damping}"/>
@@ -126,8 +127,8 @@
     <joint name="${name}_${side}_3_joint" type="revolute">
       <parent link="${name}_${side}_2_link" />
       <child link="${name}_${side}_3_link" />
-      <origin xyz="0.00000 0.00000 0.00000"
-              rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
+      <origin xyz="${0.00000 + arm_3_joint_x_offset} ${0.00000 + arm_3_joint_y_offset} ${0.00000 + arm_3_joint_z_offset}"
+              rpy="${0.00000 * deg_to_rad + arm_3_joint_roll_offset} ${0.00000 * deg_to_rad + arm_3_joint_pitch_offset} ${0.00000 * deg_to_rad + arm_3_joint_yaw_offset}"/>
       <axis xyz="0 0 1" />
       <limit lower="${-139.0 * deg_to_rad}" upper="${139.0 * deg_to_rad}" effort="${arm_3_max_effort}" velocity="${arm_3_max_vel}" />
       <dynamics friction="${arm_friction}" damping="${arm_damping}"/>
@@ -163,8 +164,8 @@
     <joint name="${name}_${side}_4_joint" type="revolute">
       <parent link="${name}_${side}_3_link" />
       <child link="${name}_${side}_4_link" />
-      <origin xyz="0.02000 0.00000 -0.27300"
-              rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
+      <origin xyz="${0.02000 + arm_4_joint_x_offset} ${0.00000 + arm_4_joint_y_offset} ${-0.27300 + arm_4_joint_z_offset}"
+              rpy="${0.00000 * deg_to_rad + arm_4_joint_roll_offset} ${0.00000 * deg_to_rad + arm_4_joint_pitch_offset} ${0.00000 * deg_to_rad + arm_4_joint_yaw_offset}"/>
       <axis xyz="0 1 0" />
       <limit lower="${-128.0 * deg_to_rad}" upper="${-0.2 * deg_to_rad}" effort="${arm_4_max_effort}" velocity="${arm_4_max_vel}" />
       <dynamics friction="${arm_friction}" damping="${arm_damping}"/>

diff --git a/talos_description/urdf/arm/wrist.urdf.xacro b/talos_description/urdf/arm/wrist.urdf.xacro
@@ -27,6 +27,7 @@
 
 
   <xacro:macro name="talos_wrist" params="name parent side reflect">
+    <xacro:include filename="$(find talos_description_calibration)/urdf/calibration/arm_${side}.xacro" />
 
     <!--************************-->
     <!--        WRIST           -->
@@ -53,8 +54,8 @@
     <joint name="${name}_${side}_5_joint" type="revolute">
       <parent link="${parent}" />
       <child link="${name}_${side}_5_link" />
-      <origin xyz="-0.02000 0.00000 -0.26430"
-                rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
+      <origin xyz="${-0.02000 + arm_5_joint_x_offset} ${0.00000 + arm_5_joint_y_offset} ${-0.26430 + arm_5_joint_z_offset}"
+              rpy="${0.00000 * deg_to_rad + arm_5_joint_roll_offset} ${0.00000 * deg_to_rad + arm_5_joint_pitch_offset} ${0.00000 * deg_to_rad + arm_5_joint_yaw_offset}"/>
       <axis xyz="0 0 1" />
       <limit lower="${-144.0 * deg_to_rad}" upper="${144.0 * deg_to_rad}" effort="${wrist_1_max_effort}" velocity="${wrist_1_max_vel}" />
       <dynamics friction="${wrist_friction}" damping="${wrist_damping}"/>
@@ -87,8 +88,8 @@
     <joint name="${name}_${side}_6_joint" type="revolute">
       <parent link="${name}_${side}_5_link" />
       <child link="${name}_${side}_6_link" />
-      <origin xyz="0.00000 0.00000 0.00000"
-              rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
+      <origin xyz="${0.00000 + arm_6_joint_x_offset} ${0.00000 + arm_6_joint_y_offset} ${0.00000 + arm_6_joint_z_offset}"
+              rpy="${0.00000 * deg_to_rad + arm_6_joint_roll_offset} ${0.00000 * deg_to_rad + arm_6_joint_pitch_offset} ${0.00000 * deg_to_rad + arm_6_joint_yaw_offset}"/>
       <axis xyz="1 0 0" />
       <limit lower="${-78.5 * deg_to_rad}" upper="${78.5 * deg_to_rad}" effort="${wrist_2_max_effort}" velocity="${wrist_2_max_vel}" />
       <dynamics friction="${wrist_friction}" damping="${wrist_damping}"/>
@@ -121,8 +122,8 @@
     <joint name="${name}_${side}_7_joint" type="revolute">
       <parent link="${name}_${side}_6_link" />
       <child link="${name}_${side}_7_link" />
-      <origin xyz="0.0 0.0 0.0"
-                rpy="${0.0 * deg_to_rad} ${0.0 * deg_to_rad} ${0.0 * deg_to_rad}"/>
+      <origin xyz="${0.00000 + arm_7_joint_x_offset} ${0.00000 + arm_7_joint_y_offset} ${0.00000 + arm_7_joint_z_offset}"
+              rpy="${0.00000 * deg_to_rad + arm_7_joint_roll_offset} ${0.00000 * deg_to_rad + arm_7_joint_pitch_offset} ${0.00000 * deg_to_rad + arm_7_joint_yaw_offset}"/>
       <axis xyz="0 1 0" />
       <limit lower="${-39.0 * deg_to_rad}" upper="${39.0 * deg_to_rad}" effort="${wrist_3_max_effort}" velocity="${wrist_3_max_vel}"/>
       <dynamics friction="${wrist_friction}" damping="${wrist_damping}"/>

diff --git a/talos_description/urdf/head/head.urdf.xacro b/talos_description/urdf/head/head.urdf.xacro
@@ -48,8 +48,8 @@
     <joint name="${name}_joint" type="revolute">
       <parent link="${parent}"/>
       <child link="${name}_link"/>
-      <origin xyz="0.02000 0.00000 0.32100"
-              rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
+      <origin xyz="${0.02000 + head_1_joint_x_offset} ${0.00000 + head_1_joint_y_offset} ${0.32100 + head_1_joint_z_offset}"
+              rpy="${0.00000 * deg_to_rad + head_1_joint_roll_offset} ${0.00000 * deg_to_rad + head_1_joint_pitch_offset} ${0.00000 * deg_to_rad + head_1_joint_yaw_offset}"/>
       <axis xyz="0 1 0" />
       <limit lower="${-12.00000 * deg_to_rad}" upper="${45.00000 * deg_to_rad}" effort="8.0" velocity="${head_max_vel}" />
       <dynamics damping="${head_damping}" friction="${head_friction}"/>
@@ -102,8 +102,8 @@
     <joint name="${name}_joint" type="revolute">
       <parent link="${parent}"/>
       <child link="${name}_link"/>
-      <origin xyz="0.00000 0.00000 0.00000"
-              rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
+      <origin xyz="${0.00000 + head_2_joint_x_offset} ${0.00000 + head_2_joint_y_offset} ${0.00000 + head_2_joint_z_offset}"
+              rpy="${0.00000 * deg_to_rad + head_2_joint_roll_offset} ${0.00000 * deg_to_rad + head_2_joint_pitch_offset} ${0.00000 * deg_to_rad + head_2_joint_yaw_offset}"/>
       <axis xyz="0 0 1" />
       <limit lower="${-75.00000 * deg_to_rad}" upper="${75.00000 * deg_to_rad}" effort="4.0" velocity="${head_max_vel}" />
       <dynamics damping="${head_damping}" friction="${head_friction}"/>

diff --git a/talos_description/urdf/torso/torso.urdf.xacro b/talos_description/urdf/torso/torso.urdf.xacro
@@ -72,8 +72,8 @@
       <parent link="base_link"/>
       <child link="${name}_1_link"/>
       <!-- XXX AS: The kinematic chain order for the torso was reversed manually. This value does not match the CAD model. XXX -->
-      <origin xyz="0.0 0.0 0.0722"
-              rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
+      <origin xyz="${0.0 + torso_1_joint_x_offset} ${0.0 + torso_1_joint_y_offset} ${0.0722 + torso_1_joint_z_offset}"
+              rpy="${0.00000 * deg_to_rad + torso_1_joint_roll_offset} ${0.00000 * deg_to_rad + torso_1_joint_pitch_offset} ${0.00000 * deg_to_rad + torso_1_joint_yaw_offset}"/>
       <axis xyz="0 0 1" />
       <limit lower="${-72.00000 * deg_to_rad}" upper="${72.00000 * deg_to_rad}"
              effort="${torso_max_effort}" velocity="${torso_max_vel}" />
@@ -110,8 +110,8 @@
     <joint name="${name}_2_joint" type="revolute">
       <parent link="${name}_1_link"/>
       <child link="${name}_2_link"/>
-      <origin xyz="0.0 0.0 0.0"
-              rpy="${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad} ${0.00000 * deg_to_rad}"/>
+      <origin xyz="${0.0 + torso_1_joint_x_offset} ${0.0 + torso_1_joint_y_offset} ${0.0 + torso_1_joint_z_offset}"
+              rpy="${0.00000 * deg_to_rad + torso_1_joint_roll_offset} ${0.00000 * deg_to_rad + torso_1_joint_pitch_offset} ${0.00000 * deg_to_rad + torso_1_joint_yaw_offset}"/>
       <axis xyz="0 1 0" />
       <limit lower="${-13.0 * deg_to_rad}" upper="${42.0 * deg_to_rad}"
              effort="${torso_max_effort}" velocity="${torso_max_vel}"/>

diff --git a/talos_description_calibration/urdf/calibration/arm_left.xacro b/talos_description_calibration/urdf/calibration/arm_left.xacro
@@ -0,0 +1,62 @@
+<?xml version="1.0"?>
+<!--
+
+  Copyright (c) 2015, PAL Robotics, S.L.
+  All rights reserved.
+
+  This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivs 3.0 Unported License.
+  To view a copy of this license, visit http://creativecommons.org/licenses/by-nc-nd/3.0/ or send a letter to
+  Creative Commons, 444 Castro Street, Suite 900, Mountain View, California, 94041, USA.
+-->
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:property name="arm_1_joint_x_offset"     value="0.0" />
+  <xacro:property name="arm_1_joint_y_offset"     value="0.0" />
+  <xacro:property name="arm_1_joint_z_offset"     value="0.0" />
+  <xacro:property name="arm_1_joint_roll_offset"  value="0.0" />
+  <xacro:property name="arm_1_joint_pitch_offset" value="0.0" />
+  <xacro:property name="arm_1_joint_yaw_offset"   value="0.0" />
+
+  <xacro:property name="arm_2_joint_x_offset"     value="0.0" />
+  <xacro:property name="arm_2_joint_y_offset"     value="0.0" />
+  <xacro:property name="arm_2_joint_z_offset"     value="0.0" />
+  <xacro:property name="arm_2_joint_roll_offset"  value="0.0" />
+  <xacro:property name="arm_2_joint_pitch_offset" value="0.0" />
+  <xacro:property name="arm_2_joint_yaw_offset"   value="0.0" />
+
+  <xacro:property name="arm_3_joint_x_offset"     value="0.0" />
+  <xacro:property name="arm_3_joint_y_offset"     value="0.0" />
+  <xacro:property name="arm_3_joint_z_offset"     value="0.0" />
+  <xacro:property name="arm_3_joint_roll_offset"  value="0.0" />
+  <xacro:property name="arm_3_joint_pitch_offset" value="0.0" />
+  <xacro:property name="arm_3_joint_yaw_offset"   value="0.0" />
+
+  <xacro:property name="arm_4_joint_x_offset"     value="0.0" />
+  <xacro:property name="arm_4_joint_y_offset"     value="0.0" />
+  <xacro:property name="arm_4_joint_z_offset"     value="0.0" />
+  <xacro:property name="arm_4_joint_roll_offset"  value="0.0" />
+  <xacro:property name="arm_4_joint_pitch_offset" value="0.0" />
+  <xacro:property name="arm_4_joint_yaw_offset"   value="0.0" />
+
+  <xacro:property name="arm_5_joint_x_offset"     value="0.0" />
+  <xacro:property name="arm_5_joint_y_offset"     value="0.0" />
+  <xacro:property name="arm_5_joint_z_offset"     value="0.0" />
+  <xacro:property name="arm_5_joint_roll_offset"  value="0.0" />
+  <xacro:property name="arm_5_joint_pitch_offset" value="0.0" />
+  <xacro:property name="arm_5_joint_yaw_offset"   value="0.0" />
+
+  <xacro:property name="arm_6_joint_x_offset"     value="0.0" />
+  <xacro:property name="arm_6_joint_y_offset"     value="0.0" />
+  <xacro:property name="arm_6_joint_z_offset"     value="0.0" />
+  <xacro:property name="arm_6_joint_roll_offset"  value="0.0" />
+  <xacro:property name="arm_6_joint_pitch_offset" value="0.0" />
+  <xacro:property name="arm_6_joint_yaw_offset"   value="0.0" />
+
+  <xacro:property name="arm_7_joint_x_offset"     value="0.0" />
+  <xacro:property name="arm_7_joint_y_offset"     value="0.0" />
+  <xacro:property name="arm_7_joint_z_offset"     value="0.0" />
+  <xacro:property name="arm_7_joint_roll_offset"  value="0.0" />
+  <xacro:property name="arm_7_joint_pitch_offset" value="0.0" />
+  <xacro:property name="arm_7_joint_yaw_offset"   value="0.0" />
+
+</robot>
diff --git a/talos_description_calibration/urdf/calibration/arm_right.xacro b/talos_description_calibration/urdf/calibration/arm_right.xacro
@@ -0,0 +1,62 @@
+<?xml version="1.0"?>
+<!--
+
+  Copyright (c) 2015, PAL Robotics, S.L.
+  All rights reserved.
+
+  This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivs 3.0 Unported License.
+  To view a copy of this license, visit http://creativecommons.org/licenses/by-nc-nd/3.0/ or send a letter to
+  Creative Commons, 444 Castro Street, Suite 900, Mountain View, California, 94041, USA.
+-->
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:property name="arm_1_joint_x_offset"     value="0.0" />
+  <xacro:property name="arm_1_joint_y_offset"     value="0.0" />
+  <xacro:property name="arm_1_joint_z_offset"     value="0.0" />
+  <xacro:property name="arm_1_joint_roll_offset"  value="0.0" />
+  <xacro:property name="arm_1_joint_pitch_offset" value="0.0" />
+  <xacro:property name="arm_1_joint_yaw_offset"   value="0.0" />
+
+  <xacro:property name="arm_2_joint_x_offset"     value="0.0" />
+  <xacro:property name="arm_2_joint_y_offset"     value="0.0" />
+  <xacro:property name="arm_2_joint_z_offset"     value="0.0" />
+  <xacro:property name="arm_2_joint_roll_offset"  value="0.0" />
+  <xacro:property name="arm_2_joint_pitch_offset" value="0.0" />
+  <xacro:property name="arm_2_joint_yaw_offset"   value="0.0" />
+
+  <xacro:property name="arm_3_joint_x_offset"     value="0.0" />
+  <xacro:property name="arm_3_joint_y_offset"     value="0.0" />
+  <xacro:property name="arm_3_joint_z_offset"     value="0.0" />
+  <xacro:property name="arm_3_joint_roll_offset"  value="0.0" />
+  <xacro:property name="arm_3_joint_pitch_offset" value="0.0" />
+  <xacro:property name="arm_3_joint_yaw_offset"   value="0.0" />
+
+  <xacro:property name="arm_4_joint_x_offset"     value="0.0" />
+  <xacro:property name="arm_4_joint_y_offset"     value="0.0" />
+  <xacro:property name="arm_4_joint_z_offset"     value="0.0" />
+  <xacro:property name="arm_4_joint_roll_offset"  value="0.0" />
+  <xacro:property name="arm_4_joint_pitch_offset" value="0.0" />
+  <xacro:property name="arm_4_joint_yaw_offset"   value="0.0" />
+
+  <xacro:property name="arm_5_joint_x_offset"     value="0.0" />
+  <xacro:property name="arm_5_joint_y_offset"     value="0.0" />
+  <xacro:property name="arm_5_joint_z_offset"     value="0.0" />
+  <xacro:property name="arm_5_joint_roll_offset"  value="0.0" />
+  <xacro:property name="arm_5_joint_pitch_offset" value="0.0" />
+  <xacro:property name="arm_5_joint_yaw_offset"   value="0.0" />
+
+  <xacro:property name="arm_6_joint_x_offset"     value="0.0" />
+  <xacro:property name="arm_6_joint_y_offset"     value="0.0" />
+  <xacro:property name="arm_6_joint_z_offset"     value="0.0" />
+  <xacro:property name="arm_6_joint_roll_offset"  value="0.0" />
+  <xacro:property name="arm_6_joint_pitch_offset" value="0.0" />
+  <xacro:property name="arm_6_joint_yaw_offset"   value="0.0" />
+
+  <xacro:property name="arm_7_joint_x_offset"     value="0.0" />
+  <xacro:property name="arm_7_joint_y_offset"     value="0.0" />
+  <xacro:property name="arm_7_joint_z_offset"     value="0.0" />
+  <xacro:property name="arm_7_joint_roll_offset"  value="0.0" />
+  <xacro:property name="arm_7_joint_pitch_offset" value="0.0" />
+  <xacro:property name="arm_7_joint_yaw_offset"   value="0.0" />
+
+</robot>
diff --git a/talos_description_calibration/urdf/calibration/calibration_constants.urdf.xacro b/talos_description_calibration/urdf/calibration/calibration_constants.urdf.xacro
@@ -20,6 +20,7 @@
   <xacro:property name="camera_orientation_r" value="0.0" />
   <xacro:property name="camera_orientation_p" value="0.0" />
   <xacro:property name="camera_orientation_y" value="0.0" />
+
   <xacro:property name="arm_left_1_joint_offset" value="0.0" />
   <xacro:property name="arm_left_2_joint_offset" value="0.0" />
   <xacro:property name="arm_left_3_joint_offset" value="0.0" />
@@ -37,6 +38,34 @@
   <xacro:property name="head_1_joint_offset" value="0.0" />
   <xacro:property name="head_2_joint_offset" value="0.0" />
 
+  <xacro:property name="head_1_joint_x_offset"     value="0.0" />
+  <xacro:property name="head_1_joint_y_offset"     value="0.0" />
+  <xacro:property name="head_1_joint_z_offset"     value="0.0" />
+  <xacro:property name="head_1_joint_roll_offset"  value="0.0" />
+  <xacro:property name="head_1_joint_pitch_offset" value="0.0" />
+  <xacro:property name="head_1_joint_yaw_offset"   value="0.0" />
+
+  <xacro:property name="head_2_joint_x_offset"     value="0.0" />
+  <xacro:property name="head_2_joint_y_offset"     value="0.0" />
+  <xacro:property name="head_2_joint_z_offset"     value="0.0" />
+  <xacro:property name="head_2_joint_roll_offset"  value="0.0" />
+  <xacro:property name="head_2_joint_pitch_offset" value="0.0" />
+  <xacro:property name="head_2_joint_yaw_offset"   value="0.0" />
+
+  <xacro:property name="torso_1_joint_x_offset"     value="0.0" />
+  <xacro:property name="torso_1_joint_y_offset"     value="0.0" />
+  <xacro:property name="torso_1_joint_z_offset"     value="0.0" />
+  <xacro:property name="torso_1_joint_roll_offset"  value="0.0" />
+  <xacro:property name="torso_1_joint_pitch_offset" value="0.0" />
+  <xacro:property name="torso_1_joint_yaw_offset"   value="0.0" />
+
+  <xacro:property name="torso_2_joint_x_offset"     value="0.0" />
+  <xacro:property name="torso_2_joint_y_offset"     value="0.0" />
+  <xacro:property name="torso_2_joint_z_offset"     value="0.0" />
+  <xacro:property name="torso_2_joint_roll_offset"  value="0.0" />
+  <xacro:property name="torso_2_joint_pitch_offset" value="0.0" />
+  <xacro:property name="torso_2_joint_yaw_offset"   value="0.0" />
+
   <xacro:property name="torso_imu_x_offset" value="0.0" />
   <xacro:property name="torso_imu_y_offset" value="0.0" />
   <xacro:property name="torso_imu_z_offset" value="0.0" />