Implement fuzzy-matching Trajectory Cache 🔥 #2838
Conversation
methylDragon
force-pushed
the
ch3/trajectory_cache
branch
10 times, most recently
from
2ce4ff5
to
37b7494
Compare
methylDragon
changed the title
methylDragon
force-pushed
the
ch3/trajectory_cache
branch
from
37b7494
to
1115477
Compare
methylDragon
changed the title
methylDragon
changed the title
Signed-off-by: methylDragon <methylDragon@gmail.com>
methylDragon
force-pushed
the
ch3/trajectory_cache
branch
3 times, most recently
from
251760a
to
421cfb2
Compare
Signed-off-by: methylDragon <methylDragon@gmail.com>
methylDragon
force-pushed
the
ch3/trajectory_cache
branch
from
421cfb2
to
0a1c92d
Compare
There was a problem hiding this comment.
Cool! Thanks for up-streaming this, it looks great 👍 I still need to test it and go over it a second time due to the size of the PR but I left a bunch of comments from the first pass. In general you'll need to align the code style with our style (I linked the relevant source in the first comment). Maybe I missed it but do you have a good testing example I can try somewhere?
| @@ -0,0 +1,1066 @@ | |||
| // Copyright 2023 Johnson & Johnson | |||
There was a problem hiding this comment.
Do you mind moving all test files into the test directory?
| using moveit::planning_interface::MoveGroupInterface; | ||
| using moveit_ros::trajectory_cache::TrajectoryCache; | ||
|
|
||
| const std::string g_robot_name = "panda"; |
| // Setup ===================================================================== | ||
| // All variants are copies. | ||
|
|
||
| /// MotionPlanRequest |
| std::swap(swapped_close_matching_plan_req.goal_constraints.at(0).joint_constraints.at(0), | ||
| swapped_close_matching_plan_req.goal_constraints.at(0).joint_constraints.at(1)); | ||
|
|
||
| // Smaller workspace start |
There was a problem hiding this comment.
General question: What impact does the workspace size have on the cache?
|
|
||
| bool TrajectoryCache::init(const std::string& db_path, uint32_t db_port, double exact_match_precision) | ||
| { | ||
| RCLCPP_INFO(node_->get_logger(), "Opening trajectory cache database at: %s (Port: %d, Precision: %f)", |
| #ifndef SRC__TRAJECTORY_CACHE_HPP | ||
| #define SRC__TRAJECTORY_CACHE_HPP |
There was a problem hiding this comment.
| #ifndef SRC__TRAJECTORY_CACHE_HPP | |
| #define SRC__TRAJECTORY_CACHE_HPP | |
| #pragma once |
| // Use rclcpp::node_interfaces::NodeInterfaces<> once warehouse_ros does. | ||
| explicit TrajectoryCache(const rclcpp::Node::SharedPtr& node); | ||
|
|
||
| bool init(const std::string& db_path = ":memory:", uint32_t db_port = 0, double exact_match_precision = 1e-6); |
There was a problem hiding this comment.
We try to keep the documentation consistent by using the doxgen documentation style. Do you mind adding this for all the functions and classing you're introducing?
/** \brief Once sentence to explain what the function does
* \param [in] ParamName What does the param do
* ...
* \return What is the returned value (only necessary for non void return values)
*/| double planning_time_s, bool delete_worse_trajectories = true); | ||
|
|
||
| // QUERY CONSTRUCTION | ||
| bool extract_and_append_trajectory_start_to_query(warehouse_ros::Query& query, |
There was a problem hiding this comment.
Would it make sense to have separate extract and append function for the code to be more modular?
| rclcpp::Node::SharedPtr node_; | ||
| warehouse_ros::DatabaseConnection::Ptr db_; | ||
|
|
||
| double exact_match_precision_ = 1e-6; |
| @@ -0,0 +1,154 @@ | |||
| # Copyright 2023 Johnson & Johnson | |||
There was a problem hiding this comment.
The test coverage is really nice, thank you!
WARNING
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
The workflow for Cartesian plans is similar. Use
fetch_best_matching_cartesian_trajectoryandput_cartesian_trajectoryinstead!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:
Full supported key list
It works generically for an arbitrary number of joints, across any number of move_groups.
It also has the following optimizations:
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:
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 keysTrainingAppendOnly: 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
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!
Test
Targeting:
rollingAll 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