机器人系统仿真(十一)——URDF集成Gazebo

参考视频：【奥特学园】ROS机器人入门课程《ROS理论与实践》零基础教程_哔哩哔哩_bilibili

参考文档：http://www.autolabor.com.cn/book/ROSTutorials/

URDF 与 Gazebo 集成流程与 Rviz 实现类似，主要步骤如下:

1. 创建功能包，导入依赖项

2. 编写 URDF 或 Xacro 文件

3. 启动 Gazebo 并显示机器人模型

1.创建功能包

创建新功能包，导入依赖包: urdf、xacro、gazebo_ros、gazebo_ros_control、gazebo_plugins

2.编写URDF文件

<robot name="mycar">
    <link name="base_link">
        <!--可视化部分-->
        <visual>
            <geometry>
                <box size="0.5 0.3 0.1" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <material name="yellow">
                <color rgba="0.5 0.3 0 0.8" />
            </material>
        </visual>
        <!--1.设置碰撞参数-->
        <!--如果是标准几何体，直接复制 visual 的 geometry 和 origin 即可-->
        <collision>
            <geometry>
                <box size="0.5 0.3 0.1" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <material name="yellow">
                <color rgba="0.5 0.3 0 0.8" />
            </material>
        </collision> 
        <!--2.设置惯性矩阵-->
        <inertial>
            <origin xyz="0 0 0" />
            <mass value="2" />
            <intertia  ixx="1" ixy="0" ixy="0" iyy="0" iyz="1" izz="1" />
        </inertial>
    </link>
    <!--gazebo 有自己的颜色设置标签-->
    <gazebo reference="base_link">
        <material>Gazebo/Red</material>
    </gazebo>
</robot>

注意， 当 URDF 需要与 Gazebo 集成时，和 Rviz 有明显区别:

1.必须使用 collision 标签，因为既然是仿真环境，那么必然涉及到碰撞检测，collision 提供碰撞检测的依据。

2.必须使用 inertial 标签，此标签标注了当前机器人某个刚体部分的惯性矩阵，用于一些力学相关的仿真计算。

3.颜色设置，也需要重新使用 gazebo 标签标注，因为之前的颜色设置为了方便调试包含透明度，仿真环境下没有此选项。

3.启动Gazebo并显示模型

可以在终端输入命令，尝试启动 gazebo

cys@ubuntu:~/demo05_ws$ roslaunch gazebo_ros empty_world.launch 

 或者，编写 demo01_helloworld.launch 再运行

<launch>
    <!--1.需要在参数服务器中载入 urdf -->
    <param name="robot_description" textfile="$(find urdf02_gazebo)/urdf/demo01_helloworld.urdf" />

    <!--2.启动 Gazebo 仿真环境 -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch" />
    <!--3.在 Gazebo 中添加机器人模型 -->


</launch>

但这样没有机器人模型，要在 gazebo 里加载机器人模型

法一：还要输入命令：

启动 gazebo_ros 下的 spawn_model 这个节点，这个节点会从参数服务器 robot_description 加载urdf 格式的数据，产生一个新的模型，并将模型命名为 car

cys@ubuntu:~$ rosrun gazebo_ros spawn_model -urdf -model car -param robot_ddescription

用以下命令可以查看 spawn_model 的参数

rosrun gazebo_ros spawn_model -urdf -model -h 

法二：或者集成进 launch 文件

<launch>
    <!--1.需要在参数服务器中载入 urdf -->
    <param name="robot_description" textfile="$(find urdf02_gazebo)/urdf/demo01_helloworld.urdf" />

    <!--2.启动 Gazebo 仿真环境 -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch" />
    <!--3.在 Gazebo 中添加机器人模型 -->
    <node pkg="gazebo_ros" type="spawn_model" name="spawn_model" args="-urdf -model car -param robot_description" />

</launch>

在 Gazebo 中加载一个机器人模型，该功能由 gazebo_ros 下的 spawn_model 提供:
-urdf 加载的是 urdf 文件
-model mycar 模型名称是 mycar
-param robot_description 从参数 robot_description 中载入模型
-x 模型载入的 x 坐标
-y 模型载入的 y 坐标
-z 模型载入的 z 坐标

启动文件

4.URDF集成Gazebo相关设置

较之于 rviz，gazebo在集成 URDF 时，需要做些许修改，比如:必须添加 collision 碰撞属性相关参数、必须添加 inertial 惯性矩阵相关参数，另外，如果直接移植 Rviz 中机器人的颜色设置是没有显示的，颜色设置也必须做相应的变更。

4.1.collision

如果机器人link是标准的几何体形状，和link的 visual 属性设置一致即可。

4.2.inertial

惯性矩阵的设置需要结合link的质量与外形参数动态生成，标准的球体、圆柱与立方体的惯性矩阵公式如下(已经封装为 xacro 实现):

球体惯性矩阵

<!-- Macro for inertia matrix -->
    <xacro:macro name="sphere_inertial_matrix" params="m r">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${2*m*r*r/5}" ixy="0" ixz="0"
                iyy="${2*m*r*r/5}" iyz="0" 
                izz="${2*m*r*r/5}" />
        </inertial>
    </xacro:macro>

圆柱惯性矩阵

<xacro:macro name="cylinder_inertial_matrix" params="m r h">
        <inertial>
            <mass value="${m}" />
            <inertia ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"
                iyy="${m*(3*r*r+h*h)/12}" iyz = "0"
                izz="${m*r*r/2}" /> 
        </inertial>
    </xacro:macro>

立方体惯性矩阵

 <xacro:macro name="Box_inertial_matrix" params="m l w h">
       <inertial>
               <mass value="${m}" />
               <inertia ixx="${m*(h*h + l*l)/12}" ixy = "0" ixz = "0"
                   iyy="${m*(w*w + l*l)/12}" iyz= "0"
                   izz="${m*(w*w + h*h)/12}" />
       </inertial>
   </xacro:macro>

需要注意的是，原则上，除了 base_footprint 外，机器人的每个刚体部分都需要设置惯性矩阵，且惯性矩阵必须经计算得出，如果随意定义刚体部分的惯性矩阵，那么可能会导致机器人在 Gazebo 中出现抖动，移动等现象。

4.3.颜色设置

在 gazebo 中显示 link 的颜色，必须要使用指定的标签:

<gazebo reference="link节点名称">
     <material>Gazebo/Blue</material>
</gazebo>

PS：material 标签中，设置的值区分大小写，颜色可以设置为 Red Blue Green Black .....