1. Background
基于上次分析，我一直认为是自己的途径点的参数设置有问题才导致实际动作与仿真区别较大，然后想看看youbot的正逆解那块，其实因为我是基于actionlib实现的，因此我的主要目标定位在了youbot的驱动文件中，因为我们加载驱动的时候，它就启动了action_server，而我们的client目前做的工作是发送途径点，及其速度，加速度，运行时间这几个参数
我的action_server的原始文件在/home/yake/catkin_ws/src/youbot_driver_ros_interface/src/下的joint_trajectory_action.cpp
大概思路是利用，Kinematics and Dynamics Library（KDL）第三方库将轨迹分割，然后利用RotationalInterpolation_SingleAxis函数计算插值，最后再组合在一起形成一条完整轨迹。
后来，再寻找资料的过程中，我发现已有针对机械臂的package——Moveit，以前看论文的时候看到过，但是当时没有接触太深，没办法，到头来还是绕不过去，因此有了这篇文章。
2.参考资料及前期工作
这里我主要参考的资料是《ROS by Example volume 2》的11章，以及网上的一些资料。
http://docs.ros.org/indigo/api/moveit_setup_assistant/html/doc/tutorial.html
http://wiki.ros.org/Industrial/Tutorials/Create_a_MoveIt_Pkg_for_an_Industrial_Robot
因为作者用的机器人是pi-robot，可以说和kuka很不一样，最大的不同在于，pirobot的机械臂是由几个舵机组成的，因此在利用moveit_assist生成config文件的过程中,有些东西会肯定不一样的，这点我有所准备。下面我只讲我和他不一样的地方。
2.1利用moveit_assist设置Virtual Joint
这个Virtual Joint就是我们将robot与现实世界连接的一个关节，比如机器人在地上跑，轮子和地面间就可以设置一个Virtual Joint。如果机器人就是一个单纯的机械臂，可以不用设置virtual joint。只有当机械臂的底座不是固定的时候才需要virtual joint，比如类似youBot机械臂固定在移动平台上的，就可以添加一个相对于odom的virtual joint。
我们都知道odometry是里程计的意思，就是用来知道机器人走了多远，处在的方位等，对navigation了解的同学应该比较熟悉。（我这里并不打算让机器人底座运动，所以设置成fixed）
如果你想配置成移动的，如下图所示
2.2 设置Planning Group和End Effector
这个地方是比较重要的，这个地方针对每个robot的不同，会有不同的设置，例如官网tutorial是针对PR2的手臂设置的Group，ros by example的作者是针对pirobot的由舵机组成的手臂设置的Group。在这里我们针对youbot 的5自由度机械臂和夹子设置的planning group，这里的类型与ros by example书中差异较大，注意区分：
这里记住一点，joint是关节，link是连杆，两个link间是通过一个joint来连接传动的（也就是link会比joint多）。夹子那里只选了link并且不选求解器，它只是简单的电机起、停，不涉及运动规划。
另外一个，要注意的地方是关于正逆解的求解器，基于第一部分，我们知道action server那里是基于的KDL
这里，不知你有没有发现多了几个求解器，我们还是首先默认的KDL就好，关于第三方求解器（上图中最后一个）以后会介绍到。
Passive Joints(被动关节)，比如像有的小车的万向轮，求解器在运动规划的时候不会考虑这些关节。
一路下来，后面基本差不多了，最后我保存的文件为youbot_moveit_config, 建议最好放在catkin_ws/src下，到时会用roslaunch启动里面的文件的。
3. Testing
利用命令
roslaunch youbot_moveit_config demo.launch就可以开启仿真
利用命令
roslaunch moveit_commander moveit_commander_cmdline.py可以通过Moveit为我们提供的接口和机器人互动，首先help
use 命令后面跟的是我们之前设置的Group的名字，tab可以补全。go rand表示让robot去到一个工作范围内的随机位置
4. 使用Arbotix在仿真中控制youbot
利用之前针对youbot修改的arbotix文件，使用命令
roslaunch ik_follow_joint_trajectory youbot_with_gripper.launch sim:=true接着再用命令
rosrun rviz rviz
rosrun ik_follow_joint_trajectory trajectory_demo_youbot_with_gripper.py _reset:=true这里我贴出上次仿真的命令
roslaunch rbx1_bring_up fake_youbot.launch
rosrun rviz rviz
rosrun ik_follow_joint_trajectory action_trajectory5. Actionlib与MoveIt Joint Controllers
MoveIt 内也有一个控制接口叫做moveit_simple_controller_manager.
moveit_simple_controller_manager - A very basic controller manager plugin. Can connect toFollowJointTrajectoryAction andGripperCommandAction servers.
https://github.com/ros-planning/moveit_plugins
看到上面加粗的地方是不是似曾相识，没错，它就是和youbot的driver中一样的server。这里参照ros by example volume2 P309页的写法
youbot的controller如下
注意:其实这里的name+action_ns合在一起就是youbot驱动中的/arm_1/arm_controller/follow_joint_trajectory，正是如此，我们才可以利用Moveit来驱动实际的youbot。同时，还要注意的是joints的名称必须是像那样arm_joint_1——arm_joint_5, gripper的名称也要是那样子gripper_finger_joint_l和gripper_finger_joint_r，这里因为youbot官方没有提供夹子的action接口，所以我们没有添加夹子的。名字这些在youbot的user manual中已经提到
这里只是Moveit的第一部分，涉及的东西有些多，下部分会介绍正、反解和第三方库的相关知识。
————————————————————————
2016.05更新
————————————————————————
后面收集资料的过程中，结合youBot的配置，我也发现了很多不一样的。配置的好不好最直接的效果就是设置目标求解的时候，Moveit能不能快速、顺利的解出来。特别是Moveit中的那个Group的设置，关于这里好像也没有太多正式的资料。下面是自己收集到的：GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc
LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included
JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included
CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group
SUBGROUPS: Groups can also be formed by referencing to already defined group names
- 那么结合手头的，ros by example里是舵机组成的手臂，用的是Chains。
- 在自己后面这篇http://blog.csdn.net/yaked/article/details/51436262文章里，作者的手臂夹子之间加了一个grasp frame也是用的Chains。我在仿真的时候因为也有grasp frame也是仿照这种，配置了一个arm为 chains的moveit config。http://blog.csdn.net/yaked/article/details/51483531也给出了配置截图
那么还有，在https://github.com/pschillinger/youbot_integration这里，作者的这个配置也值得注意
这里如果不想移动底座，可以将它设置为fixed而不是planar。
arm用的是Joints，包含了固定的arm0。而gripper则是links，包含了palm link上面这幅图要注意的是gripper的求解器那里，留空，不用设置。
这里没有一个官方的配置，一切以效果来说话，不过在我用Moveit来仿真的时候，这个还是比较不错的，它的求解器是OMPL 的RRTConnect，求解的比较快。大家可以参考一下，但也不要受限于它。