ROS与Gazebo：基于URDF实现Gazebo中的机器人仿真

前言

本文主要介绍在ROS中使用urdf实现Gazebo中仿真时需要配置的基本内容（机器人关节控制相关）。流程上包括URDF标签结构，ROS下的关节发布（robot_state_publisher），ROS与Gazebo间插件配置，launch文件配置（模型robot_description参数加载，相关ros_control控制器yaml配置与加载，一般包含使Gazebo向ROS发布关节信息的控制器类型，ROS控制Gazebo中机器人关节运动的控制器等）
另，如果由错漏的地方，希望大家及时指出，同时也多多包涵。

1. 基础预备

下面通过准备工作了解ros,ros_control与真实机器人或gazebo间的结构及数据流。

1.1 准备工作1：ros_control

https://wiki.ros.org/ros_control
大概是作为ROS与机器人交互的中介，其使用结构如图。左侧是控制器类型设置，右侧是数据流。例如对于joint_position_controller,以机器人当前关节数据及ROS目标点为输入数据，再通过PID控制器实现输出控制。

1.2 准备工作2：Gazebo与ROS间的关系

http://gazebosim.org/tutorials?tut=ros_control&cat=connect_ros
通过ros_control与Gazebo插件实现gazebo中机器人运动仿真。具体结构如下

2. 主要教程

官方教程：https://wiki.ros.org/urdf/Tutorials/Using%20a%20URDF%20in%20Gazebo
对应github: https://github.com/ros/urdf_sim_tutorial
Gazebo方面教程：http://gazebosim.org/tutorials?tut=ros_control&cat=connect_ros
urdf中的gazebo标签及其他：http://gazebosim.org/tutorials/?tut=ros_urdf
对应github: https://github.com/ros-simulation/gazebo_ros_demos

上面两个教程结合参考以后，会更加清晰。本文按官方教程顺序展开，并进行适当补充说明。
（个人认为gazebo方面的教程更有条理）

2.1 通用的启动文件格式内容

以官方教程下的gazebo.launch文件来介绍通用的启动文件格式内容。

<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="headless" value="$(arg headless)"/>
include>

<param name="robot_description" command="$(find xacro)/xacro.py $(arg model)" />


<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model"
args="-z 1.0 -unpause -urdf -model robot -param robot_description" respawn="false" output="screen" />

<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
<param name="publish_frequency" type="double" value="30.0" />
node>

它主要完成三件事：

1. Launches an empty gazebo world.
2. Loads the urdf from the macro tutorial into the parameter.
3. Runs the script to read the urdf from the parameter and spawn it in gazebo.

这里主要介绍gazebo_ros功能包与robot_state_publisher功能包

2.1.1 gazebo_ros功能包

gazebo_ros用来加载机器人模型并在GAZEBO中生成。在其参数中指定了其启动时为unpause状态，并指定读取名为robot_description的参数，指出其为 urdf类型，模型标签为robot。
关于它的一些信息参见：http://gazebosim.org/tutorials?tut=ros_roslaunch&cat=connect_ros

另外，可以用rosrun gazebo_ros spawn_model -h查看参数描述。

Commands:
-[urdf|sdf|trimesh|gazebo] - specify incoming xml is urdf, sdf or trimesh format. gazebo arg is deprecated in ROS Hydro
-[file|param|database] [||] - source of the model xml or the trimesh file
-model - name of the model to be spawned.
-reference_frame - optinal: name of the model/body where initial pose is defined.
If left empty or specified as “world”, gazebo world frame is used.
-gazebo_namespace - optional: ROS namespace of gazebo offered ROS interfaces. Defaults to /gazebo/ (e.g.
/gazebo/spawn_model).
-robot_namespace - optional: change ROS namespace of gazebo-plugins.
-unpause - optional: !!!Experimental!!! unpause physics after spawning model
-wait - optional: !!!Experimental!!! wait for model to exist
-trimesh_mass - required if -trimesh is used: linear mass
-trimesh_ixx m^2> - required if -trimesh is used: moment of inertia about x-axis
-trimesh_iyy m^2> - required if -trimesh is used: moment of inertia about y-axis
-trimesh_izz - required if -trimesh is used: moment of inertia about z-axis
-trimesh_gravity - required if -trimesh is used: gravity turned on for this trimesh model
-trimesh_material - required if -trimesh is used: E.g. Gazebo/Blue
-trimesh_name - required if -trimesh is used: name of the link containing the trimesh
-x - optional: initial pose, use 0 if left out
-y - optional: initial pose, use 0 if left out
-z - optional: initial pose, use 0 if left out
-R - optional: initial pose, use 0 if left out
-P - optional: initial pose, use 0 if left out
-Y - optional: initial pose, use 0 if left out
-J - optional: initialize the specified joint at the specified value
-package_to_model - optional: convert urdf -b - optional: bond to gazebo and delete the model when this program is interrupted

