ROS技術點滴 —— MoveIt!中的運動學插件

來自專欄古月居

MoveIt!是ROS中一個重要的集成化開發平台，由一系列移動操作的功能包組成，提供運動規劃、操作控制、3D感知、運動學等功能模塊，是ROS社區中使用度排名前三的功能包，目前已經支持眾多機器人硬體平台。

MoveIt!中的眾多功能都使用插件機制集成，其中有一個重要的功能模塊——運動學插件。

今天我們就來聊下MoveIt!中的運動學求解器。

一、KDL

Kinematics and Dynamics Library (KDL)是MoveIt!中的默認運動學插件，在使用MoveIt! Setup Assistant進行模型配置時，可以進行配置。

配置完成後，就可以運行demo.launch控制虛擬機械臂進行運動規划了。但是KDL有自己的優缺點：

比如一次逆解的求解時間：0.062192秒。。。

很多時候我們在做運動規劃的時候，MoveIt!經常會提示規劃失敗、求解失敗等錯誤，很多都是因為KDL這款運動學插件導致的，那麼問題就來了——能不能更換一個運動學插件？

當然可以，以下介紹兩個用的最多的運動學插件：TRAC-IK和IKFAST。

二、TRAC-IK

TRAC-IK和KDL類似，也是一種基於數值解的運動學插件，但是在演算法層面上進行了很多改進，求解效率高了很多。比如在下邊這張圖中，左側的紅點是KDL無法求解的姿態點，但是在右側使用TRAC-IK是可以求解的。

那麼如何將KDL更換成TRAC-IK呢，方法很簡單，ROS的軟體源中已經集成了TRAC-IK的安裝包，可以直接使用以下命令安裝：

sudo apt-get install ros-kinetic-trac-ik-kinematics-plugin

然後修改機械臂MoveIt!配置功能包下的kinematics.yaml文件就可以使用啦：

arm: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_attempts: 3 kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.05

接下來再次運行demo.launch，默認載入的就是TRAC-IK運動學插件了，試試規劃求解的效率是不是高了很多！

但是TRAC-IK也有問題，它是一種數值演算法，每次求解得到的關節位置不一定相同。

三、IKFAST

IKFAST是一種基於解析演算法的運動學插件，可以保證每次求解的一致性。

相比KDL和TRAC-IK，IKFAST的安裝過程就比較複雜了，不過就筆者的使用經驗來講，IKFAST的效果還是很推薦的，所以不妨一試，以下就是IKFAST的安裝配置過程。

• 安裝程序：

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-devgit clone https://github.com/openscenegraph/OpenSceneGraph.git --branch OpenSceneGraph-3.4cd OpenSceneGraphmkdir build; cd buildcmake .. -DDESIRED_QT_VERSION=4make -j$(nproc)sudo make install

• 安裝sympy

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

• 刪除mpmath

sudo apt remove python-mpmath

• 安裝IKFast功能包

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

• 安裝OpenRave

sudo apt-get install ros-kinetic-openrave

• 創建collada文件

export MYROBOT_NAME="marm"rosrun xacro xacro --inorder -o "$MYROBOT_NAME".urdf "$MYROBOT_NAME".xacro rosrun collada_urdf urdf_to_collada "$MYROBOT_NAME".urdf "$MYROBOT_NAME".dae

• 創建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"

• 查看生成的模型

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

openrave "$MYROBOT_NAME".dae

• 生成六軸機器人配置

$ export PLANNING_GROUP="arm"$ export BASE_LINK="1"$ export EEF_LINK="11"$ export IKFAST_OUTPUT_PATH=`pwd`/ikfast61_"$PLANNING_GROUP".cpp$ 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"

• 創建插件

export MOVEIT_IK_PLUGIN_PKG="$MYROBOT_NAME"_ikfast_"$PLANNING_GROUP"_plugincd ~/catkin_ws/srccatkin_create_pkg "$MOVEIT_IK_PLUGIN_PKG"rosrun moveit_kinematics create_ikfast_moveit_plugin.py "$MYROBOT_NAME" "$PLANNING_GROUP" "$MOVEIT_IK_PLUGIN_PKG" "$IKFAST_OUTPUT_PATH"

• 重新編譯工作空間

catkin_make

• 修改使用的插件

rosed "$MYROBOT_NAME"_moveit_config/config/kinematics.yaml<planning_group>:kinematics_solver: <myrobot_name>_<planning_group>_kinematics/IKFastKinematicsPlugin-INSTEAD OF-kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin

注意以上步驟中的機器人名稱、運動規劃組名稱、坐標系序號等需要和自己所使用的機器人匹配，也可以參考官方教程配置：

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

現在運行demo.launch文件時，使用的運動學插件就是IKFAST了，快試一試效果是不是還不錯！

更多內容可關注微信公眾號：古月居 (guyue_home)