baxter使用moveit+kdl+rvizplugin進行拖拽規劃時出現completed listing of explantions for invalid states規劃錯誤解決方法

實際上這幾天一直在學習baxter的基本操作方法，發現baxter真的是一款很好的機器人，官方給提前配置好了很多東西，使用起來很友善。雖然如此，但是在對其進行ikfast插件配置的過程中仍然艱辛滿滿，其實還是手生。

這幾天的摸索流程是這樣的

1. 按照官方教程http://sdk.rethinkrobotics.com/wiki/MoveIt_Tutorial指導的，對baxter直接進行了ikfast的配置，實際上主要是修改了/home/zzurobotics/baxter_ws/src/moveit_robots/baxter/baxter_moveit_config/config路徑下的kinematics.yaml檔案，通過屏蔽和選擇不同的解算器來達到了選擇ikfast解算器的目的，當時以為挺簡單的，實際上官方已經把幾乎所有的檔案都配置好了，你直接用就成，但是我一直不知道到底怎樣才算是确定目前改變後是用了ikfast的解算器，這裡說明一下我看了YouTube上的相關視訊，發現，當yaml檔案等一些列設定好之後，moveit rviz插件面闆中不會有任何提示，還是會顯示ompl庫，在面闆中還是顯示原來的算法，但是實際上已經變成了ikfast。
2. 查找過程中發現http://sdk.rethinkrobotics.com/wiki/Custom_IKFast_for_your_Baxter可根據自己的baxter定制一個ikfast插件，我就又對着教程搞了一遍，這個教程很好，在做自己的ikfast插件的時候也可以參考這個教程進行配置。但是這個過程确實很麻煩，計算ikfast_solver的時候很慢，一條胳膊的計算時間大約2個小時左右，很麻煩。配置來配置去還總是報錯，這裡再提一下，自動生成的baxter_right_arm_ikfast_solver.cpp、baxter_left_arm_ikfast_solver.cpp這兩個檔案裡的#include“ikfast.h”會報錯找不到，需要把具體路徑填進去，而且，裡面還需要手動為兩個變量添加空間字首std::才能夠編譯成功，否側會報錯，這個問題按下不表。
3. 最後折騰來折騰去，想着算了，先用預設的kdl吧，然後就改回來原來的配置想着先用着，重頭戲來了，結果拖拽末端執行器總是規劃失敗提示錯誤completed listing of explantions for invalid states。查來查去，終于解決了，記錄一下方法。

這個問題出現的原因是moveit在執行規劃的時候會考慮baxter的碰撞問題，包括自碰撞和障礙物碰撞，由于一些參數的配置問題導緻moveit認為規劃的結果總是會導緻自碰撞。原因是在考慮碰撞的時候會考慮模型建立不準确啥的原因，會在模型周圍多出一定距離進行碰撞檢測。而我遇到的問題原因猜測是因為兩隻手都安裝了電動夾具，夾具的兩個夾柱可能會認為自碰撞。這個參數查了之後參考http://groups.google.com/forum/#!topic/moveit-users/tMOL-GmwCGw （可能要翻牆）說修改/home/zzurobotics/baxter_ws/src/moveit_robots/baxter/baxter_moveit_config\config路徑下的ompl_planning.yaml檔案

打開之後

planner_configs:
  SBLkConfigDefault:
    type: geometric::SBL
  LBKPIECEkConfigDefault:
    type: geometric::LBKPIECE
  RRTkConfigDefault:
    type: geometric::RRT
  //range:.2      //參考連結中甚至指出添加這條語句（planner_configs到left_arm之間每塊都加）目的是為了産生更小的step，但是我沒有添加，也能正常使用
  RRTConnectkConfigDefault:
    type: geometric::RRTConnect
  LazyRRTkConfigDefault:
    type: geometric::LazyRRT
  ESTkConfigDefault:
    type: geometric::EST
  KPIECEkConfigDefault:
    type: geometric::KPIECE
  RRTStarkConfigDefault:
    type: geometric::RRTstar
  BKPIECEkConfigDefault:
    type: geometric::BKPIECE
left_arm:
  default_planner_config: RRTConnectkConfigDefault
  planner_configs:
    - SBLkConfigDefault
    - LBKPIECEkConfigDefault
    - RRTkConfigDefault
    - RRTConnectkConfigDefault
    - ESTkConfigDefault
    - KPIECEkConfigDefault
    - BKPIECEkConfigDefault
    - RRTStarkConfigDefault
  projection_evaluator: joints(left_s0,left_s1)
  longest_valid_segment_fraction: 0.05    //這裡和下面的0.05修改為0.02
right_arm:
  default_planner_config: RRTConnectkConfigDefault
  planner_configs:
    - SBLkConfigDefault
    - LBKPIECEkConfigDefault
    - RRTkConfigDefault
    - RRTConnectkConfigDefault
    - ESTkConfigDefault
    - KPIECEkConfigDefault
    - BKPIECEkConfigDefault
    - RRTStarkConfigDefault
  projection_evaluator: joints(right_s0,right_s1)
  longest_valid_segment_fraction: 0.05   //修改為0.02
both_arms:
  default_planner_config: RRTConnectkConfigDefault   //注意這個，修改的話就可以自定義預設的算法，                                                      
  planner_configs:                                   //就不用每次都手動選擇修改了。
    - SBLkConfigDefault
    - LBKPIECEkConfigDefault
    - RRTkConfigDefault
    - RRTConnectkConfigDefault
    - ESTkConfigDefault
    - KPIECEkConfigDefault
    - BKPIECEkConfigDefault
    - RRTStarkConfigDefault
  projection_evaluator: joints(right_s0,right_s1)
  longest_valid_segment_fraction: 0.05  //修改為0.02
left_hand:
  default_planner_config: RRTConnectkConfigDefault
  planner_configs:
    - SBLkConfigDefault
    - LBKPIECEkConfigDefault
    - RRTkConfigDefault
    - RRTConnectkConfigDefault
    - ESTkConfigDefault
    - KPIECEkConfigDefault
    - BKPIECEkConfigDefault
    - RRTStarkConfigDefault
right_hand:
  default_planner_config: RRTConnectkConfigDefault
  planner_configs:
    - SBLkConfigDefault
    - LBKPIECEkConfigDefault
    - RRTkConfigDefault
    - RRTConnectkConfigDefault
    - ESTkConfigDefault
    - KPIECEkConfigDefault
    - BKPIECEkConfigDefault
    - RRTStarkConfigDefault
           

猜測是夾爪兩根柱本身就很近，大約5cm也就是0.05，所有很有可能會産生誤判，修改為檢測範圍2cm即0.02，就不會誤以為碰撞了。

太晚了，準備睡覺，再說一下預設使用的RRTConnectkConfigDefault感覺很挺好用的，結算時間在0.006s之類的範圍，幾乎是瞬解，先用着這個。詳細的以後再補充吧