9. 理解PR2机器人的URDF描述

描述：本教程将会说明PR2等复杂机器人的顶层URDF Xacro文件布局。

教程级别：中级

注：本教程仍会参考pr2_defs中的描述，该描述目前已被替换为pr2_description中的描述。尽管相似，但这个URDF与PR2使用的当前URDF之间已经发生了一些变化。具体来说，Gazebo不再需要<robot>元素中指定的XML模式，而且对Gazebo插件的所有引用都是不正确的，因为这些插件的API已经发生了很大变化。

完整的PR2 URDF宏文件可以在pr2_description软件包中的文件robots/pr2.urdf.xacro中找到。下面提供了其快照以供剖析。

<?xml version="1.0"?>
<robot xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
        xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
        xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
        xmlns:xacro="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
        name="pr2" >

   <!-- The following included files set up definitions of parts of the robot body -->
   <!-- misc common stuff? -->
   <include filename="$(find pr2_description)/urdf/common.xacro" />
   <!-- PR2 Arm -->
   <include filename="$(find pr2_description)/urdf/shoulder_v0/shoulder.urdf.xacro" />
   <include filename="$(find pr2_description)/urdf/upper_arm_v0/upper_arm.urdf.xacro" />
   <include filename="$(find pr2_description)/urdf/forearm_v0/forearm.urdf.xacro" />
   <!-- PR2 gripper -->
   <include filename="$(find pr2_description)/urdf/gripper_v0/gripper.urdf.xacro" />
   <!-- PR2 head -->
   <include filename="$(find pr2_description)/urdf/head_v0/head.urdf.xacro" />
   <!-- PR2 tilting laser mount -->
   <include filename="$(find pr2_description)/urdf/tilting_laser_v0/tilting_laser.urdf.xacro" />
   <!-- PR2 torso -->
   <include filename="$(find pr2_description)/urdf/torso_v0/torso.urdf.xacro" />
   <!-- PR2 base -->
   <include filename="$(find pr2_description)/urdf/base_v0/base.urdf.xacro" />
   <!-- Head sensors -->
   <include filename="$(find pr2_description)/urdf/sensors/head_sensor_package.urdf.xacro" />
   <!-- Camera sensors -->
   <include filename="$(find pr2_description)/urdf/sensors/wge100_camera.urdf.xacro" />
   <!-- generic simulator_gazebo plugins for starting mechanism control, ros time, ros battery -->
   <include filename="$(find pr2_description)/gazebo/gazebo.urdf.xacro" />
   <!-- materials for visualization -->
   <include filename="$(find pr2_description)/urdf/materials.urdf.xacro" />

   <!-- Now we can start using the macros included above to define the actual PR2 -->

   <!-- The first use of a macro.  This one was defined in base.urdf.xacro above.
        A macro like this will expand to a set of link and joint definitions, and to additional
        Gazebo-related extensions (sensor plugins, etc).  The macro takes an argument, name, 
        that equals "base", and uses it to generate names for its component links and joints 
        (e.g., base_link).  The included origin block is also an argument to the macro.  By convention, 
        the origin block defines where the component is w.r.t its parent (in this case the parent 
        is the world frame). For more, see http://www.ros.org/wiki/xacro -->
   <xacro:pr2_base_v0 name="base" >
     <origin xyz="0 0 0.051" rpy="0 0 0" /> <!-- 5.1cm is the height of the base when wheels contact ground -->
   </xacro:pr2_base_v0>

   <xacro:pr2_torso_v0 name="torso_lift" parent="base_link">
     <origin xyz="-0.05 0 0.739675" rpy="0 0 0" />
   </xacro:pr2_torso_v0>

   <!-- The xacro preprocesser will replace the parameters below, such as ${cal_head_x}, with
        numerical values that were specified in common.xacro which was included above -->
   <xacro:pr2_head_v0 name="head" parent="torso_lift_link">
     <origin xyz="${cal_head_x}    ${cal_head_y}     ${0.3915+cal_head_z}"
             rpy="${cal_head_roll} ${cal_head_pitch} ${cal_head_yaw}" />
   </xacro:pr2_head_v0>

   <!-- Camera package: double stereo, prosilica -->
   <xacro:pr2_head_sensor_package_v0 name="sensor_mount" hd_name="high_def" 
                            stereo_name="double_stereo" 
                            parent="head_plate_frame">
     <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
   </xacro:pr2_head_sensor_package_v0>

   <xacro:pr2_tilting_laser_v0 name="laser_tilt" parent="torso_lift_link" laser_calib_ref="-0.35">
     <origin xyz="0.1 0 0.235" rpy="0 0 0" />
   </xacro:pr2_tilting_laser_v0>

   <!-- This is a common convention, to use a reflect parameter that equals +-1 to distinguish left from right -->
   <xacro:pr2_shoulder_v0 side="r" reflect="-1" parent="torso_lift_link">
     <origin xyz="0.0 -0.188 0.0" rpy="0 0 0" />
   </xacro:pr2_shoulder_v0>
   <xacro:pr2_upper_arm_v0 side="r" reflect="-1" parent="r_upper_arm_roll_link"/>
   <xacro:pr2_forearm_v0 side="r" reflect="-1" parent="r_forearm_roll_link"/>

   <xacro:pr2_gripper_v0 side="r" parent="r_wrist_roll_link"
                screw_reduction="${4.0/1000.0}"
                gear_ratio="${(729.0/25.0)*(22.0/16.0)}"
                theta0="${3.6029*M_PI/180.0}"
                phi0="${29.7089*M_PI/180.0}"
                t0="${-0.1914/1000.0}"
                L0="${37.5528/1000.0}"
                h="${0.0/1000.0}"
                a="${68.3698/1000.0}"
                b="${43.3849/1000.0}"
                r="${91.5/1000.0}" >
     <origin xyz="0 0 0" rpy="0 0 0" />
   </xacro:pr2_gripper_v0>

   <xacro:pr2_shoulder_v0 side="l" reflect="1" parent="torso_lift_link">
     <origin xyz="0.0 0.188 0.0" rpy="0 0 0" />
   </xacro:pr2_shoulder_v0>
   <xacro:pr2_upper_arm_v0 side="l" reflect="1" parent="l_upper_arm_roll_link"/>
   <xacro:pr2_forearm_v0 side="l" reflect="1" parent="l_forearm_roll_link"/>

   <xacro:pr2_gripper_v0 side="l" parent="l_wrist_roll_link"
                screw_reduction="${4.0/1000.0}"
                gear_ratio="${(729.0/25.0)*(22.0/16.0)}"
                theta0="${3.6029*M_PI/180.0}"
                phi0="${29.7089*M_PI/180.0}"
                t0="${-0.1914/1000.0}"
                L0="${37.5528/1000.0}"
                h="${0.0/1000.0}"
                a="${68.3698/1000.0}"
                b="${43.3849/1000.0}"
                r="${91.5/1000.0}" >
     <origin xyz="0 0 0" rpy="0 0 0" />
   </xacro:pr2_gripper_v0>

   <!-- Forearm Cam (Hand approximated values) -->
   <xacro:wge100_camera_v0 name="l_forearm_cam" image_format="L8" image_topic_name="l_forearm_cam/image_raw"
                           camera_info_topic_name="l_forearm_cam/camera_info"
                           parent="l_forearm_roll_link" hfov="90" focal_length="320"
                           frame_id="l_forearm_cam_frame" hack_baseline="0"
                           image_width="640" image_height="480">
     <origin xyz=".15 0 .07" rpy="${ M_PI/2} ${-45*M_PI/180} 0" />
   </xacro:wge100_camera_v0>
   <xacro:wge100_camera_v0 name="r_forearm_cam" image_format="L8" image_topic_name="r_forearm_cam/image_raw"
                           camera_info_topic_name="r_forearm_cam/camera_info"
                           parent="r_forearm_roll_link" hfov="90" focal_length="320"
                           frame_id="r_forearm_cam_frame" hack_baseline="0"
                           image_width="640" image_height="480">
     <origin xyz=".15 0 .07" rpy="${-M_PI/2} ${-45*M_PI/180} 0" />
   </xacro:wge100_camera_v0>

