利用moveit在ROS RViz下仿真控制UR机械臂

利用moveit在ROS RViz下仿真控制UR机械臂

       上一篇博客介绍了UR5机械臂仿真环境的搭建，接下来讲介绍一下如何利用ROS官方提供的moveit包在RViz控制UR5运动，以及RViz界面的几个主要功能介绍。

       首先，让我们先了解一下UR5的RViz控制界面：

       界面主要分为三部分，左上角Displays窗口主要通过修改一些参数来更改UR5控制的可视化效果；左下角Motion Planning窗口即UR5的运动规划模块；右侧三维可视化窗口用于显示UR5当前状态、目标位姿以及规划的运动轨迹。下面将对前两部分进行介绍。

1. Displays

       Display->MotionPlanning->Planned Path里面设置各种显示参数。

       上图即显示了关节扭矩，循环运动轨迹等信息。

2. Motion Planning

2.1 Context

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

       而OMPL (Open Motion Planning Library)， 开源运动规划库，就是一个运动规划的C++库，其包含了很多运动规划领域的前沿算法。虽然OMPL里面提到了最优规划，但总体来说OMPL还是一个采样规划算法库，而采样规划 (Sampling-based Planning)的主要目的在于快速的找到一个有效的解而不一定找到最优解，因此使用OMPL里的某种算法，可能前后两次生成不同的路径。

       关于OMPL，有以下两点需要介绍一下：

1. OMPL能做什么？ 简单说，就是提供一个运动轨迹。给定一个机器人结构(假设有N个关节)，给定一个目标(比如终端移到xyz)，给定一个环境，那么OMPL会提供给你一个轨迹，包含M个数组，每一个数组长度是N，也就是一个完整的关节位置。沿着这个轨迹依次移动关节，就可以最终把终端移到xyz，当然，这个轨迹应当不与环境中的任何障碍发生碰撞。
2. 为什么用OMPL？ 运动规划的软件库和算法有很多，而OMPL由于其模块化的设计和稳定的更新，成为最流行的规划软件库之一。很多新算法都在OMPL开发。很多其他软件（包括ROS/MoveIt）都使用OMPL做运动规划。

2.2 Planning

       在rviz中，我们可以用鼠标拖动机器人的终端，然后点击planning标签页中的plan and execute就可以让机器人规划路径并且到达目标位置了，gazebo中的模型也会跟随变化。

2.3 Scene Objects

       planning scene可以为机器人创建一个具体的工作环境，加入一些障碍物。这一功能主要由move group节点中的planning scene monitor来实现，主要监听：

• State Information： joint_states topic
• Sensor Information： the world geometry monitor，视觉传感器采集周围环境数据

       上面这个简单的运动规划，环境中并没有障碍物。下面我们来为环境中增加几个物体。创建一个.scene文件（你可以自己设计不同的scene）。

