基于Moveit的轨迹优化-Planning Adapter_moveit算法对运动轨迹进行后处理

        规划适配器是 MoveIt 中的一个概念，可用于修改运动计划器的轨迹（预处理和/或后处理）。MoveIt 中现有计划适配器的一些示例包括 AddTimeParameterization、FixWorkspaceBounds、FixStartBounds、FixStartStateCollision、FixStartStatePathConstraints、CHOMPOptimizerAdapter 等！使用规划适配器的概念，可以在管道中使用多种运动规划算法来生成可靠的运动规划。例如，运动规划的轨迹包括由 OMPL 生成的初始规划轨迹，然后 CHOMP 对其进行优化，以生成可能比单独由 OMPL 或 CHOMP 生成的路径更好的运动轨迹。同样，使用规划适配器的概念，可以根据机器人所处的环境混合和匹配其他运动规划器。本部分提供了有关混合和匹配不同运动规划器的分步教程，并提供了有关何时使用哪些特定运动规划器的见解。参考moveit

        在本节中，我们提供了混合和匹配不同运动规划器的不同方法。本教程的主要重点是 OMPL、CHOMP 和 STOMP，因为这些是 MoveIt 目前唯一支持的规划器。

        本文表明，CHOMP可以作为其他计划算法获得的计划的后处理优化技术。意思就是说，使用其他规划算法对 CHOMP 产生一个初始的规划轨迹。然后，CHOMP利用这些初始的轨迹进一步优化，产生更好的轨迹。为此，请按照以下步骤操作：

 1、在你的功能包中找到ompl_planning_pipeline.launch.xml文件。将其复制一份，并命名为ompl-chomp_planning_pipeline.launch.xml，编辑此文件，找到下面提到的行并将其更改为：

<arg name="planning_adapters"
     value="default_planner_request_adapters/AddTimeParameterization
            default_planner_request_adapters/FixWorkspaceBounds
            default_planner_request_adapters/FixStartStateBounds
            default_planner_request_adapters/FixStartStateCollision
            default_planner_request_adapters/FixStartStatePathConstraints
            chomp/OptimizerAdapter" />

2、在ompl-chomp_planning_pipeline.launch.xml文件找到<rosparam command="load" file="$(find moveit_config)/config/ompl_planning.yaml"/>在这行下面添加下面的语句：

<rosparam command="load" file="$(find panda_moveit_config)/config/chomp_planning.yaml"/>
<rosparam command="load" file="$(find rml_63_moveit_config)/config/chomp_planning.yaml"/>
<!--  override trajectory_initialization_method  -->
<param name="trajectory_initialization_method" value="fillTrajectory"/>

chomp_planning.yaml中的参数解释：可参考chomp

3、在进行这些必要的更改之后，打开终端并执行以下操作，启动机械臂的程序： 

roslaunch moveit_config demo.launch

在“上下文”选项卡下的“运动规划”面板中选择 OMPL。通过移动末端执行器来设置所需的开始和目标状态。最后，单击“计划”按钮开始计划。规划器现在将首先运行 OMPL，然后在 OMPL 的输出上运行 CHOMP 以生成优化路径，如下图所示。

1、Moveit暂时还没有集成stomp，因此我么需要进行源码构建。

cd ~/ws_moveit
catkin clean
source /opt/ros/noetic/setup.bash
wstool set -t src stomp_ros https://github.com/ros-industrial/stomp_ros.git --git
wstool update -t src stomp_ros
wstool merge -t src src/stomp_ros/dependencies.rosinstall
wstool update -t src stomp ros_industrial_cmake_boilerplate
catkin build

 2、在你的功能包中找到ompl_planning_pipeline.launch.xml文件。将其复制一份，并命名为ompl-chomp_planning_pipeline.launch.xml，编辑此文件，找到下面提到的行并将其更改为：

<arg name="planning_adapters"
     value="default_planner_request_adapters/AddTimeParameterization
            default_planner_request_adapters/FixWorkspaceBounds
            default_planner_request_adapters/FixStartStateBounds
            default_planner_request_adapters/FixStartStateCollision
            default_planner_request_adapters/FixStartStatePathConstraints
            stomp_moveit/StompSmoothingAdapter" />

3、在ompl-chomp_planning_pipeline.launch.xml文件找到<rosparam command="load" file="$(find moveit_config)/config/ompl_planning.yaml"/>在这行下面添加下面的语句：

<rosparam command="load" file="$(find panda_moveit_config)/config/chomp_planning.yaml"/>
<rosparam command="load" file="$(find rml_63_moveit_config)/config/stomp_planning.yaml"/>
<!--  override trajectory_initialization_method  -->
<param name="initialization_method" value="4"/>

stomp_planning.yaml中的参数解释： 可参考stomp

4、在进行这些必要的更改之后，打开终端并执行以下操作，启动机械臂的程序： 

roslaunch moveit_config demo.launch

在“上下文”选项卡下的“运动规划”面板中选择 OMPL。通过移动末端执行器来设置所需的开始和目标状态。最后，单击“计划”按钮开始计划。规划器现在将首先运行 OMPL，然后在 OMPL 的输出上运行 STOMP 以生成优化路径。

        本部分介绍了何时使用哪个规划器，以及在特定管道中使用某些规划请求适配器如何生成整体可靠的路径。在这里，我们考虑单独使用OMPL、STOMP、CHOMP，以产生从规划器获得的稳健、平滑的优化路径。对于每个规划器，都提供了一个基本的见解，使用户能够直观地在特定情况下使用特定的规划器。

• CHOMP：CHOMP 是一种优化给定初始轨迹的优化算法。根据环境，CHOMP会迅速尝试将初始轨迹从碰撞中拉出。但是，这里需要注意的一点是，该参数需要大于或等于 0.001 才能避开障碍物。这样做，CHOMP能够在避开障碍物的同时找到路径。这里应该注意的是，即使 CHOMP 可以成功避开障碍物，但它无法提供平坦的路径，在存在障碍物的情况下经常导致路径崎岖不平。对于CHOMP来说，避免碰撞是以牺牲轨迹的速度平滑度为代价的。
• STOMP：STOMP 在合理的时间内产生平滑、表现良好的无碰撞路径。该方法依赖于生成嘈杂的轨迹来探索初始（可能不可行）轨迹周围的空间，然后将其组合以产生成本更低的更新轨迹。
• OMPL 是一个开源库，用于基于采样/随机运动规划算法，如 ompl 规划教程中所述。基于采样的算法在概率上是完备的：如果存在解决方案，最终将找到解决方案，但无法报告解决方案的不存在。这些算法是有效的，通常可以快速找到解决方案。
• OMPL 作为 CHOMP 的预处理器：OMPL 可以用作基本规划器来生成初始运动规划，然后CHOMP 在此基础上生成优化的路径。在大多数情况下，产生的这种路径的质量应该优于单独使用 OMPL 或单独使用 CHOMP 产生的路径。
• OMPL 作为 STOMP 的预处理器：由于 stomp 可以用作平滑算法，因此它可用于平滑其他运动规划器生成的计划。OMPL 首先生成一条路径，然后 STOMP 可以生成该路径的平滑版本。在大多数情况下，这样的路径应该比仅由 OMPL 或 STOMP 生成的路径更好。

有关这些运动规划器的更多信息，请参阅其各自的教程页面 OMPL、CHOMP 和 STOMP。