CHOMP Planner

chomp運動規劃demo

運動規劃的協變哈密頓優化 (CHOMP) 是一種基于梯度的軌跡優化程序，它使許多日常運動規劃問題既簡單又可訓練（Ratliff 等，2009c）。 雖然大多數高維運動規劃器將軌跡生成分為不同的規劃和優化階段，但該算法在優化階段利用協變梯度和函數梯度方法來設計完全基于軌跡優化的運動規劃算法。 給定不可行的初始軌跡，CHOMP 會對周圍環境做出反應，以快速將軌跡從碰撞中拉出，同時優化關節速度和加速度等動力學量。 它迅速收斂到可以在機器人上高效執行的平滑無碰撞軌跡。 正在集成到最新版本的 MoveIt 中。 更多信息

Getting Started

如果您還沒有這樣做，請確保您已完成入門中的步驟。

您還應該完成了使用 MoveIt RViz 插件進行可視化中的步驟

Prerequisites

在 ROS Melodic 上，您不需要從源代碼構建 MoveIt，但對于舊版本的 MoveIt，您需要（請參閱以前的教程版本）。

要在您的機器人上使用 CHOMP，您必須已經為您的機器人安裝了 MoveIt 配置包。 例如，如果你有一個 Panda 機器人，它就叫做 panda_moveit_config。 這通常使用 MoveIt 設置助手進行配置。

Using CHOMP with Your Robot

注意：如果您使用 ros-planning/panda_moveit_config 存儲庫中的 panda_moveit_config 來進行此演示，則這些步驟已經為您完成，您可以跳過本節。

只需將 chomp_planning_pipeline.launch.xml 文件下載到 MoveIt 配置包的launch目錄中。 在我們的例子中，我們將把這個文件保存在 panda_moveit_config/launch 目錄中。

將 <rosparam command="load" file="$(find panda_moveit_config)/config/chomp_planning.yaml" /> 調整為 <rosparam command="load" file="$(find <robot_moveit_config>)/config/chomp_planning. yaml" /> 將 <robot_moveit_config> 替換為您的 MoveIt 配置包的名稱。

將 chomp_planning.yaml 文件下載到 MoveIt 配置包的配置目錄中。 在我們的例子中，我們將把這個文件保存在 panda_moveit_config/config 目錄中。

在您喜歡的編輯器中打開 chomp_planning.yaml 并將 animate_endeffector_segment: "panda_rightfinger" 更改為適合您的機器人的連桿。

Running the Demo

如果你有來自 ros-planning/panda_moveit_config 存儲庫的 panda_moveit_config 你應該能夠簡單地運行演示：

# roslaunch panda_moveit_config demo.launch pipeline:=chomp

roslaunch moveit_resources_panda_moveit_config demo.launch pipeline:=chomp

Running CHOMP with Obstacles in the Scene

要在有障礙物的環境中運行 CHOMP，您可以運行示例 Python 腳本：

collision_scene_example.py.

該腳本根據提供給腳本的參數創建一個帶有四個障礙物的雜亂場景或一個帶有一個障礙物的簡單場景。 還可以改變障礙物的位置/大小來改變場景。

要在有障礙的情況下運行 CHOMP 規劃器，請打開兩個shell。 在第一個 shell 中啟動 RViz 并等待一切完成加載：

roslaunch moveit_resources_panda_moveit_config demo.launch pipeline:=chomp

在第二個 shell 中，運行以下兩個命令之一：

rosrun moveit_tutorials collision_scene_example.py cluttered

or:

rosrun moveit_tutorials collision_scene_example.py sparse

接下來，在 RViz 中，在 Context 選項卡下的 MotionPlanning 面板中選擇 CHOMP。 通過使用 imarker 移動末端執行器來設置所需的開始和目標狀態，然后單擊 MotionPlanning 面板中規劃選項卡下的規劃按鈕開始規劃。 規劃器現在將嘗試在給定的開始和結束位置之間找到可行的解決方案。

