【melodic-moveit】stomp运动规划器

stomp运动规划器 避障运动规划

stomp运动规划器

Stochastic Trajectory Optimization forMotion Planning (STOMP)

运动规划的随机轨迹优化 (STOMP) 是一种概率优化框架 (Kalakrishnan et al.2011)。STOMP 在合理的时间内产生平滑良好的无碰撞路径。该方法依赖于生成嘈杂的轨迹来探索初始（可能不可行）轨迹周围的空间，然后将这些轨迹组合起来以较低的成本生成更新的轨迹。在每次迭代中优化基于障碍和平滑成本组合的成本函数。我们使用的特定优化算法不需要梯度信息，因此导数可能不可用的一般成本（例如，与约束和电机扭矩相对应的成本）可以包含在成本函数中。

STOMP 的一些优势包括：

包含额外的目标函数，例如扭矩限制、功率和工具限制。

处理不需要可微的成本函数。

使用距离场和球面近似值来快速计算距离查询和碰撞成本。

Installing STOMP from Source

cd ~/ws_moveit/src
source /opt/ros/noetic/setup.bash
wstool init
wstool set stomp https://github.com/ros-industrial/stomp_ros.git --git
wstool update          wstool update stomp
#rosintall .
rosinstall stomp_ros /opt/ros/melodic stomp_ros/dependencies.rosinstall
catkin build

Using STOMP with Your Robot

注意：如果您使用 ros-planning/panda_moveit_config 存储库中的 panda_moveit_config 来进行此演示，则这些步骤已经为您完成，您可以跳过步骤 1-3，您只需要执行步骤 4。

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

<launch>
 
 
  <!-- MoveIt 的 Stomp 插件 -->
 
 
  <arg name="planning_plugin"value="stomp_moveit/StompPlannerManager" />
 
 
 
 
 
  <!-- 请求适配器（插件）订单事项 -->
 
 
  <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" />
 
 
  <arg name="start_state_max_bounds_error"value="0.1" />
 
 
  <param name="planning_plugin"value="$(arg planning_plugin)" />
 
 
  <param name="request_adapters"value="$(arg planning_adapters)" />
 
 
  <param name="start_state_max_bounds_error"value="$(arg start_state_max_bounds_error)" />
 
 
  <rosparamcommand="load" file="$(find panda_moveit_config)/config/stomp_planning.yaml"/>
 
 
</launch>

2. 将 <rosparamcommand="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> 替换为您的 MoveIt 配置包的名称。

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

> 将此文件保存在moveit_config 包的 config 目录中。还要确保 stddev 数组参数的维数与机器人规划组名称中存在的关节数相同。此示例适用于 7DoF 机器人。工业机器人通常有 5-6 个自由度。检查机器人的自由度！

4. 修改move_group.launch 文件。打开launch目录下的move_group.launch，将`pipeline`参数值改为`stomp`，如下图：

<!-- move_group settings -->
 
 
<arg name="allow_trajectory_execution"default="true"/>
 
 
<arg name="fake_execution"default="false"/>
 
 
<arg name="max_safe_path_cost"default="1"/>
 
 
<arg name="jiggle_fraction"default="0.05" />
 
 
<arg name="publish_monitored_planning_scene"default="true"/>
 
 
 
 
 
<!-- 规划功能 -->
 
 
<include ns="move_group"file="$(find myworkcell_moveit_config)/launch/planning_pipeline.launch.xml">
 
 
  <arg name="pipeline"value="stomp" />
 
 
</include>

Running the Demo

如果你有来自 ros-planning/panda_moveit_config 存储库的 panda_moveit_config 你应该能够简单地运行演示：

catkin build
source ./devel/setup.bash
roslaunch moveit_resources_panda_moveit_config demo.launch

Running STOMP with Obstacles in the Scene

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

collision_scene_example.py.

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

要在有障碍物的情况下运行 STOMP 规划器，请打开两个终端。在第一个终端启动 RViz 并等待一切完成加载：

#roslaunch moveit_resources_panda_moveit_config demo_stomp.launch
roslaunch moveit_resources_panda_moveit_config demo.launch

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

rosrun moveit_tutorials collision_scene_example.py cluttered

or:

rosrun moveit_tutorials collision_scene_example.py sparse

