模糊匹配轨迹缓存快速入门

模糊匹配轨迹缓存（fuzzy-matching trajectory cache）允许你插入和获取机械臂轨迹，并以 MotionPlanRequest 和 GetCartesianPath 请求与对应轨迹作为键进行模糊匹配。

该缓存适用于具有任意关节数量的机械臂，可跨任意数量的 move group 使用。 此外，缓存支持对获取的轨迹进行剪枝和排序，并提供扩展点，用于注入你自己的特征键控、缓存插入、缓存剪枝和缓存排序逻辑。

更多信息，请参阅包 README。 或者观看 ROSCon 2024 演讲 和幻灯片！

警告：缓存不支持碰撞检测以及某些其他约束（例如多自由度关节、规划场景、约束区域和可见性约束）！

规划在插入和获取缓存时会忽略碰撞及这些约束。 如果你的规划场景在两次缓存查找之间预期会发生显著变化，请确保对获取的任何规划进行碰撞验证。

快速示例用法

前置条件：warehouse_plugin ROS 参数必须设置为一个你已安装的 warehouse_ros 插件，它决定了缓存使用哪个数据库后端。

auto cache = std::make_shared<TrajectoryCache>(node);
cache->init(/*db_host=*/":memory:", /*db_port=*/0, /*exact_match_precision=*/1e-6);

auto default_features = TrajectoryCache::getDefaultFeatures(start_tolerance, goal_tolerance);
std::string TrajectoryCache::getDefaultSortFeature();  // Sorts by planned execution time.

move_group.setPoseTarget(...);
moveit_msgs::msg::MotionPlanRequest motion_plan_req_msg;
move_group.constructMotionPlanRequest(motion_plan_request_msg);

auto fetched_trajectory =  // Use the cache fetch in place of planning!
    cache->fetchBestMatchingTrajectory(*move_group_interface, robot_name, motion_plan_req_msg,
                                      /*features=*/default_features,
                                      /*sort_by=*/TrajectoryCache::getDefaultSortFeature(),
                                      /*ascending=*/true);

if (fetched_trajectory)    // Great! We got a cache hit, we can execute it.
{
  move_group.execute(*fetched_trajectory);
}
else                       // Otherwise, plan... And put it for posterity!
{
  moveit::planning_interface::MoveGroupInterface::Plan plan;
  if (move_group.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS)
  {
    cache->insertTrajectory(
        *interface, robot_name, std::move(plan_req_msg), std::move(plan),
        /*cache_insert_policy=*/BestSeenExecutionTimePolicy(),
        /*prune_worse_trajectories=*/true, /*additional_features=*/{});
  }
}

上述示例中的方法都有对应的笛卡尔（Cartesian）变体。

更多信息，请阅读 moveit_ros::trajectory_cache::TrajectoryCache 的完整 API 文档。

为什么需要缓存？

轨迹缓存有助于：

• 减少规划时间

• 与随机规划器配合使用时，实现一致且可预测的行为

  • 它实际上允许你”冻结”一个动作

为了解释这一点，请考虑 MoveIt 中的规划器通常分为两大类：随机/概率型和基于优化型。概率型规划器速度快，但通常不确定；而基于优化的规划器通常较慢，但确定性强。

解决这个问题的一种方法是预先规划，手动选择和标记机器人轨迹，在轨迹数据库中”冻结”它们以便按名称重放。这样可以避免重新规划所花费的时间，并确保运动恒定且可重复。然而，这种方法不够灵活，也不容易复用。

轨迹缓存改进了这种方法：它允许用户”冻结”并存储成功的规划，同时还能以一种更通用、更自然的方式查找这些规划——直接使用规划请求本身作为缓存的键，从而有效地让缓存代替一次规划调用。

此外，该轨迹缓存的具体特性还提供了进一步的独特优势：

1. 借助模糊匹配，“冻结”的规划更容易查找和复用，同时提高了缓存命中的几率。
2. 轨迹排序能力在与随机规划器配合使用足够多次后，将使缓存最终收敛到越来越优的规划。

最后，缓存利用剪枝来优化获取时间，并设法”规范化”键控请求的特征，以提高缓存命中的几率。

工作原理

