Add Trajectory Cache Example For Refactor

[methylDragon]

Description

This PR builds on top of, and is related to:

• (WIP) Add Trajectory Cache Example #929

In the same way that

• This PR:
  • Implement fuzzy-matching Trajectory Cache 🔥 moveit2#2838
• Builds on top of and is related to:
  • Fuzzy-matching Trajectory Cache Injectable Traits refactor 🔥🔥 moveit2#2941

This PR changes the demo code to support the TrajectoryCache refactor.

It also ADDS VIDEOS AND IMAGES.

Setup

sudo apt update
source /opt/ros/rolling

git clone -b ch3/trajectory-cache-refactor https://github.com/moveit/moveit2_tutorials.git
git clone -b ch3/trajectory-cache-refactor https://github.com/moveit/moveit2.git
vcs import < moveit2_tutorials/moveit2_tutorials.repos

# For some reason these are extra steps.
git clone -b ros2 https://github.com/moveit/py_binding_tools.git
sudo apt install ros-rolling-moveit-planners-chomp

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

Running

source /opt/ros/rolling
source install/setup.bash

# In one terminal: Start the move group
ros2 launch moveit2_tutorials trajectory_cache_move_group.launch.py

# In another: Launch the demo
ros2 launch moveit2_tutorials trajectory_cache_demo.launch.py

NOTE: Sometimes a randomly generated demo goal is unreachable (or there's a weird move_group issue). Just restart the demo to resolve the issue.

Demo

The demo is a 4-phase demo. Hit next on the rviz visual tools dialog to advance the demo!

See video: It showcases cache population, pruning, and fetching (4x speed)

1. Plan and cache (no pruning)
2. Plan and cache (with pruning)
3. Fetch from cache and execute (also still planning and caching with pruning)
4. Fetch from cache and execute, except with huge start tolerances

For (4), you should see the robot arm "snap" to the fetched trajectory. The diff of the robot's start position to the trajectory is visualized in red to be obvious.

Screenrecording2024-07-3012.33.03AM-ezgif.com-crop-video.2.webm

Additional Notes

PS: I fully recognize that the template instantiations can look a bit intimidating.

e.g.

std::vector<std::unique_ptr<moveit_ros::trajectory_cache::FeaturesInterface<moveit_msgs::srv::GetCartesianPath::Request>>>
              additional_features;
          additional_features.push_back(
              std::make_unique<
                  moveit_ros::trajectory_cache::MetadataOnlyFeature<double, moveit_msgs::srv::GetCartesianPath::Request>>(
                  "planning_time_s", (cartesian_plan_end - cartesian_plan_start).seconds()));

          trajectory_cache.insertCartesianTrajectory(  // Returns bool. True if put.
              move_group, /*cache_namespace=*/PLANNING_GROUP, cartesian_path_request, cartesian_path_response,
              /*cache_insert_policy=*/*cartesian_cache_insert_policy,
              /*prune_worse_trajectories=*/reconfigurable_parameters.prune_worse_trajectories,
              /*additional_features=*/additional_features);

These can be mitigated by using declarations to carve out the namespaces. But I left it in for explanatory purposes.

(example cleaned up version)

std::vector<std::unique_ptr<FeaturesInterface<GetCartesianPath::Request>>>
    additional_features;
additional_features.push_back(
    std::make_unique<MetadataOnlyFeature<double, GetCartesianPath::Request>>(
        "planning_time_s",
        (cartesian_plan_end - cartesian_plan_start).seconds()));

trajectory_cache.insertCartesianTrajectory(
    move_group, PLANNING_GROUP, cartesian_path_request, cartesian_path_response,
    *cartesian_cache_insert_policy,
    reconfigurable_parameters.prune_worse_trajectories, additional_features);
}