0. IKFast 简介

对于求解机器人逆运动学，有数值解和解析解（封闭解）。像 OROCOS 中的KDL（Kinematics-Dynamics Library）库是数值解法，速度较慢（约1ms），一次只能求得一个解；而 IKFast 是解析解法，速度较快（约4μs），可以求得所有逆解，保证每次求解的一致性，并且能够自动生成优化的 C++ 代码。

IKFast: The Robot Kinematics Compiler
IKFast analytically solves robot inverse kinematics equations and generates optimized C++ files.

我们知道有两大机器人开源平台：OpenRAVE（Mujin公司）和 MoveIt!（Kinema Systems 公司），MoveIt! 我们应该很熟悉了，集成在 ROS 里了（括弧：当然还有其他的机器人开源平台，如前面提到的做底层控制的 OROCOS，MoveIt! 将 OROCOS 中 KDL 作为默认的运动学求解器）。 IKFast 是 OpenRAVE 库中的一个算法模块，同时也被 MoveIt! 所支持，具体原理和介绍参考 Rosen Diankov 博士论文和 OpenRAVE 官网。

http://openrave.org/docs/latest_stable/openravepy/ikfast/​openrave.org/docs/latest_stable/openravepy/ikfast/

1. OpenRAVE 源码编译安装

参考：Installing OpenRAVE on Ubuntu 16.04

  • 安装依赖
sudo apt-get install cmake g++ git ipython minizip python-dev python-h5py 
python-numpy python-scipy python-sympy 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
  • 安装 collada-dom
git clone https://github.com/rdiankov/collada-dom.git
cd collada-dom && mkdir build && cd build
cmake ..
make -j8
sudo make install
  • OpenSceneGraph
sudo apt-get install libcairo2-dev libjasper-dev libpoppler-glib-dev libsdl2-dev libtiff5-dev libxrandr-dev
git clone --branch OpenSceneGraph-3.4 https://github.com/openscenegraph/OpenSceneGraph.git
cd OpenSceneGraph && mkdir build && cd build
cmake .. -DDESIRED_QT_VERSION=4
make -j8
sudo make install
  • Flexible Collision Library
sudo apt-get install libccd-dev
git clone https://github.com/flexible-collision-library/fcl.git
cd fcl
git checkout 0.5.0  # use FCL 0.5.0
mkdir build && cd build
cmake ..
make -j8
sudo make install
  • OpenRAVE
git clone --branch latest_stable https://github.com/rdiankov/openrave.git
cd openrave && mkdir build && cd build
cmake .. -DOSG_DIR=/usr/local/lib64/
make -j8
sudo make install

添加环境变量 ~/.bashrc

export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$(openrave-config --python-dir)/openravepy/_openravepy_
export PYTHONPATH=$PYTHONPATH:$(openrave-config --python-dir)

注：添加了环境变量后，使用 Anaconda 环境可能会报出以下错误，解决办法是将 PYTHONPATH 那一行注释，用 openrave 的时候直接在命令行中添加环境变量，使当前终端有效即可。

ImportError: This package should not be accessible on Python 3. Either you are 
trying to run from the python-future src folder or your installation of 
python-future is corrupted.

OpenRAVE 的安装路径为：/usr/local，包含的目录如下图所示：

  • 测试
openrave.py --example hanoi

2. 生成 UR5 运动学逆解 C++ 源码

  • 准备 robot 文件（COLLADA 或 OpenRAVE XML）

可以从 universal-robot 官方的 ROS 包中下载 UR5 模型：ur_description/urdf，选用 ur5_joint_limited_robot.urdf.xacro

  • xacro 格式 转换成 urdf 格式
rosrun xacro xacro --inorder -o ur5.urdf ur5_joint_limited_robot.urdf.xacro
  • urdf 格式转换成 dae 格式
rosrun collada_urdf urdf_to_collada ur5.urdf ur5.dae
  • 设置精度
# 先备份
cp ur5.dae ur5.backup.dae

# 设置精度
export IKFAST_PRECISION="5"
rosrun moveit_kinematics round_collada_numbers.py ur5.dae ur5.dae "$IKFAST_PRECISION"
  • 查看关节数据
openrave-robot.py ur5.dae --info links
  • 查看三维模型
openrave ur5.dae
  • 生成 IKFast C++ 文件
python `openrave-config --python-dir`/openravepy/_openravepy_/ikfast.py 
--robot=ur5.dae --iktype=transform6d --baselink=0 --eelink=9 
--savefile=$(pwd)/ikfast61.cpp

ikfast.h 头文件路径：/usr/local/lib/python2.7/dist-packages/openravepy/_openravepy_0_9

  • openravepy

上面生成 ikfast C++ 源码的时候，用到了 openravepy 模块，其安装路径为：/usr/local/lib/python2.7/dist-packages/openravepy

官方文档介绍：

OpenRAVE Documentation​openrave.org/docs/latest_stable/openravepy/openravepy/

ikfast.py 的参数说明如下图所示：

3. 使用 ikfast61.cpp 求逆解

生成的ikfast61.cpp 文件，有 main() 函数，可用来测试；将 ikfast61.cpp 与 ikfast.h 放在同一文件夹下，进行编译。

g++ ikfast61.cpp -o ikfast -llapack -std=c++11

注：

  • IKFast 算法依赖于 LAPACK（ Linear Algebra PACKag）库，所以编译的时候要链接 liblapack.so 库；
  • 输入为 3x4 矩阵（rotation 3x3， translation 3x1）
r00 r01 r02 t1 r10 r11 r12 t2 r20 r21 r22 t3

# 转换成 3x4 矩阵
r00  r01  r02  t1
r10  r11  r12  t2
r20  r21  r22  t3
  • 生成 8 组解

参考：

http://openrave.org/docs/latest_stable/​openrave.org/docs/latest_stable/
Installing OpenRAVE on Ubuntu 16.04​scaron.info/teaching/installing-openrave-on-ubuntu-16.04.html
https://blog.csdn.net/Kalenee/article/details/80740258​blog.csdn.net/Kalenee/article/details/80740258
https://www.dazhuanlan.com/2019/11/03/5dbe9a647efc6/​www.dazhuanlan.com/2019/11/03/5dbe9a647efc6/
mujin 工业机器人设计公司的优势在哪里？在算法上获得了哪些突破？