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的实现效果：
二、虚拟关节，移动底盘问题
轮式移动机械臂软件设计：
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
再继续修改