2.1.2 robot_state_publisher功能包

http://wiki.ros.org/robot_state_publisher
使用该功能包将机器人关节角度发送至tf. 并利用机器人运动学模型能发布连杆的3D姿态。
利用robot_description指定的URDF，并结合joint_states（http://wiki.ros.org/joint_state_publisher，通过读取默认服务器参数robot_descripition，找到非固定关节，并发送其关节信息）计算机器人前向运动学，并将结果利用tf进行发布。可通过remap改变读取的机器人模型参数及监听的话题http://wiki.ros.org/robot_state_publisher/Tutorials/Using%20the%20robot%20state%20publisher%20on%20your%20own%20robot。
其均发生在ROS内，与GAZEBO无关，默认发布频率50hz

但此时无法让Gazebo中机器人做任何事，因为目前还未连通Gazebo与ROS间的信息交互。

2.2 Gazebo插件

In addition to the transmission tags, a Gazebo plugin needs to be added to your URDF that actually parses the transmission tags and loads the appropriate hardware interfaces and controller manager.

为了让ROS与GAZEBO相连，需要在URDF中添加一个GAZEBO插件，在前. 需要注意的是，ROS与Gazebo之间不同类型间的信息交互，需要不同的插件。下面提到的libgazebo_ros_control.so为控制关节运动所需，具体参见http://gazebosim.org/tutorials?tut=ros_control&cat=connect_ros。

<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/robotNamespace>
plugin>
gazebo>

若是其他类型数据交互，需要选择其他插件时参见
http://gazebosim.org/tutorials?tut=ros_gzplugins&cat=connect_ros
https://github.com/ros-simulation/gazebo_ros_pkgs/tree/kinetic-devel/gazebo_plugins/include/gazebo_plugins

2.3 URDF配置

参考链接http://gazebosim.org/tutorials/?tut=ros_urdf
单纯的URDF无法在Gazebo中使用，需要对其进行配置。例如，给予关节重量与转动惯量等
连杆配置：https://wiki.ros.org/urdf/XML/link
关节配置：https://wiki.ros.org/urdf/XML/joint

While URDFs are a useful and standardized format in ROS, they are lacking many features and have not been updated to deal with the evolving needs of robotics. URDF can only specify the kinematic and dynamic properties of a single robot in isolation. URDF can not specify the pose of the robot itself within a world. It is also not a universal description format since it cannot specify joint loops (parallel linkages), and it lacks friction and other properties. Additionally, it cannot specify things that are not robots, such as lights, heightmaps, etc.

To deal with this issue, a new format called the Simulation Description Format (SDF) was created for use in Gazebo to solve the shortcomings of URDF.

2.3.1 transmission标签

为了实现机器人运动关节运动，还需要对非固定关节在urdf中添加标签（标签详细介绍https://wiki.ros.org/urdf/XML/Transmission）。相当于在运动关节上配置驱动电机。告诉他电机类型，名称，位置，参数……
http://gazebosim.org/tutorials?tut=ros_control&cat=connect_ros
https://wiki.ros.org/urdf/XML/Transmission

Transmission-specific code (not robot-specific) implementing bidirectional (actuator <-> joint) effort and flow maps under a uniform interface shared across transmission types.

2.3.2 gazebo标签

http://gazebosim.org/tutorials/?tut=ros_urdf

The element is an extension to the URDF used for specifying additional properties needed for simulation purposes in Gazebo. It allows you to specify the properties found in the SDF format that are not included in the URDF format. None of the elements within a element are required because default values will be automatically included. There are three different types of elements - one for the tag, one for tags, and one for tags. We will discuss the attributes and elements within each type of element throughout this tutorial.

1. for link
   Convert visual colors to Gazebo format
   Convert stl files to dae files for better textures
   Add sensor plugins

2. for joint
   Set proper damping dynamics
   Add actuator control plugins

3. for the robot element

三类gazebo标签根据需要添加即可。例如，不是每个joint都需要gazebo标签。
因此，2.2其实也是这里的一部分。

2.4 控制器相关

