moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
moveit_ros::trajectory_cache::TrajectoryCache Class Reference

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.

Detailed Description

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 use

This 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.

See also
FeaturesInterface<FeatureSourceT>
CacheInsertPolicyInterface<KeyT, ValueT, CacheEntryT>

This class provides a few helper methods to have "default" behavior of:

  • Fetching and caching on "start" and "goal" parameters
  • Pruning on execution_time_s
  • Sorting on execution_time_s
See also
TrajectoryCache::getDefaultFeatures
TrajectoryCache::getDefaultCacheInsertPolicy
TrajectoryCache::getDefaultCartesianFeatures
TrajectoryCache::getDefaultCartesianCacheInsertPolicy
TrajectoryCache::getDefaultSortFeature

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 152 of file trajectory_cache.hpp.

Constructor & Destructor Documentation

◆ TrajectoryCache()

moveit_ros::trajectory_cache::TrajectoryCache::TrajectoryCache ( const rclcpp::Node::SharedPtr & node)
explicit

Constructs a TrajectoryCache.

Parameters
[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 124 of file trajectory_cache.cpp.

Here is the call graph for this function:

Member Function Documentation

◆ countCartesianTrajectories()

unsigned moveit_ros::trajectory_cache::TrajectoryCache::countCartesianTrajectories ( const std::string & cache_namespace)

Count the number of cartesian trajectories for a particular cache namespace.

Parameters
[in]cache_namespace.A namespace to separate cache entries by. The name of the robot is a good choice.
Returns
The number of cartesian trajectories for the cache namespace.

Definition at line 154 of file trajectory_cache.cpp.

◆ countTrajectories()

unsigned moveit_ros::trajectory_cache::TrajectoryCache::countTrajectories ( const std::string & cache_namespace)

Count the number of non-cartesian trajectories for a particular cache namespace.

Parameters
[in]cache_namespace.A namespace to separate cache entries by. The name of the robot is a good choice.
Returns
The number of non-cartesian trajectories for the cache namespace.

Definition at line 147 of file trajectory_cache.cpp.

◆ fetchAllMatchingCartesianTrajectories()

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.

See also
FeaturesInterface<FeatureSourceT>
Parameters
[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.
Returns
A vector of cache hits, sorted by the sort_by parameter.

Definition at line 334 of file trajectory_cache.cpp.

Here is the caller graph for this function:

◆ fetchAllMatchingTrajectories()

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.

See also
FeaturesInterface<FeatureSourceT>
Parameters
[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.
Returns
A vector of cache hits, sorted by the sort_by parameter.

Definition at line 197 of file trajectory_cache.cpp.

Here is the caller graph for this function:

◆ fetchBestMatchingCartesianTrajectory()

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.

See also
FeaturesInterface<FeatureSourceT>
Parameters
[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.
Returns
The best cache hit, with respect to the sort_by parameter.

Definition at line 358 of file trajectory_cache.cpp.

Here is the call graph for this function:

◆ fetchBestMatchingTrajectory()

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.

See also
FeaturesInterface<FeatureSourceT>
Parameters
[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.
Returns
The best cache hit, with respect to the sort_by parameter.

Definition at line 220 of file trajectory_cache.cpp.

Here is the call graph for this function:

◆ getDbPath()

std::string moveit_ros::trajectory_cache::TrajectoryCache::getDbPath ( ) const

Gets the database path.

Definition at line 161 of file trajectory_cache.cpp.

◆ getDbPort()

uint32_t moveit_ros::trajectory_cache::TrajectoryCache::getDbPort ( ) const

Gets the database port.

Definition at line 166 of file trajectory_cache.cpp.

◆ getDefaultCacheInsertPolicy()

std::unique_ptr< CacheInsertPolicyInterface< MotionPlanRequest, MoveGroupInterface::Plan, RobotTrajectory > > moveit_ros::trajectory_cache::TrajectoryCache::getDefaultCacheInsertPolicy ( )
static

Gets the default cache insert policy for MotionPlanRequest messages.

See also
BestSeenExecutionTimePolicy

Definition at line 98 of file trajectory_cache.cpp.

◆ getDefaultCartesianCacheInsertPolicy()

std::unique_ptr< CacheInsertPolicyInterface< GetCartesianPath::Request, GetCartesianPath::Response, RobotTrajectory > > moveit_ros::trajectory_cache::TrajectoryCache::getDefaultCartesianCacheInsertPolicy ( )
static

Gets the default cache insert policy for GetCartesianPath requests.

See also
CartesianBestSeenExecutionTimePolicy

Definition at line 110 of file trajectory_cache.cpp.

◆ getDefaultCartesianFeatures()

std::vector< std::unique_ptr< FeaturesInterface< GetCartesianPath::Request > > > moveit_ros::trajectory_cache::TrajectoryCache::getDefaultCartesianFeatures ( double start_tolerance,
double goal_tolerance,
double min_fraction )
static

Gets the default features for GetCartesianPath requests.

See also
CartesianBestSeenExecutionTimePolicy::getDefaultFeatures

Definition at line 104 of file trajectory_cache.cpp.

Here is the call graph for this function:

◆ getDefaultFeatures()

std::vector< std::unique_ptr< FeaturesInterface< MotionPlanRequest > > > moveit_ros::trajectory_cache::TrajectoryCache::getDefaultFeatures ( double start_tolerance,
double goal_tolerance )
static

Gets the default features for MotionPlanRequest messages.

See also
BestSeenExecutionTimePolicy::getDefaultFeatures

Definition at line 92 of file trajectory_cache.cpp.

Here is the call graph for this function:

◆ getDefaultSortFeature()

std::string moveit_ros::trajectory_cache::TrajectoryCache::getDefaultSortFeature ( )
static

Gets the default sort feature.

Definition at line 115 of file trajectory_cache.cpp.

◆ getExactMatchPrecision()

double moveit_ros::trajectory_cache::TrajectoryCache::getExactMatchPrecision ( ) const

Gets the exact match precision.

Definition at line 171 of file trajectory_cache.cpp.

◆ getNumAdditionalTrajectoriesToPreserveWhenPruningWorse()

size_t moveit_ros::trajectory_cache::TrajectoryCache::getNumAdditionalTrajectoriesToPreserveWhenPruningWorse ( ) const

Get the number of trajectories to preserve when pruning worse trajectories.

Definition at line 181 of file trajectory_cache.cpp.

◆ init()

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.

Parameters
[in]options.An instance of TrajectoryCache::Options to initialize the cache with.
See also
TrajectoryCache::Options
Returns
True if the database was successfully connected to.

Definition at line 129 of file trajectory_cache.cpp.

Here is the call graph for this function:

◆ insertCartesianTrajectory()

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.

See also
FeaturesInterface<FeatureSourceT>
CacheInsertPolicyInterface<KeyT, ValueT, CacheEntryT>
Parameters
[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.
Returns
True if the trajectory was inserted into the cache.

Definition at line 385 of file trajectory_cache.cpp.

Here is the call graph for this function:

◆ insertTrajectory()

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.

See also
FeaturesInterface<FeatureSourceT>
CacheInsertPolicyInterface<KeyT, ValueT, CacheEntryT>
Parameters
[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.
Returns
True if the trajectory was inserted into the cache.

Definition at line 246 of file trajectory_cache.cpp.

Here is the call graph for this function:

◆ setExactMatchPrecision()

void moveit_ros::trajectory_cache::TrajectoryCache::setExactMatchPrecision ( double exact_match_precision)

Sets the exact match precision.

Definition at line 176 of file trajectory_cache.cpp.

◆ setNumAdditionalTrajectoriesToPreserveWhenPruningWorse()

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 186 of file trajectory_cache.cpp.


The documentation for this class was generated from the following files: