MoveIt! 学习笔记17- STOMP Planner（随机轨迹优化）

此博文主要是用来记录ROS-Kinetic 中，用于机器人轨迹规划的MoveIt功能包的学习记录。 

英文原版教程见此链接：http://docs.ros.org/en/kinetic/api/moveit_tutorials/html/doc/time_parameterization/time_parameterization_tutorial.html

中文教程转载至：https://blog.csdn.net/nmssg1/article/details/93659720

运动规划的随机轨迹优化（STOMP）是一种概率优化框架（Kalakrishnan等，2011）。STOMP在合理的时间内产生平滑良好的无碰撞路径。该方法依赖于产生噪声轨迹以探索初始（可能不可行）轨迹周围的空间，然后将这些轨迹组合以产生具有较低成本的更新轨迹。在每次迭代中优化基于障碍物和平滑度成本的组合的成本函数。我们使用的特定优化算法不需要梯度信息，因此可能无法获得衍生物的一般成本（例如，对应于约束和电机转矩的成本）可以包括在成本函数中。STOMP的一些优势包括：它可以结合其他目标功能，如扭矩限制，能量和工具约束。STOMP可以处理不需要区分的成本函数。它使用距离场和球面近似来快速计算距离查询和碰撞成本。融入MoveIt的动力学和旋律版本！正在进行中。更多信息

入门

如果您还没有这样做，请确保已完成“ 入门”中的步骤。

您还应该使用MoveIt完成Visualization中的步骤！RViz插件

先决条件

1. 你必须拥有最新版本的MoveIt！安装。在ROS Kinetic上你需要建立MoveIt！来源。由于STOMP尚未成为正式版本的一部分，因此需要从源代码构建。因此它不包含在二进制包中。我们将在下面执行以下步骤。
2. 要将STOMP与机器人一起使用，您必须已经拥有MoveIt！已经为您的机器人配置包。例如，如果您有熊猫机器人，可能会调用它panda_moveit_config。这通常使用MoveIt构建！设置助手。
3. 您还必须从源头构建ros-industrial / industrial_moveit包。这需要从源代码构建，因为industrial_moveit尚未作为debian发布。您只需要从industrial_moveit 构建stomp_core包，因为STOMP with moveIt的功能不需要其他包。

安装MoveIt！来自Source

在工作区中添加和删除软件包时，需要清理工作区并重新运行命令以安装新的缺少的依赖项。清理工作区以删除对MoveIt系统范围安装的引用！：

1. cd~ / ws_moveit / src

2. 柳絮清洁

现在按照MoveIt上的说明进行操作！安装MoveIt的主页！源自动力学。请注意，您可以跳过“ 先决条件”部分，因为您应该已经拥有Catkin工作区。

重新获取安装文件：

source~ / ws_moveit / devel / setup.bash

将STOMP与机器人配合使用

注意：如果你正在跟踪使用此演示panda_moveit_config从ROS规划/ panda_moveit_config库，这些步骤都已经完成了你，你可以跳过步骤1-3，你只需要执行步骤4。

1. 只需将stomp_planning_pipeline.launch.xml文件下载到MoveIt的启动目录中即可！配置包。在我们的例子中，我们将此文件保存在panda_moveit_config/launch目录中。将文件“ stomp_planning_pipeline.launch.xml ”文件放在moveit_config包的启动目录中。注意：最新版本的MoveIt！Setup Assistant将为您生成此启动文件。该文件应包含以下内容：

   1. <发射>

   2. <！ - MoveIt的Stomp插件！ - >

   3. <arg name =“planning_plugin”value =“stomp_moveit / StompPlannerManager”/>

   4.  

   5. <！ - 请求适配器（插件）ORDER MATTERS - >

   6. <arg name =“planning_adapters”value =“default_planner_request_adapters / FixWorkspaceBounds

   7. default_planner_request_adapters / FixStartStateBounds

   8. default_planner_request_adapters / FixStartStateCollision

   9. default_planner_request_adapters / FixStartStatePathConstraints“/>

   10. <arg name =“start_state_max_bounds_error”value =“0.1”/>

   11. <param name =“planning_plugin”value =“$（arg planning_plugin）”/>

   12. <param name =“request_adapters”value =“$（arg planning_adapters）”/>

   13. <param name =“start_state_max_bounds_error”value =“$（arg start_state_max_bounds_error）”/>

   14. <rosparam command =“load”file =“$（find panda_moveit_config）/config/stomp_planning.yaml”/>

   15. </发射>

