UR5 Gazebo仿真配置(不使用MoveIt)

1. Introduction

1.1 动机

最近在看Bruno Siciliano的Robotics Modelling, Planning and Control, 想着把里面运动学相关的算法自己实现一下. 首先有个仿真环境, 因为之前用过ROS, 于是就用ROS-Gazebo来仿真UR5机械臂(因为实验室里用的也是UR5).
网上搜到的机械臂Gazebo仿真一般是使用MoveIt控制的. MoveIt是ROS的第三方工具包, 功能强大, 可以实现运动控制, 运动规划, 避障等功能. 但是MoveIt封装得比较抽象, 用的时候看不太出底层的算法来. 我想的是机械臂控制的接口就只是一个$1\times 6$ 的关节角向量, 上层的正逆运动学, 运动规划等算法自己实现. 于是打算自己实现一个ROS-Gazebo UR5的仿真包.

1.2 概述

先说一下实现这个包大概需要什么:

• UR5 机械臂的urdf文件 ROS包ur5_description. 这个包可以直接拿UR-ROS官方的包来用, 主要是用来生成UR5模型.
• Gazebo相关的ROS包 ur5_gazebo 主要是创建Gazebo世界, 生成机械臂模型, 以及控制的接口.

因为刚开始接触ROS, 有很多东西稀里糊涂的, 看各种教程也没有提到, 后面用了一段时间才明白, 因此本文会分享一些我的见解(个人理解, 如果有错误希望大家指正), 比较面向初学者(比如我). 熟悉的同学可能会觉得讲得啰嗦, 可以按照标题自行选择想要阅读的部分.

1.3 前置技能

建议先看一遍 Gazebo x ROS的教程, 其实本文就是从这些教程讲的知识总结而来的.

• Tutorial: Using roslaunch to start Gazebo, world files and URDF models
• Tutorial: Using a URDF in Gazebo
• Tutorial: ROS Control

建议大家还是多看ROS wiki的教程, 都学了一遍之后, 有具体问题可以再搜索看这些帖子.

2. 机器人模型描述

2.1 URDF

ROS中一般是使用URDF (Unified Robot Description Format) 来描述机器人模型的， URDF本质上是一种xml格式的文件, 长下面这样. ROS当中很多文件, 如xacro, yaml其实都是基于xml格式的.

<robot name="robot">
<link>	... </link>
<link>	... </link>
<joint>	... </joint>
</robot>
<!-- 注释 -->

比如上面的代码里, <robot>标签内的就是机器人本体的定义, 包括了连杆<link> 和 关节<joint>.
具体的语法和定义, 大家可以看XML specifications. 这里不详细说了, 后面会用到<joint>的定义.

2.2 xacro

xacro (xml macro) 其实还是xml, 但是添加了宏定义. 比如机器人urdf文件里的<link> <joint>写起来都是差不多的, 可以首先设一个<link> <joint>的宏定义, 然后后面写起来调用这个宏定义就省劲很多. xacro主要是出于代码复用(偷懒)的目的整出来的. 具体介绍大家看官方的xacro ROS wiki. 下面这段话是他们对xacro的介绍.

We also developed a macro language called xacro to make it easier to maintain the robot description files, increase their readability, and to avoid duplication in the robot description files.

xacro代码长这样, 可以定义常量等.

<!-- xacro 声明 -->
<?xml version="1.0"?>
<robot name="mrobot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- xacro 定义常量 -->
<xacro::property name="PI" value="3.14159"/>
<!-- xacro 调用常量、数学公式 -->
<origin xyz="0 0 0" rpy="${PI/2} 0 0"/>

从用户的角度来看, xacro写起来比urdf方便很多. 其实ROS还是使用urdf文件构建机器人的, 使用的时候调用xacro程序将xacro解析为urdf文件, 并上传到ROS的参数服务器.

<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find mrobot_description)/urdf/mrobot.urdf.xacro'"/>

2.3 UR 模型

UR (Universal Robot) 的ROS package可以直接去GitHub上的Universal Robot repo下载.
对于Ubuntu 16及以前的同学, 可以直接apt install. 但是对于Ubuntu 18及以上就要从源码编译了. 具体过程参见Universal Robot .

~/catkin_ws/src/universal_robot$ tree -L 1
.
├── README.md
├── universal_robot
├── universal_robots
├── ur5_e_moveit_config
├── ur5_moveit_config
├── ur_bringup
├── ur_description
├── ur_driver
├── ur_e_description
├── ur_e_gazebo
├── ur_gazebo
├── ur_kinematics
└── ur_msgs

