WIP

Signed-off-by: methylDragon <methylDragon@gmail.com>

doc/how_to_guides/how_to_guides.rst

@@ -25,6 +25,7 @@ Configuring and Using MoveIt
    pick_ik/pick_ik_tutorial
    trac_ik/trac_ik_tutorial
    benchmarking/benchmarking_tutorial
+   trajectory_cache/trajectory_cache_tutorial
 
 Developing and Documenting MoveIt
 ---------------------------------

doc/how_to_guides/trajectory_cache/src/trajectory_cache_demo.cpp

@@ -56,13 +56,20 @@
  *
  * NOTE:
  *   Tutorial notes will be commented like this block!
- *
+ * 
  * PRE-REQUISITES
  * ^^^^^^^^^^^^^^
  * This tutorial assumes knowledge of the MoveGroupInterface.
  *
  * INTERACTIVITY
  * ^^^^^^^^^^^^^
+ * This demo has four phases that can be advanced using the `rviz_visual_tools` dialog box.
+ * 
+ * 1. Plan and cache (no pruning)
+ * 2. Plan and cache (with pruning)
+ * 3. Fetch from cache and execute (while still planning and caching with pruning)
+ * 4. Fetch from cache and execute, except with large start tolerances
+ * 
  * This tutorial also supports "reconfigurable" parameters!:
  *
  * You can adjust them with:
@@ -71,7 +78,7 @@
  * Tutorial parameters:
  *   - planner:
  *       Defaults to "RRTstar". The OMPL planner used.
- *       It's better to use a random-sampling, non-optimal planner to see the cache working.
+ *       It is better to use a random-sampling, non-optimal planner to see the cache working.
  *
  * Cache parameters:
  *   - start_tolerance:
@@ -648,6 +655,19 @@ int main(int argc, char** argv)
 
   // Interactivity. ================================================================================
 
+  /** [TUTORIAL NOTE]
+   * This demo has four phases that can be advanced using the `rviz_visual_tools` dialog box.
+   * 
+   * 1. Plan and cache (no pruning)
+   *   - Showcases basic cache functionality.
+   * 2. Plan and cache (with pruning)
+   *   - Showcases pruning functionality.
+   * 3. Fetch from cache and execute (while still planning and caching with pruning)
+   *   - Showcases cache fetches.
+   * 4. Fetch from cache and execute, except with large start tolerances
+   *   - Showcases fuzzy matching, and the impact of start tolerance.
+   */
+
   std::atomic<size_t> demo_phase = 0;
   std::atomic<bool> run_execute = false;
 

doc/how_to_guides/trajectory_cache/trajectory_cache_tutorial.rst