2. 调整行以替换MoveIt的名称！配置包。<rosparam command="load" file="$(find panda_moveit_config)/config/stomp_planning.yaml" /><rosparam command="load" file="$(find <robot_moveit_config>)/config/stomp_planning.yaml" /><robot_moveit_config>

3. 将stomp_planning.yaml文件下载到MoveIt的config目录中！配置包。在我们的例子中，我们将此文件保存在panda_moveit_config/config目录中。创建“ stomp_planning.yaml ”配置文件。该文件包含STOMP所需的参数。这些参数特定于SRDF文件中定义的每个“计划组”。因此，如果有三个计划组，则配置文件为每个计划组定义一组特定参数。在我们的例子中，只有一个计划组，即“panda_arm”。

   > 将此文件保存 在moveit_config包的config 目录中。还要确保stddev数组参数的维度与机器人的计划组名称中存在的关节数相同。

4. 修改move_group.launch文件。在启动目录中打开move_group.launch并将`pipeline`参数值更改`stomp`为如下所示：

   1. <！ - move_group settings - >

   2. <arg name =“allow_trajectory_execution”default =“true”/>

   3. <arg name =“fake_execution”default =“false”/>

   4. <arg name =“max_safe_path_cost”default =“1”/>

   5. <arg name =“jiggle_fraction”default =“0.05”/>

   6. <arg name =“publish_monitored_planning_scene”default =“true”/>

   7.  

   8. <！ - 规划功能 - >

   9. <include ns =“move_group”file =“$（find myworkcell_moveit_config）/ launch/planning_pipeline.launch.xml”>

   10. <arg name =“pipeline”value =“stomp”/>

   11. </包括>

运行演示

如果你有panda_moveit_config从ROS规划/ panda_moveit_config库，你应该能够简单地运行演示：

roslaunch panda_moveit_config demo.launch

在场景中运行带有障碍物的STOMP

要在有障碍的环境中运行STOMP，可以运行示例python脚本：

collision_scene_example.py。

这个脚本根据给脚本的参数创建一个带有四个ostaps的杂乱场景或一个带有一个障碍的简单场景。人们还可以改变障碍物的位置/大小来改变场景。

要使障碍物运行STOMP规划器，请打开两个终端。在第一个终端启动RViz并等待所有内容完成加载：

roslaunch panda_moveit_config demo_stomp.launch

在第二个终端中，运行以下两个命令之一：

rosrun moveit_tutorials collision_scene_example.py杂乱无章

要么：

rosrun moveit_tutorials collision_scene_example.py稀疏

接下来，在RViz中，在“上下文”选项卡下的MotionPlanning面板中选择STOMP。通过使用imarker移动末端效应器来设置所需的开始和目标状态，然后单击MotionPlanning面板中“计划”选项卡下的“计划”按钮以开始计划。计划者现在将尝试在给定的开始和结束位置之间找到可行的解决方案。STOMP在避免障碍方面的表现优于CHOMP。这是由于STOMP的随机性质导致了非生涩的轨迹而不是CHOMP，而CHOMP通常会产生不稳定的路径以避开障碍物。

调整STOMP的一些参数

STOMP有一些与之相关的参数。这些可以针对您正在使用的给定环境/机器人进行修改，并且通常存在于您正在使用的机器人的config文件夹中的stomp_planning.yaml文件中。如果您的机器人不存在此文件，您可以创建它并根据需要设置参数值。以下是为其中一些设置这些参数值的见解：

优化参数：

• num_timesteps：优化器在终止之前可以用来查找解决方案的步数。
• num_iterations：这是计划程序在优化时可以用来查找好的解决方案的迭代次数。
• num_iterations_after_valid：找到有效路径后要执行的最大迭代次数。
• num_rollouts：这是嘈杂轨迹的数量。
• max_rollouts：每次迭代期间新旧推出的组合数量不应超过此值。
• 初始化方法：这是选择初始化方法来初始化轨迹的初始化方法。
• control_cost_weight：这是在总成本计算中应用的轨迹加速成本的百分比。

噪音发生器参数：

• class：可以设置为“NormalDistributionSampling”（默认）或“GoalGuidedMultivariateGaussian”。根据使用的类，需要设置特定参数。如果使用“GoalGuidedMultivariateGaussian”，请查看此链接以设置参数。
• stddev：这是可以应用于关节的噪音程度。该阵列中的每个值是应用于阵列中该位置处的关节的噪声幅度。对于instace，数组中最左边的值将是用于设置机器人第一个关节的噪声的值（在我们的例子中是panda_joint1）。此数组的维度应等于计划组名称中的关节数。较大的“stddev”值对应于关节的较大运动。

