Trajectory Cache manager for MoveIt. More...

#include <moveit/trajectory_cache/trajectory_cache.hpp>

Classes
struct	Options

Public Member Functions
	TrajectoryCache (const rclcpp::Node::SharedPtr &node)
	Constructs a TrajectoryCache. More...

bool	init (const Options &options)
	Initializes the TrajectoryCache. More...

unsigned	countTrajectories (const std::string &cache_namespace)
	Count the number of non-cartesian trajectories for a particular cache namespace. More...

unsigned	countCartesianTrajectories (const std::string &cache_namespace)
	Count the number of cartesian trajectories for a particular cache namespace. More...

Getters and setters.
std::string	getDbPath () const
	Gets the database path. More...

uint32_t	getDbPort () const
	Gets the database port. More...

double	getExactMatchPrecision () const
	Gets the exact match precision. More...

void	setExactMatchPrecision (double exact_match_precision)
	Sets the exact match precision. More...

size_t	getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse () const
	Get the number of trajectories to preserve when deleting worse trajectories. More...

void	setNumAdditionalTrajectoriesToPreserveWhenDeletingWorse (size_t num_additional_trajectories_to_preserve_when_deleting_worse)
	Set the number of additional trajectories to preserve when deleting worse trajectories. More...

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, double start_tolerance, double goal_tolerance, bool metadata_only=false, const std::string &sort_by="execution_time_s", bool ascending=true) const
	Fetches all plans that fit within the requested tolerances for start and goal conditions, returning them as a vector, sorted by some cache column. More...

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, double start_tolerance, double goal_tolerance, bool metadata_only=false, const std::string &sort_by="execution_time_s", bool ascending=true) const
	Fetches the best trajectory that fits within the requested tolerances for start and goal conditions, by some cache column. More...

bool	insertTrajectory (const moveit::planning_interface::MoveGroupInterface &move_group, const std::string &cache_namespace, const moveit_msgs::msg::MotionPlanRequest &plan_request, const moveit_msgs::msg::RobotTrajectory &trajectory, double execution_time_s, double planning_time_s, bool prune_worse_trajectories=true)
	Inserts a trajectory into the database if it is the best matching trajectory seen so far. More...

Cartesian trajectory caching
moveit_msgs::srv::GetCartesianPath::Request	constructGetCartesianPathRequest (moveit::planning_interface::MoveGroupInterface &move_group, const std::vector< geometry_msgs::msg::Pose > &waypoints, double max_step, double jump_threshold, bool avoid_collisions=true)
	Constructs a GetCartesianPath request. More...

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, double min_fraction, double start_tolerance, double goal_tolerance, bool metadata_only=false, const std::string &sort_by="execution_time_s", bool ascending=true) const
	Fetches all cartesian trajectories that fit within the requested tolerances for start and goal conditions, returning them as a vector, sorted by some cache column. More...

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, double min_fraction, double start_tolerance, double goal_tolerance, bool metadata_only=false, const std::string &sort_by="execution_time_s", bool ascending=true) const
	Fetches the best cartesian trajectory that fits within the requested tolerances for start and goal conditions, by some cache column. More...

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::msg::RobotTrajectory &trajectory, double execution_time_s, double planning_time_s, double fraction, bool prune_worse_trajectories=true)
	Inserts a cartesian trajectory into the database if it is the best matching cartesian trajectory seen so far. More...

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 by the start and goal conditions, and sorted by how long the trajectories took to execute. This allows for the lookup and reuse of the best performing trajectories found so far.

WARNING: RFE: !!! This cache does NOT support collision detection! Trajectories will be put 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.

!!! This cache does NOT support keying on joint velocities and efforts. The cache only keys on joint positions.

!!! This cache does NOT support multi-DOF joints.

!!! This cache does NOT support certain constraints Including: path, constraint regions, everything related to collision.

This is because they are difficult (but not impossible) to implement key logic for.

Relevant ROS Parameters:

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.

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

Motion Plan Trajectories are keyed on:

Plan Start: robot joint state
Plan Goal (either of):
- Final pose (wrt. planning_frame (usually base_link))
- Final robot joint states
Plan Constraints (but not collision)

Trajectories may be looked up with some tolerance at call time.

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.

Cartesian Trajectories are keyed on:

Plan Start: robot joint state
Plan Goal:
- Pose waypoints

Definition at line 117 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, log, and listen for TF.

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

Member Function Documentation

◆ constructGetCartesianPathRequest()

