规划适配器

规划请求适配器（Planning Request Adapter）是 MoveIt 中的一个概念，可用于为运动规划器修改轨迹（包括预处理和后处理）。MoveIt 中已有的规划适配器示例包括 AddTimeParameterization、ValidateWorkspaceBounds、FixStartBounds、CheckStartStateCollision、CHOMPOptimizerAdapter 等。利用规划适配器，可以在一条流水线（pipeline）中组合使用多种运动规划算法，从而生成更稳健的运动规划。例如，一条典型的运动规划流水线可能先由 OMPL 生成初始路径，再由 CHOMP 进行优化，最终得到的路径往往优于单独使用 OMPL 或 CHOMP 的结果。同样，借助规划适配器的概念，还可以根据机器人所处的环境灵活混搭不同的运动规划器。本节提供了混搭使用不同运动规划器的分步教程，并介绍了在何种情况下应选择何种运动规划器。

开始之前

如果尚未完成，请先完成 Getting Started 中的步骤。

此外，你还应完成 Visualization with MoveIt RViz Plugin 中的步骤。

先决条件

1. 你必须安装最新版本的 MoveIt。在 ROS Melodic 上，需要从源码构建 MoveIt。这是因为 CHOMP 和 STOMP 尚未包含在官方发行版中，因此也不在二进制软件包中。下面我们将介绍具体步骤。
2. 要在你的机器人上使用规划适配器，你必须已经拥有该机器人的 MoveIt 配置包。例如，如果你有一台 Panda 机器人，该包很可能名为 panda_moveit_config。这通常是通过 MoveIt Setup Assistant 构建的。

从源码安装 MoveIt

当向工作空间添加或移除软件包时，需要清理工作空间并重新运行命令以安装新缺失的依赖项。首先清理工作空间，移除对系统级 MoveIt 安装的引用：

cd ~/ws_moveit/src
catkin clean

现在按照 MoveIt 主页上关于从源码安装 MoveIt Melodic 的说明进行操作。注意，你可以跳过 Prerequisites（先决条件） 部分，因为你应该已经拥有一个 Catkin 工作空间。

重新 source 环境设置文件：

source ~/ws_moveit/devel/setup.bash

将规划请求适配器与你的运动规划器一起使用

在本节中，我们将介绍混搭使用不同运动规划器的几种方式。本教程主要关注 OMPL、CHOMP 和 STOMP，因为它们是目前 MoveIt 唯一支持的规划器。

将 OMPL 作为 CHOMP 的预处理器运行

这里演示如何将 CHOMP 用作其他规划算法所得规划的后处理优化技术。其基本思路是：先由某种随机规划算法为 CHOMP 生成初始猜测，然后 CHOMP 在此基础上进一步优化轨迹。 要实现这一点，请按以下步骤操作：

1. 打开你的机器人 <robot_moveit_config>/launch 文件夹中的 ompl_planning_pipeline.launch 文件。对于 Panda 机器人，就是这个文件。编辑该启动文件，找到包含 <arg name="planning_adapters"> 的行，将其改为：

<arg name="planning_adapters"
     value="default_planning_request_adapters/AddTimeParameterization
            default_planning_request_adapters/ValidateWorkspaceBounds
            default_planning_request_adapters/CheckStartStateBounds
            default_planning_request_adapters/CheckStartStateCollision
            chomp/OptimizerAdapter" />

1. planning_adapters 的值定义了上述适配器的调用顺序，顺序非常重要。在 CHOMP 适配器内部，会先调用 OMPL，再调用 CHOMP 优化求解器，因此 CHOMP 会以 OMPL 计算出的初始路径为起点进行优化。

2. 找到包含 <rosparam command="load" file="$(find panda_moveit_config)/config/ompl_planning.yaml"/> 的行，在其后添加以下内容：

<rosparam command="load" file="$(find panda_moveit_config)/config/chomp_planning.yaml"/>

1. 以上修改会添加一个 CHOMP 优化适配器（CHOMP Optimization Adapter），并加载相应的 CHOMP 规划器参数。要在你自己的机器人上实现这一点，请将 panda_moveit_config 替换为你机器人的 <my_robot>_moveit_config。

2. 在你的机器人 <robot_moveit_config>/launch 文件夹中的 move_group.launch 文件里，确保默认规划器为 ompl。

3. 在你的机器人 <robot_moveit_config>/config 文件夹中的 chomp_planning.yaml 文件里，添加以下行：

trajectory_initialization_method: "fillTrajectory"

1. 完成上述必要修改后，打开一个终端并执行以下命令：

roslaunch panda_moveit_config demo.launch pipeline:=chomp

这将启动 RViz，在 Motion Planning 面板的 Context 选项卡下选择 OMPL。像前面使用 CHOMP 时一样，通过移动末端执行器来设置期望的起始状态和目标状态，然后点击 Plan 按钮开始规划。规划器会先运行 OMPL，再对 OMPL 的输出运行 CHOMP，最终生成一条优化后的路径。

将 CHOMP 作为 STOMP 的后处理器运行

