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
methylDragon
force-pushed
the
ch3/trajectory_cache
branch
4 times, most recently
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?
| 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?
There was a problem hiding this comment.
A cache hit is valid if the requested planning constraints has a workspace that completely subsumes a cached plan's workspace.
E.g. , if workspace is defined by the extrema (x_min, y_min, x_max, y_max)
Then if:
- Request: (-1, -1, 1, 1)
- Plan in cache: (-0.5, -0.5, 0.5, 0.5)
- (potential valid match if other constraints fulfilled)
But
- Plan in cache: (-2, -0.5, 0.5, 0.5)
- (not valid, since this plan might cause the end effector to go out of bounds.)
|
This PR is stale because it has been open for 45 days with no activity. Please tag a maintainer for help on completing this PR, or close it if you think it has become obsolete. |
github-actions
bot
added
the
stale
|
@methylDragon Do you have time to address the review? |
Apologies, got caught up with urgent work elsewhere. I am planning to spend some time in the following two weeks. |
Thank you! Let me know when you have questions/need help. |
github-actions
bot
removed
the
stale
methylDragon
force-pushed
the
ch3/trajectory_cache
branch
from
408a2fd
to
889a303
Compare
methylDragon
force-pushed
the
ch3/trajectory_cache
branch
from
cb30486
to
f26f8df
Compare
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]>
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]>
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]>
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]>
Signed-off-by: methylDragon <[email protected]>
methylDragon
force-pushed
the
ch3/trajectory_cache
branch
from
04f7230
to
85aa3f7
Compare
|
CI is failing due to deprecation warnings on jump_threshold for the Cartesian planner, but:
For those two reasons, can I request we merge this PR and the refactor PR (once reviewed) despite that happening? |
Depends on:
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