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

环境:Ubuntu16.04+ROS Kinetic

 

一、IKFAST简介

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

../../_images/ikfast_robots.jpg

 

二、环境配置

安装程序

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插件

1、相关参数

(1)MYROBOT_NAME(机器人名字) - name of robot as in your URDF

(2)PLANNING_GROUP(规划组名字) - name of the planning group you would like to use this solver for, as referenced in your SRDF and kinematics.yaml

(3)MOVEIT_IK_PLUGIN_PKG (插件包名字)- name of the new package you just created

(4)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

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"

(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文件时间一般较长,在半个小时左右,若生成失败可以降低精度或者需要修改模型本身。

生成插件

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文件进行测试。

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

展开阅读全文

没有更多推荐了,返回首页