MoveIt教程[19]：IKFast Kinematics Solver

本文主要是介绍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解决方案，请选择其中一个先生成。下面的说明将假设选择了一个<planning_group_name>，将为它创建一个插件。一旦验证了这个插件是有效的，对其它规划组重复下面的说明。例如，可能有两个规划小组：

<planning_group_name> = "left_arm"
<planning_group_name> = "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

编辑这些部分：

<planning_group>:kinematics_solver: <myrobot_name>_<planning_group>_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

这篇关于MoveIt教程[19]：IKFast Kinematics Solver的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！

---