这里演示如何将 CHOMP 用作 STOMP 所得规划的后处理优化技术。其基本思路是：先由 STOMP 为 CHOMP 生成初始路径，然后 CHOMP 在此基础上进一步优化轨迹。 要实现这一点，请按以下步骤操作：

1. 打开你的机器人 <robot_moveit_config>/launch 文件夹中的 stomp_planning_pipeline.launch 文件。对于 Panda 机器人，就是这个文件。编辑该启动文件，找到包含 <arg name="planning_adapters"> 的行，将其改为：

<arg name="planning_adapters" value="default_planning_request_adapters/AddTimeParameterization
               default_planning_request_adapters/ValidateWorkspaceBounds
               default_planning_request_adapters/CheckStartStateBounds
               default_planning_request_adapters/CheckStartStateCollision
               chomp/OptimizerAdapter" />

1. planning_adapters 的值定义了上述适配器的调用顺序，顺序非常重要。在 CHOMP 适配器内部，会先调用 STOMP，再调用 CHOMP 优化求解器，因此 CHOMP 会以 STOMP 计算出的初始路径为起点进行优化。

2. 找到包含 <rosparam command="load" file="$(find panda_moveit_config)/config/stomp_planning.yaml"/> 的行，在其后添加以下内容：

<rosparam command="load" file="$(find panda_moveit_config)/config/chomp_planning.yaml"/>

1. 以上修改会添加一个 CHOMP 优化适配器，并加载相应的 CHOMP 规划器参数。要在你自己的机器人上实现这一点，请将 panda_moveit_config 替换为你机器人的 <my_robot>_moveit_config。

2. 在你的机器人 <robot_moveit_config>/launch 文件夹中的 move_group.launch 文件里，确保默认规划器为 stomp。

3. 在你的机器人 <robot_moveit_config>/config 文件夹中的 chomp_planning.yaml 文件里，添加以下行：

trajectory_initialization_method: "fillTrajectory"

1. 完成上述必要修改后，打开一个终端并执行以下命令：

roslaunch panda_moveit_config demo.launch

这将启动 RViz，在 Motion Planning 面板的 Context 选项卡下选择 STOMP。通过移动末端执行器来设置期望的起始状态和目标状态，然后点击 Plan 按钮开始规划。规划器会先运行 STOMP，再对 STOMP 的输出运行 CHOMP，最终生成一条优化后的路径。

将 OMPL 作为 STOMP 的预处理器运行

注意：STOMP 平滑适配器仍在开发中。

这里演示如何将 STOMP 用作其他规划算法所得规划的后处理平滑技术。其基本思路是：先由某种随机规划算法为 STOMP 生成初始路径，然后 STOMP 在此基础上进一步平滑轨迹。 要实现这一点，请按以下步骤操作：

1. 打开你的机器人 <robot_moveit_config>/launch 文件夹中的 ompl_planning_pipeline.launch 文件。对于 Panda 机器人，就是这个文件。编辑该启动文件，找到包含 <arg name="planning_adapters"> 的行，将其改为：

<arg name="planning_adapters" value="default_planning_request_adapters/AddTimeParameterization
               default_planning_request_adapters/ValidateWorkspaceBounds
               default_planning_request_adapters/CheckStartStateBounds
               default_planning_request_adapters/CheckStartStateCollision
               stomp_moveit/StompSmoothingAdapter" />

1. planning_adapters 的值定义了上述适配器的调用顺序，顺序非常重要。在 STOMP 适配器内部，会先调用 OMPL，再调用 STOMP 平滑求解器，因此 STOMP 会以 OMPL 计算出的初始路径为起点进行平滑处理。

2. 找到包含 <rosparam command="load" file="$(find panda_moveit_config)/config/ompl_planning.yaml"/> 的行，在其后添加以下内容：

<rosparam command="load" file="$(find panda_moveit_config)/config/stomp_planning.yaml"/>

1. 以上修改会添加一个 STOMP 平滑适配器（STOMP Smoothing Adapter），并加载相应的 STOMP 规划器参数。要在你自己的机器人上实现这一点，请将 panda_moveit_config 替换为你机器人的 <my_robot>_moveit_config。

2. 在你的机器人 <robot_moveit_config>/launch 文件夹中的 move_group.launch 文件里，确保默认规划器为 ompl。

3. 在你的机器人 <robot_moveit_config>/config 文件夹中的 stomp_planning.yaml 文件里，将以下行：

initialization_method: 1 #[1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST]

替换为这一行：

initialization_method: 4 #[1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST, 4 : FILL_TRACJECTORY]

1. 完成上述必要修改后，打开一个终端并执行以下命令：

roslaunch panda_moveit_config demo.launch

这将启动 RViz，在 Motion Planning 面板的 Context 选项卡下选择 OMPL。通过移动末端执行器来设置期望的起始状态和目标状态，然后点击 Plan 按钮开始规划。规划器会先运行 OMPL，再对 OMPL 的输出运行 STOMP，最终生成一条平滑的路径。

将 STOMP 作为 CHOMP 的后处理器运行

注意：STOMP 平滑适配器仍在开发中。

