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