</robot>

现在来逐段代码对整个XML进行说明：

<robot xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
       xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
       xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
       xmlns:xacro="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
       name="pr2" >

XML的根元素必须是带有一个name属性的robot元素。上段代码中申明了XML的命名空间。

   <!-- The following included files set up definitions of parts of the robot body -->
   <!-- misc common stuff? -->
   <include filename="$(find pr2_description)/urdf/common.xacro" />
   <!-- PR2 Arm -->
   <include filename="$(find pr2_description)/urdf/shoulder_v0/shoulder.urdf.xacro" />
   <include filename="$(find pr2_description)/urdf/upper_arm_v0/upper_arm.urdf.xacro" />
   <include filename="$(find pr2_description)/urdf/forearm_v0/forearm.urdf.xacro" />
   <!-- PR2 gripper -->
   <include filename="$(find pr2_description)/urdf/gripper_v0/gripper.urdf.xacro" />
   <!-- PR2 head -->
   <include filename="$(find pr2_description)/urdf/head_v0/head.urdf.xacro" />
   <!-- PR2 tilting laser mount -->
   <include filename="$(find pr2_description)/urdf/tilting_laser_v0/tilting_laser.urdf.xacro" />
   <!-- PR2 torso -->
   <include filename="$(find pr2_description)/urdf/torso_v0/torso.urdf.xacro" />
   <!-- PR2 base -->
   <include filename="$(find pr2_description)/urdf/base_v0/base.urdf.xacro" />
   <!-- Head sensors -->
   <include filename="$(find pr2_description)/urdf/sensors/head_sensor_package.urdf.xacro" />
   <!-- Camera sensors -->
   <include filename="$(find pr2_description)/urdf/sensors/wge100_camera.urdf.xacro" />
   <!-- generic simulator_gazebo plugins for starting mechanism control, ros time, ros battery -->
   <include filename="$(find pr2_description)/gazebo/gazebo.urdf.xacro" />
   <!-- materials for visualization -->
   <include filename="$(find pr2_description)/urdf/materials.urdf.xacro" />

