ROS下如何使用moveit驱动UR5机械臂

原来的moveit实在Ubuntu12.04上安装的，ros版本是hydro，后来重装电脑，系统变成了16.04，对应的ros版本为Kinetic。重装的时候，又重新记录了一下安装步骤。

moveit安装

安装Users: Binary版本，因为比较简单
参考文档：
Binary Installation Instructions

配置：ROS：hydro，Ubuntu：12.04 32位

1、安装UR5

安装UR5必需的包:

$ sudo apt-get install \
  ros-indigo-ur-gazebo \
  ros-indigo-ur5-moveit-config \
  ros-indigo-ur-kinematics

详情请参考网页：
1） ur_gazebo

2、MoveIt!配置助手 (MoveIt! Setup Assistant)

2.1 打开MoveIt!配置助手对UR5机械臂进行配置

#打开setup_assistant
roslaunch moveit_setup_assistant setup_assistant.launch

    MoveIt! Setup Assistant 是一个图形界面，可以让我们不用写代码看代码，直接用鼠标点击就可以配置机器人的运动规划所需要的信息。点击Create New MoveIt Configuration Package来创建新的配置包，选择UR5的urdf，然后点击Load Files 载入文件。
关于UR5如何配置可以参考网页 Setup Assistant Tutorial

2.2 创建规划群 (Planning Groups)

创建Planning Group是MoveIt的核心之一。
首先，点击Add Group，界面如图所示

Group Name: 规划群名字
Kinematic Solver: 运动学求解工具，这个就是负责求解正向运动学(Forward Kinematics)和逆运动学(IK)的。 一般我们选用KDL， The Kinematics and Dynamics Library。这是一个运动学与动力学的库，可以很好的解决6自由度以上的单链机械结构的正逆运动学问题。当然你也可以用其他IK Solver, 比如SRV或者IK_FAST，甚至你可以自己开发新的Solver然后插入进来，如果有空，我以后会发帖讲解如何创建新的运动学求解库并插入到MoveIt。
Kin. Search Resolution: 关节空间的采样密度
Kin. Search TImeout: 求解时间
Kin. Solver Attempts: 求解失败尝试次数，一般来说这三项使用默认值就可以。你也可以根据具体需要做出适当调整。

3、MoveIt 配置包详解

3.1 MoveIt! 配置文件

    使用下面命令，可以进入MoveIt! 配置文件的文件夹

cd /opt/ros/hydro/share/ur5_moveit_config/config

    先看config文件夹里面包含如下文件：
fake_controllers.yaml：这是虚拟控制器配置文件，方便我们在没有实体机器人，甚至没有任何模拟器（如gazebo）开启的情况下也能运行MoveIt。
joint_limits.yaml：这里记录了机器人各个关节的位置速度加速度的极限，这些都会被用于以后的规划中。
kinematics.yaml：用于初始化运动学求解库
ur5.srdf：这个是一个重要的MoveIt配置文件，配合URDF使用。我 们可以看到这是一个xml格式的配置文件，根是robot，并有一个属性值name=’ur5’。下面各个项目应该很明显，就是我们刚刚在Setup Assistant里面所设置的东西，包含了组群，位姿，终端控制器，虚拟关节，以及碰撞免测矩阵ACM的定义。理论上，只要有了srdf和urdf，我 们就可以完全定义一个机器人moveit信息。
ompl_planning.yaml：这里是配置OMPL各种算法的各种参数。

3.2 Launch文件

    使用下面命令，可以进入MoveIt! Launch文件夹

cd /opt/ros/hydro/share/ur5_moveit_config/launch

我们来看看几个重要的文件。

3.2.1 demo.launch

    demo是运行的总结点，打开我们可以看到他include了其他的launch文件。其中第14行说，如果有需 要，发布静态的tf。比如说，你的机器人基座不在世界坐标的原点，你可以发布一个静态tf来描述机器人在世界坐标中的位置。第17-21行，就是我们发布 虚拟机器人状态的地方了，当然，如果你有实体机器人或者有gazebo之类的模拟器，你需要去掉这一部分，有其他相应的节点来发布机器人状态。26-32 行运行了另一个moveit重要的节点，move group。

