第七章建模与仿真分为上、中、下三节。

  • 上节讲解机器人相关建模语言URDF和xacro，并用丰富的案例做实践
  • 中节讲解控制器，包括ros_control, ArbotiX差分控制器
  • 下节主要介绍gazebo，机器人如何在仿真环境中的运动

一、URDF—机器人描述语言

URDF是机器人模型的描述格式，包括机器人刚体外观、物理属性、关节类型等。

1. URDF相关标签

在urdf中，描述机器人使用link，link之间使用joint连接

在这里插入图片描述

1.1 link标签

刚体。
描述刚体外观和物理属性。

1.2joint标签

关节。连接两个刚体。
描述机器人关节的运动学和动力学属性，包括关节运动的位置和速度限制

1.3 robot标签

完整机器人的顶层标签。刚体与关节必须在该标签内。

1.4 gazebo标签

描述机器人模型在gazebo仿真中的参数。包括材料属性、Gazebo插件等。

非必须标签。

2. 实例

手动写一遍urdf来实现一个机器人模型,对理解整个仿真机器人构造很有帮助
也会更加明白引入xacro的重要性.


<?xml version="1.0"?>
<robot name="my_robot1">

    <material name="blue">
<!--        color range [0 - 1]-->
        <color rgba="0 0 0.8 1"/>
    </material>

    <material name="black">
        <color rgba="0 0 0 1"/>
    </material>

    <material name="white">
        <color rgba="1 1 1 1"/>
    </material>

    <link name="base_link">
        <visual>
            <geometry>
                <cylinder length="0.6" radius="0.2" />
            </geometry>
            <material name="blue"/>
        </visual>
    </link>


    <link name="right_leg">
        <visual>
            <geometry>
                <box size="0.6 0.1 0.2" />
            </geometry>
<!--            base your own frame -->
            <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
            <material name="white"/>
        </visual>
    </link>


    <joint name="base_to_right_leg" type="fixed">
       <parent link="base_link"/>
       <child link="right_leg"/>
       <origin xyz="0 -0.22 0.25"/>
    </joint>

    <link name="right_base">
        <visual>
            <geometry>
                <box size="0.4 0.1 0.1"/>
            </geometry>
            <material name="white"/>
        </visual>
    </link>

    <joint name="right_base_joint" type="fixed">
        <parent link="right_leg"/>
        <child link="right_base"/>
        <origin xyz="0 0 -0.6"/>
    </joint>


    <link name="right_front_wheel">
        <visual>
            <geometry>
                <cylinder length="0.1" radius="0.035"/>
            </geometry>
            <origin rpy="1.57 0 0" xyz="0 0 0"/>
            <material name="black"/>
        </visual>
    </link>

    <joint name="right_front_wheel_joint" type="fixed">
        <parent link="right_base"/>
        <child link="right_front_wheel"/>
        <origin rpy="0 0 0" xyz="0.133333333333 0 -0.085"/>
    </joint>

    <link name="right_back_wheel">
        <visual>
            <geometry>
                <cylinder length="0.1" radius="0.035"/>
            </geometry>
            <origin rpy="1.57 0 0" xyz="0 0 0"/>
            <material name="black"/>
        </visual>
    </link>

    <joint name="right_back_wheel_joint" type="fixed">
        <parent link="right_base"/>
        <child link="right_back_wheel"/>
        <origin rpy="0 0 0" xyz="-0.133333333333 0 -0.085"/>
    </joint>


    <link name="left_leg">
        <visual>
            <geometry>
                <box size="0.6 0.1 0.2" />
            </geometry>
            <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
            <material name="white"/>
        </visual>
    </link>


    <joint name="base_to_left_leg" type="fixed">
       <parent link="base_link"/>
       <child link="left_leg"/>
       <origin xyz="0 0.22 0.25"/>
    </joint>


    <link name="left_base">
        <visual>
            <geometry>
                <box size="0.4 0.1 0.1"/>
            </geometry>
            <material name="white"/>
        </visual>
    </link>

    <joint name="left_base_joint" type="fixed">
        <parent link="left_leg"/>
        <child link="left_base"/>
        <origin xyz="0 0 -0.6"/>
    </joint>


    <link name="left_front_wheel">
        <visual>
            <geometry>
                <cylinder length="0.1" radius="0.035"/>
            </geometry>
            <origin rpy="1.57 0 0" xyz="0 0 0"/>
            <material name="black"/>
        </visual>
    </link>

    <joint name="left_front_wheel_joint" type="fixed">
        <parent link="left_base"/>
        <child link="left_front_wheel"/>
        <origin rpy="0 0 0" xyz="0.133333333333 0 -0.085"/>
    </joint>

    <link name="left_back_wheel">
        <visual>
            <geometry>
                <cylinder length="0.1" radius="0.035"/>
            </geometry>
            <origin rpy="1.57 0 0" xyz="0 0 0"/>
            <material name="black"/>
        </visual>
    </link>

    <joint name="left_back_wheel_joint" type="fixed">
        <parent link="left_base"/>
        <child link="left_back_wheel"/>
        <origin rpy="0 0 0" xyz="-0.133333333333 0 -0.085"/>
    </joint>


    <joint name="gripper_extension" type="fixed">
        <parent link="base_link"/>
        <child link="gripper_pole"/>
        <origin rpy="0 0 0" xyz="0.19 0 0.2"/>
    </joint>

    <link name="gripper_pole">
        <visual>
            <geometry>
                <cylinder length="0.2" radius="0.01"/>
            </geometry>
            <material name="white"/>
            <origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
        </visual>
    </link>

    <joint name="left_gripper_joint" type="fixed">
        <origin rpy="0 0 0" xyz="0.2 0.01 0"/>
        <parent link="gripper_pole"/>
        <child link="left_gripper"/>
    </joint>

    <link name="left_gripper">
        <visual>
            <origin rpy="0.0 0 0" xyz="0 0 0"/>
            <geometry>
                <mesh filename="package://urdf_tutorial/meshes/l_finger.dae"/>
            </geometry>
        </visual>
    </link>


    <joint name="left_tip_joint" type="fixed">
        <parent link="left_gripper"/>
        <child link="left_tip"/>
    </joint>

    <link name="left_tip">
        <visual>
            <origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
            <geometry>
                <mesh filename="package://urdf_tutorial/meshes/l_finger_tip.dae"/>
            </geometry>
        </visual>
    </link>
    <joint name="right_gripper_joint" type="fixed">
        <origin rpy="0 0 0" xyz="0.2 -0.01 0"/>
        <parent link="gripper_pole"/>
        <child link="right_gripper"/>
    </joint>

    <link name="right_gripper">
        <visual>
            <origin rpy="-3.1415 0 0" xyz="0 0 0"/>
            <geometry>
                <mesh filename="package://urdf_tutorial/meshes/l_finger.dae"/>
            </geometry>
        </visual>
    </link>

    <joint name="right_tip_joint" type="fixed">
        <parent link="right_gripper"/>
        <child link="right_tip"/>
    </joint>

    <link name="right_tip">
        <visual>
            <origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
            <geometry>
                <mesh filename="package://urdf_tutorial/meshes/l_finger_tip.dae"/>
            </geometry>
        </visual>
    </link>


    <link name="head">
        <visual>
            <geometry>
                <sphere radius="0.2"/>
            </geometry>
            <material name="white"/>
        </visual>
    </link>

    <joint name="head_swivel" type="fixed">
        <parent link="base_link"/>
        <child link="head"/>
        <origin xyz="0 0 0.3"/>
    </joint>

    <link name="box">
        <visual>
            <geometry>
                <box size="0.08 0.08 0.08"/>
            </geometry>
            <material name="blue"/>
        </visual>
    </link>

    <joint name="tobox" type="fixed">
        <parent link="head"/>
        <child link="box"/>
        <origin xyz="0.15 0 0.11"/>
    </joint>

</robot>

launch 文件:


<launch>

    <arg name="model" default="$(find learn_model)/urdf/my_robot.urdf"/>
    <arg name="rvizconfig" default="$(find urdf_tutorial)/rviz/urdf.rviz" />

    <param name="robot_description" command="$(find xacro)/xacro $(arg model)" />
<!--    <param name="use_gui" value="$(arg gui)"/>-->

    <node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" />
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />

</launch>

最终效果

在这里插入图片描述

参考教程

link

3. 相关tools/pack

3.1 验证工具

sudo apt install liburdfdom-tools

在终端输入如下命令：
check_urdf **.urdf

输出结果：

在这里插入图片描述

3.2 可视化工具

在终端输入如下命令：
urdf_to_graphiz **.urdf

在选定目录下生成节点连接的pdf文件：

在这里插入图片描述没有问题!!

3.3 joint_state_publisher

节点发布每个joint的状态(除了fixed类型)，还可以通过UI对joint进行控制

3.4 robot_state_publisher

将机器人各个link，joint之间的关系，通过TF的形式整理成三维姿态发布
会订阅joint_state_publisher节点发布的joint_states话题。

订阅参数robot_description，所以需要roslaunch的时候提供对应参数。
如果参数有变的话，可以使用<remap>标签重映射，如下例：

<launch>
	<param name="my_robot_description" textfile="$(find mrobot_description)/urdf/mrobot_chassis.urdf" />
		<!-- 运行robot_state_publisher节点，发布tf  -->
	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
		<remap from="robot_description" to="my_robot_discription"/>
	</node>>
</launch>

二、让机器人动起来

机器人动起来最关键的关节joint标签。

joint标签最后需要指定type, 所有的type有如下几种:

  1. continuous: 旋转关节
  2. revolute: 旋转关节,角度有限制
  3. prismatic: 滑动关节,有位置极限
  4. fixed: 固定关节.
  5. floating: 浮动关节
  6. planar: 平面关节

之前我们的机器人模型,都是用的fixed固定关节.
本小节,笔者会探索另外几种重要的关节类型.

the head 的 continuous关节

机器人的头: 属于continuous关节

  <joint name="head_swivel" type="continuous">
    <parent link="base_link"/>
    <child link="head"/>
    <axis xyz="0 0 1"/>
    <origin xyz="0 0 0.3"/>
  </joint>

按照上面描述,这颗头可以沿着z轴360度旋转. 我们来看看效果:

在这里插入图片描述
gripper的revolute关节

前面的抓手, 属于rebolute关节,他们也可以沿着某个轴转动,但是有角度限制

  <joint name="left_gripper_joint" type="revolute">
    <axis xyz="0 0 1"/>
    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
    <origin rpy="0 0 0" xyz="0.2 0.01 0"/>
    <parent link="gripper_pole"/>
    <child link="left_gripper"/>
  </joint>

limit标签就是用来限制角度的.

看看效果:

在这里插入图片描述
gripper ARM 的 prismatic标签

抓手的手臂属于滑动标签,而且限制滑动范围.

看看urdf怎么写:

  <joint name="gripper_extension" type="prismatic">
    <parent link="base_link"/>
    <child link="gripper_pole"/>
    <limit effort="1000.0" lower="-0.38" upper="0" velocity="0.5"/>
    <origin rpy="0 0 0" xyz="0.19 0 0.2"/>
  </joint>

看看效果:

在这里插入图片描述
最后的两个标签解释:
沿着对prismatic的理解,我们将移动范围扩大到二维和三维空间之后,就是另外的两种关节移动方法.
floating: 浮动关节是在三维空间任意移动
planar: 平面关节是在平面空间任意移动

最后

思考下为什么移动滑窗能控制相关关节的移动?

回答:

首先GUI解析URDF找到所有非固定关节以及他们的活动范围.
然后GUI使用滑窗发布message:sensor_msgs/JointState
robot_state_publisher订阅该message,计算各个部分的TF
最终的tf_tree以及刚体形状被rviz可视化出来

三、添加物理与碰撞属性

1 碰撞属性

<link name="base_link">
    <visual>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
      <material name="blue">
        <color rgba="0 0 .8 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
    </collision>
  </link>
  • collision标签直接放在link下，与visual标签同等级。
  • collision标签可以设置更大的外围范围，比如保护头的时候，可以比原本的geometry大一圈；同样复杂的meshes可以用简单的geometry包裹，这样有利于快速的碰撞检测。

2 物理属性

1. 惯性：


  <link name="base_link">
    <visual>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
      <material name="blue">
        <color rgba="0 0 .8 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="10"/>
      <inertia ixx="0.4" ixy="0.0" ixz="0.0" iyy="0.4" iyz="0.0" izz="0.2"/>
    </inertial>
  </link>
  • inertial元素，同样与visual同等级
  • mass单位为kg
  • inertia元素中的6个数字分别代表如下矩阵：
    在这里插入图片描述
    具体的物理含义是惯性张量。对角元素是分别对x,y,z三轴的转动惯量，非对角元素称为惯量积。
    矩阵用于物体对其旋转运动的惯性大小的度量。
    具体解释参考维基百科

其他标签：