moveit_msgs::srv::GetCartesianPath::Request moveit_ros::trajectory_cache::TrajectoryCache::constructGetCartesianPathRequest	(	moveit::planning_interface::MoveGroupInterface &	move_group,
		const std::vector< geometry_msgs::msg::Pose > &	waypoints,
		double	max_step,
		double	jump_threshold,
		bool	avoid_collisions = `true`
	)

Constructs a GetCartesianPath request.

This mimics the move group computeCartesianPath signature (without path constraints).

Parameters

[in]	move_group.	The manipulator move group, used to get its state, frames, and link.
[in]	waypoints.	The cartesian waypoints to request the path for.
[in]	max_step.	The value to populate into the `GetCartesianPath` request's max_step field.
[in]	jump_threshold.	The value to populate into the `GetCartesianPath` request's jump_threshold field.
[in]	avoid_collisions.	The value to populate into the `GetCartesianPath` request's avoid_collisions field.

Returns

Definition at line 346 of file trajectory_cache.cpp.

◆ 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 134 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 128 of file trajectory_cache.cpp.

◆ fetchAllMatchingCartesianTrajectories()

std::vector< MessageWithMetadata< moveit_msgs::msg::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,
		double	min_fraction,
		double	start_tolerance,
		double	goal_tolerance,
		bool	metadata_only = `false`,
		const std::string &	sort_by = `"execution_time_s"`,
		bool	ascending = `true`
	)		const

Fetches all cartesian trajectories that fit within the requested tolerances for start and goal conditions, returning them as a vector, sorted by some cache column.

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 key the cache with.
[in]	min_fraction.	The minimum fraction required for a cache hit.
[in]	start_tolerance.	Match tolerance for cache entries for the `plan_request` start parameters.
[in]	goal_tolerance.	Match tolerance for cache entries for the `plan_request` goal parameters.
[in]	metadata_only.	If true, returns only the cache entry metadata.
[in]	sort_by.	The cache column to sort by, defaults to execution time.
[in]	ascending.	If true, sorts in ascending order. If false, sorts in descending order.

Returns: A vector of cache hits, sorted by the sort_by param.

Definition at line 371 of file trajectory_cache.cpp.

Here is the caller graph for this function:

◆ fetchAllMatchingTrajectories()

std::vector< MessageWithMetadata< moveit_msgs::msg::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,
		double	start_tolerance,
		double	goal_tolerance,
		bool	metadata_only = `false`,
		const std::string &	sort_by = `"execution_time_s"`,
		bool	ascending = `true`
	)		const

Fetches all plans that fit within the requested tolerances for start and goal conditions, returning them as a vector, sorted by some cache column.

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 key the cache with.
[in]	start_tolerance.	Match tolerance for cache entries for the `plan_request` start parameters.
[in]	goal_tolerance.	Match tolerance for cache entries for the `plan_request` goal parameters.
[in]	metadata_only.	If true, returns only the cache entry metadata.
[in]	sort_by.	The cache column to sort by, defaults to execution time.
[in]	ascending.	If true, sorts in ascending order. If false, sorts in descending order.

Returns: A vector of cache hits, sorted by the sort_by param.

Definition at line 182 of file trajectory_cache.cpp.

Here is the caller graph for this function:

◆ fetchBestMatchingCartesianTrajectory()

MessageWithMetadata< moveit_msgs::msg::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,
		double	min_fraction,
		double	start_tolerance,
		double	goal_tolerance,
		bool	metadata_only = `false`,
		const std::string &	sort_by = `"execution_time_s"`,
		bool	ascending = `true`
	)		const

Fetches the best cartesian trajectory that fits within the requested tolerances for start and goal conditions, by some cache column.

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 key the cache with.
[in]	min_fraction.	The minimum fraction required for a cache hit.
[in]	start_tolerance.	Match tolerance for cache entries for the `plan_request` start parameters.
[in]	goal_tolerance.	Match tolerance for cache entries for the `plan_request` goal parameters.
[in]	metadata_only.	If true, returns only the cache entry metadata.
[in]	sort_by.	The cache column to sort by, defaults to execution time.
[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 param.

Definition at line 397 of file trajectory_cache.cpp.

Here is the call graph for this function:

◆ fetchBestMatchingTrajectory()

MessageWithMetadata< moveit_msgs::msg::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,
		double	start_tolerance,
		double	goal_tolerance,
		bool	metadata_only = `false`,
		const std::string &	sort_by = `"execution_time_s"`,
		bool	ascending = `true`
	)		const