@@ -0,0 +1,203 @@
+Fuzzy-Matching Trajectory Cache - Quickstart Guide
+==================================================
+
+The fuzzy-matching trajectory cache allows you to insert and fetch manipulator trajectories, keyed fuzzily on ``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.
+
+.. raw:: html
+
+    <video width="450px" controls="true" autoplay="true" loop="true">
+        <source src="../../../_static/videos/trajectory_cache/01_cache_and_execute_loop.webm" type="video/webm">
+        Introduction video showing the trajectory cache working. 
+    </video>
+
+For more info, please see the :moveit_codedir:`package README <moveit_ros/trajectory_cache/README.md>`.
+
+.. warning::
+  The cache does NOT support collision detection and some other constraints (e.g. multi-DoF joints, planning scene, constraint regions, and visibility constraints)!
+
+  Plans will be inserted into and fetched from the cache IGNORING collision and those constraints.
+  If your planning scene is expected to change significantly between cache lookups, ensure that you validate any fetched plans for collisions.
+
+Quick Example Usage
+-------------------
+
+**PRE-REQUISITE**: The ``warehouse_plugin`` ROS parameter must be set to a `warehouse_ros <https://github.com/moveit/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);
+
+  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 variants of the methods in the above example exist.
+
+For more information, please read the extensive API documentation for :cpp_api:`moveit_ros::trajectory_cache::TrajectoryCache`.
+
+Why A Cache?
+------------
+
+A trajectory cache helps:
+
+- Cut down on planning time
+- Allows for consistent predictable behavior of used together with a stochastic planner
+
+  - It effectively allows you to "freeze" a move
+
+To explain this, consider that planners in MoveIt generally fall under two main camps: stochastic/probabilistic, and optimization based.
+The probabilistic planners are fast, but usually non-deterministic, whereas the optimization based ones are usually slow, but deterministic.
+
+One way to get around this is to pre-plan and manually select and label robot trajectories to "freeze" in a trajectory database to then replay by name, which avoids needing to spend time to replan, and allows you to ensure that motions are constant and repeatable.
+However, this approach is not very flexible and not easily reused.
+
+The trajectory cache improves upon this approach by allowing a user to "freeze" and store successful plans, but **also** look up those plans in a more generalizable and natural way, using the planning request itself to key the cache, effectively allowing the cache to stand in for a planning call.
+
+Furthermore, the specific properties of this trajectory cache provides further unique benefits:
+
+1. With fuzzy matching, "frozen" plans are much easier to look up and reuse, while simultaneously increasing the chances of a cache hit.
+2. The ability to rank trajectories will, if used with a stochastic planner over sufficient runs, cause the cache to eventually converge to increasingly optimal plans.
+
+Finally, the cache makes use of pruning to optimize fetch times, and also finds ways to "canonicalize" features of the keying request to increase chances of a cache hit.
+
+Working Principle
+-----------------
+If a plan request has features (e.g., start, goal, and constraint conditions) that are "close enough" to an entry in the cache, then the cached trajectory should be reusable for that request, allowing us to skip planning.
+
+Demo
+----
+
+.. raw:: html
+
+    <video width="700px" controls="true" autoplay="true" loop="true">
+        <source src="../../../_static/videos/trajectory_cache/02_trajectory_cache_demo.webm" type="video/webm">
+        Trajectory cache demo.
+    </video>
+
+(Video is 4x speed.)
+
+This demo has four phases that can be advanced using the `rviz_visual_tools` dialog box, which combined, showcases the highlights of the cache's capabilities.
+
+1. Plan and cache (no pruning)
+2. Plan and cache (with pruning)
+3. Fetch from cache and execute (while still planning and caching with pruning)
+4. Fetch from cache and execute, except with large start tolerances
+
+Additionally, the `demo source code itself <how_to_guides/trajectory_cache/src/trajectory_cache_demo.cpp>`_ is heavily annotated with comments and tutorial notes, should you wish to inspect the code and dive deeper.
+
+Pre-Requisites
+++++++++++++++
+
+Make sure you have built the `moveit2_tutorials <https://github.com/moveit/moveit2_tutorials>`_ package and sourced the workspace it is built in to ensure that you can run the demo.
+
+Running the Demo
+++++++++++++++++
+
+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
+
+You should see something similar to the video, except with a different set of randomly generated goal poses.
+
+Click ``next`` on the rviz window to advance the demo.
+
+.. note::
+  Sometimes a randomly generated demo goal pose is unreachable (or the ``move_group`` fails to initialize properly).
+  If this happens, the demo will halt in the first phase due to a failed planning call or other issue.
+
+  Just restart the demo, which will generate new demo goal poses, and resolve the issue.
+
+Configuring the Demo
+++++++++++++++++++++
+
+Additionally, the demo's launch file exposes launch arguments that allows you to change many aspects of the demo.
+Look at the `demo's launch file <how_to_guides/trajectory_cache/launch/trajectory_cache_demo.launch.py>`_ for the full list of configurable launch arguments.
+
+For example, you can specify a disk path for the cache to be saved to and loaded from, instead of memory:
+
+::
+
+  ros2 launch moveit2_tutorials trajectory_cache_demo.launch.py cache_db_host:="<some_directory>/trajectory_cache_demo.db
+
+Then, since the demo uses SQLite by default, you can inspect the cache database using an `SQLite database browser <https://sqlitebrowser.org/>`_.
+
+"Reconfigurable Parameters"
++++++++++++++++++++++++++++
+
+This tutorial also supports "reconfigurable" parameters!
+
+You can adjust them with ::
+
+  ros2 param set /trajectory_cache_demo <parameter_name> <parameter_value>
+
+You should then see them change on the ``rviz`` window, in the demo's visualization.
+
+- **Tutorial parameters**
+
+  - ``planner``:
+
+    -  Defaults to ``RRTstar``. The OMPL planner used.
+    -  It is better to use a random-sampling, non-optimal planner to see the cache working.
+
+- **Cache parameters**
+
+  - ``start_tolerance``:
+
+    - Defaults to ``0.025``. Determines the fuzziness of matching on planning start constraints.
+
+  - ``goal_tolerance``:
+
+    - Defaults to ``0.001``. Determines the fuzziness of matching on planning goal constraints.
+
+  - ``prune_worse_trajectories``:
+
+    - Defaults to ``true``. Setting this to ``true`` will cause cached plans to be pruned.
+
+1. CacheNoPruning
++++++++++++++++++
+
+2. CacheWithPruning
++++++++++++++++++++
+
+3. ExecuteWithCache
++++++++++++++++++++
+
+4. HighStartTolerance
++++++++++++++++++++++