安装完之后目录大概是这样的(我删掉了一些不需要的包, 比如UR3跟UR10的). ur_description就是UR机械臂的描述文件.

~/catkin_ws/src/universal_robot/ur_description$ tree -L 2
.
├── cfg
│   └── view_robot.rviz
├── CHANGELOG.rst
├── CMakeLists.txt
├── launch
│   ├── ur10_upload.launch
│   ├── ur3_upload.launch
│   ├── ur5_upload.launch
│   ├── view_ur10.launch
│   ├── view_ur3.launch
│   └── view_ur5.launch
├── meshes	# 机器人三维模型
│   ├── ur10
│   ├── ur3
│   └── ur5
├── model.pdf
├── package.xml
└── urdf	# 机器人urdf文件
    ├── common.gazebo.xacro
    ├── ur10_joint_limited_robot.urdf.xacro
    ├── ur10_robot.urdf.xacro
    ├── ur10.urdf.xacro
    ├── ur3_joint_limited_robot.urdf.xacro
    ├── ur3_robot.urdf.xacro
    ├── ur3.urdf.xacro
    ├── ur5_joint_limited_robot.urdf.xacro
    ├── ur5_robot.urdf.xacro
    ├── ur5.urdf.xacro
    ├── ur.gazebo.xacro
    └── ur.transmission.xacro

可以打开ur5.urdf.xacro看一眼ur机械臂的link和joint是如何定义的.

下面我们从官方包里提取出UR5的描述文件, 自己创建一个ur5_description包.
其实这一步没什么必要, 也可以直接用官方的包.

我主要是不喜欢有多余的东西, 而且后面会对urdf文件有改动, 所以自己新建了一个精简版的ROS package.

2.4 创建精简的ur5_description

首先创建一个ROS package

~/catkin_ws/src/ur_sim$ catkin_create_pkg ur5_description

然后复制UR5的urdf文件夹到自己新建的packageur5_description下, 可以删掉UR3跟UR10相关的文件. meshes也复制过来, 删掉ur3 ur10. 自己创建的package目录应该是这样的:

~/catkin_ws/src/ur_sim/ur5_description$ tree -L 1
.
├── CMakeLists.txt
├── launch
├── meshes
	└── ur5
        ├── collision
        └── visual
├── package.xml
└── urdf
	├── common.gazebo.xacro
    ├── ur5_joint_limited_robot.urdf.xacro
    ├── ur5_robot.urdf.xacro
    ├── ur5.urdf.xacro
    ├── ur.gazebo.xacro
    └── ur.transmission.xacro

记得修改ur5_robot.urdf.xacro 和 ur5.urdf.xacro 中的package名字.

<!-- ur5_robot.urdf.xacro -->
<!-- common stuff -->
<!-- 记得修改package名 -->
  <xacro:include filename="$(find ur5_description)/urdf/common.gazebo.xacro" />
  <!-- ur5 -->
  <xacro:include filename="$(find ur5_description)/urdf/ur5.urdf.xacro" />

<!-- ur5.urdf.xacro -->
<link name="${prefix}base_link" >
      <visual>
        <geometry>
<!-- 记得修改package名 -->
          <mesh filename="package://ur5_description/meshes/ur5/visual/base.dae" />
        </geometry>
        <material name="LightGrey">
          <color rgba="0.7 0.7 0.7 1.0"/>
        </material>
      </visual>
      <collision>
        <geometry>
          <mesh filename="package://ur5_description/meshes/ur5/collision/base.stl" />
        </geometry>
      </collision>
      <xacro:cylinder_inertial radius="0.06" length="0.05" mass="${base_mass}">
        <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
      </xacro:cylinder_inertial>
    </link>

下面我们将刚才创建的ur5_description加载Gazebo仿真环境中.

3. Gazebo 加载机器人模型

3.1 创建ur5_gazebo ROS package

创建ur5_gazebo ROS package.

~/catkin_ws/src/ur_sim$ catkin_creake_pkg ur5_gazebo

并创建一些文件夹, 创建之后这个包的目录是这样的:

~/catkin_ws/src/ur_sim/ur5_gazebo$ tree -L 2
.
├── CMakeLists.txt
├── controller
│   └── ur_gazebo_controller.yaml
├── launch
│   ├── ur_controller.launch	# "controller_spawner" load controllers
│   ├── ur_gazebo_control.launch	# include other two launch files
│   └── ur_gazebo.launch	# load gazebo world and spawn robot model
├── package.xml
└── src

3.2 launch file加载仿真环境

