UR5-配置MoveIt!运动学插件IKFAST

最近一直在用逆运动学插件，将UR5的末端位姿通过逆运动学包，规划出各个关节之间的旋转和位移关系，虽然之前看到IKFAST的配置特别麻烦，但是发现用过KDL和TRAC-IK求解效率不高，常常出现规划不出轨迹的情。所以最近决心配置IKFAST，相比于前两者是基于数值解的运动学插件，IKFAST是一种基于解析算法的运动学插件，可以保证每次求解的一致性。但IKFAST会求解出所有可行解，然后选一个，不一定是最优解，这里需要自己写解析算法选最优解。（不会算法）。
下面我就来说下我配置UR5的运动学插件的心得和注意点吧：
其实我基本按照这篇blog的配置过程配置，所以我说一下里面的注意点吧：
一，首先是环境的配置：
我的环境是：Ubuntu16.04+ROS Kinetic
包括了：
1,安装程序（直接任意目录的终端输入，按照blog里面来，很给力）
2,安装依赖库（直接任意目录的终端输入，按照blog里面来，很给力）
3,安装OpenSceneGraph-3.4 （按照blog里面来，很给力，这里我是安装到home目录下）
4,安装Flexible Collision Library 0.5.0（按照blog里面来，很给力，这里我是安装到home目录下）
5,安装sympy 0.7.1(注意版本，若版本不对会产生错误)（按照blog里面来，很给力，这里我是安装到home目录下）
6,删除mpmath（直接任意目录的终端输入，按照blog里面来，很给力）
7,安装MoveIt! IKFast（直接任意目录的终端输入，按照blog里面来，很给力）
8,安装OpenRAVE
这一步我遇到了麻烦，终端git下载过程中，总是提前终止了程序，所以自己索性直接到github链接上进行了下载，记得下载版本应该是latest_stable，然后解压后，修改文件夹名字为openrave，并剪切到home目录下，然后可以继续下面的操作。

到这里我们的环境已经配置完成，可以看到home目录下有这三个文件夹。

二，下一步就是进行配置IKFAST插件
这里一定注意：准备部分：自己编写或修改的urdf模型或者xacro模型（我的初始模型类型）的名字一定要明确，而且这里机械臂在xacro文件里的robot_name,以及xacro文件名和经过MoveIt! Setup Assistant配置出运动规划包的**_moveit_config的**要和后面的上面两个一致，这样可以在后面的操作中省去很多麻烦，如果不一致，可以再用Setup Assistant重新配置下很快。
这里提供两个Setup Assistant配置的参考网址：
https://industrial-training-master.readthedocs.io/en/kinetic/_source/session3/Build-a-Moveit!-Package.html
http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/setup_assistant/setup_assistant_tutorial.html

最后的三者的效果应该是这样：

准备好上述材料后我们就可以进行配置IKFAST插件：
（1）设置机器人名字，我的名字是myrobot2,改成自己的

export MYROBOT_NAME="panda_arm"

（2）若机器人模型为xacro格式需先转为urdf格式，这里的$MYROBOT_NAME就是第一句进行定义

rosrun xacro xacro --inorder -o "$MYROBOT_NAME".urdf "$MYROBOT_NAME".urdf.xacro

（3）机器人模型urdf格式转换为dae格式

rosrun collada_urdf urdf_to_collada "$MYROBOT_NAME".urdf "$MYROBOT_NAME".dae

（4）设置精度为小数点后5位，然后保留备份后重新设置dae格式机器人模型描述文件的精度

export IKFAST_PRECISION="5"
cp "$MYROBOT_NAME".dae "$MYROBOT_NAME".backup.dae  # create a backup of your full precision dae.
rosrun moveit_kinematics round_collada_numbers.py "$MYROBOT_NAME".dae "$MYROBOT_NAME".dae "$IKFAST_PRECISION"

（5）查看模型（关节数据，为后面设置提供参照）

openrave-robot.py "$MYROBOT_NAME".dae --info links

我的如下：

运行，查看三维模型：

   openrave "$MYROBOT_NAME".dae

（6）选择IK项（默认Transform6D ）
我的UR5本身就是6D的模型，所以默认就行
（7）设置运动规划组（与Setup Assistant配置出来的group要一致，我的是manipulator）

export PLANNING_GROUP="manipulator"

（8）设置运动规划的关节组，以上面的模型关节数据为基础设置
这里要注意下在Setup Assistant中配置运动规划组时，Add a planning group called manipulator that names the kinematic chain between base_link and tool0。这里的设置要与这里一致，参照上面的关节数据：

export BASE_LINK="0"
export EEF_LINK="10"

这里说明下在模型中我的world跟simple_arm_base_link是一致的，所以这里我选择BASE_LINK=0,当时也很纠结是1/0，最后用0成功了

---

因为自己经常会在UR5模型的基础上添加一些约束轨迹和防止碰撞的关节，所以在另外一个UR5改造后模型的配置中发现，当关节序号如下时，上面的BASE_LINK和 EEF_LINK，自己刚开始选择了2和11,但是在实际控制机械手总是可以规划但不能执行，所以后来配置又改成了0和11,可以执行，虽然在Setup Assistant中配置运动规划组时，选择了kinematic chain between base_link and tool0的运动学链，但是这里的BASE_LINK选择了base_link 的父级关节world，这是需要注意的。

（9）若关节数量大于6需设置一自由关节，若无则无需设置，我的模型是11个link

export FREE_INDEX="1"

（10）设置IKFAST输出路径

export IKFAST_OUTPUT_PATH=`pwd`/ikfast61_"$PLANNING_GROUP".cpp

（11）生成IKFAST文件，这一步花费时间很长，耐心等待，大约半小时左右

UR5是6轴

python `openrave-config --python-dir`/openravepy/_openravepy_/ikfast.py --robot="$MYROBOT_NAME".dae --iktype=transform6d --baselink="$BASE_LINK" --eelink="$EEF_LINK" --savefile="$IKFAST_OUTPUT_PATH"

（12）生成插件

export MOVEIT_IK_PLUGIN_PKG="$MYROBOT_NAME"_ikfast_"$PLANNING_GROUP"_plugin
cd ~/at_ur5/src  //自己的工作空间
catkin_create_pkg "$MOVEIT_IK_PLUGIN_PKG"
catkin_make

下面这一步执行完后，自己查看了controllers.yaml发现已经更换成了IKfast

rosrun moveit_kinematics create_ikfast_moveit_plugin.py "$MYROBOT_NAME" "$PLANNING_GROUP" "$MOVEIT_IK_PLUGIN_PKG" "$IKFAST_OUTPUT_PATH"

这一步执行完后记得要再此编译，才会生成插件相关包

catkin_make

这里编译花费时间比较长，耐心等待，最后成功效果。

这里在生成的myrobot2（自己的robot_name）_ikfast_manipulator(group_name)_plugin里的myrobot2_manipulator_moveit_ikfast_plugin_description.xml文件里，可以看到需要这个库

自己当时因为没有再次编译，所以在用时出现找不到库的情况报错如下：

所以记得这里要再次编译。

三，运用运动学插件IKFAST

因为之前已经通过Setup Assistant配置好了moveit的运动学包，里面会生成一个demo.launch的文件，我们直接运行它，会出现规划的圆圈,发现可以进行plan和execute

roslaunch myrobot2_moveit_config demo.launch 

到这里我们就已经完全配置好了运动学插件IKFAST，以后我们用这个逆运动学插件来进行规划。