Fetches the best trajectory that fits within the requested tolerances for start and goal conditions, by some cache column.

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 key the cache with.
[in]	start_tolerance.	Match tolerance for cache entries for the `plan_request` start parameters.
[in]	goal_tolerance.	Match tolerance for cache entries for the `plan_request` goal parameters.
[in]	metadata_only.	If true, returns only the cache entry metadata.
[in]	sort_by.	The cache column to sort by, defaults to execution time.
[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 param.

Definition at line 204 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 145 of file trajectory_cache.cpp.

◆ getDbPort()

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

Gets the database port.

Definition at line 150 of file trajectory_cache.cpp.

◆ getExactMatchPrecision()

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

Gets the exact match precision.

Definition at line 155 of file trajectory_cache.cpp.

◆ getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse()

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

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

Definition at line 165 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.

Exceptions

When	options.num_additional_trajectories_to_preserve_when_deleting_worse is less than 1.

Definition at line 114 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::msg::RobotTrajectory &	trajectory,
		double	execution_time_s,
		double	planning_time_s,
		double	fraction,
		bool	prune_worse_trajectories = `true`
	)

Inserts a cartesian trajectory into the database if it is the best matching cartesian trajectory seen so far.

Cartesian trajectories are matched based off their start and goal states. And are considered "better" if they are higher priority in the sorting order specified by sort_by than another exactly matching cartesian trajectory.

A trajectory is "exactly matching" if its start and goal (and fraction) are close enough to another trajectory. The tolerance for this depends on the exact_match_precision arg passed in init().

See also: init()

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

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 key the cache with.
[in]	trajectory.	The trajectory to put.
[in]	execution_time_s.	The execution time of the trajectory, in seconds.
[in]	planning_time_s.	How long the trajectory took to plan, in seconds.
[in]	fraction.	The fraction of the path that was computed.
[in]	prune_worse_trajectories.	If true, will prune the cache by deleting all cache entries that match the `plan_request` exactly, if they are worse than `trajectory`, even if it was not put.

Returns: true if the trajectory was the best seen yet and hence put into the cache.

Definition at line 426 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_msgs::msg::RobotTrajectory &	trajectory,
		double	execution_time_s,
		double	planning_time_s,
		bool	prune_worse_trajectories = `true`
	)

Inserts a trajectory into the database if it is the best matching trajectory seen so far.

Trajectories are matched based off their start and goal states. And are considered "better" if they are higher priority in the sorting order specified by sort_by than another exactly matching trajectory.

A trajectory is "exactly matching" if its start and goal are close enough to another trajectory. The tolerance for this depends on the exact_match_precision arg passed in init().

See also: init()

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

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 key the cache with.
[in]	trajectory.	The trajectory to put.
[in]	execution_time_s.	The execution time of the trajectory, in seconds.
[in]	planning_time_s.	How long the trajectory took to plan, in seconds.
[in]	prune_worse_trajectories.	If true, will prune the cache by deleting all cache entries that match the `plan_request` exactly, if they are worse than the `trajectory`, even if it was not put.

Returns: true if the trajectory was the best seen yet and hence put into the cache.

Definition at line 231 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 160 of file trajectory_cache.cpp.

◆ setNumAdditionalTrajectoriesToPreserveWhenDeletingWorse()

void moveit_ros::trajectory_cache::TrajectoryCache::setNumAdditionalTrajectoriesToPreserveWhenDeletingWorse ( size_t num_additional_trajectories_to_preserve_when_deleting_worse )

Set the number of additional trajectories to preserve when deleting worse trajectories.

Definition at line 170 of file trajectory_cache.cpp.

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

moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp
moveit_ros/trajectory_cache/src/trajectory_cache.cpp

Classes

Public Member Functions

Detailed Description

Constructor & Destructor Documentation

◆ TrajectoryCache()

Member Function Documentation

◆ constructGetCartesianPathRequest()

◆ countCartesianTrajectories()

◆ countTrajectories()

◆ fetchAllMatchingCartesianTrajectories()

◆ fetchAllMatchingTrajectories()

◆ fetchBestMatchingCartesianTrajectory()

◆ fetchBestMatchingTrajectory()

◆ getDbPath()

◆ getDbPort()

◆ getExactMatchPrecision()

◆ getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse()

◆ init()

◆ insertCartesianTrajectory()

◆ insertTrajectory()

◆ setExactMatchPrecision()

◆ setNumAdditionalTrajectoriesToPreserveWhenDeletingWorse()