接下来，在 RViz 中，在Context 选项卡下的 MotionPlanning 面板中选择 STOMP。通过使用 imarker 移动末端执行器来设置所需的开始和目标状态，然后单击 MotionPlanning 面板中规划选项卡下的规划按钮开始规划。规划器现在将尝试在给定的开始和结束位置之间找到可行的解决方案。在避开障碍物方面，STOMP 比 CHOMP 表现更好。这是由于 STOMP 的随机性会产生非急促的轨迹，而 CHOMP 则通常会产生急促jerky paths的路径来避开障碍物。

Tweaking some of the parameters for STOMP

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

优化参数：

num_timesteps: :: 优化器在终止之前找到解决方案可以采取的时间步数。

num_iterations: :: 这是规划器在优化时可以用来找到好的解决方案的迭代次数。

num_iterations_after_valid: :: 找到有效路径后要执行的最大迭代次数。

num_rollouts: :: 这是噪声轨迹的数量。

max_rollouts: :: 每次迭代期间新旧 rollout 的总数不应超过此值。

initialization method：:: 这是选择初始化轨迹的方法的初始化方法。

control_cost_weight: :: 这是要在总成本计算中应用的轨迹加速成本的百分比。

Noise Generator Parameters:噪声发生器参数：

class: :: 这可以设置为“NormalDistributionSampling”（默认）或“GoalGuidedMultivariateGaussian”。根据使用的类别，需要设置特定的参数。如果使用“GoalGuidedMultivariateGaussian”，请查看此链接以设置参数。

stddev: :: 这是可以应用于关节的噪声程度。此数组中的每个值都是应用于数组中该位置关节的噪声幅度。例如，数组中最左边的值将是用于设置机器人第一个关节（在我们的例子中为 panda_joint1）的噪声的值。此数组的维数应等于计划组名称中的关节数。较大的“stddev”值对应于较大的关节运动。

Cost Function Parameters:成本函数参数：

class: :: 在这里你可以设置你想要使用的成本函数。您可以将其设置为“CollisionCheck”、“ObstacleDistanceGradient”或“ToolGoalPose”。根据你放在这里的内容，你需要设置合适的成本函数类的参数：对于“CollisionCheck”，你需要设置参数（collision_penalty、cost_weight、kernel_window_percentage、longest_valid_joint_nove）；对于“ObstacleDistanceGradient”，您应该设置参数（cost_weight、max_distance、longest_valid_joint_move）；对于“ToolGoalPose”，您应该设置参数（受限自由度、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: :: 这个参数表示一个关节可以在连续轨迹点之间移动多远。

Update Filter parameters:更新过滤器参数：

class: :: 这可以设置为“PolynomialSmoother”或“ConstrainedCartesianGoal”。需要根据选择的类设置特定参数。要为“ConstrainedCartesianGoal”设置参数，请查看此链接。

poly_order: :: 这是用于平滑轨迹的多项式函数的阶数。

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

Difference between plans obtained by STOMP, CHOMP and OMPL

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

Local Minima Handling:::局部最小值处理：:: STOMP 由于其随机性可以避免局部最小值。然而，CHOMP 容易并经常陷入局部最小值，从而避免了最佳解决方案。根据 STOMP 和 CHOMP 论文，STOMP 表现更好。然而，CHOMP 由于其基于梯度的性质而陷入局部最小值，并且通常无法找到解决方案或返回次优解决方案。

Time requirements:::时间要求：:: 执行时间相当，即使 CHOMP 需要比 STOMP 更多的迭代才能取得成功。这主要是因为 STOMP 的每次迭代都需要多次轨迹成本评估，但可以比 CHOMP 梯度更新规则以更稳定的方式进行更大的步长。OMPL 算法是高效的，通常可以快速找到解决方案。

Parameter tuning:::参数调优：：：

CHOMP 通常需要比 STOMP 额外的参数调整才能获得成功的解决方案。OMPL 不需要大量的参数调优，默认参数在大多数情况下都做得很好。

Obstacle Handling:::障碍物处理：：：

对于包含障碍物的场景，STOMP 由于其随机性，往往能够成功避开障碍物。然而，CHOMP 通过在机器人动态量（如加速度、速度）的成本函数中添加一些噪声（ridge_factor）来生成不喜欢平滑轨迹的路径。OMPL 还可以在存在障碍物的情况下生成无碰撞的平滑路径。

参考：

https://ros-planning.github.io/moveit_tutorials/doc/stomp_planner/stomp_planner_tutorial.html#stomp-planner 

https://index.ros.org/

https://index.ros.org/search/?term=stomp

https://github.com/ros-industrial/stomp_ros