[WIP] Add draft of 6D joint pose calibration.

talos_description/urdf/arm/arm.urdf.xacro

@@ -34,6 +34,11 @@
     <!--************************-->
     <!--        SHOULDER        -->
     <!--************************-->
+    <xacro:calibration_joint name="${name}_${side}_1" parent="${parent}">
+      <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}"/>
+    </xacro:calibration_joint>
+
     <link name="${name}_${side}_1_link">
       <xacro:call macro="${name}_${side}_1_link_inertial" />
 
@@ -54,10 +59,9 @@
     </link>
 
     <joint name="${name}_${side}_1_joint" type="revolute">
-      <parent link="${parent}" />
+      <parent link="${name}_${side}_1_calibration_link" />
       <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}"/>
+      <xacro:joint_offset which="${name}_${side}" id="1"/>
       <axis xyz="0 0 1" />
 
       <xacro:arm_1_joint_limits reflect="${reflect}" />

talos_description/urdf/extras/calibration.urdf.xacro

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<!--
+
+  Copyright (c) 2020, 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:macro name="calibration_joint" params="name parent *origin">
+    <link name="${name}_calibration_link"/>
+    <joint name="${name}_calibration_joint" type="fixed">
+      <parent link="${parent}" />
+      <child link="${name}_calibration_link" />
+      <xacro:insert_block name="origin" />
+    </joint>
+  </xacro:macro>
+
+</robot>

talos_description/urdf/talos_full_common.urdf.xacro

@@ -25,6 +25,7 @@
   <xacro:include filename="$(find talos_description)/urdf/leg/leg.urdf.xacro" />
   <xacro:include filename="$(find talos_description)/urdf/leg/foot_collision_$(arg foot_collision).urdf.xacro" />
   <xacro:include filename="$(find talos_description_inertial)/urdf/inertial.xacro" />
+  <xacro:include filename="$(find talos_description)/urdf/extras/calibration.urdf.xacro" />
   <xacro:include filename="$(find talos_description)/urdf/extras/addons.urdf.xacro" />
 
   <xacro:talos_torso name="torso" />

talos_description_calibration/urdf/calibration/calibration_constants.urdf.xacro

@@ -45,4 +45,26 @@
   <xacro:property name="torso_imu_pitch_offset" value="0.0" />
   <xacro:property name="torso_imu_yaw_offset" value="0.0" />
 
+  <xacro:macro name="joint_offset" params="which id">
+    <xacro:if value="${which == 'arm_left'}">
+      <xacro:if value="${id == 1}"> <origin xyz="0 0 0" rpy="0 0 0"/> </xacro:if>
+      <xacro:if value="${id == 2}"> <origin xyz="0 0 0" rpy="0 0 0"/> </xacro:if>
+      <xacro:if value="${id == 3}"> <origin xyz="0 0 0" rpy="0 0 0"/> </xacro:if>
+      <xacro:if value="${id == 4}"> <origin xyz="0 0 0" rpy="0 0 0"/> </xacro:if>
+      <xacro:if value="${id == 5}"> <origin xyz="0 0 0" rpy="0 0 0"/> </xacro:if>
+      <xacro:if value="${id == 6}"> <origin xyz="0 0 0" rpy="0 0 0"/> </xacro:if>
+      <xacro:if value="${id == 7}"> <origin xyz="0 0 0" rpy="0 0 0"/> </xacro:if>
+    </xacro:if>
+
+    <xacro:if value="${which == 'arm_right'}">
+      <xacro:if value="${id == 1}"> <origin xyz="0 0 0" rpy="0 0 0"/> </xacro:if>
+      <xacro:if value="${id == 2}"> <origin xyz="0 0 0" rpy="0 0 0"/> </xacro:if>
+      <xacro:if value="${id == 3}"> <origin xyz="0 0 0" rpy="0 0 0"/> </xacro:if>
+      <xacro:if value="${id == 4}"> <origin xyz="0 0 0" rpy="0 0 0"/> </xacro:if>
+      <xacro:if value="${id == 5}"> <origin xyz="0 0 0" rpy="0 0 0"/> </xacro:if>
+      <xacro:if value="${id == 6}"> <origin xyz="0 0 0" rpy="0 0 0"/> </xacro:if>
+      <xacro:if value="${id == 7}"> <origin xyz="0 0 0" rpy="0 0 0"/> </xacro:if>
+    </xacro:if>
+  </xacro:macro>
+
 </robot>