MoveIt! 學習筆記10- Planning with Approximated Constraint Manifolds

本博文轉載自https://blog.csdn.net/ssw_1990/article/details/104048403；

由於目前正處在學習階段，對教程中提到的知識不是很確定含義，待以後用到的時候再新增補充。

OMPL支援自定義約束，以支援遵循期望行為的規劃軌跡。約束可以在關節空間和笛卡兒空間中定義，笛卡兒空間可以是基於方向的，也可以是基於位置的。在規劃軌跡時，每個關節狀態都需要遵循所有設定的約束，預設情況下通過拒絕取樣來執行。然而，這可能會導致非常長的計劃時間，特別是當約束非常嚴格且廢品率相應很高時。
Sucan等人提出了一種方法，他們預先計算約束流形的近似值，並據此進行軌跡規劃。OMPL外掛包含對給定約束集執行此操作並將其儲存到資料庫中的功能。在以後的例項中，可以使用任何OMPL計劃器載入資料庫來進行約束規劃，這大大減少了規劃時間。
本教程包括如何構建約束近似資料庫以及如何將其用於約束軌跡規劃的步驟。有關如何使用路徑約束進行總體規劃的更多資訊，請參見此處。

一.Creating the Constraint Database
在demo_construct_state_database.cpp中可以找到如何從約束構造近似資料庫的示例實現。主要的功能是在ConstraintsLibrary類中實現的。
通過呼叫addConstraintApproximation()來新增約束，隨後可以呼叫它來在逼近中包含多個約束。該函式需要四個引數：

• 約束訊息(moveit_msgs:: constraints)
• 機器人描述(std::string)
• 規劃場景(planning_scene::PlanningScenePtr)
• 構造選項(ompl_interface:: constraintationconstructionoptions)

機器人描述是移動組的名稱，規劃場景與相應的機器人模型一起初始化。約束訊息和選項的初始化將在下面進行說明。
1.Constraints message
可以使用任何型別和所需的容差像往常一樣初始化constraints訊息物件。關鍵的一點是，訊息的名稱應該是描述性的，並且對於約束是惟一的。在使用逼近資料庫進行規劃時，將使用該名稱來引用正確的約束。
2.Construction Options
constraintationconstructionoptions物件指定逼近流形的各種特性，例如例項大小、密度、空間引數化型別和其它。以下是每個選項的概述：

• unsigned int samples - 近似圖的大小
• unsigned int edges_per_sample - 近似圖的自由度
• double max_edge_length - 用於邊緣插入的距離閾值
• bool explicit_motions - 定義邊緣是否應遵循約束
• double explicit_points_resolution - 用於約束檢查的邊緣插值解析度
• unsigned int max_explicit_points - 檢查邊緣的最大點

[1]Graph size
顯然，穩定的規劃結果需要詳細的近似，因此，樣本越高，效能越可靠。然而，數值越大，資料庫的構建時間越長。找到合適的流形尺寸是一個高度依賴於約束條件的限制性的問題。對於大多數約束來說，使用範圍在1000到10000之間的值就足夠了，而不需要像本文中建議的那樣對更高的值進行明顯的改進。
[2]Edges
向流形新增邊是可選的，可以通過將edges_per_sample設定為0來禁用它。軌跡規劃在大多數情況下都是可行的，因為取樣過程只需要狀態就可以了。max_edge_length定義了兩個狀態之間允許新增邊緣的最大距離。通過將explicit_motion設定為true，還可以強制邊緣匹配約束，使它們表示相鄰狀態之間的有效路徑。這是有利的，特別是在非常稀疏的逼近，許多區域很難達到，因為絕對距離不是一個必要的可達性的措施。通過測試狀態對之間的線性內插點來檢查邊緣是否與約束匹配。這些內插點的數量被設定為explicit_points_resolution乘以邊緣長度，並受到max_explicit_points的限制。
新增邊緣極大地增加了資料庫的構建時間，而增加顯式動作檢查甚至會對其產生額外的影響。在對邊進行實驗時，請記住，edges_per_sample和max_edge_length值應該進行調整，以便總是有足夠多的狀態足夠接近以進行連線。這需要根據密度分析近似空間的大小，並適應狀態之間的實際距離。
3.Database Construction
將約束新增到ConstraintsLibrary物件後，可以通過呼叫saveApproximationConstraints()來構造資料庫，該約束只接受應該儲存資料庫的相對目錄。

二.Database Loading and Usage
通過設定ros引數，必須在move group節點啟動時載入constraints資料庫：

<param name="move_group/constraint_approximations_path" value="<path_to_database>"/>

• 1

要驗證資料庫是否已被找到，請檢查指定的約束是否顯示在日誌中。
對於規劃，只需像往常一樣初始化約束訊息，並將訊息名稱設定為用於構造資料庫的確切名稱。此外，還需要再次指定相同的值和容差，因為在預設情況下，規劃器只是在狀態上取樣，而不必在路徑規劃的插值期間遵循約束。沒有初始化約束的正確命名的約束訊息將使用資料庫，但因此可能導致無效的軌跡。

參考文獻：
[1]Planning with Approximated Constraint Manifolds：http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/planning_with_approximated_constraint_manifolds/planning_with_approximated_constraint_manifolds_tutorial.html