我们这里写上面的ur_gazebo.launch, 负责加载Gazebo world和机器人模型. Gazebo中加载xx一般叫spawn xx, 比如加载机器人模型会启动一个叫spawn_model的节点, 后面加载控制器会启动一个叫controller_spawner的节点. 所以大家看到spawn这个词就知道是要加载什么东西.

<!-- ur_gazebo.launch 加载Gazebo 世界和机器人模型 -->
<?xml version="1.0"?>	
<launch>
    <!-- Gazebo world -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="world_name" default="worlds/empty.world"/>
    </include>
    <!-- parse xacro file into urdf and upload urdf to parameter server -->
    <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find ur5_description)/urdf/ur5_robot.urdf.xacro'" />
    <!-- spawn robot in gazebo -->
    <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot" respawn="false" output="screen" />
</launch>

我一开始遇到的一个问题是, launch file看是能看明白, 但是不知道怎么写. 我这个其实是从别的包拿过来删删改改写成的. 如果对于launch file不明白的话, 可以看官方的ROS launch XML format 的 Tag reference.
有问题多看看ROS wiki. 其他网上的教程都是零零碎碎的, 很多疑问看一遍ROS wiki都能得到解答.

之后可以启动gazebo看看机器人模型

$ roslaunch ur5_gazebo ur_gazebo.launch

发现UR是躺在地上的, 末端还与地面形成了干涉, 这也是为什么官方的`ur5.launch`中`spawn_model`传入的参数要有`-z 0.1`, 因为要将UR基座抬高0.1m, 否则会于地面形成干涉.

<!-- UR原本包的ur5_gazebo.launch中的一行 -->
<node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -z 0.1" respawn="false" output="screen" />

一般UR的初始位姿是竖立的, 我们修改一下UR的描述文件, 将UR的初始位姿改为竖直的.

3.3 修改机器人初始位姿

看了看UR的模型, 我们要做的就是将第二个关节shoulder_lift_joint的初始关节角旋转$-\pi /2$. 我们打开UR的描述文件ur5.urdf.xacro, 找到第二个关节. 这里要参考 ROS wiki 的说明, 修改二轴的欧拉角.

(optional: defaults to identity if not specified)

This is the transform from the parent link to the child link. The joint is located at the origin of the child link, as shown in the figure above.

xyz (optional: defaults to zero vector): Represents the offset. All positions are specified in meters.
rpy (optional: defaults 'to zero vector 'if not specified): Represents the rotation around fixed axis: first roll around x, then pitch around y and finally yaw around z. All angles are specified in radians.

<joint name="${prefix}shoulder_lift_joint" type="revolute">
      <parent link="${prefix}shoulder_link" />
      <child link = "${prefix}upper_arm_link" />
      <origin xyz="0.0 ${shoulder_offset} 0.0" rpy="0.0 0.0 0.0" />

<origin>这一行原本是rpy="0 ${pi/2.0} 0", 原来二轴一开始yaw有90°的旋转, 我们将其改为三个0.

之后再启动便可以发现UR初始位姿是竖直的了.但是运行时机械臂会抖动, 目前还不知道为什么.

4. ROS Control - Gazebo中让机械臂动起来

上图介绍了gazebo_ros_control是如何模拟真实硬件接口进行仿真的, 这也是我们让机械臂动起来要用到的. 这是ROS Control的教程ROS Control tutorial.

4.1和4.2 只是跟大家说明一下, 并不需要操作什么.

4.1 transmission tag

下面我们将使用ROS control package设置模拟控制器来驱动Gazebo中机器人的关节, 让机械臂动起来. 要想用ROS control控制机器人, urdf文件中的关节必须有<transmission>标签.

我们看ur5.urdf.xacro中有这么一行

  <xacro:include filename="$(find ur_description)/urdf/ur.transmission.xacro" />

这就是把ur.transmission.xacro这个文件给include进来了, 这个文件就包含了每个关节的<transmission>信息, 比如下面这是一轴的信息.

<transmission name="${prefix}shoulder_pan_trans">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="${prefix}shoulder_pan_joint">
        <hardwareInterface>${hw_interface}</hardwareInterface>
      </joint>
      <actuator name="${prefix}shoulder_pan_motor">
        <mechanicalReduction>1</mechanicalReduction>
      </actuator>
    </transmission>

4.2 gazebo_ros_control plugin

在URDF文件中, 除了标签之外, 还需要添加一个gazebo_ros_control插件来解析标签, 并加载合适的接口和控制器.

在ur5_robot.urdf.xacro中include了common.gazebo.xacro

  <xacro:include filename="$(find ur5_description)/urdf/common.gazebo.xacro" />

