ROS进阶——MoveIt!运动学插件IKFAST配置

一、IKFAST简介

IKFAST是一种基于解析算法的运动学插件，可以保证每次求解的一致性。

 

二、环境配置

Melodic版本可参考：https://ros-planning.github.io/moveit_tutorials/doc/ikfast/ikfast_tutorial.html

安装程序

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

安装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

安装Flexible Collision Library 0.5.0

cd ~/git
git clone https://github.com/flexible-collision-library/fcl
cd fcl; git checkout 0.5.0
mkdir build; cd build
cmake ..
make -j `nproc`
sudo make install

安装sympy 0.7.1(注意版本，若版本不对会产生错误)

pip install --upgrade --user sympy==0.7.1

删除mpmath

sudo apt remove python-mpmath

安装MoveIt! IKFast

sudo apt-get install ros-kinetic-moveit-kinematics

安装OpenRAVE

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

注意：不建议采用sudo apt-get install ros-kinetic-openrave，会一并安装mpmath，导致后面生成IKFAET文件失败，因此采用源码安装的方式。

补充：若出现错误RPC failed; curl 56 GnuTLS recv error (-54): Error in the pull function. 可运行下面命令解决。

git config --global http.postBuffer 2097152000

 

三、配置IKFAST插件

3.1 相关参数

• MYROBOT_NAME（机器人名字） - name of robot as in your URDF
• PLANNING_GROUP（规划组名字） - name of the planning group you would like to use this solver for, as referenced in your SRDF and kinematics.yaml
• MOVEIT_IK_PLUGIN_PKG （插件包名字）- name of the new package you just created
• IKFAST_OUTPUT_PATH（IKFAST输出路径） - file path to the location of your generated IKFast output.cpp file

export MYROBOT_NAME="panda_arm"
export IKFAST_PRECISION="5"//保留精度小数点后5位
export PLANNING_GROUP="panda_arm"
export BASE_LINK="0"//起始关节
export EEF_LINK="8"//末端关节
export FREE_INDEX="1"//若关节数大于6需设置一自由关节
export IKFAST_OUTPUT_PATH=`pwd`/ikfast61_"$PLANNING_GROUP".cpp

3.2 配置流程

（1）设置机器人名字

export MYROBOT_NAME="panda_arm"

（2）若机器人模型为xacro格式需先转为urdf格式

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"

设置的精度会影响IKFAST的生成，设置过小会导致无法生成或求解精度过低，过大也会导致无法生成或者求解变慢，可以根据需求调整精度。

（5）查看模型

模型关节数据（数据如下图）

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

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

模型三维结构

  openrave "$MYROBOT_NAME".dae

（6）选择IK项（默认Transform6D ）

http://openrave.org/docs/latest_stable/openravepy/ikfast/#ik-types

（7）设置运动规划组

export PLANNING_GROUP="panda_arm"

（8）设置运动规划的关节组，以上面的模型关节数据为基础设置

export BASE_LINK="0"
export EEF_LINK="8"

（9）若关节数量大于6需设置一自由关节，若无则无需设置

export FREE_INDEX="1"

（10）设置IKFAST输出路径

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

（11）生成IKFAST文件

• 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轴

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"

备注：生成IKFAST文件时间一般不会太长，在10~20分钟左右，若时间过长有几率代表失败（即使成功生成），生成失败的话可以降低精度或者需要修改模型本身。

生成插件

export MOVEIT_IK_PLUGIN_PKG="$MYROBOT_NAME"_ikfast_"$PLANNING_GROUP"_plugin
cd ~/catkin_ws/src
catkin_create_pkg "$MOVEIT_IK_PLUGIN_PKG"

catkin build

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

注意：create_ikfast_moveit_plugin.py默认该目录下存在名字为"$MYROBOT_NAME"_moveit_config的功能包如下，该功能包为机器人模型通过moveit_setup_assistant配置生成的功能包。

      plan_pkg = robot_name + '_moveit_config'
      plan_pkg_dir = roslib.packages.get_pkg_dir(plan_pkg)
      print 'Loading robot from \''+plan_pkg+'\' package ... '

 

四、使用IKFAST插件

cd到"$MYROBOT_NAME"_moveit_config功能包的config文件目录下，修改该目录下的kinematics.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

测试

    建议先采用方式二或三确保生成output_ikfast61.cpp文件正常才进行方式一测试。

• 方式一：运行"$MYROBOT_NAME"_moveit_config功能包的demo.launch检查是否成功。
• 方式二：直接编译ikfast61_"$PLANNING_GROUP".cpp文件测试

g++ ikfast61_hand.cpp -lstdc++ -llapack -o compute -lrt

Usage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...

Returns the ik solutions given the transformation of the end effector specified by
a 3x3 rotation R (rXX), and a 3x1 translation (tX).
There are 0 free parameters that have to be specified.

• 方式三：通过ikfastdemo.cpp编译ikfast61_"$PLANNING_GROUP".cpp文件进行测试。

  （1）下载ikfastdemo.cpp文件（https://github.com/davetcoleman/clam_rosbuild/blob/master/clam_ik/models/ikfastdemo.cpp）

  （2）从OpenRAVE目录下复制ikfast.h文件到同一目录下。

  （3）修改output_ikfast61.cpp文件为生成的ikfast61_"$PLANNING_GROUP".cpp

#define IK_VERSION 61
#include "output_ikfast61.cpp"

  （4）编译

g++ ikfastdemo.cpp -lstdc++ -llapack -o compute -lrt

若发生错误改用如下

g++ ikfastdemo.cpp -lstdc++ -llapack -o compute -lrt -I<openravepy>

（4）运行

./compute fk j0 j1 ... j%d
Returns the forward kinematic solution given the joint angles (in radians).

./compute ik  t0 t1 t2  qw qi qj qk  free0 
Returns the ik solutions given the transformation of the end effector specified by a 3x1 translation (tX), and a 1x4 quaternion (w + i + j + k). There are %d free parameters that have to be specified. 

./compute ik  r00 r01 r02 t0  r10 r11 r12 t1  r20 r21 r22 t2  free0
Returns the ik solutions given the transformation of the end effector specified by a 3x3 rotation R (rXX), and a 3x1 translation (tX).There are %d free parameters that have to be specified. 

./compute iktiming
For fixed number of iterations, generates random joint angles, then calculates fk, calculates ik, measures average time taken. 

./compute iktiming2 
For fixed number of iterations, with one set of joint variables, this finds the ik solutions and measures the average time taken.

 

参考

http://www.guyuehome.com/1921

http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/ikfast/ikfast_tutorial.html

https://scaron.info/teaching/installing-openrave-on-ubuntu-16.04.html

http://wiki.ros.org/Industrial/Tutorials/Create_a_Fast_IK_Solution

http://openrave.org/docs/0.8.2/openravepy/ikfast/