Tweaking some of the parameters for CHOMP

調整 CHOMP 的一些參數

CHOMP 有一些與之相關的優化參數。 這些可以針對您正在使用的給定環境/機器人進行修改，并且通常存在于您正在使用的機器人的 config 文件夾中的 chomp_planning.yaml 文件中。 如果您的機器人不存在此文件，您可以創建它并根據需要設置參數值。 以下是為其中一些設置這些參數值的一些見解：

Planning_time_limit: 優化器在終止前找到解決方案的最長時間

max_iterations: 這是規劃器在優化時找到一個好的解決方案可以采取的最大迭代次數

max_iterations_after_collision_free: 找到無碰撞路徑后要執行的最大迭代次數。

smoothness_cost_weight: 參數控制其在 CHOMP 實際優化的最終成本中的權重。

obstacle_cost_weight:這控制著給予障礙物的權重，以達到 CHOMP 優化的最終成本。例如，0.0 將有障礙被忽略，1.0 將是一個硬約束

learning_rate:這是優化器用來尋找局部/全局最小值同時降低總成本的學習率。

smoothness_cost_velocity、smoothness_cost_acceleration、smoothness_cost_jerk：與速度、加速度和加加速度成本相關的變量。

ridge_factor: 添加到目標函數中總二次成本矩陣的對角線上的噪聲。添加小噪聲（例如 0.001）允許 CHOMP 以軌跡平滑為代價避開障礙物。

use_pseudo_inverse: :: 是否啟用偽逆計算。

pseudo_inverse_ridge_factor: :: 如果啟用了偽逆，則設置脊因子。

Joint_update_limit: :: 設置機器人關節的更新限制

collision_clearance: :: 避開障礙物需要保持的最小距離。

collision_threshold: :: 需要維護以避免碰撞的碰撞閾值成本

use_stochastic_descent: :: 如果您想在優化成本的同時使用隨機下降，請將其設置為 true/false。在隨機下降中，使用軌跡中的一個隨機點，而不是所有軌跡點。這更快并保證收斂，但在最壞的情況下可能需要更多的迭代。

enable failure recovery：:: 如果設置為 true，CHOMP 會調整 ceratin 參數，希望在 chomp_planning.yaml 文件中指定的默認參數不存在時找到解決方案。

max_recovery_attempts: :: 這是首次嘗試使用默認參數后，CHOMP 使用不同參數集運行的最大次數。

trajectory_initializaiton_method: :: 可以在此處為 CHOMP 提供軌跡的初始化類型，可以是五次樣條、線性、三次或填充軌跡。前三個選項是指用于開始和目標狀態之間軌跡初始化的插值方法。 fillTrajectory 提供了從現有運動規劃器（如 OMPL）計算出的路徑初始化軌跡的選項。

為 CHOMP 選擇參數需要一些由規劃環境告知的直觀intuition?。 例如，CHOMP 的默認參數在沒有障礙物的環境中運行良好； 然而，在有許多障礙物的環境中，默認參數可能會導致 CHOMP 卡在局部最小值中。 通過調整參數，我們可以提高 CHOMP 生成的規劃的質量。

一些未使用/注釋的參數是：

hmc_stochasticity

hmc_annealing_factor

hmc_discretization

use_hamiltonian_montecarlo

animate_endeffector

animate_endeffector_segment

animate_path

random_jump_amount

add_randomness

Difference between plans obtained by CHOMP and OMPL

優化規劃器優化的成本函數有時可能會導致令人驚訝的結果：穿過薄障礙物的成本可能低于避免所有碰撞的長而蜿蜒的軌跡。在本節中，我們將對從 CHOMP 獲得的路徑進行區分，并將其與從 OMPL 獲得的路徑進行對比。