这里演示如何将 STOMP 用作 CHOMP 所得规划的后处理平滑技术。 要实现这一点，请按以下步骤操作：

1. 打开你的机器人 <robot_moveit_config>/launch 文件夹中的 chomp_planning_pipeline.launch 文件。对于 Panda 机器人，就是这个文件。编辑该启动文件，找到包含 <arg name="planning_plugins"> 的行，并在其下方添加以下内容：

<arg name="planning_adapters" value="stomp_moveit/StompSmoothingAdapter" />
<param name="request_adapters" value="$(arg planning_adapters)" />

1. planning_adapters 的值定义了上述适配器的调用顺序，顺序非常重要。在 STOMP 适配器内部，会先调用 CHOMP，再调用 STOMP 平滑求解器，因此 STOMP 会以 CHOMP 计算出的初始路径为起点进行平滑处理。

2. 找到包含 <rosparam command="load" file="$(find panda_moveit_config)/config/chomp_planning.yaml"/> 的行，在其后添加以下内容：

<rosparam command="load" file="$(find panda_moveit_config)/config/stomp_planning.yaml"/>

1. 以上修改会添加一个 STOMP 平滑适配器，并加载相应的 STOMP 规划器参数。要在你自己的机器人上实现这一点，请将 panda_moveit_config 替换为你机器人的 <my_robot>_moveit_config。

2. 在你的机器人 <robot_moveit_config>/launch 文件夹中的 move_group.launch 文件里，确保默认规划器为 ompl。

3. 在你的机器人 <robot_moveit_config>/config 文件夹中的 stomp_planning.yaml 文件里，将以下行：

initialization_method: 1 #[1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST]

替换为这一行：

initialization_method: 4 #[1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST, 4 : FILL_TRACJECTORY]

1. 完成上述必要修改后，打开一个终端并执行以下命令：

roslaunch panda_moveit_config demo.launch

这将启动 RViz，在 Motion Planning 面板的 Context 选项卡下选择 CHOMP。通过移动末端执行器来设置期望的起始状态和目标状态，然后点击 Plan 按钮开始规划。规划器会先运行 CHOMP，再对 CHOMP 的输出运行 STOMP，最终生成一条平滑的路径。

不同运动规划器及规划适配器的规划见解

本节介绍何时使用哪种规划器，以及如何在特定流水线中使用规划请求适配器以生成稳健路径的经验。这里分别考虑单独使用 OMPL、STOMP、CHOMP 以及组合使用它们的情况，目标都是生成稳健、平滑且优化良好的路径。针对每种规划器，我们给出一条基本见解，帮助你在特定场景下直观地选择合适的规划器。

• CHOMP：CHOMP 是一种优化算法，用于优化给定的初始轨迹。根据环境的不同，CHOMP 会快速尝试将初始轨迹拉出碰撞区域。需要注意的是，参数 ridge_factor 必须大于或等于 0.001 才能避开障碍物，这样 CHOMP 就能在避开障碍物的同时找到路径。不过，尽管 CHOMP 能够成功避开障碍物，但它往往无法提供平滑的路径——在存在障碍物的情况下，生成的路径常常会出现颠簸。对 CHOMP 而言，碰撞规避是以牺牲轨迹的速度平滑性为代价的。

• STOMP：STOMP 能在合理的时间内生成平滑、良好且无碰撞的路径。该方法通过生成带噪声的轨迹来探索初始（可能是不可行的）轨迹周围的空间，然后将这些轨迹组合起来，生成一条成本更低的更新轨迹。

• OMPL 是一个用于基于采样/随机运动规划算法的开源库，如 OMPL 规划教程中所述。基于采样的算法是概率完备的：如果存在解，最终会找到解，但无法报告解不存在。这些算法效率很高，通常能快速找到解。

关于上述每种运动规划器的更多信息，请参阅各自的教程页面：OMPL、CHOMP 和 STOMP。

• OMPL 作为 CHOMP 的预处理器：OMPL 可以用作基础规划器来生成初始运动规划，作为 CHOMP 的初始猜测，然后由 CHOMP 生成优化后的路径。在大多数情况下，这样得到的路径质量优于单独使用 OMPL 或 CHOMP。

• OMPL 作为 STOMP 的预处理器：由于 STOMP 可以用作平滑算法，因此可以用来平滑其他运动规划器生成的规划。OMPL 首先生成一条路径，然后由 STOMP 生成该路径的平滑版本。在大多数情况下，这样的路径优于仅使用 OMPL 或 STOMP 生成的路径。

• STOMP 作为 CHOMP 的预处理器：在这种情况下，首先由 STOMP 生成一条路径，然后 CHOMP 将其作为初始猜测，生成平滑路径的优化版本。

• CHOMP 作为 STOMP 的预处理器：CHOMP 用来生成一条路径，然后由 STOMP 对其进行平滑。这有助于消除仅使用 CHOMP 时在存在障碍物情况下产生的颠簸运动。

STOMP 平滑适配器的开发完成后，我们将在本节发布一段演示不同规划器在各种场景下工作效果的视频。