WARNING

 *   This cache does NOT support collision detection!
 *   Plans will be put into and fetched from the cache IGNORING collision.
 *   If your planning scene is expected to change between cache lookups, do NOT
 *   use this cache, fetched plans are likely to result in collision then.
 *
 *   To handle collisions this class will need to hash the planning scene world
 *   msg (after zeroing out std_msgs/Header timestamps and sequences) and do an
 *   appropriate lookup, or otherwise find a way to determine that a planning scene
 *   is "less than" or "in range of" another (e.g. checking that every obstacle/mesh
 *   exists within the other scene's). (very out of scope)

Preamble

Apologies for the large PR size, there isn't really a way to meaningfully break this down.

To make up for that, here is a very comprehensive description. Also, this class is fully tested (165 test cases! Wow!), and already used in production.

Please inspect the tests to see all the capabilities of the cache in action.

Quick Example Usage

// Be sure to set some relevant ROS parameters:
// Relevant ROS Parameters:
//   - `warehouse_plugin`: What database to use
//   - `warehouse_host`: Where the database should be created
//   - `warehouse_port`: The port used for the database
auto traj_cache = std::make_shared<TrajectoryCache>(node);

auto fetched_trajectory = traj_cache->fetch_best_matching_trajectory(
      *move_group_interface, robot_name, motion_plan_req_msg,
      _cache_start_match_tolerance, _cache_goal_match_tolerance, /*sort_by=*/"execution_time_s");

if (fetched_trajectory) {
  // Great! We got a cache hit
  // Do something with the plan.
} else {
  // Plan... And put it for posterity!
  traj_cache->put_trajectory(
        *interface, robot_name, std::move(plan_req_msg), std::move(res->result.trajectory),
        rclcpp::Duration(
          res->result.trajectory.joint_trajectory.points.back().time_from_start
        ).seconds(),
        res->result.planning_time, /*overwrite=*/true)
}

The workflow for Cartesian plans is similar. Use fetch_best_matching_cartesian_trajectory and put_cartesian_trajectory instead!

An alternate signature to the move_group plan methods could also be added in the future to add caching ability to plan calls without asking MoveIt 2 users to implement the caching logic on their own. But I am wary of doing that just yet with the lack of support of collisions.

What Is This?

As part of the Nexus development effort, and while working for Open Robotics, I developed a trajectory cache for MoveIt 2, leveraging warehouse_ros, for motion plans and cartesian plans that supports fuzzy lookup.

This PR upstreams that work to MoveIt2.

It also basically answers this (7!?!?) year old thread asking for a MoveIt trajectory cache. Better late than never.

The cache allows you to insert trajectories and fetch keyed fuzzily on the following:

Starting robot (joint) state
Planning request constraints
- This includes ALL joint, position, and orientation constraints!
- And also workspace parameters, and some others.
Planning request goal parameters

Full supported key list

```cpp // MotionPlanRequest "group_name" "workspace_parameters.header.frame_id" "workspace_parameters.min_corner.x" "workspace_parameters.min_corner.y" "workspace_parameters.min_corner.z" "workspace_parameters.max_corner.x" "workspace_parameters.max_corner.y" "workspace_parameters.max_corner.z" "start_state.joint_state.name_i" "start_state.joint_state.position_i" "start_state.joint_state.name_i" "start_state.joint_state.position_i" "max_velocity_scaling_factor" "max_acceleration_scaling_factor" "max_cartesian_speed" "goal_constraints.joint_constraints_i.joint_name" "goal_constraints.joint_constraints_i.position" "goal_constraints.joint_constraints_i.tolerance_above" "goal_constraints.joint_constraints_i.tolerance_below" "goal_constraints.position_constraints.header.frame_id" "goal_constraints.position_constraints_i.link_name" "goal_constraints.position_constraints_i.target_point_offset.x" "goal_constraints.position_constraints_i.target_point_offset.y" "goal_constraints.position_constraints_i.target_point_offset.z" "goal_constraints.orientation_constraints.header.frame_id" "goal_constraints.orientation_constraints_i.link_name" "goal_constraints.orientation_constraints_i.target_point_offset.x" "goal_constraints.orientation_constraints_i.target_point_offset.y" "goal_constraints.orientation_constraints_i.target_point_offset.z" "goal_constraints.orientation_constraints_i.target_point_offset.w" // GetCartesianPathRequest "group_name" "start_state.joint_state.name_i" "start_state.joint_state.position_i" "start_state.joint_state.name_i" "start_state.joint_state.position_i" "max_velocity_scaling_factor" "max_acceleration_scaling_factor" "max_step" "jump_threshold" "waypoints_i.position.x" "waypoints_i.position.y" "waypoints_i.position.z" "waypoints_i.orientation.x" "waypoints_i.orientation.y" "waypoints_i.orientation.z" "waypoints_i.orientation.w" "link_name" "header.frame_id" ```

It works generically for an arbitrary number of joints, across any number of move_groups.

It also has the following optimizations:

Separate caches for separate move groups
The cache does "canonicalization" of the poses to be relative to the planning frame to increase the chances of cache hits.
Configurable fuzzy lookup for the keys above.
It supports "overwriting" of worse trajectories with better trajectories
- If a trajectory with keys extremely close to a pre-existing cache entry is inserted with a better planned execution time, the cache can delete all "worse" trajectories.
- IsThisMachineLearning (it's not.)
- It also prunes the database and mitigates a lot of query slow-down as the cache grows

Working Principle

If a plan request has start, goal, and constraint conditions that are "close enough" (configurable per request) to an entry in the cache, then the cached trajectory should be suitable (as long as obstacles have not changed. More on that later.)

Goal fuzziness is a lot less lenient than start fuzziness by default.

Finally, the databases are sharded by move group, and the constraints are added as columns in a name agnostic fashion (they are even somewhat robust to ordering, because they're sorted!)

Why?

A trajectory cache helps:

Cut down on planning time (especially for known moves)
Allows for consistent predictable behavior of used together with a stochastic planner
- It effectively allows you to "freeze" a move

A user may also choose when to leverage the cache (e.g. when planning moves from a static "home" position, or repetitive/cartesian moves) to get these benefits.

Additionally, because the cache class has the ability to sort by planned execution time, over sufficient runs, the stochastic plans eventually converge to better and better plans (execution time wise).

We build on top of the class in this PR to expose the following behaviors (not implemented in this PR):

TrainingOverwrite: Always plan, and write to cache, deleting all worse trajectories for "matching" cache keys
TrainingAppendOnly: Always plan, and always add to the cache.
ExecuteBestEffort: Rely on cache wherever possible, but plan otherwise.
ExecuteReadOnly: Only execute if cache hit occurred.

You can see how such behaviors effectively model the "dev" and "deploy" phases of a robot deployment, and how they could be useful.

Sample Results

We saw 5%-99% reduction in planning time in production.

Image

![image](https://github.com/moveit/moveit2/assets/20265670/c0edc75f-fe05-45e4-ba9c-0041e365f4c5)

Cache hits and pushes!

A view of the schema

With database collections for each move group

Populated Entries

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!
- Trajectories will be put into and fetched from the cache IGNORING collision.
- If your planning scene is expected to change between cache lookups, do NOT use this cache, fetched trajectories are likely to result in collision then.
- To handle collisions this class 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 less obstructed version of the scene in the cache entry.
- !!! This cache does NOT support keying on joint velocities and efforts.
- The cache only keys on joint positions.
!!! This cache does NOT support multi-DOF joints.
!!! This cache does NOT support certain constraints
- Including: path, constraint regions, everything related to collision.
The fuzzy lookup can't be configured on a per-joint basis.

Test

Targeting: rolling

source /opt/ros/rolling/setup.bash
sudo rosdep init
rosdep update

mkdir -p traj_cache_ws/src
cd traj_cache_ws/src
git clone https://github.com/methyldragon/moveit2.git -b ch3/trajectory_cache
for repo in moveit2/moveit2.repos $(f="moveit2/moveit2_rolling.repos"; test -r $f && echo $f); do vcs import < "$repo"; done

cd ..
rosdep install --from-paths src --ignore-src --rosdistro rolling -y -r
colcon build --packages-up-to moveit_ros_trajectory_cache --cmake-args -DCMAKE_BUILD_TYPE=Release

source install/setup.bash
colcon test --packages-select moveit_ros_trajectory_cache

All 165 test cases pass, (no [FAIL] in log, 165 [PASS]. [ERROR] logs are expected since the tests also test invalid cases.)

The tests are launch tests even though I would prefer them to be unit tests; because move_group requires a running node.

Test Output

```shell 1: [test_trajectory_cache-8] [INFO] [1716031915.906177908] [test_node]: Opening trajectory cache database at: :memory: (Port: 0, Precision: 0.000100) 1: [test_trajectory_cache-8] [PASS] init: Cache init 1: [move_group-1] [WARN] [1716031915.915125008] [kdl_parser]: The root link panda_link0 has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF. 1: [move_group-1] [INFO] [1716031915.915213428] [move_group.moveit.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'panda_arm': 1 1 1 1 1 1 1 1: [test_trajectory_cache-8] [INFO] [1716031915.918141848] [test_trajectory_cache_node.moveit.ros.rdf_loader]: Loaded robot model in 0.00205568 seconds 1: [test_trajectory_cache-8] [INFO] [1716031915.918204538] [test_trajectory_cache_node.moveit.core.robot_model]: Loading robot model 'panda'... 1: [test_trajectory_cache-8] [WARN] [1716031915.928728938] [kdl_parser]: The root link panda_link0 has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF. 1: [test_trajectory_cache-8] [INFO] [1716031915.928822188] [test_trajectory_cache_node.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'panda_arm': 1 1 1 1 1 1 1 1: [move_group-1] [INFO] [1716031915.951168767] [move_group.moveit.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' 1: [move_group-1] [INFO] [1716031915.951473138] [move_group.moveit.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states 1: [move_group-1] [INFO] [1716031915.953246827] [move_group.moveit.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' 1: [move_group-1] [INFO] [1716031915.953958707] [move_group.moveit.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects 1: [move_group-1] [INFO] [1716031915.953990867] [move_group.moveit.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. 1: [move_group-1] [INFO] [1716031915.954258597] [move_group.moveit.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. 1: [move_group-1] [INFO] [1716031915.954646377] [move_group.moveit.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' 1: [move_group-1] [INFO] [1716031915.954748417] [move_group.moveit.moveit.ros.planning_scene_monitor]: Starting planning scene monitor 1: [move_group-1] [INFO] [1716031915.955193077] [move_group.moveit.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' 1: [move_group-1] [INFO] [1716031915.955220027] [move_group.moveit.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. 1: [move_group-1] [INFO] [1716031915.956105797] [move_group.moveit.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' 1: [move_group-1] [INFO] [1716031915.956574547] [move_group.moveit.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry 1: [move_group-1] [WARN] [1716031915.957054387] [move_group.moveit.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead 1: [move_group-1] [ERROR] [1716031915.957085667] [move_group.moveit.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates 1: [move_group-1] [INFO] [1716031915.975037547] [move_group.moveit.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' 1: [move_group-1] [INFO] [1716031915.977461317] [move_group]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' 1: [move_group-1] [INFO] [1716031915.978431077] [move_group]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' 1: [move_group-1] [INFO] [1716031915.978455127] [move_group]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' 1: [move_group-1] [INFO] [1716031915.978746577] [move_group]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' 1: [move_group-1] [INFO] [1716031915.978771117] [move_group]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' 1: [move_group-1] [INFO] [1716031915.978796647] [move_group]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' 1: [move_group-1] [INFO] [1716031915.978803257] [move_group]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' 1: [move_group-1] [INFO] [1716031915.978815457] [move_group]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' 1: [move_group-1] [INFO] [1716031915.981106127] [move_group]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' 1: [move_group-1] [INFO] [1716031915.983534637] [move_group]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' 1: [move_group-1] [INFO] [1716031915.983555487] [move_group]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' 1: [move_group-1] [INFO] [1716031915.985705137] [move_group]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' 1: [move_group-1] [INFO] [1716031915.985731557] [move_group]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' 1: [move_group-1] [INFO] [1716031915.986349867] [move_group]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' 1: [move_group-1] [INFO] [1716031916.038589177] [move_group.moveit.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for panda_arm_controller 1: [move_group-1] [INFO] [1716031916.038664217] [move_group.moveit.moveit.plugins.simple_controller_manager]: Max effort set to 0.0 1: [move_group-1] [INFO] [1716031916.041401486] [move_group.moveit.moveit.plugins.simple_controller_manager]: Added GripperCommand controller for panda_hand_controller 1: [move_group-1] [INFO] [1716031916.041584756] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list 1: [move_group-1] [INFO] [1716031916.041631466] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list 1: [move_group-1] [INFO] [1716031916.042251016] [move_group.moveit.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers 1: [move_group-1] [INFO] [1716031916.042280546] [move_group]: MoveGroup debug mode is ON 1: [move_group-1] [INFO] [1716031916.065692586] [move_group.moveit.moveit.ros.move_group.executable]: 1: [move_group-1] 1: [move_group-1] ******************************************************** 1: [move_group-1] * MoveGroup using: 1: [move_group-1] * - apply_planning_scene_service 1: [move_group-1] * - clear_octomap_service 1: [move_group-1] * - get_group_urdf 1: [move_group-1] * - CartesianPathService 1: [move_group-1] * - execute_trajectory_action 1: [move_group-1] * - get_planning_scene_service 1: [move_group-1] * - kinematics_service 1: [move_group-1] * - move_action 1: [move_group-1] * - motion_plan_service 1: [move_group-1] * - query_planners_service 1: [move_group-1] * - state_validation_service 1: [move_group-1] ******************************************************** 1: [move_group-1] 1: [move_group-1] [INFO] [1716031916.065774356] [move_group.moveit.moveit.ros.move_group.context]: MoveGroup context using pipeline ompl 1: [move_group-1] [INFO] [1716031916.065789836] [move_group.moveit.moveit.ros.move_group.context]: MoveGroup context initialization complete 1: [test_trajectory_cache-8] [INFO] [1716031916.069942436] [test_trajectory_cache_node.moveit.ros.move_group_interface]: Ready to take commands for planning group panda_arm. 1: [test_trajectory_cache-8] [INFO] [1716031916.072211106] [test_trajectory_cache_node.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' 1: [test_trajectory_cache-8] [WARN] [1716031916.082699776] [test_trajectory_cache_node.moveit.ros.current_state_monitor]: Unable to update multi-DOF joint 'virtual_joint':Failure to lookup transform between 'world'and 'panda_link0' with TF exception: "world" passed to lookupTransform argument target_frame does not exist. 1: [ros2_control_node-4] [INFO] [1716031916.531930089] [controller_manager]: Loading controller 'joint_state_broadcaster' 1: [ros2_control_node-4] [INFO] [1716031916.559834818] [controller_manager]: Loading controller 'panda_hand_controller' 1: [ros2 run controller_manager spawner joint_state_broadcaster-7] [INFO] [1716031916.583473377] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster 1: [ros2_control_node-4] [INFO] [1716031916.585386557] [controller_manager]: Configuring controller 'joint_state_broadcaster' 1: [ros2_control_node-4] [INFO] [1716031916.585539357] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published 1: [ros2 run controller_manager spawner panda_hand_controller-6] [INFO] [1716031916.597440578] [spawner_panda_hand_controller]: Loaded panda_hand_controller 1: [ros2_control_node-4] [INFO] [1716031916.598982468] [controller_manager]: Configuring controller 'panda_hand_controller' 1: [ros2_control_node-4] [INFO] [1716031916.599078648] [panda_hand_controller]: Action status changes will be monitored at 20Hz. 1: [ros2 run controller_manager spawner joint_state_broadcaster-7] [INFO] [1716031916.627831697] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster 1: [ros2 run controller_manager spawner panda_hand_controller-6] [INFO] [1716031916.647980687] [spawner_panda_hand_controller]: Configured and activated panda_hand_controller 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.empty: Trajectory cache initially empty 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.empty: Fetch all trajectories on empty cache returns empty 1: [test_trajectory_cache-8] [INFO] [1716031916.818075454] [test_node]: No matching trajectories found. 1: [INFO] [ros2 run controller_manager spawner joint_state_broadcaster-7]: process has finished cleanly [pid 2480904] 1: [test_trajectory_cache-8] [ERROR] [1716031916.818141824] [test_node]: Skipping plan insert: Frame IDs cannot be empty. 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.empty: Fetch best trajectory on empty cache returns nullptr 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_trajectory_empty_frame: Put empty frame trajectory, no delete_worse_trajectories, not ok 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_trajectory_empty_frame: No trajectories in cache 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_trajectory_req_empty_frame: Put empty frame req trajectory, no delete_worse_trajectories, not ok 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_trajectory_req_empty_frame: No trajectories in cache 1: [test_trajectory_cache-8] [ERROR] [1716031916.818231104] [test_node]: Skipping plan insert: Frame IDs cannot be empty. 1: [test_trajectory_cache-8] [INFO] [1716031916.818670524] [test_node]: Inserting trajectory: New trajectory execution_time (9.990000e+02s) is better than best trajectory's execution_time (infs) 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first: Put first valid trajectory, no delete_worse_trajectories, ok 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first: One trajectory in cache 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_matching_no_tolerance: Fetch all returns one 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_matching_no_tolerance: Fetch best trajectory is not nullptr 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_matching_no_tolerance: Fetched trajectory on both fetches match 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_matching_no_tolerance: Fetched trajectory matches original 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_matching_no_tolerance: Fetched trajectory has correct execution time 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_matching_no_tolerance: Fetched trajectory has correct planning time 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_is_diff_no_tolerance: Fetch all returns one 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_is_diff_no_tolerance: Fetch best trajectory is not nullptr 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_is_diff_no_tolerance: Fetched trajectory on both fetches match 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_is_diff_no_tolerance: Fetched trajectory matches original 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_is_diff_no_tolerance: Fetched trajectory has correct execution time 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_is_diff_no_tolerance: Fetched trajectory has correct planning time 1: [test_trajectory_cache-8] [INFO] [1716031916.838228644] [test_node]: No matching trajectories found. 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_non_matching_out_of_tolerance: Fetch all returns empty 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_non_matching_out_of_tolerance: Fetch best trajectory is nullptr 1: [test_trajectory_cache-8] [INFO] [1716031916.839152504] [test_node]: No matching trajectories found. 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_non_matching_only_start_in_tolerance: Fetch all returns empty 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_non_matching_only_start_in_tolerance: Fetch best trajectory is nullptr 1: [test_trajectory_cache-8] [INFO] [1716031916.839659694] [test_node]: No matching trajectories found. 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_non_matching_only_goal_in_tolerance: Fetch all returns empty 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_non_matching_only_goal_in_tolerance: Fetch best trajectory is nullptr 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_non_matching_in_tolerance: Fetch all returns one 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_non_matching_in_tolerance: Fetch best trajectory is not nullptr 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_non_matching_in_tolerance: Fetched trajectory on both fetches match 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_non_matching_in_tolerance: Fetched trajectory matches original 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_non_matching_in_tolerance: Fetched trajectory has correct execution time 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_non_matching_in_tolerance: Fetched trajectory has correct planning time 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_swapped: Fetch all returns one 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_swapped: Fetch best trajectory is not nullptr 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_swapped: Fetched trajectory on both fetches match 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_swapped: Fetched trajectory matches original 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_swapped: Fetched trajectory has correct execution time 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_swapped: Fetched trajectory has correct planning time 1: [test_trajectory_cache-8] [INFO] [1716031916.842295294] [test_node]: No matching trajectories found. 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_smaller_workspace: Fetch all returns empty 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_smaller_workspace: Fetch best trajectory is nullptr 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_larger_workspace: Fetch all returns one 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_larger_workspace: Fetch best trajectory is not nullptr 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_larger_workspace: Fetched trajectory on both fetches match 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_larger_workspace: Fetched trajectory matches original 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_larger_workspace: Fetched trajectory has correct execution time 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_larger_workspace: Fetched trajectory has correct planning time 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_larger_workspace: Fetched trajectory has more restrictive workspace max_corner 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_larger_workspace: Fetched trajectory has more restrictive workspace min_corner 1: [test_trajectory_cache-8] [INFO] [1716031916.843924924] [test_node]: Skipping plan insert: New trajectory execution_time (1.099000e+03s) is worse than best trajectory's execution_time (9.990000e+02s) 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_worse: Put worse trajectory, no delete_worse_trajectories, not ok 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_worse: One trajectory in cache 1: [test_trajectory_cache-8] [INFO] [1716031916.844476654] [test_node]: Inserting trajectory: New trajectory execution_time (8.990000e+02s) is better than best trajectory's execution_time (9.990000e+02s) 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_better: Put better trajectory, no delete_worse_trajectories, ok 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_better: Two trajectories in cache 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_better.fetch_sorted: Fetch all returns two 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_better.fetch_sorted: Fetch best trajectory is not nullptr 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_better.fetch_sorted: Fetched trajectory on both fetches match 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_better.fetch_sorted: Fetched trajectory matches original 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_better.fetch_sorted: Fetched trajectory has correct execution time 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_better.fetch_sorted: Fetched trajectory has correct planning time 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_better.fetch_sorted: Fetched trajectories are sorted correctly 1: [test_trajectory_cache-8] [INFO] [1716031916.846008374] [test_node]: Overwriting plan (id: 2): execution_time (8.990000e+02s) > new trajectory's execution_time (7.990000e+02s) 1: [test_trajectory_cache-8] [INFO] [1716031916.846087244] [test_node]: Overwriting plan (id: 1): execution_time (9.990000e+02s) > new trajectory's execution_time (7.990000e+02s) 1: [test_trajectory_cache-8] [INFO] [1716031916.846138374] [test_node]: Inserting trajectory: New trajectory execution_time (7.990000e+02s) is better than best trajectory's execution_time (8.990000e+02s) 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_better_delete_worse_trajectories: Put better trajectory, delete_worse_trajectories, ok 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_better_delete_worse_trajectories: One trajectory in cache 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_better_delete_worse_trajectories.fetch: Fetch all returns one 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_better_delete_worse_trajectories.fetch: Fetch best trajectory is not nullptr 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_better_delete_worse_trajectories.fetch: Fetched trajectory on both fetches match 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_better_delete_worse_trajectories.fetch: Fetched trajectory matches original 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_better_delete_worse_trajectories.fetch: Fetched trajectory has correct execution time 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_better_delete_worse_trajectories.fetch: Fetched trajectory has correct planning time 1: [test_trajectory_cache-8] [INFO] [1716031916.847252094] [test_node]: Inserting trajectory: New trajectory execution_time (8.990000e+02s) is better than best trajectory's execution_time (infs) 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_different_req: Put different trajectory req, delete_worse_trajectories, ok 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_different_req: Two trajectories in cache 1: [INFO] [ros2 run controller_manager spawner panda_hand_controller-6]: process has finished cleanly [pid 2480903] 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_different_req.fetch: Fetch all returns one 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_different_req.fetch: Fetch best trajectory is not nullptr 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_different_req.fetch: Fetched trajectory on both fetches match 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_different_req.fetch: Fetched trajectory matches original 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_different_req.fetch: Fetched trajectory has correct execution time 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_different_req.fetch: Fetched trajectory has correct planning time 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.different_robot.empty: No trajectories in cache 1: [test_trajectory_cache-8] [INFO] [1716031916.848691324] [test_node]: Inserting trajectory: New trajectory execution_time (8.990000e+02s) is better than best trajectory's execution_time (infs) 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.different_robot.put_first: Put different trajectory req, delete_worse_trajectories, ok 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.different_robot.put_first: One trajectories in cache 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.different_robot.put_first: One trajectories in cache 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.different_robot.put_first: Two trajectories in original robot's cache 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.different_robot.put_first: Fetch all on original still returns one 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.construct_get_cartesian_path_request: Ok 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.empty: Trajectory cache initially empty 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.empty: Fetch all trajectories on empty cache returns empty 1: [test_trajectory_cache-8] [INFO] [1716031916.857743763] [test_node]: No matching cartesian trajectories found. 1: [test_trajectory_cache-8] [ERROR] [1716031916.857776503] [test_node]: Skipping cartesian trajectory insert: Frame IDs cannot be empty. 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.empty: Fetch best trajectory on empty cache returns nullptr 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_trajectory_empty_frame: Put empty frame trajectory, no delete_worse_trajectories, not ok 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_trajectory_empty_frame: No trajectories in cache 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_trajectory_req_empty_frame: Put empty frame req trajectory, no delete_worse_trajectories, not ok 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_trajectory_req_empty_frame: No trajectories in cache 1: [test_trajectory_cache-8] [ERROR] [1716031916.857845583] [test_node]: Skipping cartesian trajectory insert: Frame IDs cannot be empty. 1: [test_trajectory_cache-8] [INFO] [1716031916.858312403] [test_node]: Inserting cartesian trajectory: New trajectory execution_time (9.990000e+02s) is better than best trajectory's execution_time (infs) at fraction (5.000000e-01s) 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first: Put first valid trajectory, no delete_worse_trajectories, ok 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first: One trajectory in cache 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first.fetch_matching_no_tolerance: Fetch all returns one 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first.fetch_matching_no_tolerance: Fetch best trajectory is not nullptr 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first.fetch_matching_no_tolerance: Fetched trajectory on both fetches match 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first.fetch_matching_no_tolerance: Fetched trajectory matches original 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first.fetch_matching_no_tolerance: Fetched trajectory has correct execution time 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first.fetch_matching_no_tolerance: Fetched trajectory has correct planning time 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first.fetch_matching_no_tolerance: Fetched trajectory has correct fraction 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first.fetch_is_diff_no_tolerance: Fetch all returns one 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first.fetch_is_diff_no_tolerance: Fetch best trajectory is not nullptr 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first.fetch_is_diff_no_tolerance: Fetched trajectory on both fetches match 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first.fetch_is_diff_no_tolerance: Fetched trajectory matches original 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first.fetch_is_diff_no_tolerance: Fetched trajectory has correct execution time 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first.fetch_is_diff_no_tolerance: Fetched trajectory has correct planning time 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first.fetch_is_diff_no_tolerance: Fetched trajectory has correct fraction 1: [test_trajectory_cache-8] [INFO] [1716031916.888140753] [test_node]: No matching cartesian trajectories found. 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first.fetch_non_matching_out_of_tolerance: Fetch all returns empty 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first.fetch_non_matching_out_of_tolerance: Fetch best trajectory is nullptr 1: [test_trajectory_cache-8] [INFO] [1716031916.888692013] [test_node]: No matching cartesian trajectories found. 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_non_matching_only_start_in_tolerance: Fetch all returns empty 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_non_matching_only_start_in_tolerance: Fetch best trajectory is nullptr 1: [test_trajectory_cache-8] [INFO] [1716031916.889167503] [test_node]: No matching cartesian trajectories found. 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_non_matching_only_goal_in_tolerance: Fetch all returns empty 1: [test_trajectory_cache-8] [PASS] test_motion_trajectories.put_first.fetch_non_matching_only_goal_in_tolerance: Fetch best trajectory is nullptr 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first.fetch_non_matching_in_tolerance: Fetch all returns one 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first.fetch_non_matching_in_tolerance: Fetch best trajectory is not nullptr 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first.fetch_non_matching_in_tolerance: Fetched trajectory on both fetches match 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first.fetch_non_matching_in_tolerance: Fetched trajectory matches original 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first.fetch_non_matching_in_tolerance: Fetched trajectory has correct execution time 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first.fetch_non_matching_in_tolerance: Fetched trajectory has correct planning time 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first.fetch_non_matching_in_tolerance: Fetched trajectory has correct fraction 1: [test_trajectory_cache-8] [INFO] [1716031916.890435153] [test_node]: No matching cartesian trajectories found. 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first.fetch_higher_fraction: Fetch all returns empty 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first.fetch_higher_fraction: Fetch best trajectory is nullptr 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first.fetch_lower_fraction: Fetch all returns one 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first.fetch_lower_fraction: Fetch best trajectory is not nullptr 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first.fetch_lower_fraction: Fetched trajectory on both fetches match 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first.fetch_lower_fraction: Fetched trajectory matches original 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first.fetch_lower_fraction: Fetched trajectory has correct execution time 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first.fetch_lower_fraction: Fetched trajectory has correct planning time 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_first.fetch_lower_fraction: Fetched trajectory has correct fraction 1: [test_trajectory_cache-8] [INFO] [1716031916.891883133] [test_node]: Skipping cartesian trajectory insert: New trajectory execution_time (1.099000e+03s) is worse than best trajectory's execution_time (9.990000e+02s) at fraction (5.000000e-01s) 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_worse: Put worse trajectory, no delete_worse_trajectories, not ok 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_worse: One trajectory in cache 1: [test_trajectory_cache-8] [INFO] [1716031916.892428183] [test_node]: Inserting cartesian trajectory: New trajectory execution_time (8.990000e+02s) is better than best trajectory's execution_time (9.990000e+02s) at fraction (5.000000e-01s) 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_better: Put better trajectory, no delete_worse_trajectories, ok 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_better: Two trajectories in cache 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_better.fetch_sorted: Fetch all returns two 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_better.fetch_sorted: Fetch best trajectory is not nullptr 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_better.fetch_sorted: Fetched trajectory on both fetches match 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_better.fetch_sorted: Fetched trajectory matches original 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_better.fetch_sorted: Fetched trajectory has correct execution time 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_better.fetch_sorted: Fetched trajectory has correct planning time 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_better.fetch_sorted: Fetched trajectory has correct fraction 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_better.fetch_sorted: Fetched trajectories are sorted correctly 1: [test_trajectory_cache-8] [INFO] [1716031916.894319463] [test_node]: Overwriting cartesian trajectory (id: 2): execution_time (8.990000e+02s) > new trajectory's execution_time (7.990000e+02s) 1: [test_trajectory_cache-8] [INFO] [1716031916.894368903] [test_node]: Overwriting cartesian trajectory (id: 1): execution_time (9.990000e+02s) > new trajectory's execution_time (7.990000e+02s) 1: [test_trajectory_cache-8] [INFO] [1716031916.894402943] [test_node]: Inserting cartesian trajectory: New trajectory execution_time (7.990000e+02s) is better than best trajectory's execution_time (8.990000e+02s) at fraction (5.000000e-01s) 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_better_delete_worse_trajectories: Put better trajectory, delete_worse_trajectories, ok 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_better_delete_worse_trajectories: One trajectory in cache 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_better_delete_worse_trajectories.fetch: Fetch all returns one 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_better_delete_worse_trajectories.fetch: Fetch best trajectory is not nullptr 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_better_delete_worse_trajectories.fetch: Fetched trajectory on both fetches match 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_better_delete_worse_trajectories.fetch: Fetched trajectory matches original 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_better_delete_worse_trajectories.fetch: Fetched trajectory has correct execution time 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_better_delete_worse_trajectories.fetch: Fetched trajectory has correct planning time 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_better_delete_worse_trajectories.fetch: Fetched trajectory has correct fraction 1: [test_trajectory_cache-8] [INFO] [1716031916.895401613] [test_node]: Inserting cartesian trajectory: New trajectory execution_time (8.990000e+02s) is better than best trajectory's execution_time (infs) at fraction (5.000000e-01s) 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_different_req: Put different trajectory req, delete_worse_trajectories, ok 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_different_req: Two trajectories in cache 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_different_req.fetch: Fetch all returns one 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_different_req.fetch: Fetch best trajectory is not nullptr 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_different_req.fetch: Fetched trajectory on both fetches match 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_different_req.fetch: Fetched trajectory matches original 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_different_req.fetch: Fetched trajectory has correct execution time 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_different_req.fetch: Fetched trajectory has correct planning time 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.put_different_req.fetch: Fetched trajectory has correct fraction 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.different_robot.empty: No trajectories in cache 1: [test_trajectory_cache-8] [INFO] [1716031916.896685943] [test_node]: Inserting cartesian trajectory: New trajectory execution_time (8.990000e+02s) is better than best trajectory's execution_time (infs) at fraction (5.000000e-01s) 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.different_robot.put_first: Put different trajectory req, delete_worse_trajectories, ok 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.different_robot.put_first: One trajectories in cache 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.different_robot.put_first: One trajectories in cache 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.different_robot.put_first: Two trajectories in original robot's cache 1: [test_trajectory_cache-8] [PASS] test_cartesian_trajectories.different_robot.put_first: Fetch all on original still returns one ```

moveit / moveit2

Implement fuzzy-matching Trajectory Cache 🔥 #2838