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

57
0
6天前

源码:

 

<?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

发表评论

后才能评论