common.gazebo.xacro中又加载了ros_control的插件.

 <plugin name="ros_control" filename="libgazebo_ros_control.so">
      <!--robotNamespace>/</robotNamespace-->
      <!--robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType-->
    </plugin>

4.3 ROS control package

下面我们写虚拟的控制器文件ur_gazebo_controller.yaml

# Publish all joint states -----------------------------------
joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 50  
  
# Position Controllers ---------------------------------------
joint1_position_controller:
  type: position_controllers/JointPositionController
  joint: shoulder_pan_joint
  pid: {p: 100.0, i: 0.01, d: 10.0}
joint2_position_controller:
  type: position_controllers/JointPositionController
  joint: shoulder_lift_joint
  pid: {p: 100.0, i: 0.01, d: 10.0}
joint3_position_controller:
  type: position_controllers/JointPositionController
  joint: elbow_joint
  pid: {p: 100.0, i: 0.01, d: 10.0}
joint4_position_controller:
  type: position_controllers/JointPositionController
  joint: wrist_1_joint
  pid: {p: 100.0, i: 0.01, d: 10.0}
joint5_position_controller:
  type: position_controllers/JointPositionController
  joint: wrist_2_joint
  pid: {p: 100.0, i: 0.01, d: 10.0}
joint6_position_controller:
  type: position_controllers/JointPositionController
  joint: wrist_3_joint
  pid: {p: 100.0, i: 0.01, d: 10.0}

你要是问为什么这么写, 我只能说看ROS control教程.

然后写一个launch file ur_controller.launch启动控制器.

<?xml version="1.0"?>
<launch>
    
    <rosparam file="$(find ur5_gazebo)/controller/ur_gazebo_controller.yaml" command="load"/>

    <node name="controller_spawner" pkg="controller_manager" type="spawner"  respawn="false" output="screen" ns="/"
    args="joint_state_controller
          joint1_position_controller
          joint2_position_controller
          joint3_position_controller
          joint4_position_controller
          joint5_position_controller
          joint6_position_controller"/>

    <node name="ur_state_publisher"  pkg="robot_state_publisher"  type="robot_state_publisher" respawn="false" output="screen">
      <!-- <remap from="/joint_states" to="/arm/joint_states"/> -->
    </node>
</launch>

先启动ur_gazebo.launch在Gazebo中加载UR5模型后, 再启动这个控制器的launch file, 就可以对每个关节角进行控制了.

[INFO] [1619959151.260079, 0.000000]: Loading controller: joint_state_controller
[INFO] [1619959195.207835, 5.160000]: Loading controller: joint1_position_controller
[INFO] [1619959195.245979, 5.196000]: Loading controller: joint2_position_controller
[INFO] [1619959195.270927, 5.218000]: Loading controller: joint3_position_controller
[INFO] [1619959195.298858, 5.240000]: Loading controller: joint4_position_controller
[INFO] [1619959195.332093, 5.268000]: Loading controller: joint5_position_controller
[INFO] [1619959195.348003, 5.283000]: Loading controller: joint6_position_controller
[INFO] [1619959195.368850, 5.300000]: Controller Spawner: Loaded controllers: joint_state_controller, joint1_position_controller, joint2_position_controller, joint3_position_controller, joint4_position_controller, joint5_position_controller, joint6_position_controller
# 能够看到控制器加载成功
[INFO] [1619959195.387982, 5.316000]: Started controllers: joint_state_controller, joint1_position_controller, joint2_position_controller, joint3_position_controller, joint4_position_controller, joint5_position_controller, joint6_position_controller

这时看一下有什么topic, 可以看到有1到6轴的/joint1_position_controller/command, 直接向这些topic发送关节角就可以控制机械臂运动.

~/catkin_ws$ rostopic list 
/clock
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/set_link_state
/gazebo/set_model_state
/joint1_position_controller/command
/joint2_position_controller/command
/joint3_position_controller/command
/joint4_position_controller/command
/joint5_position_controller/command
/joint6_position_controller/command
/joint_states
/rosout
/rosout_agg
/tf
/tf_static

更省劲一点可以再写一个launch file把上面两个launch include进来.

<?xml version="1.0"?>
<launch>
    <include file="$(find ur5_gazebo)/launch/ur_gazebo.launch"/>
    <include file="$(find ur5_gazebo)/launch/ur_controller.launch"/>
</launch>

---

Reference

胡春旭. 《ROS机器人开发实践》

Tutorial: Using roslaunch to start Gazebo, world files and URDF models

Tutorial: Using a URDF in Gazebo

Tutorial: ROS Control