Fuzzy-Matching Trajectory Cache

A trajectory cache based on warehouse_ros for the move_group planning interface that supports fuzzy lookup for MotionPlanRequest and GetCartesianPath requests and trajectories.

The cache will work on manipulators with an arbitrary number of joints, across any number of move groups. Furthermore, the cache supports pruning and ranking of fetched trajectories, with extension points for injecting your own feature keying, cache insert, cache prune and cache sorting logic.

Citations

If you use this package in your work, please cite it using the following:

@software{ong_2024_11215428,
  author       = {Ong, Brandon},
  title        = {A Fuzzy-Matching Trajectory Cache for MoveIt 2},
  month        = may,
  year         = 2024,
  publisher    = {GitHub},
  version      = {0.1.0},
  doi          = {10.5281/zenodo.11215428},
  url          = {https://doi.org/10.5281/zenodo.11215428}
}

WARNING: The following are unsupported / RFE

Since this is an initial release, the following features are unsupported because they were a little too difficult for the time I had to implement this. So I am leaving it to the community to help!

• !!! This cache does NOT support collision detection, multi-DOF joints, or constraint regions!
  • Trajectories will be put into and fetched from the cache IGNORING collision. If your planning scene is expected to change significantly between cache lookups, it is likely that the fetched plan will result in collisions.
  • To natively handle collisions this cache will need to hash the planning scene world msg (after zeroing out std_msgs/Header timestamps and sequences) and do an appropriate lookup, or do more complicated checks to see if the scene world is “close enough” or is a strictly less obstructed version of the scene in the cache entry.
• The fuzzy lookup can’t be configured on a per-joint basis.

That said, there are ways to get around the lack of native collision support to enable use of this cache, such as:

• Validating a fetched plan for collisions before execution.
• Make use of the hybrid planning pipeline, using local planners for collision avoidance, while keeping the cache as a stand-in for a “global planner”, where applicable.

Example Usage

PRE-REQUISITE: The warehouse_plugin ROS parameter must be set to a warehouse_ros plugin you have installed, which determines what database backend should be used for the cache.

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

// The default feature extractors key the cache on starting robot state and goal constraints in the plan request.
// Keyed fuzzily with separate fuzziness on start and goal features.
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);

// Use the cache INSTEAD of planning!
auto fetched_trajectory =
    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=*/{});
  }
}

Main Features

Overview

This trajectory cache package supports:

• Inserting and fetching trajectories, keyed fuzzily on any feature of the plan request and plan response.
• Ranking cache entries on any keying feature that is supported (e.g. sorting by execution time).
• Optional cache pruning to keep fetch times and database sizes low.
• Generic support for manipulators with any arbitrary number of joints, across any number of move_groups.
• Cache namespacing and partitioning
• Extension points for injecting your own feature keying, cache insert, cache prune, and cache sorting logic.

The cache supports MotionPlanRequest and GetCartesianPaths::Request out of the box!

Fully Customizable Behavior

This trajectory cache allows you to inject your own implementations to affect:

• What features of the plan request and plan response to key the cache on
• What cache insert and cache pruning policy to adopt

For example, you may decide to write your own feature extractor to key the cache, and decide when to insert or prune a cache entry on features such as:

File truncated at 100 lines see the full file

Changelog for package moveit_ros_trajectory_cache

2.14.0 (2025-06-13)

2.13.2 (2025-04-16)

2.13.1 (2025-04-15)

2.13.0 (2025-02-15)

• Fuzzy-matching Trajectory Cache Injectable Traits refactor 🔥🔥 (#2941)
• Contributors: methylDragon

2.12.0 (2024-11-29)

• Enhancement/use hpp for headers (#3113)
• Contributors: Tom Noble

2.11.0 (2024-09-16)

0.1.0 (2024-05-17)

• Add moveit_ros_trajectory_cache package for trajectory caching.