OMPL 是一個開源庫，用于基于采樣的/隨機運動規劃算法。基于采樣的算法在概率上是完整的：如果存在，最終會找到解決方案，但是無法報告不存在解決方案。這些算法是有效的，通常很快就能找到解決方案。 OMPL 不包含任何與碰撞檢查或可視化相關的代碼，因為 OMPL 的設計者不想將其綁定到任何特定的碰撞檢查器或可視化前端。該庫的設計使其可以輕松集成到提供附加組件的系統中。 MoveIt 直接與 OMPL 集成，并使用來自 OMPL 的運動規劃器作為其默認規劃器集。 OMPL 中的規劃器是抽象的；即 OMPL 沒有機器人的概念。相反，MoveIt 配置 OMPL 并為 OMPL 提供后端以處理機器人技術中的問題。

CHOMP：雖然大多數高維運動規劃器將軌跡生成分為不同的規劃和優化階段，但 CHOMP 在優化階段利用協變梯度和函數梯度方法來設計完全基于軌跡優化的運動規劃算法。給定不可行的初始軌跡，CHOMP 會對周圍環境做出反應，以快速將軌跡從碰撞中拉出，同時優化關節速度和加速度等動力學量。它迅速收斂到可以在機器人上高效執行的平滑無碰撞軌跡。協變更新規則確保 CHOMP 快速收斂到局部最優軌跡。

對于包含障礙物的場景，CHOMP 通常通過在機器人動態量（如加速度、速度）的成本函數中添加一些噪聲（ridge_factor）來生成不喜歡平滑軌跡的路徑。在大多數情況下，CHOMP 能夠避開障礙物，但如果由于對軌跡的初始猜測錯誤而陷入局部最小值，則它可能會失敗。 OMPL 可用于為 CHOMP 生成無碰撞種子軌跡以緩解此問題。

Using CHOMP as a post-processor for OMPL

使用 CHOMP 作為 OMPL 的后處理器

在這里，證明了 CHOMP 也可以用作其他規劃算法獲得的規劃的后處理優化技術。 這背后的直覺是一些隨機規劃算法產生了對 CHOMP 的初始猜測。 CHOMP 然后采用這個初始猜測并進一步優化軌跡。 為此，請按照以下步驟操作：

在機器人的 <robot_moveit_config>/launch 文件夾中創建一個新的啟動文件 ompl-chomp_planning_pipeline.launch ，內容如下：

<launch><!-- load OMPL planning pipeline, but add the CHOMP planning adapter. --><include file="$(find panda_moveit_config)/launch/ompl_planning_pipeline.launch.xml"><arg name="planning_adapters" value="default_planner_request_adapters/AddTimeParameterizationdefault_planner_request_adapters/FixWorkspaceBoundsdefault_planner_request_adapters/FixStartStateBoundsdefault_planner_request_adapters/FixStartStateCollisiondefault_planner_request_adapters/FixStartStatePathConstraintschomp/OptimizerAdapter"/></include><!-- load chomp config --><rosparam command="load" file="$(find panda_moveit_config)/config/chomp_planning.yaml"/><!-- override trajectory_initialization_method --><param name="trajectory_initialization_method" value="fillTrajectory"/></launch>

這個啟動文件定義了新的規劃管道 ompl-chomp，派生自 ompl 管道，但添加了 CHOMP 后處理器作為規劃適配器。 此外，trajectory_initialization_method 被覆蓋以使用 OMPL 生成的軌跡。

現在您可以按如下方式啟動新配置的規劃管道：

# roslaunch panda_moveit_config demo.launch pipeline:=ompl-chomp

roslaunch moveit_resources_panda_moveit_config demo.launch pipeline:=ompl-chomp

這將啟動 RViz，在 Context 選項卡下的 Motion Planning 面板中選擇 OMPL。 通過以與上述 CHOMP 相同的方式移動末端執行器來設置所需的開始和目標狀態。 最后點擊規劃按鈕開始規劃。 規劃器現在將首先運行 OMPL，然后在 OMPL 的輸出上運行 CHOMP 以生成優化路徑。

總結

以上是生活随笔為你收集整理的【neotic-moveit】CHOMP Planner的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。