对于link而言，在碰撞collisiontag下还有其他属性可以设置：

  • mu: 摩擦系数
  • kp：刚体系数
  • kd：阻尼系数

对于joint而言，有如下物理属性：

  • friction： 摩擦
  • damping：阻尼

四、xacro语言简化URDF模型

在我们生成机器人URDF过程中，一定会对很多对称的重复的定义感到繁琐。针对URDF中冗长重复的定义，xacro语言作为一种macro语言可以实现对urdf的简化。

1. constants常量定义

很多尺寸坐标等常量的使用，使得原本的URDF模型可读性极差，并且难以维护。

xacro提供一种常量属性的定义方式：

<xacro:property name="width" value="0.2" />

我们先用常量来给前文中的urdf瘦身。查看如下原来的base_link:

  <link name="base_link">
    <visual>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
      <material name="blue"/>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.6" radius="0.2"/>
      </geometry>
    </collision>
  </link>

使用常量的base_link:

<xacro:property name="width" value="0.2" />
<xacro:property name="bodylen" value="0.6" />
<link name="base_link">
    <visual>
        <geometry>
            <cylinder radius="${width}" length="${bodylen}"/>
        </geometry>
        <material name="blue"/>
    </visual>
    <collision>
        <geometry>
            <cylinder radius="${width}" length="${bodylen}"/>
        </geometry>
    </collision>
</link>

两行属性的定义可以出现在xacro文件中的任何位置。

2. 数学公式

<cylinder radius="${wheeldiam/2}" length="0.1"/>
<origin xyz="${reflect*(width+.02)} 0 0.25" />

在${}符号中，除了调用常量，还可以使用一些常用的数学运算：+ - * / （） sin cos等

3. Macros宏定义

像写函数一样，宏定义声明基于输入的参数，可以重复使用代码模块。

比如一辆车有四个轮子，我们写一个轮子的定义够了。其他只是参数的不同。

我们看一下，前文中机器人的两条腿，来实现代码复用。

<xacro:macro name="leg" params="prefix reflect">
    <link name="${prefix}_leg">
        <visual>
            <geometry>
                <box size="${leglen} 0.1 0.2"/>
            </geometry>
            <origin xyz="0 0 -${leglen/2}" rpy="0 ${pi/2} 0"/>
            <material name="white"/>
        </visual>
        <collision>
            <geometry>
                <box size="${leglen} 0.1 0.2"/>
            </geometry>
            <origin xyz="0 0 -${leglen/2}" rpy="0 ${pi/2} 0"/>
        </collision>
        <xacro:default_inertial mass="10"/>
    </link>

    <joint name="base_to_${prefix}_leg" type="fixed">
        <parent link="base_link"/>
        <child link="${prefix}_leg"/>
        <origin xyz="0 ${reflect*(width+.02)} 0.25" />
    </joint>
    <!-- A bunch of stuff cut -->
</xacro:macro>
<xacro:leg prefix="right" reflect="1" />
<xacro:leg prefix="left" reflect="-1" />

trick

  • 使用名称前缀命名两个相似物体
  • 使用math计算关节朝向
  • 使用镜像参数，1 和 -1实现两条腿对称

其他有用的特性

  • 如果传入的参数为属性块，需要在参数栏对应参数名字前添加*号
  • 在对应的宏定义块中，插入属性块部分需要使用insert_block 命令

举个例子：

<xacro:macro name="blue_shape" params="name *shape">
    <link name="${name}">
        <visual>
            <geometry>
                <xacro:insert_block name="shape" />
            </geometry>
            <material name="blue"/>
        </visual>
        <collision>
            <geometry>
                <xacro:insert_block name="shape" />
            </geometry>
        </collision>
    </link>
</xacro:macro>

在定义blue_shape的时候，由于传入的参数为属性块，需要在参数栏对应参数名字shape前添加*号

同样在插入属性块部分需要使用insert_block 命令：
<xacro:insert_block name="shape" />

在使用blue_shape宏定义的时候，需要传入两种参数，一个是name，一个是从属性块*shape。 如下所示：

<xacro:blue_shape name="base_link">
    <cylinder radius=".42" length=".01" />
</xacro:blue_shape>

4. 使用xacro语言

使用命令行可以直接将xacro文件解析为urdf文件

xacro  model.xacro > model.urdf

在launch文件中，我们同样可以使用对应命令生成urdf文件

<param name="robot_description"
  command="xacro --inorder '$(find pr2_description)/robots/pr2.urdf.xacro'" />