上段代码中会包含各xacro宏文件，这些xacro宏文件含有对机器人单个组件的描述。这就像在C语言中包含头文件一样：设置了一堆定义，但实际上并没有调用它们中的任何一个。例如，pr2_common/pr2_description/urdf/shoulder_v0/shoulder.urdf.xacro文件含有PR2机器人肩部的xacro宏。而PR2机器人的底座宏xacro:pr2_base_v0则是在pr2_description/urdf/base_v0/base.urdf.xacro包含文件中。该宏具有以下嵌套结构：

● xacro:pr2_base_v0—定义PR2机器人的底座，配有4个脚轮和8个轮子以及一个激光测距仪。

● xacro:pr2_caster_v0—定义PR2上连接到底座的脚轮。

● xacro:pr2_wheel_v0—定义PR2上连接到脚轮的轮子。

   <!-- Now we can start using the macros included above to define the actual PR2 -->

   <!-- The first use of a macro.  This one was defined in base.urdf.xacro above.
        A macro like this will expand to a set of link and joint definitions, and to additional
        Gazebo-related extensions (sensor plugins, etc).  The macro takes an argument, name, 
        that equals "base", and uses it to generate names for its component links and joints 
        (e.g., base_link).  The included origin block is also an argument to the macro.  By convention, 
        the origin block defines where the component is w.r.t its parent (in this case the parent 
        is the world frame). For more, see http://www.ros.org/wiki/xacro -->

上段代码是一个非常有用的注释。

   <xacro:pr2_base_v0 name="base" >
     <origin xyz="0 0 0.051" rpy="0 0 0" /> <!-- 5.1cm is the height of the base when wheels contact ground -->
   </xacro:pr2_base_v0>

现在来实际使用上面定义的宏。这段代码中我们使用xacro:pr2_base_v0宏来定义PR2机器人的底座，其name参数等于“base”且具有一个origin（原点）参数块。通过xacro预处理器运行原始文件后，该元素将会被替换为一个相当长的底座及其组件的URDF定义。

   <xacro:pr2_torso_v0 name="torso_lift" parent="base_link">
     <origin xyz="-0.05 0 0.739675" rpy="0 0 0" />
   </xacro:pr2_torso_v0>

类似地，我们使用xacro:pr2_torso_v0宏来定义PR2机器人的躯干，该宏可以在包含的文件pr2_description/urdf/torso_v0/torso.urdf.xacro中找到。

此处看到的origin块会在大多数宏中出现。我们用它来表示所定义的链接与其父链接相关的位置。在这段代码中，origin块用于表示torso_lift_link相对于其父级链接（base_link）的起始位置。

   <!-- The xacro preprocesser will replace the parameters below, such as ${cal_head_x}, with
        numerical values that were specified in common.xacro which was included above -->
   <xacro:pr2_head_v0 name="head" parent="torso_lift_link">
     <origin xyz="${cal_head_x}    ${cal_head_y}     ${0.3915+cal_head_z}"
             rpy="${cal_head_roll} ${cal_head_pitch} ${cal_head_yaw}" />
   </xacro:pr2_head_v0>

接下来我们使用xacro:pr2_head_v0宏。该宏可以在包含的文件pr2_description/urdf/head_v0/head.urdf.xacro中找到。请注意，这里使用的是常量（这些常量是在上面包含的校准文件中定义的：pr2_description/urdf/common.xacro），而不是原点的硬编码数字。

PR2机器人头部宏的嵌套结构如下：

● xacro:pr2_head_v0

