MoveIt教程[19]:IKFast Kinematics Solver

在本节中,将介绍为MoveIt配置IKFast插件。
一.What is IKFast
来自Wikipedia: IKFast,机器人运动学编译器,是Rosen Diankov的OpenRAVE运动规划软件中提供的一个强大的逆运动学求解器。与大多数逆运动学求解器不同,IKFast可以解析求解任何复杂运动学链的运动学方程,并生成语言特定的文件[如C++]供以后使用。最终的结果是非常稳定的解决方案,可以在最近的处理器上以5微秒的速度运行。

二.MoveIt! IKFast
MoveIt IKFast是一个使用OpenRAVE生成的cpp文件为MoveIt生成IKFast运动学插件的工具。本教程将通过设置机器人来利用IKFast的力量。MoveIt IKFast使用OpenRAVE 0.8和6DOF和7DOF机械臂机械手在ROS上进行动力学测试。虽然它在理论上可以工作,但目前IKFast插件生成器工具还不能与>7自由度臂一起工作。

三.Getting Started
如果还没有这样做,确保你已经完成了开始的步骤。应该使用安装助手创建机器人的MoveIt配置包。
在Ubuntu 16.04上安装OpenRAVE是一件棘手的事情。这里有两篇博文提供了安装OpenRAVE的不同方法。

  • Stephane Caron’s Installing OpenRAVE on Ubuntu 16.04
  • Francisco Suarez-Ruiz’s Robotics Workstation Setup in Ubuntu 16.04

确保安装了这些程序:

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

要启用OpenRAVE查看器,可能还需要从源代码安装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

为了让IkFast正常工作,必须安装正确版本的sympy:

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

不应该安装mpmath:

sudo apt remove python-mpmath

四.MoveIt! IKFast Installation
安装MoveIt IKFast包来自debs或从源。
二进制安装:

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

从源安装,在catkin工作空间内部:

git clone https://github.com/ros-planning/moveit.git

五.OpenRAVE Installation
二进制安装[仅Indigo / Ubuntu 14.04]:

sudo apt-get install ros-indigo-openrave

注意:必须设置:

export PYTHONPATH=$PYTHONPATH:`openrave-config --python-dir`

从源安装:

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

六.Create Collada File For Use With OpenRAVE
参数如下所示:

  • MYROBOT_NAME:URDF中机器人的名称
  • PLANNING_GROUP:希望使用此求解器的规划组的名称,请参考SRDF和kinemtics.yaml
  • MOVEIT_IK_PLUGIN_PKG:刚刚创建的新包的名称
  • IKFAST_OUTPUT_PATH:文件路径到生成的IKFast output.cpp文件的位置

为了方便使用本教程的复制/粘贴功能,请使用机器人的名称设置MYROBOT_NAME环境变量:

export MYROBOT_NAME="panda_arm"

首先,需要的机器人描述文件是Collada或OpenRAVE机器人格式。如果机器人不是这种格式,建议创建一个ROS URDF文件。如果机器人是xacro格式的,可以使用以下命令将其转换为urdf:

rosrun xacro xacro --inorder -o "$MYROBOT_NAME".urdf "$MYROBOT_NAME".urdf.xacro

有了URDF格式的机器人后,可以使用以下命令将其转换为Collada[.dae]文件:

rosrun collada_urdf urdf_to_collada "$MYROBOT_NAME".urdf "$MYROBOT_NAME".dae

在将URDF文件转换为Collada文件时,经常会出现浮点数问题,因此创建了一个脚本来将.dae文件中的所有数字四舍五入到小数点后x位。如果一开始跳过这一步,看看IKFast是否可以用默认值生成一个解决方案,这可能是最好的,但是如果生成时间超过,比如说,一个小时,试试下面的方法:

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 IKFast生成器需要很长时间才能找到解决方案,那么降低小数位数应该会有所帮助。
查看新生成的Collada文件中的链接。可能需要安装软件包libsoqt4-dev使显示工作:

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

如果有一个7自由度的arm,并且需要填充一个-freeindex参数[稍后将讨论],那么这是非常有用的。在OpenRAVE中测试新生成的Collada文件:

