博客

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

  • Post author:
  • Post category:其他



一、IKFAST简介


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

../../_images/ikfast_robots.jpg

二、环境配置


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

../../_images/openrave_panda.png

（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/