(noname)+++++
* obs1.dae
1
mesh
24 12
-0.0315993 0.126397 -0.0315993
-0.0315993 -0.126397 -0.0315993
-0.0315993 0.126397 0.0315993
-0.0315993 -0.126397 0.0315993
0.0315993 0.126397 0.0315993
0.0315993 -0.126397 -0.0315993
0.0315993 0.126397 -0.0315993
0.0315993 -0.126397 0.0315993
-0.0315993 -0.126397 0.0315993
0.0315993 -0.126397 0.0315993
0.0315993 0.126397 0.0315993
-0.0315993 0.126397 0.0315993
0.0315993 0.126397 -0.0315993
0.0315993 -0.126397 -0.0315993
-0.0315993 -0.126397 -0.0315993
-0.0315993 0.126397 -0.0315993
-0.0315993 -0.126397 0.0315993
-0.0315993 -0.126397 -0.0315993
0.0315993 -0.126397 -0.0315993
0.0315993 -0.126397 0.0315993
0.0315993 0.126397 -0.0315993
-0.0315993 0.126397 -0.0315993
-0.0315993 0.126397 0.0315993
0.0315993 0.126397 0.0315993
0 1 2
1 3 2
4 5 6
4 7 5
8 9 10
11 8 10
12 13 14
12 14 15
16 17 18
19 16 18
20 21 22
20 22 23
0.7 0 0.8
0.706825 0 0 0.707388
0.5 0 0 1
* obs2.dae
1
mesh
24 12
-0.0315993 0.126397 -0.0315993
-0.0315993 -0.126397 -0.0315993
-0.0315993 0.126397 0.0315993
-0.0315993 -0.126397 0.0315993
0.0315993 0.126397 0.0315993
0.0315993 -0.126397 -0.0315993
0.0315993 0.126397 -0.0315993
0.0315993 -0.126397 0.0315993
-0.0315993 -0.126397 0.0315993
0.0315993 -0.126397 0.0315993
0.0315993 0.126397 0.0315993
-0.0315993 0.126397 0.0315993
0.0315993 0.126397 -0.0315993
0.0315993 -0.126397 -0.0315993
-0.0315993 -0.126397 -0.0315993
-0.0315993 0.126397 -0.0315993
-0.0315993 -0.126397 0.0315993
-0.0315993 -0.126397 -0.0315993
0.0315993 -0.126397 -0.0315993
0.0315993 -0.126397 0.0315993
0.0315993 0.126397 -0.0315993
-0.0315993 0.126397 -0.0315993
-0.0315993 0.126397 0.0315993
0.0315993 0.126397 0.0315993
0 1 2
1 3 2
4 5 6
4 7 5
8 9 10
11 8 10
12 13 14
12 14 15
16 17 18
19 16 18
20 21 22
20 22 23
0.6 0.5 0.8
0.706825 0 0 0.707388
0.5 0 0 1
* obs3.dae
1
mesh
24 12
-0.0315993 0.126397 -0.0315993
-0.0315993 -0.126397 -0.0315993
-0.0315993 0.126397 0.0315993
-0.0315993 -0.126397 0.0315993
0.0315993 0.126397 0.0315993
0.0315993 -0.126397 -0.0315993
0.0315993 0.126397 -0.0315993
0.0315993 -0.126397 0.0315993
-0.0315993 -0.126397 0.0315993
0.0315993 -0.126397 0.0315993
0.0315993 0.126397 0.0315993
-0.0315993 0.126397 0.0315993
0.0315993 0.126397 -0.0315993
0.0315993 -0.126397 -0.0315993
-0.0315993 -0.126397 -0.0315993
-0.0315993 0.126397 -0.0315993
-0.0315993 -0.126397 0.0315993
-0.0315993 -0.126397 -0.0315993
0.0315993 -0.126397 -0.0315993
0.0315993 -0.126397 0.0315993
0.0315993 0.126397 -0.0315993
-0.0315993 0.126397 -0.0315993
-0.0315993 0.126397 0.0315993
0.0315993 0.126397 0.0315993
0 1 2
1 3 2
4 5 6
4 7 5
8 9 10
11 8 10
12 13 14
12 14 15
16 17 18
19 16 18
20 21 22
20 22 23
0.3 0 0.8
0.999784 0 0 0.0207948
0.5 0 0 1
* table.dae
1
mesh
24 12
-0.420619 0.0420619 -0.841237
-0.420619 -0.0420619 -0.841237
-0.420619 0.0420619 0.841237
-0.420619 -0.0420619 0.841237
0.420619 0.0420619 0.841237
0.420619 -0.0420619 -0.841237
0.420619 0.0420619 -0.841237
0.420619 -0.0420619 0.841237
-0.420619 0.0420619 0.841237
-0.420619 -0.0420619 0.841237
0.420619 -0.0420619 0.841237
0.420619 0.0420619 0.841237
0.420619 -0.0420619 -0.841237
-0.420619 -0.0420619 -0.841237
-0.420619 0.0420619 -0.841237
0.420619 0.0420619 -0.841237
-0.420619 -0.0420619 0.841237
-0.420619 -0.0420619 -0.841237
0.420619 -0.0420619 -0.841237
0.420619 -0.0420619 0.841237
0.420619 0.0420619 -0.841237
-0.420619 0.0420619 -0.841237
-0.420619 0.0420619 0.841237
0.420619 0.0420619 0.841237
0 1 2
1 3 2
4 5 6
4 7 5
8 9 10
8 10 11
12 13 14
15 12 14
16 17 18
19 16 18
20 21 22
20 22 23
0.7 0 0.63
0.706825 0 0 0.707388
0 0.5 0.5 1
* target
1
mesh
24 12
-0.0315993 0.126397 -0.0315993
-0.0315993 -0.126397 -0.0315993
-0.0315993 0.126397 0.0315993
-0.0315993 -0.126397 0.0315993
0.0315993 0.126397 0.0315993
0.0315993 -0.126397 -0.0315993
0.0315993 0.126397 -0.0315993
0.0315993 -0.126397 0.0315993
-0.0315993 -0.126397 0.0315993
0.0315993 -0.126397 0.0315993
0.0315993 0.126397 0.0315993
-0.0315993 0.126397 0.0315993
0.0315993 0.126397 -0.0315993
0.0315993 -0.126397 -0.0315993
-0.0315993 -0.126397 -0.0315993
-0.0315993 0.126397 -0.0315993
-0.0315993 -0.126397 0.0315993
-0.0315993 -0.126397 -0.0315993
0.0315993 -0.126397 -0.0315993
0.0315993 -0.126397 0.0315993
0.0315993 0.126397 -0.0315993
-0.0315993 0.126397 -0.0315993
-0.0315993 0.126397 0.0315993
0.0315993 0.126397 0.0315993
0 1 2
1 3 2
4 5 6
4 7 5
8 9 10
11 8 10
12 13 14
12 14 15
16 17 18
19 16 18
20 21 22
20 22 23
0.6 0.2 0.8
0.706825 0 0 0.707388
0 1 0 1
.

       然后在SceneObjects模块中点击ImportFromText，选择刚刚创建的demo_obstacle.scene文件，一个简单的桌面环境就被导入进了rviz。你可以通过选择各个物体，来调整他们的位置。

       回到Context子模块，点击Publish Current Scene，将当前的环境发布出去。然后再次点击Plan，你会看到一条不同的轨迹，这一轨迹应该绕过所有障碍物并且达到目标位姿。

       因为OMPL是采样算法，由于其随机采样的特性，每次的路径是不同的，如下图。而且有可能失败。

3. 参考文献：

1. 运动规划 (Motion Planning): MoveIt! 与 OMPL
   
2. ROS下如何使用moveit驱动UR5机械臂