MoveIt!运动学插件IKFAST(八)
一、MoveIt!运动学插件IKFAST
实现了IK_FAST逆运动学求解。IK_FAST可以在ABB机械臂还有古月居的机械臂上面跑通。
完成了IK_FAST的可视化效果Openrave。通过大量的尝试,在网上寻找了许多的解决办法,仍然没有成功,最后我分析了源码包中的CMakeList文件,终于发现了问题。通过修改CMakeList文件当中的查找Boost版本语句,将其改为Boost自定义安装的路径,最后终于成功安装。
1.KDL
Kinematics and Dynamics Library (KDL)是MoveIt!中的默认运动学插件,在使用MoveIt! Setup Assistant进行模型配置时,可以进行配置。KDL的优点是可以求解封闭情况下你运动学;但是速度慢、可能找不到解。
多时候我们在做运动规划的时候,MoveIt!经常会提示规划失败、求解失败等错误,很多都是因为KDL这款运动学插件导致的,所以,需要更换一个运动学插件。
2.TRAC-IK
将KDL更换成TRAC-IK呢,方法很简单,ROS的软件源中已经集成了TRAC-IK的安装包,可以直接使用以下命令安装:
然后修改机械臂MoveIt!配置功能包下的kinematics.yaml文件就可以使用啦:
1 arm: 2 kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin 3 kinematics_solver_attempts: 3 4 kinematics_solver_search_resolution: 0.005
接下来再次运行demo.launch,默认加载的就是TRAC-IK运动学插件了,试试规划求解的效率是不是高了很多!
IK_Fast比默认的KDL和现在用的Trac_IK安装起来也更麻烦。
查看boost的版本:
1 dpkg -S /usr/include/boost/version.hpp
3. IKFAST
IKFAST是一种基于解析算法的运动学插件,可以保证每次求解的一致性。IKFAST,机器人运动学的编译器,在Rosen Diankov OpenRAVE运动规划软件提供,是一个强大的逆运动学求解器。不像大多数的逆运动学求解,IKFAST可以求解任意复杂运动链的运动学方程,并产生特定语言的文件(如C++)后供使用。最终的结果是非常稳定的解决方案。
4.MoveIt! IKFast
MoveIt! IKFast是一种可利用OpenRAVE生成的cpp文件来生成 IKFast 运动学插件的工具。
MoveIt! IKFast测试环境是ROS Groovy,使用catkin编译,使用带有6dof和7dof的机器臂的OpenRave 0.8 。
理论上可以工作在任何自动度的机械臂,但目前 IKFast插件生成器不能工作在大于7自动度的机械臂上。
1.环境配置
(1)安装程序
1 sudo apt-get install cmake g++ git ipython minizip python-dev python-h5py python-numpy python-scipy qt4-dev-tools
(2)安装依赖库
1 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
(3)安装OpenSceneGraph-3.4
1 sudo apt-get install libcairo2-dev libjasper-dev libpoppler-glib-dev libsdl2-dev libtiff5-dev libxrandr-dev 2 git clone https://github.com/openscenegraph/OpenSceneGraph.git --branch OpenSceneGraph-3.4 3 cd OpenSceneGraph 4 mkdir build; cd build 5 cmake .. -DDESIRED_QT_VERSION=4 6 make -j$(nproc) 7 sudo make install
(4)安装灵活的碰撞库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(注意版本,若版本不对会产生错误)
1 pip install --upgrade --user sympy==0.7.1
删除mpmath
1 sudo apt remove python-mpmath
安装MoveIt! IKFast
1 sudo apt-get install ros-melodic-moveit-kinematics
安装OpenRAVE(采用的源码安装) 安装了2天,很多报错并且缓慢
1 git clone --branch latest_stable https://github.com/rdiankov/openrave.git 2 cd openrave && mkdir build && cd build 3 cmake -DODE_USE_MULTITHREAD=ON -DOSG_DIR=/usr/local/lib64/ .. 4 make -j$(nproc) 5 sudo make install
注意:不建议采用sudo apt-get install ros-melodic-openrave,会一并安装mpmath,导致后面生成IKFAET文件失败,因此采用源码安装的方式。
openrave装了很久都没装好,可以查考下面这个网站:https://github.com/crigroup/openrave-installation
下载脚本后,按顺序执行命令即可:
1 ./install-dependencies.sh 2 ./install-osg.sh 3 ./install-fcl.sh 4 ./install-openrave.sh
前面两条脚本都运行正确。但是./install-openrave.sh还是报错,尝试了许多方法,还是没有解决。
参考博客:https://blog.csdn.net/weixin_40512647/article/details/105719908
之后采用以下方法:
1 sudo apt install git # probably already installed 2 sudo apt install libboost-filesystem-dev libboost-system-dev libboost-python-dev libboost-thread-dev libboost-iostreams-dev libboost-numpy-dev 3 sudo apt install libqt4-dev qt4-dev-tools libxml2-dev libode-dev 4 sudo apt install libsoqt4-dev libcoin80-dev 5 sudo apt install liblapack-dev 6 sudo apt install libcollada-dom2.4-dp-dev # Open .zae files, avoid cmake 3.19 error on 18.04 Bionic 7 cd # go home 8 mkdir -p repos; cd repos # create $HOME/repos if it doesn't exist; then, enter it 9 git clone --branch master https://github.com/rdiankov/openrave.git 10 cd openrave; mkdir build; cd build 11 cmake .. -DOPT_VIDEORECORDING=OFF -DOPT_PYTHON=OFF 12 make -j$(nproc) 13 sudo make install; cd # install and go home
问题:
python/bindings/CMakeFiles/pyANN_int.dir/build.make:86: recipe for target 'python/bindings/CMakeFiles/pyANN_int.dir/bindings.cpp.o' failed make[3]: * [python/bindings/CMakeFiles/pyANN_int.dir/bindings.cpp.o] Error 1 make[3]: 离开目录“/home/wei/repos/openrave/build” CMakeFiles/Makefile2:1199: recipe for target 'python/bindings/CMakeFiles/pyANN_int.dir/all' failed make[2]: * [python/bindings/CMakeFiles/pyANN_int.dir/all] Error 2
还是报错:
脚本安装方式和以上这种方式的报错都是一样的,主要是boost问题,我尝试删除boost,重新安装boost。
自己摸索出来:CMakeList.txt里面修改了动态链接库的路径,cmake .. -DOPT_VIDEORECORDING=OFF -DOPT_PYTHON=OFF:
运行 :make -j$(nproc)的时候,磁盘60G满了。。。
5、配置IKFAST插件
(1)用OpenRave创建Collada碰撞文件
相关参数
-
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
配置流程
参考博客:https://blog.csdn.net/Kalenee/article/details/80740258
(1)设置机器人名字
1 export MYROBOT_NAME="robot_arm"
(2)若机器人模型为xacro格式需先转为urdf格式
1 osrun xacro xacro --inorder -o "$MYROBOT_NAME".urdf arm.xacro
(3)机器人模型urdf格式转换为dae格式
1 rosrun collada_urdf urdf_to_collada "$MYROBOT_NAME".urdf "$MYROBOT_NAME".dae
(4)设置精度为小数点后5位,然后保留备份后重新设置dae格式机器人模型描述文件的精度
1 export IKFAST_PRECISION="5" 2 cp "$MYROBOT_NAME".dae "$MYROBOT_NAME".backup.dae # create a backup of your full precision dae. 3 rosrun moveit_kinematics round_collada_numbers.py "$MYROBOT_NAME".dae "$MYROBOT_NAME".dae "$IKFAST_PRECISION"
设置的精度会影响IKFAST的生成,设置过小会导致无法生成或求解精度过低,过大也会导致无法生成或者求解变慢,可以根据需求调整精度。
(5)查看模型
查看模型关节数据
1 openrave-robot.py "$MYROBOT_NAME".dae --info links
查看模型三维结构
1 openrave "$MYROBOT_NAME".dae
(6)选择IK项(默认Transform6D )
http://openrave.org/docs/latest_stable/openravepy/ikfast/#ik-types
(7)设置运动规划组
1 export PLANNING_GROUP="robot_arm"
(8)设置运动规划的关节组,以上面的模型关节数据为基础设置
1 export BASE_LINK="0" 2 export EEF_LINK="8"
(9)若关节数量大于6需设置一自由关节,若无则无需设置
1 export FREE_INDEX="1"
(10)设置IKFAST输出路径
1 export IKFAST_OUTPUT_PATH=`pwd`/ikfast61_"$PLANNING_GROUP".cpp
(11)生成IKFAST文件
-
6轴
1 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轴
1 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分钟左右,若时间过长有几率代表失败(即使成功生成),生成失败的话可以降低精度或者需要修改模型本身。
生成插件
1 export MOVEIT_IK_PLUGIN_PKG="$MYROBOT_NAME"_ikfast_"$PLANNING_GROUP"_plugin 2 cd ~/catkin_ws/src 3 catkin_create_pkg "$MOVEIT_IK_PLUGIN_PKG" 4 catkin build 5 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配置生成的功能包。
1 plan_pkg = robot_name + '_moveit_config' 2 plan_pkg_dir = roslib.packages.get_pkg_dir(plan_pkg) 3 print 'Loading robot from \''+plan_pkg+'\' package ... '
问题1:
1 libdvd-pkg: `apt-get check` failed, you may have broken packages. Aborting...
解决办法:
1 sudo dpkg-reconfigure libdvd-pkg
问题2:
1 collect2: error: ld returned 1 exit status 2 test/CMakeFiles/test_fcl_shape_mesh_consistency.dir/build.make:121: recipe for target 'test/test_fcl_shape_mesh_consistency' failed 3 make[2]: *** [test/test_fcl_shape_mesh_consistency] Error 1 4 CMakeFiles/Makefile2:216: recipe for target 'test/CMakeFiles/test_fcl_shape_mesh_consistency.dir/all' failed 5 make[1]: *** [test/CMakeFiles/test_fcl_shape_mesh_consistency.dir/all] Error 2 6 Makefile:140: recipe for target 'all' failed 7 make: *** [all] Error 2
解决办法:
安装libccd:https://github.com/danfis/libccd
问题3:
boost卸载和重新安装
解决:
https://stackoverflow.com/questions/8430332/uninstall-boost-and-install-another-version
问题4:
1 [rospack] Error: package 'collada_urdf' not found
解决:
1 sudo apt-get install ros-melodic-collada-urdf
参考博客:https://github.com/ros/collada_urdf/issues/31
问题5:
1 -- Configuring incomplete, errors occurred! 2 See also "/home/wei/git/openrave/build/CMakeFiles/CMakeOutput.log". 3 See also "/home/wei/git/openrave/build/CMakeFiles/CMakeError.log". 4 make: *** 没有指明目标并且找不到 makefile。 停止。
解决:
之前已经配置过IKfast插件,但是在用到marm_moveit_config这个包时,出现了许多错误。
问题1:using empty link_prefix
问题2: link "10" not found in model "arm"
前提
为了解决这些问题,查询了大量的资料和尝试了许许多多的方法,例如通过"setup_assistant",由marm.xacro文件重新生成配置文件marm_moveit_config_wwd,在kinematics.yaml
文件中修改kinematics_solver
为marm_marm/IKFastKinematicsPlugin
,然后运行roslaunch marm_moveit_config_wwd demo.launch
,依旧报错。起初我以为是因为group_name和ikfast生成时使用的group_name
不一样。于是使用之前的方法重新生成一个ikfast插件,再次运行roslaunch
仍然报错。
解决方法
经过一次次失败,无论是重新生成新的配置文件marm_moveit_config
还是创建新的IKfast
插件,都会报那几个鲜红的错误。也使用了Google进行搜索这些Error,期待别人也遇到相同的错误,可惜事与愿违。于是我仔细的分析了这些鲜红的错误,尝试自己解决。看了许久,终于在这些Error当中发现了线索,其中有一条语句很奇怪==Link "10" not found in model " arm"==,这条语句让我受到了启发,让我想起来了之前创建IKfast
插件时,运行了以下语句
1 rosrun moveit_kinematics create_ikfast_moveit_plugin.py "$MYROBOT_NAME" "$PLANNING_GROUP" "$MOVEIT_IK_PLUGIN_PKG" "$BASE_LINK" "$EEF_LINK" "$IKFAST_OUTPUT_PATH"
其中$BASE_LINK=0,$EEF_LINK=10。于是就猜想 很大可能是这两个参数的问题。因为官网没有指明这两个参数应该传的什么,而且这两个参数又是必需的,当时就以为是
1 export BASE_LINK="0" 2 export EEF_LINK="10"
所以传参进去的就是0和10。这会儿是不是明白了,报的错误是"Link 10"没有被找到,指的就是这个参数10。为了确定这两个参数指的什么,我就去查看了创建插件的源码"create_ikfast_moveit_plugin.py",一下子就明白了,原来这两个参数是"base_link"的名称"base_link"和End Effectors的name“grasping_frame”。有了这个想法就好办了。再次重新生成IKfast插件就行。
本次重新生成IKfast插件的步骤如下:
## 默认已经准备好了arm.dae文件
export BASE_LINK="0" export EEF_LINK="10" export IKFAST_OUTPUT_PATH=`pwd`/ikfast61_arm.cpp python `openrave-config --python-dir`/openravepy/_openravepy_/ikfast.py --robot=arm.dae --iktype=transform6d --baselink="$BASE_LINK" --eelink="$EEF_LINK" --savefile="$IKFAST_OUTPUT_PATH" export MOVEIT_IK_PLUGIN_PKG=arm_ikfast_arm_plugin cd ~/catkin_ws/src catkin_create_pkg "$MOVEIT_IK_PLUGIN_PKG"
## 这里是关键,也是容易出错的地方
1 rosrun moveit_kinematics create_ikfast_moveit_plugin.py "marm" "arm" "$MOVEIT_IK_PLUGIN_PKG" "base_link" "grasping_frame" "$IKFAST_OUTPUT_PATH"
### 运行完这条命令可能会报错(提示 "arm"没有在marm.dae文件中定义),这时候只需要将/marm_moveit_config/config/marm.dae文件里的group名称、home所属group名称以及forward所属的group名称均改为"arm"即可。
重新创建ikfast包的时候要修改marm.srdf文件:
安装完IKfast插件后,再次运行
1 roslaunch marm_moveit_config demo.launch
这时候可能会出现一些错误
这是因为安装好IKfast后,工作空间没有重新编译,只要重新编译,生成链接库就行
1 cd ~/catkin 2 catkin_make
配置文件kinematics.yaml里面的IKfast修改效果:
开门任务的逆运动学求解器IKFast的实现效果:
![](https://img2020.cnblogs.com/blog/1343253/202106/1343253-20210623174207286-23741119.png)
二、虚拟关节,移动底盘问题
轮式移动机械臂软件设计:
1.编写URDF文件,建立各连杆与关节的连接关系;
1)包含机械臂各零件部件外观、物理属性和关节类型等方面信息。
2)关节坐标系的位姿以父连杆坐标系为参考,而子连杆坐标系参考于关节坐标系,一般情况下与关节坐标系一致。
2.使用MoveIt!设置助手加载URDF文件。对轮式移动机械臂运动规划进行配置,生成基本的配置与启动文件;
3.根据自己的功能需求开发节点,基于MoveIt!框架对机械臂进行运动规划。
参考博客:https://blog.csdn.net/weixin_44381217/article/details/109680817
问题:
1 The following directory should be added to compiler include paths:
2
3 /home/wei/boost_1_58_0
4
5 The following directory should be added to linker library paths:
6
7 /home/wei/boost_1_58_0/stage/lib
解决:
可以先查看boost版本
1 dpkg -S /usr/local/include/boost/version.hpp
再继续修改