moveit2
The MoveIt Motion Planning Framework for ROS 2.
|
Trajectory Cache manager for MoveIt. More...
#include <moveit/trajectory_cache/trajectory_cache.hpp>
Classes | |
struct | Options |
Public Member Functions | |
Cache configuration. | |
TrajectoryCache (const rclcpp::Node::SharedPtr &node) | |
Constructs a TrajectoryCache. | |
bool | init (const Options &options) |
Initializes the TrajectoryCache. | |
Getters and setters. | |
unsigned | countTrajectories (const std::string &cache_namespace) |
Count the number of non-cartesian trajectories for a particular cache namespace. | |
unsigned | countCartesianTrajectories (const std::string &cache_namespace) |
Count the number of cartesian trajectories for a particular cache namespace. | |
std::string | getDbPath () const |
Gets the database path. | |
uint32_t | getDbPort () const |
Gets the database port. | |
double | getExactMatchPrecision () const |
Gets the exact match precision. | |
void | setExactMatchPrecision (double exact_match_precision) |
Sets the exact match precision. | |
size_t | getNumAdditionalTrajectoriesToPreserveWhenPruningWorse () const |
Get the number of trajectories to preserve when pruning worse trajectories. | |
void | setNumAdditionalTrajectoriesToPreserveWhenPruningWorse (size_t num_additional_trajectories_to_preserve_when_pruning_worse) |
Set the number of additional trajectories to preserve when pruning worse trajectories. | |
Motion plan trajectory caching | |
std::vector< warehouse_ros::MessageWithMetadata< moveit_msgs::msg::RobotTrajectory >::ConstPtr > | fetchAllMatchingTrajectories (const moveit::planning_interface::MoveGroupInterface &move_group, const std::string &cache_namespace, const moveit_msgs::msg::MotionPlanRequest &plan_request, const std::vector< std::unique_ptr< FeaturesInterface< moveit_msgs::msg::MotionPlanRequest > > > &features, const std::string &sort_by, bool ascending=true, bool metadata_only=false) const |
Fetches all trajectories keyed on user-specified features, returning them as a vector, sorted by some cache feature. | |
warehouse_ros::MessageWithMetadata< moveit_msgs::msg::RobotTrajectory >::ConstPtr | fetchBestMatchingTrajectory (const moveit::planning_interface::MoveGroupInterface &move_group, const std::string &cache_namespace, const moveit_msgs::msg::MotionPlanRequest &plan_request, const std::vector< std::unique_ptr< FeaturesInterface< moveit_msgs::msg::MotionPlanRequest > > > &features, const std::string &sort_by, bool ascending=true, bool metadata_only=false) const |
Fetches the best trajectory keyed on user-specified features, with respect to some cache feature. | |
bool | insertTrajectory (const moveit::planning_interface::MoveGroupInterface &move_group, const std::string &cache_namespace, const moveit_msgs::msg::MotionPlanRequest &plan_request, const moveit::planning_interface::MoveGroupInterface::Plan &plan, CacheInsertPolicyInterface< moveit_msgs::msg::MotionPlanRequest, moveit::planning_interface::MoveGroupInterface::Plan, moveit_msgs::msg::RobotTrajectory > &cache_insert_policy, bool prune_worse_trajectories=true, const std::vector< std::unique_ptr< FeaturesInterface< moveit_msgs::msg::MotionPlanRequest > > > &additional_features={}) |
Inserts a trajectory into the database, with user-specified insert policy. | |
Cartesian trajectory caching | |
std::vector< warehouse_ros::MessageWithMetadata< moveit_msgs::msg::RobotTrajectory >::ConstPtr > | fetchAllMatchingCartesianTrajectories (const moveit::planning_interface::MoveGroupInterface &move_group, const std::string &cache_namespace, const moveit_msgs::srv::GetCartesianPath::Request &plan_request, const std::vector< std::unique_ptr< FeaturesInterface< moveit_msgs::srv::GetCartesianPath::Request > > > &features, const std::string &sort_by, bool ascending=true, bool metadata_only=false) const |
Fetches all cartesian trajectories keyed on user-specified features, returning them as a vector, sorted by some cache feature. | |
warehouse_ros::MessageWithMetadata< moveit_msgs::msg::RobotTrajectory >::ConstPtr | fetchBestMatchingCartesianTrajectory (const moveit::planning_interface::MoveGroupInterface &move_group, const std::string &cache_namespace, const moveit_msgs::srv::GetCartesianPath::Request &plan_request, const std::vector< std::unique_ptr< FeaturesInterface< moveit_msgs::srv::GetCartesianPath::Request > > > &features, const std::string &sort_by, bool ascending=true, bool metadata_only=false) const |
Fetches the best cartesian trajectory keyed on user-specified features, with respect to some cache feature. | |
bool | insertCartesianTrajectory (const moveit::planning_interface::MoveGroupInterface &move_group, const std::string &cache_namespace, const moveit_msgs::srv::GetCartesianPath::Request &plan_request, const moveit_msgs::srv::GetCartesianPath::Response &plan, CacheInsertPolicyInterface< moveit_msgs::srv::GetCartesianPath::Request, moveit_msgs::srv::GetCartesianPath::Response, moveit_msgs::msg::RobotTrajectory > &cache_insert_policy, bool prune_worse_trajectories=true, const std::vector< std::unique_ptr< FeaturesInterface< moveit_msgs::srv::GetCartesianPath::Request > > > &additional_features={}) |
Inserts a cartesian trajectory into the database, with user-specified insert policy. | |
Static Public Member Functions | |
Default cache behavior helpers. | |
static std::vector< std::unique_ptr< FeaturesInterface< moveit_msgs::msg::MotionPlanRequest > > > | getDefaultFeatures (double start_tolerance, double goal_tolerance) |
Gets the default features for MotionPlanRequest messages. | |
static std::unique_ptr< CacheInsertPolicyInterface< moveit_msgs::msg::MotionPlanRequest, moveit::planning_interface::MoveGroupInterface::Plan, moveit_msgs::msg::RobotTrajectory > > | getDefaultCacheInsertPolicy () |
Gets the default cache insert policy for MotionPlanRequest messages. | |
static std::vector< std::unique_ptr< FeaturesInterface< moveit_msgs::srv::GetCartesianPath::Request > > > | getDefaultCartesianFeatures (double start_tolerance, double goal_tolerance, double min_fraction) |
Gets the default features for GetCartesianPath requests. | |
static std::unique_ptr< CacheInsertPolicyInterface< moveit_msgs::srv::GetCartesianPath::Request, moveit_msgs::srv::GetCartesianPath::Response, moveit_msgs::msg::RobotTrajectory > > | getDefaultCartesianCacheInsertPolicy () |
Gets the default cache insert policy for GetCartesianPath requests. | |
static std::string | getDefaultSortFeature () |
Gets the default sort feature. | |
Trajectory Cache manager for MoveIt.
This manager facilitates cache management for MoveIt 2's MoveGroupInterface
by using warehouse_ros
to manage a database of executed trajectories, keyed with injectable feature extractors, and pruned and inserted by cache insert policies. This allows for the lookup and reuse of the best performing trajectories found so far in a user-specified manner.
Trajectories may be looked up with some tolerance at call time.
The following ROS Parameters MUST be set:
warehouse_plugin
: What database to useThis class supports trajectories planned from move_group MotionPlanRequests as well as GetCartesianPath requests. That is, both normal motion plans and cartesian plans are supported.
A cache fetch is intended to be usable as a stand-in for the MoveGroupInterface plan
and computeCartesianPath
methods.
WARNING: RFE: !!! The default set of feature extractors and cache insert policies do NOT support collision detection!
Trajectories using them will be inserted 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 trajectories 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, or do more complicated checks to see if the scene world is "close enough" or is a less obstructed version of the scene in the cache entry.
Alternatively, use your planning scene after fetching the cache entry to validate if the cached trajectory will result in collisions or not.
!!! They also do NOT support keying on joint velocities and efforts. The cache only keys on joint positions.
!!! They also do NOT support multi-DOF joints.
!!! They also do NOT support certain constraints Including: constraint regions, everything related to collision.
This is because they are difficult (but not impossible) to implement key logic for.
Thread-Safety ^^^^^^^^^^^^^ This class is NOT thread safe. Synchronize use of it if you need it in multi-threaded contexts.
Injectable Feature Extraction and Cache Insert Policies ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ The specific features of cache entries and cache insertion candidates is determined by passing the TrajectoryCache's insert and fetch methods with the appropriate FeaturesInterface<FeatureSourceT> implementations, which will extract and append the appropriate features to the query.
Similarly, a CacheInsertPolicyInterface<KeyT, ValueT, CacheEntryT> implementation must be passed to the insert method to determine what pruning and insertion logic to apply.
Each cache insert policy implementation constrains what features can be used to fetch cache entries inserted with them. See the related interface classes for more information.
This class provides a few helper methods to have "default" behavior of:
execution_time_s
execution_time_s
Cache Sections ^^^^^^^^^^^^^^ Motion plan trajectories are stored in the move_group_trajectory_cache
database within the database file, with trajectories for each move group stored in a collection named after the relevant move group's name.
For example, the "my_move_group" move group will have its cache stored in move_group_trajectory_cache@my_move_group
.
Similarly, the cartesian trajectories are stored in the move_group_cartesian_trajectory_cache
database within the database file, with trajectories for each move group stored in a collection named after the relevant move group's name.
Definition at line 151 of file trajectory_cache.hpp.
|
explicit |
Constructs a TrajectoryCache.
[in] | node. | An rclcpp::Node::SharedPtr, which will be used to lookup warehouse_ros parameters and log. |
TODO: methylDragon - We explicitly need a Node::SharedPtr because warehouse_ros ONLY supports it... Use rclcpp::node_interfaces::NodeInterfaces<> once warehouse_ros does.
Definition at line 125 of file trajectory_cache.cpp.
unsigned moveit_ros::trajectory_cache::TrajectoryCache::countCartesianTrajectories | ( | const std::string & | cache_namespace | ) |
Count the number of cartesian trajectories for a particular cache namespace.
[in] | cache_namespace. | A namespace to separate cache entries by. The name of the robot is a good choice. |
Definition at line 155 of file trajectory_cache.cpp.
unsigned moveit_ros::trajectory_cache::TrajectoryCache::countTrajectories | ( | const std::string & | cache_namespace | ) |
Count the number of non-cartesian trajectories for a particular cache namespace.
[in] | cache_namespace. | A namespace to separate cache entries by. The name of the robot is a good choice. |
Definition at line 148 of file trajectory_cache.cpp.
std::vector< MessageWithMetadata< RobotTrajectory >::ConstPtr > moveit_ros::trajectory_cache::TrajectoryCache::fetchAllMatchingCartesianTrajectories | ( | const moveit::planning_interface::MoveGroupInterface & | move_group, |
const std::string & | cache_namespace, | ||
const moveit_msgs::srv::GetCartesianPath::Request & | plan_request, | ||
const std::vector< std::unique_ptr< FeaturesInterface< moveit_msgs::srv::GetCartesianPath::Request > > > & | features, | ||
const std::string & | sort_by, | ||
bool | ascending = true , |
||
bool | metadata_only = false |
||
) | const |
Fetches all cartesian trajectories keyed on user-specified features, returning them as a vector, sorted by some cache feature.
[in] | move_group. | The manipulator move group, used to get its state. |
[in] | cache_namespace. | A namespace to separate cache entries by. The name of the robot is a good choice. |
[in] | plan_request. | The cartesian plan request to extract features from to key the cache with. |
[in] | features. | The features to key the cache with. |
[in] | sort_by. | The cache feature to sort by, defaults to execution time. |
[in] | ascending. | If true, sorts in ascending order. If false, sorts in descending order. |
[in] | metadata_only. | If true, returns only the cache entry metadata. |
sort_by
parameter. Definition at line 335 of file trajectory_cache.cpp.
std::vector< MessageWithMetadata< RobotTrajectory >::ConstPtr > moveit_ros::trajectory_cache::TrajectoryCache::fetchAllMatchingTrajectories | ( | const moveit::planning_interface::MoveGroupInterface & | move_group, |
const std::string & | cache_namespace, | ||
const moveit_msgs::msg::MotionPlanRequest & | plan_request, | ||
const std::vector< std::unique_ptr< FeaturesInterface< moveit_msgs::msg::MotionPlanRequest > > > & | features, | ||
const std::string & | sort_by, | ||
bool | ascending = true , |
||
bool | metadata_only = false |
||
) | const |
Fetches all trajectories keyed on user-specified features, returning them as a vector, sorted by some cache feature.
[in] | move_group. | The manipulator move group, used to get its state. |
[in] | cache_namespace. | A namespace to separate cache entries by. The name of the robot is a good choice. |
[in] | plan_request. | The motion plan request to extract features from to key the cache with. |
[in] | features. | The features to key the cache with. |
[in] | sort_by. | The cache feature to sort by. |
[in] | ascending. | If true, sorts in ascending order. If false, sorts in descending order. |
[in] | metadata_only. | If true, returns only the cache entry metadata. |
sort_by
parameter. Definition at line 198 of file trajectory_cache.cpp.
MessageWithMetadata< RobotTrajectory >::ConstPtr moveit_ros::trajectory_cache::TrajectoryCache::fetchBestMatchingCartesianTrajectory | ( | const moveit::planning_interface::MoveGroupInterface & | move_group, |
const std::string & | cache_namespace, | ||
const moveit_msgs::srv::GetCartesianPath::Request & | plan_request, | ||
const std::vector< std::unique_ptr< FeaturesInterface< moveit_msgs::srv::GetCartesianPath::Request > > > & | features, | ||
const std::string & | sort_by, | ||
bool | ascending = true , |
||
bool | metadata_only = false |
||
) | const |
Fetches the best cartesian trajectory keyed on user-specified features, with respect to some cache feature.
[in] | move_group. | The manipulator move group, used to get its state. |
[in] | cache_namespace. | A namespace to separate cache entries by. The name of the robot is a good choice. |
[in] | plan_request. | The cartesian plan request to extract features from to key the cache with. |
[in] | features. | The features to key the cache with. |
[in] | sort_by. | The cache feature to sort by. |
[in] | ascending. | If true, sorts in ascending order. If false, sorts in descending order. |
[in] | metadata_only. | If true, returns only the cache entry metadata. |
sort_by
parameter. Definition at line 359 of file trajectory_cache.cpp.
MessageWithMetadata< RobotTrajectory >::ConstPtr moveit_ros::trajectory_cache::TrajectoryCache::fetchBestMatchingTrajectory | ( | const moveit::planning_interface::MoveGroupInterface & | move_group, |
const std::string & | cache_namespace, | ||
const moveit_msgs::msg::MotionPlanRequest & | plan_request, | ||
const std::vector< std::unique_ptr< FeaturesInterface< moveit_msgs::msg::MotionPlanRequest > > > & | features, | ||
const std::string & | sort_by, | ||
bool | ascending = true , |
||
bool | metadata_only = false |
||
) | const |
Fetches the best trajectory keyed on user-specified features, with respect to some cache feature.
[in] | move_group. | The manipulator move group, used to get its state. |
[in] | cache_namespace. | A namespace to separate cache entries by. The name of the robot is a good choice. |
[in] | plan_request. | The motion plan request to extract features from to key the cache with. |
[in] | features. | The features to key the cache with. |
[in] | metadata_only. | If true, returns only the cache entry metadata. |
[in] | sort_by. | The cache feature to sort by. |
[in] | ascending. | If true, sorts in ascending order. If false, sorts in descending order. |
sort_by
parameter. Definition at line 221 of file trajectory_cache.cpp.
std::string moveit_ros::trajectory_cache::TrajectoryCache::getDbPath | ( | ) | const |
Gets the database path.
Definition at line 162 of file trajectory_cache.cpp.
uint32_t moveit_ros::trajectory_cache::TrajectoryCache::getDbPort | ( | ) | const |
Gets the database port.
Definition at line 167 of file trajectory_cache.cpp.
|
static |
Gets the default cache insert policy for MotionPlanRequest messages.
Definition at line 99 of file trajectory_cache.cpp.
|
static |
Gets the default cache insert policy for GetCartesianPath requests.
Definition at line 111 of file trajectory_cache.cpp.
|
static |
Gets the default features for GetCartesianPath requests.
Definition at line 105 of file trajectory_cache.cpp.
|
static |
Gets the default features for MotionPlanRequest messages.
Definition at line 93 of file trajectory_cache.cpp.
|
static |
Gets the default sort feature.
Definition at line 116 of file trajectory_cache.cpp.
double moveit_ros::trajectory_cache::TrajectoryCache::getExactMatchPrecision | ( | ) | const |
Gets the exact match precision.
Definition at line 172 of file trajectory_cache.cpp.
size_t moveit_ros::trajectory_cache::TrajectoryCache::getNumAdditionalTrajectoriesToPreserveWhenPruningWorse | ( | ) | const |
Get the number of trajectories to preserve when pruning worse trajectories.
Definition at line 182 of file trajectory_cache.cpp.
bool moveit_ros::trajectory_cache::TrajectoryCache::init | ( | const Options & | options | ) |
Initializes the TrajectoryCache.
This sets up the database connection, and sets any configuration parameters. You must call this before calling any other method of the trajectory cache.
[in] | options. | An instance of TrajectoryCache::Options to initialize the cache with. |
Definition at line 130 of file trajectory_cache.cpp.
bool moveit_ros::trajectory_cache::TrajectoryCache::insertCartesianTrajectory | ( | const moveit::planning_interface::MoveGroupInterface & | move_group, |
const std::string & | cache_namespace, | ||
const moveit_msgs::srv::GetCartesianPath::Request & | plan_request, | ||
const moveit_msgs::srv::GetCartesianPath::Response & | plan, | ||
CacheInsertPolicyInterface< moveit_msgs::srv::GetCartesianPath::Request, moveit_msgs::srv::GetCartesianPath::Response, moveit_msgs::msg::RobotTrajectory > & | cache_insert_policy, | ||
bool | prune_worse_trajectories = true , |
||
const std::vector< std::unique_ptr< FeaturesInterface< moveit_msgs::srv::GetCartesianPath::Request > > > & | additional_features = {} |
||
) |
Inserts a cartesian trajectory into the database, with user-specified insert policy.
Optionally deletes all worse cartesian trajectories by default to prune the cache.
[in] | move_group. | The manipulator move group, used to get its state. |
[in] | cache_namespace. | A namespace to separate cache entries by. The name of the robot is a good choice. |
[in] | plan_request. | The cartesian path plan request to extract features from to key the cache with. |
[in] | plan. | The plan containing the trajectory to insert. |
[in,out] | cache_insert_policy. | The cache insert policy to use. Will determine what features can be used to fetch entries, and pruning and insertion logic. Will be reset at the end of the call. |
[in] | prune_worse_trajectories. | If true, will prune the cache according to the cache_insert_policy 's pruning logic. |
[in] | additional_features. | Additional features to key the cache with, must not intersect with the set of features supported by the cache_insert_policy . |
Definition at line 386 of file trajectory_cache.cpp.
bool moveit_ros::trajectory_cache::TrajectoryCache::insertTrajectory | ( | const moveit::planning_interface::MoveGroupInterface & | move_group, |
const std::string & | cache_namespace, | ||
const moveit_msgs::msg::MotionPlanRequest & | plan_request, | ||
const moveit::planning_interface::MoveGroupInterface::Plan & | plan, | ||
CacheInsertPolicyInterface< moveit_msgs::msg::MotionPlanRequest, moveit::planning_interface::MoveGroupInterface::Plan, moveit_msgs::msg::RobotTrajectory > & | cache_insert_policy, | ||
bool | prune_worse_trajectories = true , |
||
const std::vector< std::unique_ptr< FeaturesInterface< moveit_msgs::msg::MotionPlanRequest > > > & | additional_features = {} |
||
) |
Inserts a trajectory into the database, with user-specified insert policy.
Optionally deletes all worse trajectories by default to prune the cache.
[in] | move_group. | The manipulator move group, used to get its state. |
[in] | cache_namespace. | A namespace to separate cache entries by. The name of the robot is a good choice. |
[in] | plan_request. | The motion plan request to extract features from to key the cache with. |
[in] | plan. | The plan containing the trajectory to insert. |
[in,out] | cache_insert_policy. | The cache insert policy to use. Will determine what features can be used to fetch entries, and pruning and insertion logic. Will be reset at the end of the call. |
[in] | prune_worse_trajectories. | If true, will prune the cache according to the cache_insert_policy 's pruning logic. |
[in] | additional_features. | Additional features to key the cache with. Must not intersect with the set of features supported by the cache_insert_policy . |
Definition at line 247 of file trajectory_cache.cpp.
void moveit_ros::trajectory_cache::TrajectoryCache::setExactMatchPrecision | ( | double | exact_match_precision | ) |
Sets the exact match precision.
Definition at line 177 of file trajectory_cache.cpp.
void moveit_ros::trajectory_cache::TrajectoryCache::setNumAdditionalTrajectoriesToPreserveWhenPruningWorse | ( | size_t | num_additional_trajectories_to_preserve_when_pruning_worse | ) |
Set the number of additional trajectories to preserve when pruning worse trajectories.
Definition at line 187 of file trajectory_cache.cpp.