Class TrajectoryCache

Defined in File trajectory_cache.hpp

Nested Relationships

Nested Types

Struct TrajectoryCache::Options

Class Documentation

class TrajectoryCache

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.

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

FeaturesInterface<FeatureSourceT>

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.

See also

TrajectoryCache::getDefaultFeatures

See also

TrajectoryCache::getDefaultSortFeature

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.

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.

See also

BestSeenExecutionTimePolicy::getDefaultFeatures

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.

See also

BestSeenExecutionTimePolicy

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.

See also

CartesianBestSeenExecutionTimePolicy::getDefaultFeatures

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.

See also

CartesianBestSeenExecutionTimePolicy

static std::string getDefaultSortFeature(): Gets the default sort feature.

Cache configuration.

explicit TrajectoryCache(const rclcpp::Node::SharedPtr &node)

Constructs a TrajectoryCache.

TODO: methylDragon - We explicitly need a Node::SharedPtr because warehouse_ros ONLY supports it… Use rclcpp::node_interfaces::NodeInterfaces<> once warehouse_ros does.

Parameters:: node. – [in] An rclcpp::Node::SharedPtr, which will be used to lookup warehouse_ros parameters and log.

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

See also

TrajectoryCache::Options

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

Getters and setters.

unsigned countTrajectories(const std::string &cache_namespace)

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

Parameters:: cache_namespace. – [in] 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.

unsigned countCartesianTrajectories(const std::string &cache_namespace)

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

Parameters:: cache_namespace. – [in] 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.

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.

See also

FeaturesInterface<FeatureSourceT>

Parameters:

move_group. – [in] The manipulator move group, used to get its state.
cache_namespace. – [in] A namespace to separate cache entries by. The name of the robot is a good choice.
plan_request. – [in] The motion plan request to extract features from to key the cache with.
features. – [in] The features to key the cache with.
sort_by. – [in] The cache feature to sort by.
ascending. – [in] If true, sorts in ascending order. If false, sorts in descending order.
metadata_only. – [in] If true, returns only the cache entry metadata.

Returns:

A vector of cache hits, sorted by the sort_by parameter.

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.

See also

FeaturesInterface<FeatureSourceT>

Parameters:

move_group. – [in] The manipulator move group, used to get its state.
cache_namespace. – [in] A namespace to separate cache entries by. The name of the robot is a good choice.
plan_request. – [in] The motion plan request to extract features from to key the cache with.
features. – [in] The features to key the cache with.
metadata_only. – [in] If true, returns only the cache entry metadata.
sort_by. – [in] The cache feature to sort by.
ascending. – [in] If true, sorts in ascending order. If false, sorts in descending order.

Returns:

The best cache hit, with respect to the sort_by parameter.

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.

Optionally deletes all worse trajectories by default to prune the cache.

See also

FeaturesInterface<FeatureSourceT>

Parameters:

move_group. – [in] The manipulator move group, used to get its state.
cache_namespace. – [in] A namespace to separate cache entries by. The name of the robot is a good choice.
plan_request. – [in] The motion plan request to extract features from to key the cache with.
plan. – [in] The plan containing the trajectory to insert.
cache_insert_policy. – [inout] 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.
prune_worse_trajectories. – [in] If true, will prune the cache according to the cache_insert_policy’s pruning logic.
additional_features. – [in] 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.

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.

See also

FeaturesInterface<FeatureSourceT>

Parameters:

move_group. – [in] The manipulator move group, used to get its state.
cache_namespace. – [in] A namespace to separate cache entries by. The name of the robot is a good choice.
plan_request. – [in] The cartesian plan request to extract features from to key the cache with.
features. – [in] The features to key the cache with.
sort_by. – [in] The cache feature to sort by, defaults to execution time.
ascending. – [in] If true, sorts in ascending order. If false, sorts in descending order.
metadata_only. – [in] If true, returns only the cache entry metadata.

Returns:

A vector of cache hits, sorted by the sort_by parameter.

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.

See also

FeaturesInterface<FeatureSourceT>

Parameters:

move_group. – [in] The manipulator move group, used to get its state.
cache_namespace. – [in] A namespace to separate cache entries by. The name of the robot is a good choice.
plan_request. – [in] The cartesian plan request to extract features from to key the cache with.
features. – [in] The features to key the cache with.
sort_by. – [in] The cache feature to sort by.
ascending. – [in] If true, sorts in ascending order. If false, sorts in descending order.
metadata_only. – [in] If true, returns only the cache entry metadata.

Returns:

The best cache hit, with respect to the sort_by parameter.

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.

Optionally deletes all worse cartesian trajectories by default to prune the cache.

See also

FeaturesInterface<FeatureSourceT>

Parameters:

move_group. – [in] The manipulator move group, used to get its state.
cache_namespace. – [in] A namespace to separate cache entries by. The name of the robot is a good choice.
plan_request. – [in] The cartesian path plan request to extract features from to key the cache with.
plan. – [in] The plan containing the trajectory to insert.
cache_insert_policy. – [inout] 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.
prune_worse_trajectories. – [in] If true, will prune the cache according to the cache_insert_policy’s pruning logic.
additional_features. – [in] 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.

struct Options

Public Members

std::string db_path = ":memory:"

uint32_t db_port = 0

double exact_match_precision = 1e-6

size_t num_additional_trajectories_to_preserve_when_pruning_worse = 1