Implement motion plan cache #16

methylDragon · 2023-09-22T06:02:34Z

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. (very out of scope)

Description

This PR adds a MotionPlanCache class that wraps a warehouse_ros database interface.
It is then incorporated into the motion_planning_server.

Features:

Put and fetch RobotTrajectory plans keyed by MotionPlanRequest message values
- This includes ALL joint, position, and orientation constraints!
Supports fuzzy matching on the start and goal params in the request
Supports overwriting "worse" plans with "better" plans (this prunes the database and keeps it tidy!)
Separate caches for separate move groups

Furthermore, because our goals are stated in zero-offset TFs, the cache does a TF lookup and restates them in the database with respect to the workspace frame! (i.e. wrt. base_link.)

I could only test this on sqlite3. I couldn't set up MongoDB on my setup.

Also, while this is not 100% generalizable to all moveit plannable plans, I think this is a good candidate for upstreaming! 🎉

Why is this PR so big?

It's not trivial to key the motion plans. This is because there are many factors that go into the plan request, and the fulfilling plan. It should be visible from how I'm handling each factor on review.

I'll attach example requests, and an example generated trajectory. On inspecting those messages, it should be clear that it's not trivial to key them (but I think I managed to do it!)

Start State (MotionPlanRequest) Variations

`is_diff=true`, which requires getting the current state

workspace_parameters:
  header:
    stamp:
      sec: 1695075603
      nanosec: 61941176
    frame_id: "base_link"
  min_corner:
    x: -0.570000
    y: -0.300000
    z: 0.00000
  max_corner:
    x: 0.830000
    y: 0.850000
    z: 1.39000
start_state:
  joint_state:
    header:
      stamp:
        sec: 0
        nanosec: 0
      frame_id: ""
    name: []
    position: []
    velocity: []
    effort: []
  multi_dof_joint_state:
    header:
      stamp:
        sec: 0
        nanosec: 0
      frame_id: ""
    joint_names: []
    transforms: []
    twist: []
    wrench: []
  attached_collision_objects: []
  is_diff: true
goal_constraints:
-
  name: ""
  joint_constraints: []
  position_constraints:
  -
    header:
      stamp:
        sec: 0
        nanosec: 0
      frame_id: "pickup_label_goal_pose"
    link_name: "gripper_link"
    target_point_offset:
      x: 0.00000
      y: 0.00000
      z: 0.00000
    constraint_region:  // (CH3) WE'RE IGNORING THIS
      primitives:
      -
        type: 2
        dimensions:
        - 0.000100000
        polygon:
          points: []
      primitive_poses:
      -
        position:
          x: 0.00000
          y: 0.00000
          z: -0.0100000
        orientation:
          x: 0.00000
          y: 0.00000
          z: 0.00000
          w: 1.00000
      meshes: []
      mesh_poses: []
    weight: 1.00000
  orientation_constraints:
  -
    header:
      stamp:
        sec: 0
        nanosec: 0
      frame_id: "pickup_label_goal_pose"
    orientation:
      x: 0.00000
      y: 0.00000
      z: 0.00000
      w: 1.00000
    link_name: "gripper_link"
    absolute_x_axis_tolerance: 0.000100000
    absolute_y_axis_tolerance: 0.000100000
    absolute_z_axis_tolerance: 0.000100000
    parameterization: 0
    weight: 1.00000
  visibility_constraints: []
path_constraints:
  name: ""
  joint_constraints: []
  position_constraints: []
  orientation_constraints: []
  visibility_constraints: []
trajectory_constraints:
  constraints: []
reference_trajectories: []
pipeline_id: ""
planner_id: ""
group_name: "manipulator"
num_planning_attempts: 1
allowed_planning_time: 5.00000
max_velocity_scaling_factor: 0.300000
max_acceleration_scaling_factor: 0.300000
cartesian_speed_end_effector_link: ""
max_cartesian_speed: 0.00000