3.2.2 move_group.launch

    顾名思义，move group的功能是让一个规划组群动起来。怎么动，那就要做运动规划了，在move_group.launch第24-26行定义了运动规划库的使用，我 们可以看到，默认的是使用ompl运动规划库。同样的，如果以后有时间，我会发帖详解如何创建新的运动规划库插件并让moveit使用其他的运动规划算 法。其他的都是设置一些基本参数，暂时可以略过。

3.2.3 planning_context.launch

    这里我们可以看到，定义了所使用的urdf和srdf文件，以及运动学求解库。不建议手动更改这些，但是如果你需要使用不同的urdf，srdf，可以在这里更改。

3.2.4 setup_assistant.launch

4、运行demo

    现在我们可以来尝试运行moveit了！

roslaunch  /opt/ros/hydro/share/ur5_moveit_config/launch/demo.launch

    等待几秒，当看到绿色字体 All is well! Everyone is happy! You can start planning now! 的时候，就代表启动成功了。
我们可以看到一个Rivz窗口，其中有一个运动规划Motion Planning模块。

    第一个进入视野的就是Planning Library, OMPL，这里告诉你当前用的是OMPL运动规划算法。点击unspecified按钮，在中间的下来菜单里面有很多的具体算法，之后你可以尝试不同的算法，看看他们的区别。

如果你需要更改一些配置，那么可以直接运行

5、使用Move Group Interface/C++ API规划路径

    想对UR5进行运动学规划，可以参考下网站上给出的PR2运动规划到例子，在上面作出相应到修改。