如果一个规划请求的特征（例如起始状态、目标状态和约束条件）与缓存中的某个条目”足够接近”，那么该缓存中的轨迹就应可复用于这个请求，从而跳过规划。

缓存从规划请求和规划响应中提取这些特征，并将它们存储在缓存数据库中。当使用新的规划请求尝试获取匹配的规划时，缓存会尝试将该请求与预先存在的、以相似请求为键的缓存条目进行模糊匹配。任何”足够接近”的匹配都会作为有效的缓存命中返回以供规划复用，而”足够接近”的定义取决于所提取特征的类型。

关于缓存如何工作、使用哪些具体特征作为缓存条目的键，请参阅包 README，以及 moveit_ros::trajectory_cache::TrajectoryCache 及其相关类的完整 API 文档。

演示

（视频为 4 倍速。）

这个演示包含四个阶段，可通过 rviz_visual_tools 对话框推进，它们共同展示了缓存能力的亮点。

1. 规划并缓存（无剪枝）
2. 规划并缓存（带剪枝）
3. 从缓存获取并执行（同时仍带剪枝地进行规划和缓存）
4. 从缓存获取并执行，但使用较大的起始容差

此外，演示源码本身带有大量注释和教程说明，如果你想检查代码并深入了解，可以参考。

前置条件

确保你已经构建了 moveit2_tutorials 包，并 source 了构建它的工作区，以确保你能够运行演示。

运行演示

在一个终端中启动 move group：

ros2 launch moveit2_tutorials trajectory_cache_move_group.launch.py

在另一个终端中启动演示：

ros2 launch moveit2_tutorials trajectory_cache_demo.launch.py

你应该会看到与视频类似的内容，只是随机生成的目标位姿不同。

点击 rviz 窗口上的 next 推进演示。

注意：有时随机生成的演示目标位姿不可达（或者 move_group 无法正确初始化）。 如果发生这种情况，演示会因规划调用失败或其他问题而在第一阶段停止。

只需重新启动演示，它会生成新的演示目标位姿并解决问题。

配置演示

此外，演示的 launch 文件暴露了 launch 参数，允许你更改演示的许多方面。查看演示的 launch 文件，获取可配置 launch 参数的完整列表。

例如，你可以为缓存指定一个磁盘路径用于保存和加载，而不是使用内存：

ros2 launch moveit2_tutorials trajectory_cache_demo.launch.py cache_db_host:="<some_directory>/trajectory_cache_demo.db

然后，如果你指定了磁盘路径，由于演示默认使用 SQLite，你可以使用 SQLite database browser 检查缓存数据库文件。

1. CacheNoPruning

演示的第一阶段展示了从单个起始点到多个目标位姿的缓存插入和获取功能，且不进行剪枝。

左上角的文本包含演示阶段的名称，以及关于缓存状态的有用信息。

在此阶段，我们为每个目标多次运行规划器，并每次都尝试插入缓存。我们还会获取并可视化特定目标的所有匹配规划，以及最佳匹配规划。

可视化效果解读：

• 半透明深灰色的轨迹线是当前目标的已规划轨迹
• 白色的轨迹线是缓存中的轨迹
• 黄色的轨迹线是匹配起始条件和规划请求的匹配缓存轨迹
• 绿色的轨迹线是给定起始条件和规划请求下的最佳匹配缓存轨迹（按执行时间）

注意：可视化演示了可以为给定目标获取”匹配”的轨迹，并且可以对获取的规划进行排序，从中确定”最佳”的轨迹。

注意：你可能注意到在可视化中，绿色轨迹（最佳轨迹）看起来比一些其他候选规划要优化得多。

而且，大多数时候，获取时间都比规划时间短，即使在这个无障碍物的情况下也是如此，这说明了缓存如何节省规划时间。

默认的缓存插入策略只在轨迹是迄今为止所见最佳（按执行时间）时才插入缓存，这就是为什么并非每条规划轨迹都会被插入缓存。

你应该会发现，随着缓存不断保存越来越好的规划，缓存规划的数量会逐渐趋于稳定。

让演示阶段运行一段时间以填满缓存，然后在 rviz_visual_tools 对话框中点击 next，将演示推进到下一阶段——我们将在那里剪枝掉较不优的缓存条目。