Or if joint states were explicitly provided as start

workspace_parameters:
  header:
    stamp:
      sec: 1695079404
      nanosec: 4589339
    frame_id: "base_link"
  min_corner:
    x: -0.570000
    y: -0.300000
    z: 0.00000
  max_corner:
    x: 0.830000
    y: 0.850000
    z: 1.39000
start_state:
  joint_state:
    header:
      stamp:
        sec: 0
        nanosec: 0
      frame_id: "base_link"
    name:
    - "joint_1"
    - "joint_2"
    - "joint_3"
    - "joint_4"
    - "joint_5"
    - "joint_6"
    position:
    - 0.00000
    - 0.00000
    - 0.00000
    - 0.00000
    - 0.00000
    - 0.00000
    velocity: []
    effort: []
  multi_dof_joint_state:
    header:
      stamp:
        sec: 0
        nanosec: 0
      frame_id: "base_link"
    joint_names: []
    transforms: []
    twist: []
    wrench: []
  attached_collision_objects: []
  is_diff: false
goal_constraints:
-
  name: ""
  joint_constraints: []
  position_constraints:
  -
    header:
      stamp:
        sec: 0
        nanosec: 0
      frame_id: "pickup_label_goal_pose"
    link_name: "gripper_link"
    target_point_offset:
      x: 0.00000
      y: 0.00000
      z: 0.00000
    constraint_region:
      primitives:
      -
        type: 2
        dimensions:
        - 0.000100000
        polygon:
          points: []
      primitive_poses:
      -
        position:
          x: 0.00000
          y: 0.00000
          z: -0.0100000
        orientation:
          x: 0.00000
          y: 0.00000
          z: 0.00000
          w: 1.00000
      meshes: []
      mesh_poses: []
    weight: 1.00000
  orientation_constraints:
  -
    header:
      stamp:
        sec: 0
        nanosec: 0
      frame_id: "pickup_label_goal_pose"
    orientation:
      x: 0.00000
      y: 0.00000
      z: 0.00000
      w: 1.00000
    link_name: "gripper_link"
    absolute_x_axis_tolerance: 0.000100000
    absolute_y_axis_tolerance: 0.000100000
    absolute_z_axis_tolerance: 0.000100000
    parameterization: 0
    weight: 1.00000
  visibility_constraints: []
path_constraints:
  name: ""
  joint_constraints: []
  position_constraints: []
  orientation_constraints: []
  visibility_constraints: []
trajectory_constraints:
  constraints: []
reference_trajectories: []
pipeline_id: ""
planner_id: ""
group_name: "manipulator"
num_planning_attempts: 1
allowed_planning_time: 5.00000
max_velocity_scaling_factor: 0.300000
max_acceleration_scaling_factor: 0.300000
cartesian_speed_end_effector_link: ""
max_cartesian_speed: 0.00000

Notice for both cases, the goal is stated in terms of a TF frame (which can change.)
The motion plan cache restates the goal in terms of an x, y, z and orientation offset from the planning frame (which should not change.)

Planning frame is consequently part of the key for cache lookups.

Generated Plan Example