5.1、PR2的demo

    在网站（参考文件[5]）上下载pr2的相关文件(MoveIt! Packages for PR2）（hydro版本可能是自带装好的，我在运行demo时，发现是运行/opt/ros/hydro/share/pr2_moveit_tutorials/launch里的文件）。之后，在工作空间内编译。关于launch文件的含义，可以参考文件[6]。

#确保此时路径在工作空间内，如我自己在~/catkin_ws2$，编译工作空间
$catkin_make
#运行demo,我这里写的是绝对路径。
$roslaunch  /home/exbot/catkin_ws2/src/moveit_pr2-hydro-devel/pr2_moveit_tutorials/planning/launch/move_group_python_interface_tutorial.launch

    运行时，会自动调出rivz的窗口，显示模拟结果。勾选左侧loop Animation，可以循环显示动画。这个demo就是对给定的目标点和初始点进行运动规划，同时调用rviz软件进行仿真显示。

    如果roslaunch后没有结果，可以打开move_group_interface_tutorial.launch文件，看一下里面< include file=”$(find pr2_moveit_config)/launch/demo.launch”/>这句，可以直接改成编译完成后到demo.launch路径。

本文参考文档：

[1] : 运动规划 (Motion Planning): MoveIt! 与 OMPL
[2] : PR2/Gazebo/Quick Start
[3] : Moveit Tutorials for the PR2
[4] : Move Group Interface/C++ API
[5] : MoveIt! Packages for PR2
[6] : .launch文件分析
[7] : MoveIt Tutorial，新手教程Programmatic_interaction_for_planning
[8] : Move Group Interface/C++ API
[9] : ros-industrial/universal_robot代码，可以Usage with real Hardware，MoveIt! with real Hardware，Usage with Gazebo Simulation，MoveIt! with a simulated robot
[10] :Getting Started with a Universal Robot and ROS-Industrial

---

在ubuntu 16.04下利用moveit 调试ur5

环境：ROS Kinetic；ubuntu 16.04 64位版

安装moveit

ROS Kinetic
Install Ubuntu Packages
Simply run:

sudo apt-get install ros-kinetic-moveit

Setup your environment

source /opt/ros/kinetic/setup.bash

参考文件
Moveit安装官方文档

安装ur5的配置文件

从GitHub的universal_robot项目的README.md文件上，可以看到，再安装高于indigo的版本时，需要从source中编译。

Installation from Source
There are releases available for ROS Hydro and ROS Indigo. However, for the latest features and developments you might want to install from source.

First set up a catkin workspace (see this tutorials).
Then clone the repository into the src/ folder. It should look like /path/to/your/catkin_workspace/src/universal_robot.
Make sure to source the correct setup file according to your workspace hierarchy, then use catkin_make to compile.
根据文件指示，首先将在ros-industrial/universal_robot项目中找到ur5的对应版本，这里是选择kinetic-devel

将文件保存在～/catkin_ws/src里，之后

$ cd ~/catkin_ws
$ catkin_make

注：每次打开新终端，调试ur5时，先输入下面语句（Don’t forget to source the correct setup shell files and use a new terminal for each command!）：

$ . ~/catkin_ws/devel/setup.bash

UR5的配置文件路径为：~/catkin_ws/src/universal_robot-kinetic-devel/ur_description/urdf/
可以选择其中的ur5_joint_limited_robot.urdf.xacro file，或者 ur5_robot.urdf.xacro 。

For the UR5, load either the ur5_robot.urdf.xacro or the ur5_joint_limited_robot.urdf.xacro file. The former sets the joint limits to their actual values (+-2pi for all joints), while the latter restricts that to half of that (+-pi). MoveIt (and/or some of its planners) don’t work too well with full 2pi joint limits, hence the joint limited variant of that file.
Import UR5 urdf to Moveit!原文链接

代码文档：
ros-industrial/universal_robot

调试

调试前出现的一个小问题

在我调试的时候，出现了导入ur5_joint_limited_robot.urdf.xacro时出现错误的问题，后来我重新执行了如下语句，导入成功。

$ cd ~/catkin_ws
$ catkin_make

模拟

输入下面命令，可以在 rviz 中模拟ur5下面三个命令，需要在三个不同终端中调用，同时在调用之前，需要分别重新输入

$ . ~/catkin_ws/devel/setup.bash

roslaunch ur_gazebo ur5.launch limited:=true

roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch sim:=true limited:=true

roslaunch ur5_moveit_config moveit_rviz.launch config:=true

在rivz中，规划库中有多种算法，某一个规划算法可能会遇到failed，多试几个，总有一个成功。

实体联调

使用ur_driver的问题

官方文档ros-industrial/universal_robot提供了一条测试文档

roslaunch ur_bringup ur5_bringup.launch robot_ip:=IP_OF_THE_ROBOT [reverse_port:=REVERSE_PORT]
rosrun ur_driver test_move.py

我在运行第二句rosrun ur_driver test_move.py时，发现程序卡在“Waiting for server…”这句。后来上网差原因，看资料显示，可能是版本兼容的问题。有人提出用
ur_modern_driver代替universal_robot。但是我从官网ur_modern_driver下载，按官网步骤，拷入~/catkin_ws/src文件夹下，执行catkin_make命令时，报错。后来又去网上查原因，发现有人说是ur_modern_driver不支持ROS kinetic版本（作者说近期会对其进行改进），但是经过一些修改后可以使用。此处WangJIankun1992 说他将ur_modern_driver里的 ur_hardware_interface.cpp,文件中的 controller_it->hardware_interface改成 controller_it->type. 这样可以保证运行成功。按此方法对文件进行修改后，执行catkin_make可以编译成功。

实际与ur5控制时，无法链接查了github上的注释，说是目前对3.3.2.266还不支持（Support for UR3 version 3.3.2.266? #75）。很可惜我这台ur5前段时间刚升到3.3。问题待解决。

注

1. Gazebo 与Rviz 的区别
   Gazebo is the actual real world physics simulator that you will set up a world and simulate your robot moving around. Rviz is the visualization software, that will allow you to view that gazebo data (if you are simulating) or real world data (if you are not using gazebo, but a real robot).原文链接

2. 列表内容

参考文档：

1. 运动规划 (Motion Planning): MoveIt! 与 OMPL
2. ROS下利用 moveit 控制gazebo模型并在rviz中显示的探索总结
3. ur_description
4. MoveIt!教程
5. ur_driver（wiki.ros.org/ur_driver）