-
Notifications
You must be signed in to change notification settings - Fork 531
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Fuzzy-matching Trajectory Cache Injectable Traits refactor 🔥🔥 #2941
base: main
Are you sure you want to change the base?
Fuzzy-matching Trajectory Cache Injectable Traits refactor 🔥🔥 #2941
Conversation
91b48e8
to
cc0feb3
Compare
79b7f95
to
e64cad8
Compare
Quick question: clang-format/tidy is erroneously editing the template parameters. How do I get around it? Is it acceptable to throw in NOLINT directives? |
77789f3
to
1feb211
Compare
41fde4f
to
68e4268
Compare
Rebased! |
CI is failing due to the |
This pull request is in conflict. Could you fix it @methylDragon? |
Are we good to merge? (: Quoting from before:
|
🙈 I've been busy but maybe I'll get to it on the weekend, sorry. The API of |
d9f16d0
to
0d96af3
Compare
Signed-off-by: methylDragon <methylDragon@gmail.com>
0d96af3
to
a70b660
Compare
Signed-off-by: methylDragon <methylDragon@gmail.com>
a4a668a
to
ff61b50
Compare
Small poke @sjahr (: |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Nice work! Tests seem to work now, thanks.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Whoops, I did not read the log correctly. Any idea what might call this error:
$ ( source /home/runner/work/moveit2/moveit2/.work/upstream_ws/install/setup.bash && cd /home/runner/work/moveit2/moveit2/.work/target_ws && colcon test-result --verbose; )
build/moveit_ros_trajectory_cache/Testing/20241107-2346/Test.xml: 7 tests, 0 errors, 1 failure, 0 skipped
- test_utils
<<< failure message
-- run_test.py: invoking following command in '/home/runner/work/moveit2/moveit2/.work/target_ws/build/moveit_ros_trajectory_cache/test':
- /home/runner/work/moveit2/moveit2/.work/target_ws/build/moveit_ros_trajectory_cache/test/test_utils --gtest_output=xml:/home/runner/work/moveit2/moveit2/.work/target_ws/build/moveit_ros_trajectory_cache/test_results/moveit_ros_trajectory_cache/test_utils.gtest.xml
[==========] Running 3 tests from 2 test suites.
[----------] Global test environment set-up.
[----------] 1 test from WarehouseFixture
[ RUN ] WarehouseFixture.QueryAppendCenterWithToleranceWorks
Error: ROR] [1731023211.703482413] [warehouse_ros_sqlite.query]: Preparing Query failed: no such column: M_unrelated_metadata
[ OK ] WarehouseFixture.QueryAppendCenterWithToleranceWorks (14 ms)
[----------] 1 test from WarehouseFixture (14 ms total)
[----------] 2 tests from TestUtils
[ RUN ] TestUtils.GetExecutionTimeWorks
[ OK ] TestUtils.GetExecutionTimeWorks (0 ms)
[ RUN ] TestUtils.ConstraintSortingWorks
[ OK ] TestUtils.ConstraintSortingWorks (0 ms)
[----------] 2 tests from TestUtils (0 ms total)
[----------] Global test environment tear-down
[==========] 3 tests from 2 test suites ran. (14 ms total)
https://github.com/moveit/moveit2/actions/runs/11733210217/job/32686999595
This is coming from this test of the warehouse_ros utils: moveit2/moveit_ros/trajectory_cache/test/utils/test_utils.cpp Lines 45 to 47 in 8575486
Which double checks that a query for a metadata key that doesn't exist returns empty, so this is working as expected. I think the emission is just a warning from warehouse_ros since we're deliberately looking up a non-existent column in the test. If we're instead talking this segfault on cleanup:
I think it's an issue with the warehouse_ros database loader, specific to humble. So not an issue with the tests here. |
Sorry to keep poking; is it possible to exclude the trajectory cache targets/tests from the humble CI? Or is there anything that I need to do on my end? |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I tried to find the error cause but I did not succeed for now and due to a lack of more time, I'd say, just skip this test on humble. warehouse_ros throws when the test fixture is teared down 🤷 Memory management is not trivial I guess 🙈
As requested in the original PR, this PR refactors the TrajectoryCache to allow users to inject their own behaviors (which will allow them to cache on and sort by any arbitrary feature, as long as it can be represented as a series of warehouse_ros columns).
Depends on:
Builds on top of:
TODOs:
Preamble
I apologize that this PR is even larger than the one it builds on top of. Most of the added lines are docstrings or tests, and boilerplate to support the behavior injection pattern this refactor is concerned with.
On the other hand, the average file length has decreased, so the code is MUCH more modular and hopefully easy to digest.
I can't split up this PR into multiple smaller ones since technically speaking, in order to preserve cache functionality, all the feature extractors and cache insert policies introduced in this PR will need to go in together.
I would suggest looking at the tests to aid in review (they run the gamut of unit and integration tests).
You can also build and run the example:
PS: If the size is still prohibitive, and we are okay with having partial implementations live in moveit2 while reviews are pending, let me know and I will split up the PR into smaller PRs (though I suspect at that point, that a logical number of splits might end up being somewhere close to 5-10 PRs.)
Navigating This PR
Since this PR is so large, here is a proposed order for comprehending the PR.
features/features_interface.hpp
,cache_insert_policies/cache_insert_policy_interface.hpp
features/
,cache_insert_policies/
trajectory_cache.hpp
,trajectory_cache.cpp
Additionally, all docstrings are filled, including file ones, as appropriate. So hopefully any clarificatory questions would have already been pre-empted and answered.
Description
This PR builds on top of the fuzzy-matching Trajectory Cache introduced in:
The implementation in that PR coupled the cache tightly with assumptions about what features to extract and sort by (i.e., a specific set of start/goal constraints, and pruning by execution time.)
This means that users who might want to store different features or a subset of those features, or who might want to fetch and prune on different features (e.g., minimum jerk, path length, etc.) will be unable to.
This PR refactors the cache to allow users to inject their own feature extractors and pruning/insertion logic!
This is done by introducing two new abstract classes that can be injected into the cache methods, acting a lot like class "traits":
FeaturesInterface<>
: Governs what query/metadata items to extract and append to the warehouse_ros query.CacheInserterInterface<>
: Governs the pruning and insertion logic.For more details on FeaturesInterface, see the Docstrings: https://github.com/moveit/moveit2/blob/cc0feb37cf423076e133523ccdbbf3038b84a01e/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/features/features_interface.hpp
Some notes:
Example
In other words, before. the cache was used like this:
Now the cache is used like this:
See the motion plan request features here: 79b7f95
The Feature Contract
Users may use an instance of FeaturesInterface<> to fetch a cache entry only if it was supported by the CacheInserterInterface<> instance that they used (or on insert, the feature was added in additional_features).
I could not think of a way to create a coupling between uses of the cache inserters and the features. This is the cost of generality and allowing users to inject arbitrary logic into the cache.
As such, users must take care to look at the docs of the cache inserter to see what features can be used with them.
(This can be mitigated by adding helper methods to get "standard" bundles of features and a "standard" CacheInserter.)
Bonus
I added new features to the default feature extractors set and cleaned up some utilities!
There are now
FeaturesInterface<>
implementations that can handle path and trajectory constraints!Multiple goal constraints are also handled (at the cost of increased cardinality.)
I also added "atomic" features that wrap the basic ops you can do with warehouse_ros, to allow users to specify their own metadata to tag cache entries with.
Here: cc0feb3
Pre-Existing Support
The package now provides some starter implementations that covers most general cases of motion planning.
For more information, see the implementations of:
FeaturesInterface
CacheInsertPolicyInterface
Cache Keying Features
The following are features of the plan request and response that you can key the cache on.
These support separately configurable fuzzy lookup on start and goal conditions!
Additionally, these features "canonicalize" the inputs to reduce the cardinality of the cache, increasing the chances of cache hits. (e.g., restating poses relative to the planning frame).
Supported Features:
WorkspaceFeatures
: Planning workspaceStartStateJointStateFeatures
: Starting robot joint stateMaxSpeedAndAccelerationFeatures
: Max velocity, acceleration, and cartesian speed limitsGoalConstraintsFeatures
: Planning requestgoal_constraints
PathConstraintsFeatures
: Planning requestpath_constraints
TrajectoryConstraintsFeatures
: Planning requesttrajectory_constraints
Additionally, support for user-specified features are provided for query-only or cache metadata tagging constant features.
Similar support exists for the cartesian variants of these.
Cache Insert and Pruning Policies
The following are cache insertion and pruning policies to govern when cache entries are inserted, and how they are (optionally) pruned.
Supported Cache Insert Policies:
BestSeenExecutionTimePolicy
: Only insert best seen execution time, optionally prune on best execution time.AlwaysInsertNeverPrunePolicy
: Always insert, never pruneCaveat
The increased functionality is now no longer 100% covered. But I tried adding tests where I had time to. I am unfortunately running out of time to iterate on this, so let's be targeted with the improvements!