KUKA youbot机械臂与Moveit工具包（3）

1. 背景

这段时间以来，先是按照by example 的作者的例子，将它看懂，改动，以运用到youbot上来。其中修改完毕并测试成功的有：moveit_fk_demo.py(关节空间)， moveit_ik_demo.py(笛卡尔空间)，arm_tracker.py（追踪空中的球）

视频地址

http://v.youku.com/v_show/id_XMTQ0NDUwOTQ4NA==.html?from=y1.7-1.2

以及moveit_cartesian_demo.py（加入途经点）。但是后面的那个moveit_constraints_demo.py（path_constraints）修改后，提示

也就是invalid start state, 造成Motion Planning 失败。这个问题困扰了半个月，期间查ros answer 和 moveit mailing list都没有找到较好的解决方案。然后，一边郁闷，一边尝试利用moveit 的c++接口实现之前的动作，后来发现有些功能python中没有涉及到，于是有了这篇博客。

2. 你需要知道的

我们秉承着“最好的学习资料来自官网“，在http://moveit.ros.org/documentation/tutorials/ 中有关于tutorial的部分。然后就转到了http://docs.ros.org/hydro/api/pr2_moveit_tutorials/html/index.html 这里。下面就具体说明：

上面提到的tutorial程序的完整版在https://github.com/ros-planning/moveit_pr2/tree/hydro-devel/pr2_moveit_tutorials/planning/src

它这里起名比较乱，我最开始也绕晕了，在这里整理出来，也方便大家查阅。

The Move Group Interface tutorial  ====对应于github的======>>  move_group_interface_tutorial.cpp 转到Github（C++中可以调用）

The Move Group Python Interface tutorial ====对应于github的======>>  moveit_pr2/pr2_moveit_tutorials/planning/scripts/move_group_python_interface_tutorial.py转到Github
（Python中可以调用）

上面这两个是官方建议我们用的。也是moveit中用的最多的一些功能。

下面的这几个是进阶用法，主要是C++中调用，Python版本的没有，也就是说进阶功能你必须用C++来写，这也就是我之所以要转到C++这里来的原因，另外用C++较多，也熟悉一点，不必再重头来学Python。

The Kinematics tutorial ==对应于github的==>>  moveit_pr2/pr2_moveit_tutorials/kinematics/src/kinematic_model_tutorial.cpp 转到Github（注意前面tutorial那里还有关于state_display的教程）

The Planning Scene tutorial ====对应于github的===>>  planning_scene_tutorial.cpp（selfcollision and constraint check ）

The Planning Scene ROS API tutorial  ==对应于github的3个地方==>>move_group_interface_tutorial.cpp、move_group_tutorial.cpp和planning_scene_ros_api_tutorials.cpp(主要功能就是添加/移除物品到规划场景中)

The Motion Planning tutorial ===对应于github的==>> planning_scene_ros_api_tutorial.cpp 转到Github （动态加载motion planner）

The Motion Planning Pipeline tutorial ==对应于github的===>> planning_scene_ros_api_tutorial.cpp转到Github （规划的预处理和后处理）

3. 运行与分析

1. 关于Move Group interface

左上角是启动FollowJointserver，右上角是启动Move_group.launch。左下角rviz图形界面，右下角是测试程序（后面的结果页面也都是如此，我就不再具体指明了）

由于自己的那个constraint原因，造成右下角窗口的Plan fail。后来求助ROS QQ群的群友，有说可能是碰撞问题，因此我就找寻Moveit中关于碰撞检测的部分。

2. 关于collision check和constrait check

这个按照官网例程是属于The Planning Scene tutorial部分。

在这里我给定了一个必然碰撞的位置，Moveit能够较好的指出contact的关节。即下图中的test4.

测试结果如下图所示：

这里得说明一个地方，就是这里的Move_group那个窗口，十分明显的一点，它没有数据滚动，是不是就是说它没有起作用呢？也就是没有数据交给它处理，我直接关闭它，然后再启动测试程序，结果测试就定在那个地方不动了。说明猜测不成立。另外关于constraints，也分为了好几种，如下图：在req的3个constraints中，它的每一个都可以分别添加前面那四个。比如goal_constraints可以加入PositonConstraint和OrientionConstraint，这个depends on you

这里再说一个小技巧，终端运行rosrun rqt_logger_level rqt_logger_level

将模式选为Debug,输出就会包含更多有用的信息。我原本以为这样就可以解决问题了，但是还是不行。碰撞那里一切都ok，test4是我强制设置的一个碰撞位置，有效果，但是之前的都没有碰撞。

4. 关于Kinematics

这部分比较有用的就是根据robot_model生成的一个robot_state::RobotStatePtr 类型的Kinematic_state, 后面的操作都与它相关，包括enforce joint limit， IK，GetJacobian等。

5. Motion Planning 

官网解释是动态加载motion planner，但是我们的Planer只有个OMPL，所以感觉意义不大。但是做下来，改动完，才发现有些细节的地方，比如，工作空间要提前设置大小，不然会报错。官网这里没有提及，它放在了程序后面。测试后发现要改到前面去。另外执行的时候，它有个命令行参数，rosrun moveit_obstacles_cpp moveit_obstacles_cpp_node _planner_plugin_name:=ompl_interface/OMPLPlanner这是我这边的执行语句，大家根据自己的情况改成对应的package名字和node名字。

这个错误还是由于path constraint造成。

6. Planning PipeLine

对motion planning进行前处理和后处理。

写在最后

以上基于官方修改的代码我放在了https://github.com/StanleyYake/Kuka_youbot/tree/master/change_tutorial

我是按照a、b、c、d、e来命名的，a2表示在适配youBot以后，我又做了自己的一些修改等等。

• Moveit很多东西帮我们封装起来了，但是实际中用的时候老是觉得心里发虚，不踏实。写出来希望大家可以参考看看，但是最好的东西还应该是官方的那些tutorials
• 由于添加path constraints时，我并没有成功，但是这也应该是最精华的一部分，但由于花费了太多时间，我想先暂时放一放，不过这个问题我会持续关注的。