成本函数参数：

• class：在这里你可以设置你想要使用的成本函数。您可以将其设置为“CollisionCheck”，“ObstacleDistanceGradient”或“ToolGoalPose”。根据您放在这里的内容，您需要设置相应的cost函数类的参数：对于“CollisionCheck”，您需要设置参数（collision_penalty，cost_weight，kernel_window_percentage，longest_valid_joint_nove）; 对于“ObstacleDistanceGradient”，您应该设置参数（cost_weight，max_distance，longest_valid_joint_move），对于“ToolGoalPose”，您应该设置参数（constrained_dofs，position_error_range，orientation_error_range，position_cost_weight，orientation_cost_weight）。请查看此链接以设置“ToolGoalPose”类的参数。
• collision_penalty：这是分配给碰撞状态的值。
• cost_weight：未使用的参数。
• kernel_window_percentage：用于计算window_size以进行内核平滑的乘法因子。
• longest_valid_joint_move：此参数指示关节在连续轨迹点之间移动的距离。

更新过滤器参数：

• class：可以设置为“PolynomialSmoother”或“ConstrainedCartesianGoal”。需要根据所选类别设置特定参数。要设置“ConstrainedCartesianGoal”的参数，请查看此链接。
• poly_order：这是用于平滑轨迹的多项式函数的阶数。

选择STOMP的参数需要比CHOMP更少的直觉。可以使用STOMP的默认参数，这在大多数环境中都能很好地工作。但是，您可以增加时间步数，推出数量和使用stddev数组来使STOMP在复杂环境下表现良好，以便STOMP可以在这些情况下找到最佳路径。或者，您可以通过在stomp_config.yaml文件中设置您想要的任何一个来尝试不同的成本函数，噪声生成器，更新过滤器类。

STOMP，CHOMP和OMPL获得的计划之间的差异

在本节中，区分了从STOMP，CHOMP和OMPL获得的路径。STOMP。一些移动计划者倾向于产生不稳定的轨迹并且可能引入不必要的机器人运动。通常需要后处理平滑步骤。相比之下，由于STOMP倾向于在短时间内产生平滑良好的运动计划，因此不需要一些其他运动规划者所要求的后处理平滑步骤。CHOMP基于方差和功能梯度方法优化给定的初始幼稚轨迹。CHOMP完全基于轨迹优化。OMPL是一个基于采样/随机运动规划算法的开源库。基于采样的算法在概率上是完整的：如果存在一个解决方案，最终会找到解决方案，但是不能报告解决方案的不存在。这些算法很有效，通常可以快速找到解决方案。现在，对以下规划者进行定性分析：

• 局部最小值处理：由于其随机性，STOMP可以避免局部最小值。然而，CHOMP倾向于并经常陷入局部最小值，从而避免了最佳解决方案。根据STOMP和CHOMP论文，STOMP表现更好。然而，CHOMP由于其基于梯度的性质而陷入局部最小值并且通常无法找到解决方案或返回次优解决方案。
• 时间要求：执行时间具有可比性，即使CHOMP需要更多迭代才能获得成功，而不是STOMP。这主要是因为STOMP的每次迭代都需要多次轨迹成本评估，但是可以以比CHOMP梯度更新规则更稳定的方式制作更大的步骤。OMPL算法很有效，通常可以快速找到解决方案。
• 参数调整：CHOMP通常需要额外的参数调整而不是STOMP才能获得成功的解决方案。OMPL不需要大量的参数调整，默认参数在大多数情况下都能很好地完成。
• 障碍物处理：对于包含障碍物的场景，STOMP通常能够成功地避开障碍物，因为它具有随机性。然而，CHOMP通过在成本函数中为机器人的动态量（如加速度，速度）添加一些噪声（ridge_factor）来生成不喜欢平滑轨迹的路径。在存在障碍物的情况下，OMPL还可以生成无碰撞的平滑路径。

运行STOMP单元测试

要运行单元测试，您必须拥有ros-industrial / industrial_moveit存储库中的stomp_core包。如果这些测试成功运行，则意味着STOMP已成功构建并在您的系统上运行。要运行STOMP单元测试，请打开终端并在catkin工作区中执行以下操作：

catkin_make run_tests_stomp_moveit stomp_moveit_utest