一、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
再繼續修改