此博文主要是用来记录ROS-Kinetic 中，用于机器人轨迹规划的MoveIt功能包的学习记录。
中文教程转载至：https://blog.csdn.net/ssw_1990/article/details/104100140
Covariant Hamiltonian optimization for motion planning[CHOMP]是一种新型的基于梯度的轨迹优化方法，它使得许多日常的运动规划问题变得既简单又可训练([Ratliff et al., 2009c]。当高维运动规划将轨迹生成划分为不同的规划和优化阶段时，该算法利用协变梯度法和函数梯度法优化阶段，设计了完全基于轨迹优化的运动规划算法。给定一个不可行时的初始轨迹，CHOMP通过对周围环境的反应，快速地将轨迹从碰撞中拉出，同时优化关节速度、加速度等动力学量。它迅速收敛到一个光滑的无碰撞的轨迹，可以有效地执行机器人。集成到最新版本的MoveIt正在进行中。更多信息。
一.Getting Started
如果还没有这样做，确保已经完成了开始的步骤。也应该完成这些步骤在Visualization with MoveIt! RViz Plugin。
二.Prerequisites
1.必须有最新版本的MoveIt安装。在ROS Kinetic将需要编译MoveIt从源代码。由于CHOMP还不是正式版本的一部分，所以需要从源代码进行构建。因此，它不包括在二进制包中。将通过下面的步骤来实现这一点。
2.要使用CHOMP与机器人，必须已经有一个MoveIt机器人配置包。例如，如果有一个Panda机器人，它可能被称为panda_moveit_config。这通常是使用MoveIt Setup Assistant构建的。
三.Installing MoveIt! from Source
当从工作空间中添加和删除包时，将需要清理工作空间并重新运行命令来安装新的丢失的依赖项。清理工作空间，删除对系统范围内安装MoveIt的引用：
cd ~/ws_moveit/src
catkin clean
现在按照MoveIt主页上的说明去安装MoveIt Kinetic从源码。请注意，可以跳过先决条件部分，因为应该已经有了一个Catkin工作区。
重新启动安装文件：
source ~/ws_moveit/devel/setup.bash
四.Using CHOMP with Your Robot
注意：如果正在使用来自ros-planning/panda_moveit_config存储库中的panda_moveit_config进行演示，那么这些步骤已经完成了，可以跳过这一部分。
1.只需将chomp_planning_pipeline.launch.xml文件下载到MoveIt配置包的启动目录中。在例子中，将这个文件保存在panda_moveit_config/launch目录中。
2.调整<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的配置包的名称。
3.下载chomp_planning.yaml文件放入MoveIt的配置包的配置目录中。在例子中，将这个文件保存在panda_moveit_config/config目录中。
4.打开chomp_planning.yaml在最喜欢的编辑器中，并将animate_endeffector_segment:"panda_rightfinger"更改为机器人的适当链接。
5.复制demo.launch文件到demo_chomp.launch。注意，这个文件也在MoveIt配置包的启动目录中。在例子中，是panda_moveit_config/launch目录。
6.找到包括move_group.launch所在的行，并更改为：
<!-- Replace <robot_moveit_config> with the name of your MoveIt! configuration package -->
<include file="$(find <robot_moveit_config>)/launch/move_group.launch">
<arg name="allow_trajectory_execution" value="true"/>
<arg name="fake_execution" value="true"/>
<arg name="info" value="true"/>
<arg name="debug" value="$(arg debug)"/>
<arg name="planner" value="chomp" />
</include>
7.打开move_group.launch文件，在<robot_moveit_config>/launch/文件夹中，并进行两个更改：
[1]在<launch>标签下添加<arg name="planner" default="ompl" />。
[2]在<include ns="move_group">标签中，将<arg name="pipeline" value="ompl" />替换为<arg name="pipeline" value="$(arg planner)"/ >。
五.Running the Demo
如果有panda_moveit_config来从 ros-planning/panda_moveit_config仓库，应该能够简单地运行演示：
roslaunch panda_moveit_config demo_chomp.launch
1.Running CHOMP with Obstacles in the Scene
要在有障碍的环境中运行CHOMP，以运行Python脚本示例：collision_scene_example.py
这个脚本创建一个有四个障碍物的杂乱场景，或者一个只有一个障碍物的简单场景，这取决于脚本的参数。也可以改变障碍物的位置/大小来改变场景。
要运行CHOMP计划与障碍，打开两个shell。在第一个shell启动RViz和等待一切完成加载：
roslaunch panda_moveit_config demo_chomp.launch
在第二个shell中，运行这两个命令之一：
rosrun moveit_tutorials collision_scene_example.py cluttered
或
rosrun moveit_tutorials collision_scene_example.py sparse
接下来，在RViz中，在Context选项卡下的MotionPlanning窗格中选择CHOMP。通过使用imarker移动末端执行器来设置所需的开始和目标状态，然后单击MotionPlanning窗格中Planning选项卡下的Plan按钮来开始规划。规划器现在将尝试在给定的开始和结束位置之间找到一个可行的解决方案。
六.Tweaking some of the parameters for CHOMP
CHOMP有一些与之相关的优化参数。这些可以针对正在使用的特定环境/机器人进行修改，并且通常出现在chomp_planning.yaml文件中。在工作的机器人的配置文件夹中。如果机器人不存在此文件，可以创建它并设置所需的参数值。以下是为其中一些设置这些参数值的一些见解：
1.planning_time_limit：优化器在终止之前找到解决方案所花费的最大时间
2.max_iterations：这是规划人员在进行优化时能够找到一个好的解决方案最大迭代次数
3.max_iterations_after_collision_free：找到无碰撞路径后要执行的最大迭代
4.smoothness_cost_weight：smoothness_cost_weight参数在CHOMP实际优化的最终成本中控制其权重
5.obstacle_cost_weight：这控制了重量，以给予障碍的最终成本CHOMP优化。例如，0.0将会有一些可以忽略的障碍，1.0将会是一个硬约束
6.learning_rate：这是优化器用来在降低总成本的同时找到本地/全局最小值的学习率
7.smoothness_cost_velocity，smoothness_cost_acceleration, smoothness_cost_jerk：与velocity、acceleration和jerk相关的变量
8.ridge_factor：目标函数中总quadratic cost matrix对角线上的噪声。增加小的噪声[如0.001]可以使CHOMP避开障碍物，但代价是轨迹的平顺性
9.use_pseudo_inverse：是否启用伪逆计算
10.pseudo_inverse_ridge_factor：如果启用伪逆，则设置ridge因子
11.joint_update_limit：设置机器人关节的更新限制
12.collision_clearance：为避开障碍物而需要保持的最小距离
13.collision_threshold：为了避免冲突，需要维护的冲突阈值成本
14.use_stochastic_descent：如果想在优化成本的同时使用随机下降，就把这个设置为true/false。在随机下降中，使用的是轨迹上的一个随机点，而不是所有的轨迹点。这是更快的，并保证收敛，但它可能需要更多的迭代在最坏的情况下
15.enable failure recovery：如果设置为true，CHOMP将调整ceratin参数，希望在chomp_planning.yaml文件中指定的默认参数不存在的情况下找到解决方案
16.max_recovery_attempts：这是CHOMP在第一次尝试使用默认参数后，使用一组不同的参数运行的最大时间
17.trajectory_initializaiton_method：CHOMP可以在这里提供轨迹初始化的类型，可以是quintic-spline、linear、cubic或fillTrajectory。前三个选项是用于初始化起始和目标状态之间的轨迹的插值方法。fillTrajectory提供了一个选项，可以从现有的运动规划器[如OMPL]计算出的路径初始化轨迹。
为CHOMP选择参数需要一些规划环境提供的直觉。例如，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 OMPL as a pre-processor for CHOMP
这里证明了CHOMP也可以作为其它规划算法获得的规划的后处理优化技术。这背后的直觉是，一些随机规划算法生成CHOMP的初始猜测。然后CHOMP进行了初步的猜测，并进一步优化了轨迹。要做到这一点，请遵循以下步骤：
1.打开ompl_planning_pipeline.launch文件，在<robot_moveit_config>/launch机器人文件夹中。Panda机器人就是这个文件。编辑此启动文件，找到<arg name="planning_adapter ">被提及的行，并将其更改为：
<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
default_planner_request_adapters/CHOMPOptimizerAdapter" />
2.planning_adapter的顺序是上述适配器called/invoked的顺序。在CHOMP适配器内部，在调用CHOMP优化解决程序之前调用OMPL，因此CHOMP以OMPL计算的初始路径为起点，进一步优化它。
3.查找行，那里<rosparam command="load" file="$(find panda_moveit_config)/config/ompl_planning.yaml"/>被提高，在这一行后面加上以下内容：
<rosparam command="load" file="$(find panda_moveit_config)/config/chomp_planning.yaml"/>
4.这些添加将添加一个CHOMP优化适配器并加载相应的CHOMP计划器的参数。要做到这一点，可以将panda_moveit_config替换为机器人的<my_robot>_moveit_config。
5.在机器人的<robot_moveit_config>/launch文件夹的move_group.launch文件中，确保默认规划器是ompl。
6.在机器人的<robot_moveit_config>/config文件夹的chomp_planning.yaml文件中，添加以下行：
trajectory_initialization_method: "fillTrajectory"
7.在对启动文件进行这些必要的更改之后，打开终端并执行以下操作：
roslaunch panda_moveit_config demo_chomp.launch
这将启动RViz，在Context选项卡下的Motion Planning面板中选择OMPL。通过移动末端执行器来设置所需的起始和目标状态，方法与上述CHOMP相同。最后点击Plan按钮开始规划。规划器现在将首先运行OMPL，然后在OMPL的输出上运行CHOMP以生成优化的路径。
参考文献：
[1]CHOMP Planner：http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/chomp_planner/chomp_planner_tutorial.html