moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
Classes | List of all members
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:

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:

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

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 155 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 148 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 335 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 198 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 359 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 221 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 162 of file trajectory_cache.cpp.

◆ getDbPort()

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

Gets the database port.

Definition at line 167 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 99 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 111 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 105 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 93 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 116 of file trajectory_cache.cpp.

◆ getExactMatchPrecision()

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

Gets the exact match precision.

Definition at line 172 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 182 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 130 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 386 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 247 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 177 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 187 of file trajectory_cache.cpp.


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