openrave "$MYROBOT_NAME".dae

七.Create IKFast Solution CPP File
一旦有一个数值四舍五入的Collada文件,它的时间来生成C++ .h头文件,其中包含机器人的分析IK解决方案。
1.Select IK Type
需要选择想要的知识。更多信息见此页。最常见的IK类型是transform6d。
2.Choose Planning Group
如果机器人有多个手臂或“规划组”,希望为其生成一个IKFast解决方案,请选择其中一个先生成。下面的说明将假设选择了一个,将为它创建一个插件。一旦验证了这个插件是有效的,对其它规划组重复下面的说明。例如,可能有两个规划小组:

 = "left_arm"
 = "right_arm"

为了使本教程的其余部分易于使用复制/粘贴。设置一个PLANNING_GROUP环境变量。例如:

export PLANNING_GROUP="panda_arm"

3.Identify Link Numbers
还需要用于计算IK的base_link和end_link的链接索引号。可以通过查看模型中的链接列表来计算链接的数量:

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

一个典型的6-DOF机械手应该有6 arm links+一个虚拟的base_link根据ROS规范的要求。如果模型中没有额外的链接,则得到:baselink=0,eelink=6。通常,会提供一个额外的tool_link来定位抓取/工具框架,给出eelink=7。
下面的机械手还有另一个虚拟的mounting_link,给出baselink=1和eelink=8。

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

将基本链接和EEF链接设置为需要的索引:

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

如果有一个7自由度的手臂,需要指定一个自由链接:

export FREE_INDEX="1"

4.Generate IK Solver
要生成IK解决方案之间的机械手的基础和工具框架的6自由度手臂,使用以下命令格式。建议将输出命名为ikfast61_"$PLANNING_GROUP".cpp:

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

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自由度的arm,需要指定一个自由链接:

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"

这个过程的速度和成功将取决于机器人的复杂性。一个典型的6自由度机械手与3相交轴在base或wrist将只需要几分钟来产生IK。
已知的问题:freeindex参数有一个错误,它不能正确地处理树索引。假设baselink = 2 -eelink = 16,链接索引从3到9与当前规划组链无关。在这种情况下,freeindex将期望索引2作为链接2,但索引3作为链接10…索引9作为链接16。
应该参考OpenRAVE邮件列表和ROS答案,以获得关于5和7自由度操纵器的信息。

八.Create Plugin
创建包含IK插件的包。建议将包命名为"$MYROBOT_NAME"_ikfast_"$ PLANNING_GROUP"_plugin

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

构建工作空间,以便检测新包[可以是roscd]:

catkin build

创建插件源代码:

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

或没有ROS:

python /path/to/create_ikfast_moveit_plugin.py "$MYROBOT_NAME" "$PLANNING_GROUP" "$MOVEIT_IK_PLUGIN_PKG" "$IKFAST_OUTPUT_PATH"

九.Usage
IKFast插件应该与默认的KDL IK求解器功能相同,但是性能大大提高。MoveIt配置文件由moveit_ikfast脚本自动编辑,但是可以使用机器人运动学中的kinematics_solver参数在KDL和IKFast求解器之间进行切换。yaml文件:

rosed "$MYROBOT_NAME"_moveit_config kinematics.yaml

编辑这些部分:

:
  kinematics_solver: __kinematics/IKFastKinematicsPlugin
-INSTEAD OF-
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin

1.Test the Plugin
使用MoveIt RViz运动规划插件和使用交互式标记,看看是否找到了正确的IK解决方案。

十.Updating the Plugin
如果MoveIt或者IKFast将来有任何变化,可能需要使用脚本重新生成这个插件。为了能够轻松地做到这一点,在IKFast包的根目录下自动创建了一个bash脚本,名为update_ikfast_plugin.sh。这与之前手动执行的操作相同,但是使用的是复制到ROS包中的IKFast解决方案头文件。

参考文献:
[1]IKFast Kinematics Solver:http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/ikfast/ikfast_tutorial.html

你可能感兴趣的:(计算机视觉,ikfast,solver,openrave,ros,moveit)