MoveIt教程[25]：CHOMPPlanner
Covariant Hamiltonian optimization for motion planning[CHOMP]是⼀种新型的基于梯度的轨迹优化⽅法，它使得许多⽇常的运动规划问题变得既简单⼜可训练([Ratliff et al., 2009c]。当⾼维运动规划将轨迹⽣成划分为不同的规划和优化阶段时，该算法利⽤协变梯度法和函数梯度法优化阶段，设计了完全基于轨迹优化的运动规划算法。给定⼀个不可⾏时的初始轨迹，CHOMP通过对周围环境的反应，快速地将轨迹从碰撞中拉出，同时优化关节速度、加速度等动⼒学量。它迅速收敛到⼀个光滑的⽆碰撞的轨迹，可以有效地执⾏机器⼈。集成到最新版本的MoveIt。。
⼀.Getting Started
如果还没有这样做，确保已经完成了。也应该完成这些步骤在。
⼆.Prerequisites
1.必须有最新版本的MoveIt安装。在ROS Kinetic将需要编译MoveIt从源代码。由于CHOMP还不是正式版本的⼀部分，所以需要从源代码进⾏构建。因此，它不包括在⼆进制包中。将通过下⾯的步骤来实现这⼀点。
2.要使⽤CHOMP与机器⼈，必须已经有⼀个MoveIt机器⼈配置包。例如，如果有⼀个Panda机器⼈，它
可能被称为
panda_moveit_config。这通常是使⽤构建的。
三.Installing MoveIt! from Source
当从⼯作空间中添加和删除包时，将需要清理⼯作空间并重新运⾏命令来安装新的丢失的依赖项。清理⼯作空间，删除对系统范围内安装MoveIt的引⽤：
cd ~/ws_moveit/src
catkin clean
现在按照MoveIt主页上的说明去。请注意，可以跳过先决条件部分，因为应该已经有了⼀个Catkin⼯作区。
重新启动安装⽂件：
source ~/ws_moveit/devel/tup.bash
四.Using CHOMP with Your Robot
注意：如果正在使⽤来⾃ros-planning/panda_moveit_config存储库中的panda_moveit_config进⾏演⽰，那么这些步骤已经完成了，可以跳过这⼀部分。
吹牛的近义词1.只需将⽂件下载到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_gment:"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脚本⽰例：
这个脚本创建⼀个有四个障碍物的杂乱场景，或者⼀个只有⼀个障碍物的简单场景，这取决于脚本的参数。也可以改变障碍物的位置/⼤⼩来改变场景。
要运⾏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 spar
接下来，在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.u_pudo_inver：是否启⽤伪逆计算
10.pudo_inver_ridge_factor：如果启⽤伪逆，则设置ridge因⼦
11.joint_update_limit：设置机器⼈关节的更新限制
14.u_stochastic_descent：如果想在优化成本的同时使⽤随机下降，就把这个设置为true/fal。在随机下降中，使⽤的是轨迹上的⼀个随机点，⽽不是所有的轨迹点。这是更快的，并保证收敛，但它可能需要更多的迭代在最坏的情况下
厉害的近义词
16.max_recovery_attempts：这是CHOMP在第⼀次尝试使⽤默认参数后，使⽤⼀组不同的参数运⾏的最⼤时间
为CHOMP选择参数需要⼀些规划环境提供的直觉。例如，CHOMP的默认参数在没有障碍的环境中⼯作得很好。但是，在有很多障碍的环境中，默认参数可能会导致CHOMP陷⼊局部极⼩值。通过调整参数，可以提⾼由CHOMP⽣成的规划的质量。
⼀些未使⽤/注释的参数是
hmc_stochasticity，hmc_annealing_factor，hmc_discretization，u_hamiltonian_montecarlo，animate_endeffector，anima te_endeffector_gment，animate_path，random_jump_amount，add_randomness。班风班训
七.Difference between plans obtained by CHOMP and OMPL
优化规划器优化⼀个成本函数，这有时可能会导致令⼈惊讶的结果：通过⼀个薄的障碍可能⽐⼀个长的，弯曲的轨迹更低的成本，避免所有的碰撞。在本节中，将区分从CHOMP获得的路径，并将其与从OMPL获得的路径进⾏⽐较。
OMPL是⼀个开放源码库，⽤于基于采样/随机化的运动规划算法。基于抽样的算法在概率上是完整的：如果⼀个解决⽅案存在，那么它最终会被找到，但是解决⽅案的不存在是不能被报告的。这些算法是有效的，通常能快速找到解决⽅案。OMPL不包含任何与冲突检查或可视化相关的代码，因为OM
PL的设计者不希望将其绑定到任何特定的冲突检查器或可视化前端。这个库被设计成可以很容易地集成到提供附加组件的系统中。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：
