【neotic-moveit】CHOMP Planner

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 以生成优化路径。