ROS URDF(四) joint和link坐标系及变换释义

源码：

<?xml version="1.0"?>
<robot name="pan_tilt">
    <link name="base_link">
        <visual>
            <geometry>
                <cylinder length="0.01" radius="0.2"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <material name="yellow">
                <color rgba="1 1 0 1"/>
            </material>
        </visual>
    </link>

    <joint name="pan_joint" type="revolute">
        <limit effort="1000.0" lower="0.0" upper="3.1415926" velocity="0.5"/>
        <parent link="base_link"/>
        <child link="pan_link"/>
        <origin xyz="0 0 0.1" rpy="0 0 -0.7854"/>
        <axis xyz="0 0 1"/>
    </joint>

    <link name="pan_link">
        <visual>
            <geometry>
                <cylinder length="0.4" radius="0.04"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.1"/>
            <material name="red">
                <color rgba="0 0 1 1"/>
            </material>
        </visual>
    </link>

    <joint name="tilt_joint" type="revolute">
        <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
        <parent link="pan_link"/>
        <child link="tilt_link"/>
        <origin xyz="0 0 0.2" rpy="0 0 0.7854"/>
        <axis xyz="0 0 1" />
    </joint>

    <link name="tilt_link">
    <visual>
        <geometry>
            <cylinder length="0.4" radius="0.04"/>
        </geometry>
        <origin rpy="0 1.5708 0" xyz="0 0 0"/>
        <material name="green">
            <color rgba="1 0 0 1"/>
        </material>
    </visual>
    </link>
</robot>

源码对应的Rviz显示模型：

图1

上述模型由两个joint(pan_joint、tilt_joint)和三个link(base_link、pan_link、tilt_link)组成。其中，base_link固定于大地上，pan_link和tilt_link分别为pan_joint和tilt_joint的child link。

注：上图中显示的base_link、pan_link、tilt_link都是坐标系(frame)名称，而不是具有圆柱几何形状的杆件。

1 base_link

1.1 base_link frame
base_link frame 可以认为是默认的大地(世界坐标系)，作为整个模型系统的参考。
1.2 base_link 连杆
base_link 连杆 是上图中黄色的圆盘杆件，该杆件仅用于更形象的显示模型结构，而不是模型本身，模型本身是frame。
如将URDF中 <origin rpy=“0 0 0” xyz=“0.1 0 0”/ >中x量改为0.1，仅仅是圆盘显示构件x方向发生了0.1的平移，而base_link坐标系不动，如 图2 所示。

	<link name="base_link">
        <visual>
            <geometry>
                <cylinder length="0.01" radius="0.2"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0.1 0 0"/>
            <material name="yellow">
                <color rgba="1 1 0 1"/>
            </material>
        </visual>
    </link>

图2

2 pan_link

2.1 pan_link frame
pan_link frame对应于joint标签

	<joint name="pan_joint" type="revolute">
        <limit effort="1000.0" lower="0.0" upper="3.1415926" velocity="0.5"/>
        <parent link="base_link"/>
        <child link="pan_link"/>
        <origin xyz="0 0 0.1" rpy="0 0 -0.7854"/>
        <axis xyz="0 0 1"/>
    </joint>

其中最重要的配置为 origin xyz=“0 0 0.1” rpy=“0 0 -0.7854”，该配置表征了pan_link frame的位置和姿态，该位置和姿态是基于pan_joint的parent link对应的frame而言的，即base_link frame.
（1）origin xyz=“0 0 0” rpy="0 0 0"表示：pan_link frame与base_link frame 重合
（2）xyz=“0 0 0.1” rpy=“0 0 -0.7854"表示：将base_link frame 沿自身的z轴方向平移0.1m，再沿平移后自身的x轴平移0.1m，再沿变换后的自身的z轴旋转-0.7854rad（-45°）得到pan_link frame。即pan_link frame是将base_link frame按 xyz=“0 0 0.1” rpy="0 0 -0.7854"变换得到的，并且是先平移后旋转，并且每次变换都是以变换后的base_link为参考。
origin xyz=“0.1 0 0.1” rpy=” 0 0 -0.7854"的变换 如图3所示。

图3
2.2 pan_link 连杆
pan_link 连杆对应于link标签

    <link name="pan_link">
        <visual>
            <geometry>
                <cylinder length="0.4" radius="0.04"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.1"/>
            <material name="red">
                <color rgba="0 0 1 1"/>
            </material>
        </visual>
    </link>

其中最重要的配置为 origin xyz=“0 0 0.1” rpy=“0 0 0”，该配置表征了pan_link 连杆的位置和姿态，该位置和姿态是基于pan_link frame而言。此时可以认为在连杆几何中心上固连一个隐藏的坐标系，初始时，该隐藏坐标系与pan_link frame重合，然后将隐藏的坐标系按origin xyz=“0 0 0.1” rpy="0 0 0"沿隐藏的坐标自身轴进行变换，同样是先平移后旋转得到连杆最终的姿态。
origin xyz=“0 0 0.1” rpy="0.7854 0 0"的变换结果如图4所示，变换的仅仅是连杆的状态，坐标系没有变化。

图4

3 tilt_link

3.1 tilt_link frame
原理与pan_link frame 相同，origin xyz=“0 0 0.2” rpy=“0 0 0.7854” 表征了tilt_link frame的位置和姿态，该位置和姿态是基于tilt_joint的parent link对应的frame而言的，即pan_link frame，其余不再赘述,参考2 pan_link。
3.2 tilt_link 连杆
原理与tilt_link 连杆 相同，origin rpy=“0 1.5708 0” xyz=“0 0 0” 表征了tilt_link 连杆的位置和姿态，该位置和姿态是基于tilt_link frame而言,其余不再赘述，参考2 pan_link。

4 为什么显示的关节坐标系名与连杆名相同

从图5可以看出child fram = joint frame，所以Rviz中显示的关节旋转坐标系，不是pan_joint、tilt_joint而是pan_link、tilt_link，如图6。

图5

图6