MoveIt教程[19]：IKFast Kinematics Solver
在本节中，将介绍为MoveIt配置IKFast插件。
一.What is IKFast
来自Wikipedia: IKFast，机器人运动学编译器，是Rosen Diankov的OpenRAVE运动规划软件中提供的一个强大的逆运动学求解器。与大多数逆运动学求解器不同，IKFast可以解析求解任何复杂运动学链的运动学方程，并生成语言特定的文件[如C++]供以后使用。最终的结果是非常稳定的解决方案，可以在最近的处理器上以5微秒的速度运行。
二.MoveIt! IKFast
MoveIt IKFast是一个使用OpenRAVE生成的cpp文件为MoveIt生成IKFast运动学插件的工具。本教程将通过设置机器人来利用IKFast的力量。MoveIt IKFast使用OpenRAVE 0.8和6DOF和7DOF机械臂机械手在ROS上进行动力学测试。虽然它在理论上可以工作，但目前IKFast插件生成器工具还不能与>7自由度臂一起工作。
三.Getting Started
如果还没有这样做，确保你已经完成了开始的步骤。应该使用安装助手创建机器人的MoveIt配置包。
在Ubuntu 16.04上安装OpenRAVE是一件棘手的事情。这里有两篇博文提供了安装OpenRAVE的不同方法。
- Stephane Caron’s Installing OpenRAVE on Ubuntu 16.04
- Francisco Suarez-Ruiz’s Robotics Workstation Setup in Ubuntu 16.04
确保安装了这些程序：
sudo apt-get install cmake g++ git ipython minizip python-dev python-h5py python-numpy python-scipy qt4-dev-tools
可能还需要下列类库：
sudo apt-get install libassimp-dev libavcodec-dev libavformat-dev libavformat-dev libboost-all-dev libboost-date-time-dev libbullet-dev libfaac-dev libglew-dev libgsm1-dev liblapack-dev liblog4cxx-dev libmpfr-dev libode-dev libogg-dev libpcrecpp0v5 libpcre3-dev libqhull-dev libqt4-dev libsoqt-dev-common libsoqt4-dev libswscale-dev libswscale-dev libvorbis-dev libx264-dev libxml2-dev libxvidcore-dev
要启用OpenRAVE查看器，可能还需要从源代码安装OpenSceneGraph-3.4：
sudo apt-get install libcairo2-dev libjasper-dev libpoppler-glib-dev libsdl2-dev libtiff5-dev libxrandr-dev
git clone https://github.com/openscenegraph/OpenSceneGraph.git --branch OpenSceneGraph-3.4
cd OpenSceneGraph
mkdir build; cd build
cmake .. -DDESIRED_QT_VERSION=4
make -j$(nproc)
sudo make install
为了让IkFast正常工作，必须安装正确版本的sympy：
pip install --upgrade --user sympy==0.7.1
不应该安装mpmath：
sudo apt remove python-mpmath
四.MoveIt! IKFast Installation
安装MoveIt IKFast包来自debs或从源。
二进制安装：
sudo apt-get install ros-kinetic-moveit-kinematics
从源安装，在catkin工作空间内部：
git clone https://github.com/ros-planning/moveit.git
五.OpenRAVE Installation
二进制安装[仅Indigo / Ubuntu 14.04]：
sudo apt-get install ros-indigo-openrave
注意：必须设置：
export PYTHONPATH=$PYTHONPATH:`openrave-config --python-dir`
从源安装：
git clone --branch latest_stable https://github.com/rdiankov/openrave.git
cd openrave && mkdir build && cd build
cmake -DODE_USE_MULTITHREAD=ON -DOSG_DIR=/usr/local/lib64/ ..
make -j$(nproc)
sudo make install
六.Create Collada File For Use With OpenRAVE
参数如下所示：
- MYROBOT_NAME：URDF中机器人的名称
- PLANNING_GROUP：希望使用此求解器的规划组的名称，请参考SRDF和kinemtics.yaml
- MOVEIT_IK_PLUGIN_PKG：刚刚创建的新包的名称
- IKFAST_OUTPUT_PATH：文件路径到生成的IKFast output.cpp文件的位置
为了方便使用本教程的复制/粘贴功能，请使用机器人的名称设置MYROBOT_NAME环境变量：
export MYROBOT_NAME="panda_arm"
首先，需要的机器人描述文件是Collada或OpenRAVE机器人格式。如果机器人不是这种格式，建议创建一个ROS URDF文件。如果机器人是xacro格式的，可以使用以下命令将其转换为urdf：
rosrun xacro xacro --inorder -o "$MYROBOT_NAME".urdf "$MYROBOT_NAME".urdf.xacro
有了URDF格式的机器人后，可以使用以下命令将其转换为Collada[.dae]文件：
rosrun collada_urdf urdf_to_collada "$MYROBOT_NAME".urdf "$MYROBOT_NAME".dae
在将URDF文件转换为Collada文件时，经常会出现浮点数问题，因此创建了一个脚本来将.dae文件中的所有数字四舍五入到小数点后x位。如果一开始跳过这一步，看看IKFast是否可以用默认值生成一个解决方案，这可能是最好的，但是如果生成时间超过，比如说，一个小时，试试下面的方法：
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 IKFast生成器需要很长时间才能找到解决方案，那么降低小数位数应该会有所帮助。
查看新生成的Collada文件中的链接。可能需要安装软件包libsoqt4-dev使显示工作：
openrave-robot.py "$MYROBOT_NAME".dae --info links
如果有一个7自由度的arm，并且需要填充一个-freeindex参数[稍后将讨论]，那么这是非常有用的。在OpenRAVE中测试新生成的Collada文件：
openrave "$MYROBOT_NAME".dae
七.Create IKFast Solution CPP File
一旦有一个数值四舍五入的Collada文件，它的时间来生成C++ .h头文件，其中包含机器人的分析IK解决方案。
1.Select IK Type
需要选择想要的知识。更多信息见此页。最常见的IK类型是transform6d。
2.Choose Planning Group
如果机器人有多个手臂或“规划组”，希望为其生成一个IKFast解决方案，请选择其中一个先生成。下面的说明将假设选择了一个
= "left_arm"
= "right_arm"
为了使本教程的其余部分易于使用复制/粘贴。设置一个PLANNING_GROUP环境变量。例如：
export PLANNING_GROUP="panda_arm"
3.Identify Link Numbers
还需要用于计算IK的base_link和end_link的链接索引号。可以通过查看模型中的链接列表来计算链接的数量：
openrave-robot.py "$MYROBOT_NAME".dae --info links
一个典型的6-DOF机械手应该有6 arm links+一个虚拟的base_link根据ROS规范的要求。如果模型中没有额外的链接，则得到：baselink=0，eelink=6。通常，会提供一个额外的tool_link来定位抓取/工具框架，给出eelink=7。
下面的机械手还有另一个虚拟的mounting_link，给出baselink=1和eelink=8。
| name | index | parents |
|---|---|---|
| panda_link0 | 0 | |
| panda_link1 | 1 | panda_link0 |
| panda_link2 | 2 | panda_link1 |
| panda_link3 | 3 | panda_link2 |
| panda_link4 | 4 | panda_link3 |
| panda_link5 | 5 | panda_link4 |
| panda_link6 | 6 | panda_link5 |
| panda_link7 | 7 | panda_link6 |
| panda_link8 | 8 | panda_link7 |
将基本链接和EEF链接设置为需要的索引：
export BASE_LINK="0"
export EEF_LINK="8"
如果有一个7自由度的手臂，需要指定一个自由链接：
export FREE_INDEX="1"
4.Generate IK Solver
要生成IK解决方案之间的机械手的基础和工具框架的6自由度手臂，使用以下命令格式。建议将输出命名为ikfast61_"$PLANNING_GROUP".cpp：
export IKFAST_OUTPUT_PATH=`pwd`/ikfast61_"$PLANNING_GROUP".cpp
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"
对于7自由度的arm，需要指定一个自由链接：
python `openrave-config --python-dir`/openravepy/_openravepy_/ikfast.py --robot="$MYROBOT_NAME".dae --iktype=transform6d --baselink="$BASE_LINK" --eelink="$EEF_LINK" --freeindex="$FREE_INDEX" --savefile="$IKFAST_OUTPUT_PATH"
这个过程的速度和成功将取决于机器人的复杂性。一个典型的6自由度机械手与3相交轴在base或wrist将只需要几分钟来产生IK。
已知的问题：freeindex参数有一个错误，它不能正确地处理树索引。假设baselink = 2 -eelink = 16，链接索引从3到9与当前规划组链无关。在这种情况下，freeindex将期望索引2作为链接2，但索引3作为链接10…索引9作为链接16。
应该参考OpenRAVE邮件列表和ROS答案，以获得关于5和7自由度操纵器的信息。
八.Create Plugin
创建包含IK插件的包。建议将包命名为"$MYROBOT_NAME"_ikfast_"$ PLANNING_GROUP"_plugin。
export MOVEIT_IK_PLUGIN_PKG="$MYROBOT_NAME"_ikfast_"$PLANNING_GROUP"_plugin
cd ~/catkin_ws/src
catkin_create_pkg "$MOVEIT_IK_PLUGIN_PKG"
构建工作空间，以便检测新包[可以是roscd]：
catkin build
创建插件源代码：
rosrun moveit_kinematics create_ikfast_moveit_plugin.py "$MYROBOT_NAME" "$PLANNING_GROUP" "$MOVEIT_IK_PLUGIN_PKG" "$IKFAST_OUTPUT_PATH"
或没有ROS：
python /path/to/create_ikfast_moveit_plugin.py "$MYROBOT_NAME" "$PLANNING_GROUP" "$MOVEIT_IK_PLUGIN_PKG" "$IKFAST_OUTPUT_PATH"
九.Usage
IKFast插件应该与默认的KDL IK求解器功能相同，但是性能大大提高。MoveIt配置文件由moveit_ikfast脚本自动编辑，但是可以使用机器人运动学中的kinematics_solver参数在KDL和IKFast求解器之间进行切换。yaml文件：
rosed "$MYROBOT_NAME"_moveit_config kinematics.yaml
编辑这些部分：
:
kinematics_solver: __kinematics/IKFastKinematicsPlugin
-INSTEAD OF-
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
1.Test the Plugin
使用MoveIt RViz运动规划插件和使用交互式标记，看看是否找到了正确的IK解决方案。
十.Updating the Plugin
如果MoveIt或者IKFast将来有任何变化，可能需要使用脚本重新生成这个插件。为了能够轻松地做到这一点，在IKFast包的根目录下自动创建了一个bash脚本，名为update_ikfast_plugin.sh。这与之前手动执行的操作相同，但是使用的是复制到ROS包中的IKFast解决方案头文件。
参考文献：
[1]IKFast Kinematics Solver：http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/ikfast/ikfast_tutorial.html