ROS与Gazebo间的数据交互方式由控制器相关yaml文件指定.不论是，由GAZEBO向ROS发送机器人状态，还是由ROS向gazebo发布机器人运动指令。
目前来看，主要是与关节运动相关的控制器需要另行配置yaml文件，之后再加载控制器。其他插件控制器不需要这一步，只要配好自己的urdf文件即可。

例如：
http://gazebosim.org/tutorials?tut=ros_gzplugins&cat=connect_ros#RunningtheRRBotExample

2.4.1 控制器加载

控制器相关yaml文件将直接加载到参数空间。例如：

<rosparam command="load"
file="$(find urdf_sim_tutorial)/config/joints.yaml"
ns="r2d2_joint_state_controller" />

<node name="r2d2_controller_spawner" pkg="controller_manager" type="spawner"
args="r2d2_joint_state_controller
--shutdown-timeout 3"/>

从教程上看，控制器最好自己添加命名空间实现更好的区分话题。
当然，添加命名空间后，注意在rontroller_manager中用args指明。

关于controller_maneger的用法，参见：https://wiki.ros.org/controller_manager

2.4.2 joint_state_controller/JointStateController

joint_state_controller/JointStateController， 用来发送运动关节的实时数据。基本任何类型机器人都需要这个控制器。

This controller is found in the joint_state_controller package and publishes the state of the robot’s joints into ROS directly from Gazebo.

#yaml文件示例
type: "joint_state_controller/JointStateController"
publish_rate: 50

在添加命名空间后，该控制器发布的joint_states话题也会变化。需要更改robot_state_publisher的接受话题。
参考：http://gazebosim.org/tutorials?tut=ros_control&cat=connect_ros#Createaroslaunchfile

2.4.3 position_controllers/JointPositionController

配置控制器为position_controllers/JointPositionController， 使用position_controllers package 下的JointPositionController 进行运动控制。需要在yaml中指出对应的joint名称，并匹配URDF标签下的控制器类型。
此时，GAZEBO将会监听相应话题，在接收后驱动对应关节。

#yaml文件示例
type: "position_controllers/JointPositionController"
joint: head_swivel

	  
   1   <transmission name="head_swivel_trans">
   2     <type>transmission_interface/SimpleTransmissiontype>
   3     <actuator name="$head_swivel_motor">
   4       <mechanicalReduction>1mechanicalReduction>
   5     actuator>
   6     <joint name="head_swivel">
   7       <hardwareInterface>PositionJointInterfacehardwareInterface>
   8     joint>
   9   transmission>

<rosparam command="load"
file="$(find urdf_sim_tutorial)/config/joints.yaml"
ns="r2d2_joint_state_controller" />
<rosparam command="load"
file="$(find urdf_sim_tutorial)/config/head.yaml"
ns="r2d2_head_controller" />

<node name="r2d2_controller_spawner" pkg="controller_manager" type="spawner"
args="r2d2_joint_state_controller
r2d2_head_controller
--shutdown-timeout 3"/>

可以对多个控制器依次分开启动，也可以如上同时启动多个控制器
或者
rrbot：https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_control/launch/rrbot_control.launch

2.4.4 position_controllers/JointGroupPositionController

另外，当需要同时控制多个关节时，需要使用group。例如，同时控制抓手上的多个关节。串联机器人一般都使用这个类型控制器。

#yaml文件示例
type: "position_controllers/JointGroupPositionController"
joints:
  - gripper_extension
  - left_gripper_joint
  - right_gripper_joint

<rosparam command="load"
file="$(find urdf_sim_tutorial)/config/joints.yaml"
ns="r2d2_joint_state_controller" />
<rosparam command="load"
file="$(find urdf_sim_tutorial)/config/head.yaml"
ns="r2d2_head_controller" />
<rosparam command="load"
file="$(find urdf_sim_tutorial)/config/gripper.yaml"
ns="r2d2_gripper_controller" />

<node name="r2d2_controller_spawner" pkg="controller_manager" type="spawner"
args="r2d2_joint_state_controller
r2d2_head_controller
r2d2_gripper_controller
--shutdown-timeout 3"/>

2.4.5 其它控制器

另外还有轮式机器人底座运动控制器，请查看教程。
更多类型ROS控制器参见https://github.com/ros-controls/ros_controllers。例如在默认UR包中，除了2.4.2, 2.4.4，还使用了joint_trajectory_controller（http://wiki.ros.org/joint_trajectory_controller）

Controller for executing joint-space trajectories on a group of joints. Trajectories are specified as a set of waypoints to be reached at specific time instants, which the controller attempts to execute as well as the mechanism allows. Waypoints consist of positions, and optionally velocities and accelerations.