joint_trajectory:
  header:
    stamp:
      sec: 0
      nanosec: 0
    frame_id: "base_link"
  joint_names:
  - "joint_1"
  - "joint_2"
  - "joint_3"
  - "joint_4"
  - "joint_5"
  - "joint_6"
  points:
  -
    positions:
    - 0.548301
    - 0.835924
    - 0.543629
    - -0.000849880
    - 0.191294
    - -1.02156
    velocities:
    - 0.0100000
    - 0.000238418
    - 0.00142367
    - -0.00630237
    - -0.00170240
    - -0.00574253
    accelerations:
    - 10.0000
    - 0.238418
    - 1.42367
    - -6.30237
    - -1.70240
    - -5.74253
    effort: []
    time_from_start:
      sec: 0
      nanosec: 0
  -
    positions:
    - 0.598300
    - 0.837116
    - 0.550747
    - -0.0323615
    - 0.182782
    - -1.05027
    velocities:
    - 1.00681
    - 0.0240042
    - 0.143337
    - -0.634529
    - -0.171400
    - -0.578163
    accelerations:
    - 10.0000
    - 0.238418
    - 1.42367
    - -6.30237
    - -1.70240
    - -5.74253
    effort: []
    time_from_start:
      sec: 0
      nanosec: 100000000
  -
    positions:
    - 0.748297
    - 0.840692
    - 0.572102
    - -0.126895
    - 0.157246
    - -1.13641
    velocities:
    - 2.00296
    - 0.0477543
    - 0.285157
    - -1.26234
    - -0.340985
    - -1.15021
    accelerations:
    - 10.0000
    - 0.238418
    - 1.42367
    - -6.30237
    - -1.70240
    - -5.74253
    effort: []
    time_from_start:
      sec: 0
      nanosec: 200000000
  -
    positions:
    - 0.987439
    - 0.846394
    - 0.606148
    - -0.277611
    - 0.116535
    - -1.27373
    velocities:
    - 2.34000
    - 0.0557899
    - 0.333140
    - -1.47476
    - -0.398362
    - -1.34375
    accelerations:
    - -10.0000
    - -0.238418
    - -1.42367
    - 6.30237
    - 1.70240
    - 5.74253
    effort: []
    time_from_start:
      sec: 0
      nanosec: 300000000
  -
    positions:
    - 1.17155
    - 0.850783
    - 0.632359
    - -0.393642
    - 0.0851921
    - -1.37946
    velocities:
    - 1.34000
    - 0.0319480
    - 0.190772
    - -0.844518
    - -0.228122
    - -0.769498
    accelerations:
    - -10.0000
    - -0.238418
    - -1.42367
    - 6.30237
    - 1.70240
    - 5.74253
    effort: []
    time_from_start:
      sec: 0
      nanosec: 400000000
  -
    positions:
    - 1.25565
    - 0.852788
    - 0.644333
    - -0.446650
    - 0.0708737
    - -1.42776
    velocities:
    - 0.340000
    - 0.00810622
    - 0.0484049
    - -0.214281
    - -0.0578817
    - -0.195246
    accelerations:
    - -10.0000
    - -0.238418
    - -1.42367
    - 6.30237
    - 1.70240
    - 5.74253
    effort: []
    time_from_start:
      sec: 0
      nanosec: 500000000
  -
    positions:
    - 1.26147
    - 0.852927
    - 0.645161
    - -0.450315
    - 0.0698835
    - -1.43110
    velocities:
    - 3.00625e-16
    - 7.16745e-18
    - 4.27992e-17
    - -1.89465e-16
    - -5.11785e-17
    - -1.72635e-16
    accelerations:
    - -10.0000
    - -0.238418
    - -1.42367
    - 6.30237
    - 1.70240
    - 5.74253
    effort: []
    time_from_start:
      sec: 0
      nanosec: 534110678
multi_dof_joint_trajectory:
  header:
    stamp:
      sec: 0
      nanosec: 0
    frame_id: ""
  joint_names: []
  points: []

Insight

Notice that keying them requires considering many things, like the varying number of joints, joint constraints; orientation constraints; position constraints; planning frames, etc.

I picked what I think is a good set that covers most of our cases. What I am explicitly not supporting is stated below.

Images

Cache hits and pushes!

A view of the schema

With database collections for each move group

Populated Entries

Notes

Does not support:

Constraint regions
Collision objects
Cartesian planning (introduced in Implement cartesian plan cache #18)
Velocity/Acceleration constraints
Obstacles in the planning scene

It also only partially handles matching in cases where constraints get jumbled up in order. (But this really shouldn't happen, I think.)

TODO

Retrofit the launch files (but what path do I use for the db?)
Test on physical setup. I was only able to test by spoofing "execute_trajectory=True" and not actually executing the trajectory.
- The fuzzy matching parameters should be tuned depending on how much precision we want. (I currently set them to about ~0.5 degrees per joint for the start, and 1mm goal offsets in all axes.)
Unit tests??
Benchmark cache time vs planning times?

Yadunund

Thanks for great effort here! Did a first pass review with some comments.

I'm concerned we're not using the APIs from PlanningSceneStorage class that is already implemented upstream for such caching. It looks like they serialize the MotionPlanRequest msg to check if it's cached before updating the database. Is there a reason we're not able to simply reuse the approach there?

Lastly, as part of this implementation we should also

Update the nexus_msgs/stv/GetMotionPlan request to add a boolean to force lookup from the database (ReadOnly in the review comments) and fail the response if the trajectory does not exist in the databse.
Update the PlanMotion BT node to add a similar input port as above which will append the value to the request.

nexus_motion_planner/src/motion_plan_cache.hpp

Yadunund · 2023-09-29T08:27:14Z

nexus_motion_planner/src/motion_plan_cache.hpp

+ *     - Final pose (wrt. `planning_frame` (usually `base_link`))
+ *     - Final robot joint states
+ *
+ * Motion plans may be looked up with some tolerance.


mention the name of the ROS param to set the tolerance here.

There is no ROS param that sets the tolerance here.
Instead it's passed in per lookup call on the motion plan server, since this class is meant to be a standalone class.

Yadunund · 2023-09-29T09:53:58Z

nexus_motion_planner/src/motion_plan_cache.hpp

+    const moveit_msgs::msg::RobotTrajectory& plan,
+    double execution_time_s,
+    double planning_time_s,
+    bool overwrite = true);


Is there a scenario where we do not want to overwrite?

In most cases, no. However:

If exact_match_precision is set very high (relative to start and goal match tolerance), then overwriting will result in the cache being pretty sparse.

Implication: Not overwriting results in the cache space being maximally dense, which maximizes the chances of a cache hit on lookup. This is at the cost of cache storage size as well as (probably) the speed of lookups.

Also it can be used to see historically how plans have evolved over time.

I think for all those reasons I'd like to keep a parameter here to disable overwriting, especially if we plan to upstream this cache class to MoveIt!

Note: Overwriting in the context of the cache means deleting ALL "worse" plans that have an "exact" match (according to the exact_match_precision amount, across all cache entry metadata fields), before inserting the new "better" entry.

I think this behavior should be documented, iirc put_plan only inserts a new plan if it is better than existing plans. So overwrite has no effect if it is not the best plan.

The insertion check only happens at the end, so technically speaking overwrite will delete all worse plans.

I decided to name it overwrite -> delete_worse_plans to be more clear.

d1847ba in #26

Yadunund · 2023-09-29T09:55:46Z

nexus_motion_planner/src/motion_plan_cache.hpp

+  rclcpp::Node::SharedPtr node_;
+  warehouse_ros::DatabaseConnection::Ptr db_;
+
+  double exact_match_precision_ = 0;


Numerical precision.

Suggested change

double exact_match_precision_ = 0;

double exact_match_precision_ = 1e-6;

Ah, I set this as a default on the ROS param, but no harm setting it on the variable as well!

(Note: I noticed that, even on sim when the robot is commanded to the same location each time, a value of 0.001 (~0.05 degrees per joint) seemed more appropriate.)

nexus_motion_planner/src/motion_plan_cache.cpp

nexus_motion_planner/src/motion_planner_server.hpp

Yadunund · 2023-09-29T10:13:45Z

nexus_motion_planner/src/motion_planner_server.hpp

+  std::string _cache_db_plugin;
+  std::string _cache_db_host;
+  int _cache_db_port;
+  double _cache_exact_match_tolerance;


Instead of three doubles, let's define an enum class for each of these cases. Then have two params: 1) To get the string name of the enum type and match as defined above 2) The value of the tolerance. You could also consider using std::variant here and define 3 three different structs with boolean values of tolerance.

Any cache will use all three of these tolerance values (exact matches for population, and start and goal for plan fetches.)

More importantly, these values should be configurable by the user.

I don't understand why we would use an enum or variant in that case?
Maybe I'm missing something here?

I think enums and variants don't quite make sense here also. Perhaps there is confusion with the naming having both "exact_match" and "tolerance", the fomer suggest a bool and the latter suggest a double.

I'll add comments to explain:
ccbf91c in #26

Yadunund · 2023-09-29T10:22:52Z

nexus_motion_planner/src/motion_planner_server.hpp

+  // Motion plan caching
+  std::shared_ptr<nexus::motion_planner::MotionPlanCache> _motion_plan_cache;
+
+  bool _use_motion_plan_cache;


Instead of three bools, let's define class which defines how the databse will be used for better extensibility.

enum class PlannerDatabaseMode: public unint8_t { // The planner will generate a plan. Find if an entry exists in the databse and overwrite if execution time is better. Training, // Try to lookup a cached plan and if non is available, plan live but do not update the cache. Optional, // Return a plan only if present in the database cache. // ReadOnly, };

And below we would store an optional of this type.

std::optional<PlannerDatabaseMode> = std::nullopt;

We can parse a similarly named ROS param which takes in a string with the same name as the enum variants above. If we have a matching name, we set the member to the appropriate enum; Else leave it as optional. In the service response, if this member is nullopt, do a live plan (same as now). Else if not nullopt, plan and update database accordingly.
We should also instantiate the _motion_plan_cache only if this variable is not nullopt and other database specific parameters are defined (port, etc).

d85907d , with an unset enum value instead of using a nullopt.

Yadunund · 2023-09-29T10:41:01Z

nexus_motion_planner/src/motion_plan_cache.cpp

+  tf_listener_ =
+    std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+
+  warehouse_ros::DatabaseLoader loader(node_);


I don't think we should use the DatabaseLoader class as it is forcing us to store (and spin) a separate ROS node in the MotionPlanner class. Looking at it's implementation, it is a simple wrapper to retrieve ROS Params and load the right storage plugin. We're already retrieving some of the params in the MotionPlanner class, we can also try loading the plugin within the on_configure() and directly instantiate the db object via pluginlib. This would save us the need to spin another node which brings the total node count to three! (we already have one for this server, one for the move_group_interface).

We unfortunately still need a node for the tf listener.
I will just use the motion planner's node for both the tf listener and the database loading.

Edit: I can't use the motion planner's node because it's a lifecycle node :( I will use the internal_node instead...

methylDragon · 2023-10-02T22:03:58Z

Thanks for great effort here! Did a first pass review with some comments.

I'm concerned we're not using the APIs from PlanningSceneStorage class that is already implemented upstream for such caching. It looks like they serialize the MotionPlanRequest msg to check if it's cached before updating the database. Is there a reason we're not able to simply reuse the approach there?

Hey Yadu, I just saw this review! Thanks for the review!

The moveit_warehouse storage classes don't support fuzzy lookups. That class (PlanningSceneStorage) is more used for saving and loading scenes and states in the RViz window (as per the tutorial), and so only needs the ability to label messages with user defined IDs for saving and retrieval. (It then does an exact match on these IDs when looking the entries up to load them.)

Whereas for us, we need to be able to do fuzzy lookups to match "close enough" start and goal states to support caching and lookups for the purposes of planning.

Lastly, as part of this implementation we should also

Update the nexus_msgs/stv/GetMotionPlan request to add a boolean to force lookup from the database (ReadOnly in the review comments) and fail the response if the trajectory does not exist in the databse.

Update the PlanMotion BT node to add a similar input port as above which will append the value to the request.

Ah, I had put it as a parameter on motion planning service's node, but I can just as easily put it in the motion plan request instead.
Will do!

koonpeng

There is quite a lot of logic going on in maintaining the cache, the lack of tests is abit worrying. But I will leave it to @Yadunund to decide if this is a blocker.

nexus_motion_planner/src/motion_plan_cache.cpp

nexus_motion_planner/src/motion_planner_server.cpp

nexus_motion_planner/src/motion_plan_cache.cpp

nexus_motion_planner/src/motion_plan_cache.hpp

nexus_motion_planner/src/motion_plan_cache.cpp

koonpeng · 2023-11-08T05:43:29Z

nexus_motion_planner/src/motion_planner_server.hpp

+  std::string _cache_db_plugin;
+  std::string _cache_db_host;
+  int _cache_db_port;
+  double _cache_exact_match_tolerance;


I think enums and variants don't quite make sense here also. Perhaps there is confusion with the naming having both "exact_match" and "tolerance", the fomer suggest a bool and the latter suggest a double.

koonpeng · 2023-11-08T05:51:29Z

nexus_motion_planner/src/motion_plan_cache.hpp

+    const moveit_msgs::msg::RobotTrajectory& plan,
+    double execution_time_s,
+    double planning_time_s,
+    bool overwrite = true);


I think this behavior should be documented, iirc put_plan only inserts a new plan if it is better than existing plans. So overwrite has no effect if it is not the best plan.

Signed-off-by: methylDragon <[email protected]>

methylDragon · 2023-11-20T19:22:43Z

Thanks for the review!

I'll address all the comments in a separate PR (the branching structure makes it difficult to be updating it here, since changes need to be duplicated across the two PRs at the moment.)

koonpeng

Pre-approving for when the tests are merged.

* Add cartesian plan caching interfaces Signed-off-by: methylDragon <[email protected]> * Add construct_get_cartesian_plan_request Signed-off-by: methylDragon <[email protected]> * Add goal query and metadata Signed-off-by: methylDragon <[email protected]> * Add start query and metadata Signed-off-by: methylDragon <[email protected]> * Implement top level cache ops Signed-off-by: methylDragon <[email protected]> * Use motion plan cache for cartesian plans Signed-off-by: methylDragon <[email protected]> * Allow mismatched plan frames since we coerce anyway Signed-off-by: methylDragon <[email protected]> * Fix move bug Signed-off-by: methylDragon <[email protected]> * Plan cache code review fixes (sans unit tests) (#26) * Remove query appending macro Signed-off-by: methylDragon <[email protected]> * Default to warehouse_ros plugin if warehouse plugin isn't set Signed-off-by: methylDragon <[email protected]> * Return and use init result Signed-off-by: methylDragon <[email protected]> * Add todo for catching exceptions Signed-off-by: methylDragon <[email protected]> * Implement plan fetching with configurable key Signed-off-by: methylDragon <[email protected]> * Add comments for exact match tolerance Signed-off-by: methylDragon <[email protected]> * Slightly refactor put plan Signed-off-by: methylDragon <[email protected]> * Rename overwrite to delete_worse_plans Signed-off-by: methylDragon <[email protected]> * Split out motion plan cache into its own library Signed-off-by: methylDragon <[email protected]> * Sort constraints for reduced cardinality Signed-off-by: methylDragon <[email protected]> * Rename util function Signed-off-by: methylDragon <[email protected]> * Add todo for is_diff Signed-off-by: methylDragon <[email protected]> * Add unit tests for motion plan cache (#28) * Add count methods Signed-off-by: methylDragon <[email protected]> * Enable shared from this for cache class Signed-off-by: methylDragon <[email protected]> * Add unit test build rules Signed-off-by: methylDragon <[email protected]> * Add tests for motion plan cache (but not cartesian) Signed-off-by: methylDragon <[email protected]> * Fix bugs in cartesian caching Signed-off-by: methylDragon <[email protected]> * Add tests for cartesian plan cache Signed-off-by: methylDragon <[email protected]> * Exit if a test fails Signed-off-by: methylDragon <[email protected]> * Remove gtest import Signed-off-by: methylDragon <[email protected]> * Remove enable_shared_from_this Signed-off-by: methylDragon <[email protected]> * Only check for failure Signed-off-by: methylDragon <[email protected]> * Test half in-tolerance Signed-off-by: methylDragon <[email protected]> * Test different robot cache Signed-off-by: methylDragon <[email protected]> * Add force_cache_mode_execute_read_only (#29) * Add force_cache_mode_execute_read_only Signed-off-by: methylDragon <[email protected]> * Add force_cache_mode_execute_read_only input port Signed-off-by: methylDragon <[email protected]> --------- Signed-off-by: methylDragon <[email protected]> --------- Signed-off-by: methylDragon <[email protected]> --------- Signed-off-by: methylDragon <[email protected]> --------- Signed-off-by: methylDragon <[email protected]>

Signed-off-by: methylDragon <[email protected]>

* Fix and add motion planner server tests Signed-off-by: methylDragon <[email protected]> * Add test deps Signed-off-by: methylDragon <[email protected]> * Add motion planner server test to CI Signed-off-by: methylDragon <[email protected]> --------- Signed-off-by: methylDragon <[email protected]>

Yadunund

Thanks for this amazing effort @methylDragon!

methylDragon requested a review from Yadunund September 22, 2023 06:08

methylDragon force-pushed the ch3/motion-plan-cache branch 11 times, most recently from 5827839 to 6a7a3d9 Compare September 27, 2023 06:10

Yadunund requested changes Sep 29, 2023

View reviewed changes

methylDragon force-pushed the ch3/motion-plan-cache branch 7 times, most recently from 3ed60c6 to d8f3791 Compare October 17, 2023 23:54

methylDragon mentioned this pull request Oct 18, 2023

Implement cartesian plan cache #18

Merged

koonpeng requested changes Nov 8, 2023

View reviewed changes

methylDragon and others added 7 commits November 20, 2023 11:17

Add motion plan cache

15e7370

Signed-off-by: methylDragon <[email protected]>

Switch to snake case for function names

c715b29

Signed-off-by: methylDragon <[email protected]>

Print cache fetch time and key plans on planned execution time

16784bb

Signed-off-by: methylDragon <[email protected]>

Add overwrite_worse_plans param

8889982

Signed-off-by: methylDragon <[email protected]>

Fix plan fetching printout and don't recache fetched plans

32659fd

Signed-off-by: methylDragon <[email protected]>

Update only use cached plans parameter name

b683eab

Signed-off-by: methylDragon <[email protected]>

Add copyright headers and motion_planner namespace

5c94f49

Signed-off-by: methylDragon <[email protected]>

methylDragon added 7 commits November 20, 2023 11:17

Make motion_plan_cache a unique_ptr instead of shared_ptr

856c009

Signed-off-by: methylDragon <[email protected]>

Set numerical precision

dd323a5

Signed-off-by: methylDragon <[email protected]>

Use enum for planner database mode

52db5b5

Signed-off-by: methylDragon <[email protected]>

Rename cache methods to prepare for cartesian caching

e0d4cbc

Signed-off-by: methylDragon <[email protected]>

Add very important warning to cache class

d7bd142

Signed-off-by: methylDragon <[email protected]>

Fix access after move bug

f161f45

Signed-off-by: methylDragon <[email protected]>

Remove internal cache node

ca0f123

Signed-off-by: methylDragon <[email protected]>

methylDragon force-pushed the ch3/motion-plan-cache branch from 1c09ef5 to ca0f123 Compare November 20, 2023 19:19

methylDragon mentioned this pull request Nov 20, 2023

Plan cache code review fixes (sans unit tests) #26

Merged

methylDragon requested a review from koonpeng November 21, 2023 03:57

koonpeng approved these changes Nov 27, 2023

View reviewed changes

methylDragon and others added 2 commits December 5, 2023 00:25

Fix nullptr dereference

d757b8c

Signed-off-by: methylDragon <[email protected]>

methylDragon force-pushed the ch3/motion-plan-cache branch from b713af3 to d757b8c Compare December 5, 2023 04:23

methylDragon mentioned this pull request Dec 5, 2023

Fix and add motion planner server tests #30

Merged

methylDragon requested a review from Yadunund December 5, 2023 07:42

Yadunund approved these changes Jan 22, 2024

View reviewed changes

Yadunund merged commit edce5e4 into main Jan 22, 2024
1 of 2 checks passed

Yadunund deleted the ch3/motion-plan-cache branch January 22, 2024 05:37

Yadunund mentioned this pull request Jan 22, 2024

Return error if not plan was fetched for non-cartesian requests #31

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Implement motion plan cache #16

Implement motion plan cache #16

methylDragon commented Sep 22, 2023 •

edited

Loading

Yadunund left a comment

Yadunund Sep 29, 2023

methylDragon Oct 17, 2023 •

edited

Loading

Yadunund Sep 29, 2023

methylDragon Oct 2, 2023 •

edited

Loading

koonpeng Nov 8, 2023

methylDragon Nov 20, 2023 •

edited

Loading

Yadunund Sep 29, 2023

methylDragon Oct 2, 2023

methylDragon Oct 17, 2023 •

edited

Loading

Yadunund Sep 29, 2023

methylDragon Oct 16, 2023

koonpeng Nov 8, 2023

methylDragon Nov 20, 2023

Yadunund Sep 29, 2023

methylDragon Oct 17, 2023 •

edited

Loading

Yadunund Sep 29, 2023

methylDragon Oct 17, 2023 •

edited

Loading

methylDragon commented Oct 2, 2023 •

edited

Loading

koonpeng left a comment

koonpeng Nov 8, 2023

koonpeng Nov 8, 2023

methylDragon commented Nov 20, 2023 •

edited

Loading

koonpeng left a comment

Yadunund left a comment

	double exact_match_precision_ = 0;
	double exact_match_precision_ = 1e-6;

Implement motion plan cache #16

Implement motion plan cache #16

Conversation

methylDragon commented Sep 22, 2023 • edited Loading

WARNING

Description

Why is this PR so big?

Start State (MotionPlanRequest) Variations

Generated Plan Example

Insight

Images

Cache hits and pushes!

A view of the schema

With database collections for each move group

Populated Entries

Notes

TODO

Yadunund left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

methylDragon Oct 17, 2023 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

methylDragon Oct 2, 2023 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

methylDragon Nov 20, 2023 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

methylDragon Oct 17, 2023 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

methylDragon Oct 17, 2023 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

methylDragon Oct 17, 2023 • edited Loading

Choose a reason for hiding this comment

methylDragon commented Oct 2, 2023 • edited Loading

koonpeng left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

methylDragon commented Nov 20, 2023 • edited Loading

koonpeng left a comment

Choose a reason for hiding this comment

Yadunund left a comment

Choose a reason for hiding this comment

methylDragon commented Sep 22, 2023 •

edited

Loading

methylDragon Oct 17, 2023 •

edited

Loading

methylDragon Oct 2, 2023 •

edited

Loading

methylDragon Nov 20, 2023 •

edited

Loading

methylDragon Oct 17, 2023 •

edited

Loading

methylDragon Oct 17, 2023 •

edited

Loading

methylDragon Oct 17, 2023 •

edited

Loading

methylDragon commented Oct 2, 2023 •

edited

Loading

methylDragon commented Nov 20, 2023 •

edited

Loading