2. CacheWithPruning

演示的第二阶段展示了从单个起始点到多个目标位姿的缓存插入和获取功能，并带有剪枝。

建议进行缓存剪枝，以保持缓存中轨迹数量较少，节省缓存数据库存储空间，并提高缓存获取效率。

你还应该看到 delete_worse_trajectories 参数被设置为 true。

随着规划继续进行，你应该会立即看到缓存条目逐渐被剪枝，直到最终缓存为每个目标收敛出一条”最佳”轨迹。

缓存需要多次规划运行才能收敛的原因在于：我们只尝试插入从当前规划运行中获得的规划，而缓存只剪枝那些比当前插入候选更差（默认按执行时间）的完全匹配的缓存条目。因此，一个规划要被剪枝，规划出的轨迹必须比它更好。

注意：如果你想要立即剪枝所有更差的规划，可以先获取最佳规划，然后重新插入它以触发剪枝。

在 rviz_visual_tools 对话框中点击 next，将演示推进到下一阶段——我们将开始执行。

3. ExecuteWithCache

演示的第三阶段开始执行。

我们仍然进行规划和缓存，现在也包括笛卡尔规划。你应该预期缓存规划的数量会随时间增加，包括缓存的笛卡尔规划。

注意，执行的轨迹始终锁定在最佳匹配轨迹上，如绿色轨迹所示。

此外，请注意可匹配轨迹的集合（以白色表示）是一个更加受限的集合。这表明缓存是可分区的，并且能够依据键控它们的约束的性质过滤掉不匹配的条目。

可视化效果解读，使用与之前相同的图例：

• 新增：灰色的轨迹线是缓存中不匹配的轨迹

在 rviz_visual_tools 对话框中点击 next，将演示推进到下一阶段——我们将在那里展示缓存的模糊匹配能力以及高起始容差的影响。

4. HighStartTolerance

演示的最后阶段设置了过高的起始容差，以展示缓存的模糊匹配能力。

我们将起始容差设置为 2（从 0.025 提高），这意味着我们允许每个起始机器人关节状态最多相差 2 弧度、每个起始约束最多相差 2 个单位的轨迹进行匹配（对目标仍然保持严格容差）。

可视化效果解读，使用与之前相同的图例：

• 新增：红色的线是机器人状态与匹配轨迹的差（diff）

请注意，对于某些起始状态，机器人现在会使用一条起始状态比当前状态更远的不同的匹配轨迹。这个差异被可视化为一条粗红线。

这是因为该匹配轨迹的执行时间更短，会被判定为”最佳”，并且由于我们设置了更高的起始容差，它现在可以匹配。这展示了缓存处理模糊匹配的能力——尽管在这里使用了过高的起始容差，因为它会导致机器人剧烈地”猛拉”到轨迹的起点。

最佳实践

作为本操作指南的结尾，这里有一些关于如何最好地使用缓存的建议。

碰撞

由于缓存尚不支持碰撞：

• 确保规划场景和障碍物保持静态，或者
• 单独验证获取的规划
• 额外加分项（BONUS）：使用 TrajectoryCache::Options::num_additional_trajectories_to_preserve_when_deleting_worse 选项在剪枝后保留多条轨迹，希望能存下足够多种类的、能够避开障碍物的可行轨迹，然后按从最佳到最差的顺序逐一验证

提高缓存命中率

• 在获取之前将机器人移动到静态的已知位姿（例如 home）
• 在可能发生重复性、非动态运动的地方使用缓存
• 使用较宽松的起始容差和较严格的目标容差

进一步阅读

可自定义行为

可以扩展缓存，使其以自定义的用户定义特征作为键，也可以更改缓存的插入、排序和剪枝逻辑。你可以通过实现特征接口和缓存插入策略接口，然后将它们提供给相应的缓存方法来实现。

例如，你可能决定编写自己的特征提取器来为缓存键控，并决定何时基于以下特征插入或剪枝缓存条目：

• 最小加加速度时间（minimum jerk time）
• 路径长度（path length）
• 此包不支持的任何其他特征！

参见：

• FeaturesInterface<FeatureSourceT
• CacheInsertPolicyInterface