○ xacro:pr2_head_pan_v0

○ xacro:pr2_head_tilt_v0

   <!-- Camera package: double stereo, prosilica -->
   <xacro:pr2_head_sensor_package_v0 name="sensor_mount" hd_name="high_def" 
                            stereo_name="double_stereo" 
                            parent="head_plate_frame">
     <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
   </xacro:pr2_head_sensor_package_v0>

上段代码是PR2机器人头部传感器包，包括两个隔行立体摄像头和一个高分辨率Prosilica GC2450摄像头。

   <xacro:pr2_tilting_laser_v0 name="laser_tilt" parent="torso_lift_link" laser_calib_ref="-0.35">
     <origin xyz="0.1 0 0.235" rpy="0 0 0" />
   </xacro:pr2_tilting_laser_v0>

这段代码定义了PR2机器人倾斜式Hokuyo激光测距仪。

   <!-- This is a common convention, to use a reflect parameter that equals +-1 to distinguish left from right -->
   <xacro:pr2_shoulder_v0 side="r" reflect="-1" parent="torso_lift_link">
     <origin xyz="0.0 -0.188 0.0" rpy="0 0 0" />
   </xacro:pr2_shoulder_v0>
   <xacro:pr2_upper_arm_v0 side="r" reflect="-1" parent="r_upper_arm_roll_link"/>
   <xacro:pr2_forearm_v0 side="r" reflect="-1" parent="r_forearm_roll_link"/>

   <xacro:pr2_gripper_v0 side="r" parent="r_wrist_roll_link"
                screw_reduction="${4.0/1000.0}"
                gear_ratio="${(729.0/25.0)*(22.0/16.0)}"
                theta0="${3.6029*M_PI/180.0}"
                phi0="${29.7089*M_PI/180.0}"
                t0="${-0.1914/1000.0}"
                L0="${37.5528/1000.0}"
                h="${0.0/1000.0}"
                a="${68.3698/1000.0}"
                b="${43.3849/1000.0}"
                r="${91.5/1000.0}" >
     <origin xyz="0 0 0" rpy="0 0 0" />
   </xacro:pr2_gripper_v0>

   <xacro:pr2_shoulder_v0 side="l" reflect="1" parent="torso_lift_link">
     <origin xyz="0.0 0.188 0.0" rpy="0 0 0" />
   </xacro:pr2_shoulder_v0>
   <xacro:pr2_upper_arm_v0 side="l" reflect="1" parent="l_upper_arm_roll_link"/>
   <xacro:pr2_forearm_v0 side="l" reflect="1" parent="l_forearm_roll_link"/>

   <xacro:pr2_gripper_v0 side="l" parent="l_wrist_roll_link"
                screw_reduction="${4.0/1000.0}"
                gear_ratio="${(729.0/25.0)*(22.0/16.0)}"
                theta0="${3.6029*M_PI/180.0}"
                phi0="${29.7089*M_PI/180.0}"
                t0="${-0.1914/1000.0}"
                L0="${37.5528/1000.0}"
                h="${0.0/1000.0}"
                a="${68.3698/1000.0}"
                b="${43.3849/1000.0}"
                r="${91.5/1000.0}" >
     <origin xyz="0 0 0" rpy="0 0 0" />
   </xacro:pr2_gripper_v0>

上段代码定义了PR2机器人的左右臂，从肩部平移链接到抓手。

   <!-- Forearm Cam (Hand approximated values) -->
   <xacro:wge100_camera_v0 name="l_forearm_cam" image_format="L8" image_topic_name="l_forearm_cam/image_raw"
                           camera_info_topic_name="l_forearm_cam/camera_info"
                           parent="l_forearm_roll_link" hfov="90" focal_length="320"
                           frame_id="l_forearm_cam_frame" hack_baseline="0"
                           image_width="640" image_height="480">
     <origin xyz=".15 0 .07" rpy="${ M_PI/2} ${-45*M_PI/180} 0" />
   </xacro:wge100_camera_v0>
   <xacro:wge100_camera_v0 name="r_forearm_cam" image_format="L8" image_topic_name="r_forearm_cam/image_raw"
                           camera_info_topic_name="r_forearm_cam/camera_info"
                           parent="r_forearm_roll_link" hfov="90" focal_length="320"
                           frame_id="r_forearm_cam_frame" hack_baseline="0"
                           image_width="640" image_height="480">
     <origin xyz=".15 0 .07" rpy="${-M_PI/2} ${-45*M_PI/180} 0" />
   </xacro:wge100_camera_v0>

这段代码定义了前臂相机。

英语原文地址：wiki.ros.org/urdf/Tutor