diff --git a/moveit_ros/trajectory_cache/CHANGELOG.rst b/moveit_ros/trajectory_cache/CHANGELOG.rst new file mode 100644 index 0000000000..bbb2a3679d --- /dev/null +++ b/moveit_ros/trajectory_cache/CHANGELOG.rst @@ -0,0 +1,7 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package moveit_ros_trajectory_cache +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.0 (2024-05-17) +------------------ +* Add ``moveit_ros_trajectory_cache`` package for trajectory caching. diff --git a/moveit_ros/trajectory_cache/CMakeLists.txt b/moveit_ros/trajectory_cache/CMakeLists.txt new file mode 100644 index 0000000000..d013111c53 --- /dev/null +++ b/moveit_ros/trajectory_cache/CMakeLists.txt @@ -0,0 +1,77 @@ +cmake_minimum_required(VERSION 3.22) +project(moveit_ros_trajectory_cache) + +# Common cmake code applied to all moveit packages +find_package(moveit_common REQUIRED) +moveit_package() + +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(moveit_ros_planning_interface REQUIRED) +find_package(moveit_ros_warehouse REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(trajectory_msgs REQUIRED) +find_package(warehouse_ros REQUIRED) + +include(GenerateExportHeader) +include_directories(include) + +set(TRAJECTORY_CACHE_DEPENDENCIES + geometry_msgs + moveit_ros_planning_interface + moveit_ros_warehouse + rclcpp + tf2 + tf2_ros + trajectory_msgs + warehouse_ros) + +# Trajectory cache library +add_library(moveit_ros_trajectory_cache_lib SHARED src/trajectory_cache.cpp) +generate_export_header(moveit_ros_trajectory_cache_lib) +target_include_directories( + moveit_ros_trajectory_cache_lib + PUBLIC $ + $) +ament_target_dependencies(moveit_ros_trajectory_cache_lib + ${TRAJECTORY_CACHE_DEPENDENCIES}) + +install( + TARGETS moveit_ros_trajectory_cache_lib + EXPORT moveit_ros_trajectory_cacheTargets + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES + DESTINATION include/moveit_ros_trajectory_cache) + +install(DIRECTORY include/ DESTINATION include/moveit_ros_trajectory_cache) +install( + FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_ros_trajectory_cache_lib_export.h + DESTINATION include/moveit_ros_trajectory_cache) + +if(BUILD_TESTING) + find_package(ament_cmake_pytest REQUIRED) + find_package(launch_testing_ament_cmake REQUIRED) + find_package(rmf_utils REQUIRED) + find_package(warehouse_ros_sqlite REQUIRED) + + # This test executable is run by the pytest_test, since a node is required for + # testing move groups + add_executable(test_trajectory_cache test/test_trajectory_cache.cpp) + target_link_libraries(test_trajectory_cache moveit_ros_trajectory_cache_lib) + ament_target_dependencies(test_trajectory_cache warehouse_ros_sqlite) + + install(TARGETS test_trajectory_cache RUNTIME DESTINATION lib/${PROJECT_NAME}) + + ament_add_pytest_test( + test_trajectory_cache_py "test/test_trajectory_cache.py" WORKING_DIRECTORY + "${CMAKE_CURRENT_BINARY_DIR}") + +endif() + +ament_export_targets(moveit_ros_trajectory_cacheTargets HAS_LIBRARY_TARGET) +ament_export_dependencies(${TRAJECTORY_CACHE_DEPENDENCIES}) +ament_package() diff --git a/moveit_ros/trajectory_cache/README.md b/moveit_ros/trajectory_cache/README.md new file mode 100644 index 0000000000..659ee0f7a2 --- /dev/null +++ b/moveit_ros/trajectory_cache/README.md @@ -0,0 +1,136 @@ +# Trajectory Cache + +``` + * This cache does NOT support collision detection! + * Plans 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 plans 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 otherwise find a way to determine that a planning scene + * is "less than" or "in range of" another (e.g. checking that every obstacle/mesh + * exists within the other scene's). (very out of scope) + ``` + +A trajectory cache based on [`warehouse_ros`](https://github.com/moveit/warehouse_ros) for the move_group planning interface that supports fuzzy lookup for `MotionPlanRequest` and `GetCartesianPath` requests and trajectories. + +The cache allows you to insert trajectories and fetch keyed fuzzily on the following: + +- Starting robot (joint) state +- Planning request constraints + - This includes ALL joint, position, and orientation constraints! + - And also workspace parameters, and some others. +- Planning request goal parameters + +It works generically for an arbitrary number of joints, across any number of move_groups. + +Furthermore, the strictness of the fuzzy lookup can be configured for start and goal conditions. + +## Citations + +If you use this package in your work, please cite it using the following: + +``` +@software{ong_2024_11215428, + author = {Ong, Brandon}, + title = {A Fuzzy-Matching Trajectory Cache for MoveIt 2}, + month = may, + year = 2024, + publisher = {GitHub}, + version = {0.1.0}, + doi = {10.5281/zenodo.11215428}, + url = {https://doi.org/10.5281/zenodo.11215428} +} +``` + +## Example usage + +```cpp +// Be sure to set some relevant ROS parameters: +// Relevant ROS Parameters: +// - `warehouse_plugin`: What database to use +auto traj_cache = std::make_shared(node); +traj_cache->init(/*db_host=*/":memory:", /*db_port=*/0, /*exact_match_precision=*/1e-6); + +auto fetched_trajectory = + traj_cache->fetchBestMatchingTrajectory(*move_group_interface, robot_name, motion_plan_req_msg, + _cache_start_match_tolerance, _cache_goal_match_tolerance, + /*sort_by=*/"execution_time_s"); + +if (fetched_trajectory) +{ + // Great! We got a cache hit + // Do something with the plan. +} +else +{ + // Plan... And put it for posterity! + traj_cache->insertTrajectory( + *interface, robot_name, std::move(plan_req_msg), std::move(res->result.trajectory), + rclcpp::Duration(res->result.trajectory.joint_trajectory.points.back().time_from_start).seconds(), + res->result.planning_time, /*prune_worse_trajectories=*/true); +} +``` + +It also has the following optimizations: +- Separate caches for separate move groups +- The cache does "canonicalization" of the poses to be relative to the planning frame to increase the chances of cache hits. +- Configurable fuzzy lookup for the keys above. +- It supports "overwriting" of worse trajectories with better trajectories + - If a trajectory with keys extremely close to a pre-existing cache entry is inserted with a better planned execution time, the cache can delete all "worse" trajectories. + - #IsThisMachineLearning + - It also prunes the database and mitigates a lot of query slow-down as the cache grows + +## Working Principle + +If a plan request has start, goal, and constraint conditions that are "close enough" (configurable per request) to an entry in the cache, then the cached trajectory should be suitable (as long as obstacles have not changed). + +Goal fuzziness is a lot less lenient than start fuzziness by default. + +Finally, the databases are sharded by move group, and the constraints are added as columns in a name agnostic fashion (they are even somewhat robust to ordering, because they're sorted!) + +## Benefits + +A trajectory cache helps: +- Cut down on planning time (especially for known moves) +- Allows for consistent predictable behavior of used together with a stochastic planner + - It effectively allows you to "freeze" a move + +These benefits come from the fact that the cache acts as a lookup table of plans that were already made for a given scenario and constraints, allowing the cache to be substituted for a planner call. The reuse of cached plans then allow you to get predictable execution behavior. + +A user may also choose when to leverage the cache (e.g. when planning moves from a static "home" position, or repetitive/cartesian moves) to get these benefits. + +Additionally, because the cache class has the ability to sort by planned execution time, over sufficient runs, the stochastic plans eventually converge to better and better plans (execution time wise). + +You may build abstractions on top of the class, for example, to expose the following behaviors: +- `TrainingOverwrite`: Always plan, and write to cache, deleting all worse trajectories for "matching" cache keys +- `TrainingAppendOnly`: Always plan, and always add to the cache. +- `ExecuteBestEffort`: Rely on cache wherever possible, but plan otherwise. +- `ExecuteReadOnly`: Only execute if cache hit occurred. + +You can see how such behaviors effectively model the "dev" and "deploy" phases of a robot deployment, and how they could be useful. + +## Best Practices + +- Since this cache does not yet support collisions, ensure the planning scene and obstacles remain static +- Have looser start fuzziness, and stricter goal fuzziness +- Move the robot to static known poses where possible before planning to increase the chances of a cache hit +- Use the cache where repetitive, non-dynamic motion is likely to occur (e.g. known plans, short planned moves, etc.) + +## WARNING: The following are unsupported / RFE + +Since this is an initial release, the following features are unsupported because they were a little too difficult for the time I had to implement this. So I am leaving it to the community to help! + +- **!!! 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. +- The fuzzy lookup can't be configured on a per-joint basis. +- Alternate ordinal lookup metrics for the cache + - Currently only execution time is explicitly supported as a way to compare cache entries. Ideally we should be able to inject lambdas to save custom cache DB metadata to represent and sort on custom cost functions (e.g. minimum jerk, path length, etc.). (e.g. https://github.com/moveit/moveit2/pull/2153) diff --git a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp new file mode 100644 index 0000000000..2af5058840 --- /dev/null +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp @@ -0,0 +1,544 @@ +// Copyright 2024 Intrinsic Innovation LLC. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/** @file + * @brief Fuzzy-Matching Trajectory Cache. + * @author methylDragon + */ + +#pragma once + +#include +#include +#include + +#include + +#include +#include + +// TF2 +#include +#include + +// ROS2 Messages +#include +#include + +// moveit modules +#include +#include + +namespace moveit_ros +{ +namespace trajectory_cache +{ + +/** @class TrajectoryCache trajectory_cache.hpp moveit/trajectory_cache/trajectory_cache.hpp + * + * @brief 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 + */ +class TrajectoryCache +{ +public: + /** + * @brief Constructs a TrajectoryCache. + * + * @param[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. + */ + explicit TrajectoryCache(const rclcpp::Node::SharedPtr& node); + + /** + * @brief Options struct for TrajectoryCache. + * + * @property db_path. The database path. + * @property db_port. The database port. + * @property exact_match_precision. Tolerance for float precision comparison for what counts as an exact match. + * An exact match is when: + * (candidate >= value - (exact_match_precision / 2) + * && candidate <= value + (exact_match_precision / 2)) + * @property num_additional_trajectories_to_preserve_when_deleting_worse. The number of additional cached trajectories + * to preserve when `prune_worse_trajectories` is true. It is useful to keep more than one matching trajectory to + * have alternative trajectories to handle obstacles. + */ + struct Options + { + std::string db_path = ":memory:"; + uint32_t db_port = 0; + + double exact_match_precision = 1e-6; + size_t num_additional_trajectories_to_preserve_when_deleting_worse = 1; + }; + + /** + * @brief 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. + * + * @param[in] options. An instance of TrajectoryCache::Options to initialize the cache with. + * @see TrajectoryCache::Options + * @returns true if the database was successfully connected to. + * @throws When options.num_additional_trajectories_to_preserve_when_deleting_worse is less than 1. + * */ + bool init(const Options& options); + + /** + * @brief Count the number of non-cartesian trajectories for a particular cache namespace. + * + * @param[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. + */ + unsigned countTrajectories(const std::string& cache_namespace); + + /** + * @brief Count the number of cartesian trajectories for a particular cache namespace. + * + * @param[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. + */ + unsigned countCartesianTrajectories(const std::string& cache_namespace); + + /** + * @name Getters and setters. + */ + /**@{*/ + + /// @brief Gets the database path. + std::string getDbPath() const; + + /// @brief Gets the database port. + uint32_t getDbPort() const; + + /// @brief Gets the exact match precision. + double getExactMatchPrecision() const; + + /// @brief Sets the exact match precision. + void setExactMatchPrecision(double exact_match_precision); + + /// @brief Get the number of trajectories to preserve when deleting worse trajectories. + size_t getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse() const; + + /// @brief Set the number of additional trajectories to preserve when deleting worse trajectories. + void setNumAdditionalTrajectoriesToPreserveWhenDeletingWorse( + size_t num_additional_trajectories_to_preserve_when_deleting_worse); + + /**@}*/ + + /** + * @name Motion plan trajectory caching + */ + /**@{*/ + + /** + * @brief Fetches all plans that fit within the requested tolerances for start and goal conditions, returning them as + * a vector, sorted by some cache column. + * + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. + * @param[in] plan_request. The motion plan request to key the cache with. + * @param[in] start_tolerance. Match tolerance for cache entries for the `plan_request` start parameters. + * @param[in] goal_tolerance. Match tolerance for cache entries for the `plan_request` goal parameters. + * @param[in] metadata_only. If true, returns only the cache entry metadata. + * @param[in] sort_by. The cache column to sort by, defaults to execution time. + * @param[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. + */ + std::vector::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; + + /** + * @brief Fetches the best trajectory that fits within the requested tolerances for start and goal conditions, by some + * cache column. + * + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. + * @param[in] plan_request. The motion plan request to key the cache with. + * @param[in] start_tolerance. Match tolerance for cache entries for the `plan_request` start parameters. + * @param[in] goal_tolerance. Match tolerance for cache entries for the `plan_request` goal parameters. + * @param[in] metadata_only. If true, returns only the cache entry metadata. + * @param[in] sort_by. The cache column to sort by, defaults to execution time. + * @param[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. + */ + warehouse_ros::MessageWithMetadata::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; + + /** + * @brief 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 init() + * + * Optionally deletes all worse trajectories by default to prune the cache. + * + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. + * @param[in] plan_request. The motion plan request to key the cache with. + * @param[in] trajectory. The trajectory to put. + * @param[in] execution_time_s. The execution time of the trajectory, in seconds. + * @param[in] planning_time_s. How long the trajectory took to plan, in seconds. + * @param[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. + */ + 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); + + /**@}*/ + + /** + * @name Cartesian trajectory caching + */ + /**@{*/ + + /** + * @brief Constructs a GetCartesianPath request. + * + * This mimics the move group computeCartesianPath signature (without path constraints). + * + * @param[in] move_group. The manipulator move group, used to get its state, frames, and link. + * @param[in] waypoints. The cartesian waypoints to request the path for. + * @param[in] max_step. The value to populate into the `GetCartesianPath` request's max_step field. + * @param[in] jump_threshold. The value to populate into the `GetCartesianPath` request's jump_threshold field. + * @param[in] avoid_collisions. The value to populate into the `GetCartesianPath` request's avoid_collisions field. + * @returns + */ + moveit_msgs::srv::GetCartesianPath::Request + constructGetCartesianPathRequest(moveit::planning_interface::MoveGroupInterface& move_group, + const std::vector& waypoints, double max_step, + double jump_threshold, bool avoid_collisions = true); + + /** + * @brief 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. + * + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. + * @param[in] plan_request. The cartesian plan request to key the cache with. + * @param[in] min_fraction. The minimum fraction required for a cache hit. + * @param[in] start_tolerance. Match tolerance for cache entries for the `plan_request` start parameters. + * @param[in] goal_tolerance. Match tolerance for cache entries for the `plan_request` goal parameters. + * @param[in] metadata_only. If true, returns only the cache entry metadata. + * @param[in] sort_by. The cache column to sort by, defaults to execution time. + * @param[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. + */ + std::vector::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; + + /** + * @brief Fetches the best cartesian trajectory that fits within the requested tolerances for start and goal + * conditions, by some cache column. + * + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. + * @param[in] plan_request. The cartesian plan request to key the cache with. + * @param[in] min_fraction. The minimum fraction required for a cache hit. + * @param[in] start_tolerance. Match tolerance for cache entries for the `plan_request` start parameters. + * @param[in] goal_tolerance. Match tolerance for cache entries for the `plan_request` goal parameters. + * @param[in] metadata_only. If true, returns only the cache entry metadata. + * @param[in] sort_by. The cache column to sort by, defaults to execution time. + * @param[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. + */ + warehouse_ros::MessageWithMetadata::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; + + /** + * @brief 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 init() + * + * Optionally deletes all worse cartesian trajectories by default to prune the cache. + * + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] cache_namespace. A namespace to separate cache entries by. The name of the robot is a good choice. + * @param[in] plan_request. The cartesian plan request to key the cache with. + * @param[in] trajectory. The trajectory to put. + * @param[in] execution_time_s. The execution time of the trajectory, in seconds. + * @param[in] planning_time_s. How long the trajectory took to plan, in seconds. + * @param[in] fraction. The fraction of the path that was computed. + * @param[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. + */ + 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); + + /**@}*/ + +private: + /** + * @name Motion plan trajectory query and metadata construction + */ + /**@{*/ + + /** + * @brief Extracts relevant parameters from a motion plan request's start parameters to populate a cache db query, + * with some match tolerance. + * + * These parameters will be used to look-up relevant sections of a cache element's key. + * + * @param[out] query. The query to add parameters to. + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] plan_request. The motion plan request to key the cache with. + * @param[in] match_tolerance. The match tolerance (additive with exact_match_precision) for the query. + * @returns true if successfully added to. If false, the query might have been partially modified and should not be + * used. + */ + bool extractAndAppendTrajectoryStartToQuery(warehouse_ros::Query& query, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request, + double match_tolerance) const; + + /** + * @brief Extracts relevant parameters from a motion plan request's goal parameters to populate a cache db query, with + * some match tolerance. + * + * These parameters will be used to look-up relevant sections of a cache element's key. + * + * @param[out] query. The query to add parameters to. + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] plan_request. The motion plan request to key the cache with. + * @param[in] match_tolerance. The match tolerance (additive with exact_match_precision) for the query. + * @returns true if successfully added to. If false, the query might have been partially modified and should not be + * used. + */ + bool extractAndAppendTrajectoryGoalToQuery(warehouse_ros::Query& query, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request, + double match_tolerance) const; + + /** + * @brief Extracts relevant parameters from a motion plan request's start parameters to populate a cache entry's + * metadata. + * + * These parameters will be used key the cache element. + * + * @param[out] metadata. The metadata to add parameters to. + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] plan_request. The motion plan request to key the cache with. + * @returns true if successfully added to. If false, the metadata might have been partially modified and should not be + * used. + */ + bool extractAndAppendTrajectoryStartToMetadata(warehouse_ros::Metadata& metadata, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request) const; + + /** + * @brief Extracts relevant parameters from a motion plan request's goal parameters to populate a cache entry's + * metadata. + * + * These parameters will be used key the cache element. + * + * @param[out] metadata. The metadata to add parameters to. + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] plan_request. The motion plan request to key the cache with. + * @returns true if successfully added to. If false, the metadata might have been partially modified and should not be + * used. + */ + bool extractAndAppendTrajectoryGoalToMetadata(warehouse_ros::Metadata& metadata, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request) const; + + /**@}*/ + + /** + * @name Cartesian trajectory query and metadata construction + */ + /**@{*/ + + /** + * @brief Extracts relevant parameters from a cartesian plan request's start parameters to populate a cache db query, + * with some match tolerance. + * + * These parameters will be used to look-up relevant sections of a cache element's key. + * + * @param[out] query. The query to add parameters to. + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] plan_request. The cartesian plan request to key the cache with. + * @param[in] match_tolerance. The match tolerance (additive with exact_match_precision) for the query. + * @returns true if successfully added to. If false, the query might have been partially modified and should not be + * used. + */ + bool extractAndAppendCartesianTrajectoryStartToQuery(warehouse_ros::Query& query, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, + double match_tolerance) const; + + /** + * @brief Extracts relevant parameters from a cartesian plan request's goal parameters to populate a cache db query, + * with some match tolerance. + * + * These parameters will be used to look-up relevant sections of a cache element's key. + * + * @param[out] query. The query to add parameters to. + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] plan_request. The cartesian plan request to key the cache with. + * @param[in] match_tolerance. The match tolerance (additive with exact_match_precision) for the query. + * @returns true if successfully added to. If false, the query might have been partially modified and should not be + * used. + */ + bool extractAndAppendCartesianTrajectoryGoalToQuery(warehouse_ros::Query& query, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, + double match_tolerance) const; + + /** + * @brief Extracts relevant parameters from a cartesian plan request's goal parameters to populate a cache entry's + * metadata. + * + * These parameters will be used key the cache element. + * + * @param[out] metadata. The metadata to add parameters to. + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] plan_request. The cartesian plan request to key the cache with. + * @returns true if successfully added to. If false, the metadata might have been partially modified and should not be + * used. + */ + bool extractAndAppendCartesianTrajectoryStartToMetadata( + warehouse_ros::Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request) const; + + /** + * @brief Extracts relevant parameters from a cartesian plan request's goal parameters to populate a cache entry's + * metadata. + * + * These parameters will be used key the cache element. + * + * @param[out] metadata. The metadata to add parameters to. + * @param[in] move_group. The manipulator move group, used to get its state. + * @param[in] plan_request. The cartesian plan request to key the cache with. + * @returns true if successfully added to. If false, the metadata might have been partially modified and should not be + * used. + */ + bool extractAndAppendCartesianTrajectoryGoalToMetadata( + warehouse_ros::Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request) const; + + /**@}*/ + + rclcpp::Node::SharedPtr node_; + rclcpp::Logger logger_; + warehouse_ros::DatabaseConnection::Ptr db_; + + Options options_; + + std::unique_ptr tf_buffer_; + std::shared_ptr tf_listener_; +}; + +} // namespace trajectory_cache +} // namespace moveit_ros diff --git a/moveit_ros/trajectory_cache/package.xml b/moveit_ros/trajectory_cache/package.xml new file mode 100644 index 0000000000..cbd862e050 --- /dev/null +++ b/moveit_ros/trajectory_cache/package.xml @@ -0,0 +1,41 @@ + + + + moveit_ros_trajectory_cache + 0.1.0 + A trajectory cache for MoveIt 2 motion plans and cartesian plans. + Brandon Ong + Apache License 2.0 + + ament_cmake + + moveit_common + geometry_msgs + moveit_ros_planning_interface + rclcpp + rclcpp_action + tf2_ros + trajectory_msgs + + moveit_ros + python3-yaml + xacro + + ament_cmake_pytest + ament_cmake_uncrustify + launch_pytest + launch_testing_ament_cmake + + moveit_configs_utils + moveit_planners_ompl + moveit_resources + python3-pytest + rmf_utils + robot_state_publisher + ros2_control + warehouse_ros_sqlite + + + ament_cmake + + diff --git a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp new file mode 100644 index 0000000000..81bceebe5c --- /dev/null +++ b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp @@ -0,0 +1,1438 @@ +// Copyright 2024 Intrinsic Innovation LLC. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/** @file + * @brief Implementation of the Fuzzy-Matching Trajectory Cache. + * @author methylDragon + */ + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace moveit_ros +{ +namespace trajectory_cache +{ + +using warehouse_ros::MessageWithMetadata; +using warehouse_ros::Metadata; +using warehouse_ros::Query; + +// Utils ======================================================================= + +// Ensure we always have a workspace frame ID. +// +// It makes sense to use getModelFrame() in the absence of a workspace parameter frame ID because the same method is +// used to populate the workspace header frame ID in the MoveGroupInterface's setWorkspace() method, which this function +// is associated with. +std::string getWorkspaceFrameId(const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::WorkspaceParameters& workspace_parameters) +{ + if (workspace_parameters.header.frame_id.empty()) + { + return move_group.getRobotModel()->getModelFrame(); + } + else + { + return workspace_parameters.header.frame_id; + } +} + +// Ensure we always have a cartesian path request frame ID. +// +// It makes sense to use getPoseReferenceFrame() in the absence of a frame ID in the request because the same method is +// used to populate the header frame ID in the MoveGroupInterface's computeCartesianPath() method, which this function is associated with. +std::string getCartesianPathRequestFrameId(const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& path_request) +{ + if (path_request.header.frame_id.empty()) + { + return move_group.getPoseReferenceFrame(); + } + else + { + return path_request.header.frame_id; + } +} + +// Append a range inclusive query with some tolerance about some center value. +void queryAppendRangeInclusiveWithTolerance(Query& query, const std::string& name, double center, double tolerance) +{ + query.appendRangeInclusive(name, center - tolerance / 2, center + tolerance / 2); +} + +// Sort constraint components by joint or link name. +void sortConstraints(std::vector& joint_constraints, + std::vector& position_constraints, + std::vector& orientation_constraints) +{ + std::sort(joint_constraints.begin(), joint_constraints.end(), + [](const moveit_msgs::msg::JointConstraint& l, const moveit_msgs::msg::JointConstraint& r) { + return l.joint_name < r.joint_name; + }); + + std::sort(position_constraints.begin(), position_constraints.end(), + [](const moveit_msgs::msg::PositionConstraint& l, const moveit_msgs::msg::PositionConstraint& r) { + return l.link_name < r.link_name; + }); + + std::sort(orientation_constraints.begin(), orientation_constraints.end(), + [](const moveit_msgs::msg::OrientationConstraint& l, const moveit_msgs::msg::OrientationConstraint& r) { + return l.link_name < r.link_name; + }); +} + +// Trajectory Cache ============================================================ + +TrajectoryCache::TrajectoryCache(const rclcpp::Node::SharedPtr& node) + : node_(node), logger_(moveit::getLogger("moveit.ros.trajectory_cache")) +{ + tf_buffer_ = std::make_unique(node_->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); +} + +bool TrajectoryCache::init(const TrajectoryCache::Options& options) +{ + RCLCPP_DEBUG(logger_, "Opening trajectory cache database at: %s (Port: %d, Precision: %f)", options.db_path.c_str(), + options.db_port, options.exact_match_precision); + + // If the `warehouse_plugin` parameter isn't set, defaults to warehouse_ros' + // default. + db_ = moveit_warehouse::loadDatabase(node_); + options_ = options; + + db_->setParams(options.db_path, options.db_port); + return db_->connect(); +} + +unsigned TrajectoryCache::countTrajectories(const std::string& cache_namespace) +{ + auto coll = db_->openCollection("move_group_trajectory_cache", cache_namespace); + return coll.count(); +} + +unsigned TrajectoryCache::countCartesianTrajectories(const std::string& cache_namespace) +{ + auto coll = + db_->openCollection("move_group_cartesian_trajectory_cache", cache_namespace); + return coll.count(); +} + +// ============================================================================= +// GETTERS AND SETTERS +// ============================================================================= + +std::string TrajectoryCache::getDbPath() const +{ + return options_.db_path; +} + +uint32_t TrajectoryCache::getDbPort() const +{ + return options_.db_port; +} + +double TrajectoryCache::getExactMatchPrecision() const +{ + return options_.exact_match_precision; +} + +void TrajectoryCache::setExactMatchPrecision(double exact_match_precision) +{ + options_.exact_match_precision = exact_match_precision; +} + +size_t TrajectoryCache::getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse() const +{ + return options_.num_additional_trajectories_to_preserve_when_deleting_worse; +} + +void TrajectoryCache::setNumAdditionalTrajectoriesToPreserveWhenDeletingWorse( + size_t num_additional_trajectories_to_preserve_when_deleting_worse) +{ + options_.num_additional_trajectories_to_preserve_when_deleting_worse = + num_additional_trajectories_to_preserve_when_deleting_worse; +} + +// ============================================================================= +// MOTION PLAN TRAJECTORY CACHING +// ============================================================================= + +std::vector::ConstPtr> +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, + const std::string& sort_by, bool ascending) const +{ + auto coll = db_->openCollection("move_group_trajectory_cache", cache_namespace); + + Query::Ptr query = coll.createQuery(); + + bool start_ok = this->extractAndAppendTrajectoryStartToQuery(*query, move_group, plan_request, start_tolerance); + bool goal_ok = this->extractAndAppendTrajectoryGoalToQuery(*query, move_group, plan_request, goal_tolerance); + + if (!start_ok || !goal_ok) + { + RCLCPP_ERROR(logger_, "Could not construct trajectory query."); + return {}; + } + + return coll.queryList(query, metadata_only, sort_by, ascending); +} + +MessageWithMetadata::ConstPtr 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, const std::string& sort_by, bool ascending) const +{ + // First find all matching, but metadata only. + // Then use the ID metadata of the best plan to pull the actual message. + auto matching_trajectories = this->fetchAllMatchingTrajectories( + move_group, cache_namespace, plan_request, start_tolerance, goal_tolerance, true, sort_by, ascending); + + if (matching_trajectories.empty()) + { + RCLCPP_DEBUG(logger_, "No matching trajectories found."); + return nullptr; + } + + auto coll = db_->openCollection("move_group_trajectory_cache", cache_namespace); + + // Best plan is at first index, since the lookup query was sorted by + // execution_time. + int best_trajectory_id = matching_trajectories.at(0)->lookupInt("id"); + Query::Ptr best_query = coll.createQuery(); + best_query->append("id", best_trajectory_id); + + return coll.findOne(best_query, metadata_only); +} + +bool 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) +{ + std::string workspace_frame_id = getWorkspaceFrameId(move_group, plan_request.workspace_parameters); + + // Check pre-conditions + if (!trajectory.multi_dof_joint_trajectory.points.empty()) + { + RCLCPP_ERROR(logger_, "Skipping plan insert: Multi-DOF trajectory plans are not supported."); + return false; + } + if (workspace_frame_id.empty()) + { + RCLCPP_ERROR(logger_, "Skipping plan insert: Workspace frame ID cannot be empty."); + return false; + } + if (trajectory.joint_trajectory.header.frame_id.empty()) + { + RCLCPP_ERROR(logger_, "Skipping plan insert: Trajectory frame ID cannot be empty."); + return false; + } + if (workspace_frame_id != trajectory.joint_trajectory.header.frame_id) + { + RCLCPP_ERROR(logger_, + "Skipping plan insert: " + "Plan request frame (%s) does not match plan frame (%s).", + workspace_frame_id.c_str(), trajectory.joint_trajectory.header.frame_id.c_str()); + return false; + } + + auto coll = db_->openCollection("move_group_trajectory_cache", cache_namespace); + + // Pull out trajectories "exactly" keyed by request in cache. + Query::Ptr exact_query = coll.createQuery(); + + bool start_query_ok = this->extractAndAppendTrajectoryStartToQuery(*exact_query, move_group, plan_request, 0); + bool goal_query_ok = this->extractAndAppendTrajectoryGoalToQuery(*exact_query, move_group, plan_request, 0); + + if (!start_query_ok || !goal_query_ok) + { + RCLCPP_ERROR(logger_, "Skipping plan insert: Could not construct lookup query."); + return false; + } + + auto exact_matches = + coll.queryList(exact_query, /*metadata_only=*/true, /*sort_by=*/"execution_time_s", /*ascending=*/true); + + double best_execution_time = std::numeric_limits::infinity(); + if (!exact_matches.empty()) + { + best_execution_time = exact_matches.at(0)->lookupDouble("execution_time_s"); + + if (prune_worse_trajectories) + { + size_t preserved_count = 0; + for (auto& match : exact_matches) + { + double match_execution_time_s = match->lookupDouble("execution_time_s"); + if (++preserved_count > options_.num_additional_trajectories_to_preserve_when_deleting_worse && + execution_time_s < match_execution_time_s) + { + int delete_id = match->lookupInt("id"); + RCLCPP_DEBUG(logger_, + "Overwriting plan (id: %d): " + "execution_time (%es) > new trajectory's execution_time (%es)", + delete_id, match_execution_time_s, execution_time_s); + + Query::Ptr delete_query = coll.createQuery(); + delete_query->append("id", delete_id); + coll.removeMessages(delete_query); + } + } + } + } + + // Insert if candidate is best seen. + if (execution_time_s < best_execution_time) + { + Metadata::Ptr insert_metadata = coll.createMetadata(); + + bool start_meta_ok = this->extractAndAppendTrajectoryStartToMetadata(*insert_metadata, move_group, plan_request); + bool goal_meta_ok = this->extractAndAppendTrajectoryGoalToMetadata(*insert_metadata, move_group, plan_request); + insert_metadata->append("execution_time_s", execution_time_s); + insert_metadata->append("planning_time_s", planning_time_s); + + if (!start_meta_ok || !goal_meta_ok) + { + RCLCPP_ERROR(logger_, "Skipping plan insert: Could not construct insert metadata."); + return false; + } + + RCLCPP_DEBUG(logger_, + "Inserting trajectory: New trajectory execution_time (%es) " + "is better than best trajectory's execution_time (%es)", + execution_time_s, best_execution_time); + + coll.insert(trajectory, insert_metadata); + return true; + } + + RCLCPP_DEBUG(logger_, + "Skipping plan insert: New trajectory execution_time (%es) " + "is worse than best trajectory's execution_time (%es)", + execution_time_s, best_execution_time); + return false; +} + +// ============================================================================= +// CARTESIAN TRAJECTORY CACHING +// ============================================================================= + +moveit_msgs::srv::GetCartesianPath::Request +TrajectoryCache::constructGetCartesianPathRequest(moveit::planning_interface::MoveGroupInterface& move_group, + const std::vector& waypoints, + double max_step, double jump_threshold, bool avoid_collisions) +{ + moveit_msgs::srv::GetCartesianPath::Request out; + + move_group.constructRobotState(out.start_state); + + out.group_name = move_group.getName(); + out.max_velocity_scaling_factor = move_group.getMaxVelocityScalingFactor(); + out.max_acceleration_scaling_factor = move_group.getMaxVelocityScalingFactor(); + + out.header.frame_id = move_group.getPoseReferenceFrame(); + out.waypoints = waypoints; + out.max_step = max_step; + out.jump_threshold = jump_threshold; + out.path_constraints = moveit_msgs::msg::Constraints(); + out.avoid_collisions = avoid_collisions; + out.link_name = move_group.getEndEffectorLink(); + out.header.stamp = move_group.getNode()->now(); + + return out; +} + +std::vector::ConstPtr> +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, + const std::string& sort_by, bool ascending) const +{ + auto coll = + db_->openCollection("move_group_cartesian_trajectory_cache", cache_namespace); + + Query::Ptr query = coll.createQuery(); + + bool start_ok = + this->extractAndAppendCartesianTrajectoryStartToQuery(*query, move_group, plan_request, start_tolerance); + bool goal_ok = this->extractAndAppendCartesianTrajectoryGoalToQuery(*query, move_group, plan_request, goal_tolerance); + + if (!start_ok || !goal_ok) + { + RCLCPP_ERROR(logger_, "Could not construct cartesian trajectory query."); + return {}; + } + + query->appendGTE("fraction", min_fraction); + return coll.queryList(query, metadata_only, sort_by, ascending); +} + +MessageWithMetadata::ConstPtr 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, const std::string& sort_by, bool ascending) const +{ + // First find all matching, but metadata only. + // Then use the ID metadata of the best plan to pull the actual message. + auto matching_trajectories = + this->fetchAllMatchingCartesianTrajectories(move_group, cache_namespace, plan_request, min_fraction, + start_tolerance, goal_tolerance, true, sort_by, ascending); + + if (matching_trajectories.empty()) + { + RCLCPP_DEBUG(logger_, "No matching cartesian trajectories found."); + return nullptr; + } + + auto coll = + db_->openCollection("move_group_cartesian_trajectory_cache", cache_namespace); + + // Best plan is at first index, since the lookup query was sorted by + // execution_time. + int best_trajectory_id = matching_trajectories.at(0)->lookupInt("id"); + Query::Ptr best_query = coll.createQuery(); + best_query->append("id", best_trajectory_id); + + return coll.findOne(best_query, metadata_only); +} + +bool 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) +{ + std::string path_request_frame_id = getCartesianPathRequestFrameId(move_group, plan_request); + + // Check pre-conditions + if (!trajectory.multi_dof_joint_trajectory.points.empty()) + { + RCLCPP_ERROR(logger_, "Skipping cartesian trajectory insert: " + "Multi-DOF trajectory plans are not supported."); + return false; + } + if (path_request_frame_id.empty()) + { + RCLCPP_ERROR(logger_, "Skipping cartesian trajectory insert: Path request frame ID cannot be empty."); + return false; + } + if (trajectory.joint_trajectory.header.frame_id.empty()) + { + RCLCPP_ERROR(logger_, "Skipping cartesian trajectory insert: Trajectory frame ID cannot be empty."); + return false; + } + + auto coll = + db_->openCollection("move_group_cartesian_trajectory_cache", cache_namespace); + + // Pull out trajectories "exactly" keyed by request in cache. + Query::Ptr exact_query = coll.createQuery(); + + bool start_query_ok = + this->extractAndAppendCartesianTrajectoryStartToQuery(*exact_query, move_group, plan_request, 0); + bool goal_query_ok = this->extractAndAppendCartesianTrajectoryGoalToQuery(*exact_query, move_group, plan_request, 0); + exact_query->append("fraction", fraction); + + if (!start_query_ok || !goal_query_ok) + { + RCLCPP_ERROR(logger_, "Skipping cartesian trajectory insert: Could not construct lookup query."); + return false; + } + + auto exact_matches = + coll.queryList(exact_query, /*metadata_only=*/true, /*sort_by=*/"execution_time_s", /*ascending=*/true); + double best_execution_time = std::numeric_limits::infinity(); + if (!exact_matches.empty()) + { + best_execution_time = exact_matches.at(0)->lookupDouble("execution_time_s"); + + if (prune_worse_trajectories) + { + size_t preserved_count = 0; + for (auto& match : exact_matches) + { + double match_execution_time_s = match->lookupDouble("execution_time_s"); + if (++preserved_count > options_.num_additional_trajectories_to_preserve_when_deleting_worse && + execution_time_s < match_execution_time_s) + { + int delete_id = match->lookupInt("id"); + RCLCPP_DEBUG(logger_, + "Overwriting cartesian trajectory (id: %d): " + "execution_time (%es) > new trajectory's execution_time (%es)", + delete_id, match_execution_time_s, execution_time_s); + + Query::Ptr delete_query = coll.createQuery(); + delete_query->append("id", delete_id); + coll.removeMessages(delete_query); + } + } + } + } + + // Insert if candidate is best seen. + if (execution_time_s < best_execution_time) + { + Metadata::Ptr insert_metadata = coll.createMetadata(); + + bool start_meta_ok = + this->extractAndAppendCartesianTrajectoryStartToMetadata(*insert_metadata, move_group, plan_request); + bool goal_meta_ok = + this->extractAndAppendCartesianTrajectoryGoalToMetadata(*insert_metadata, move_group, plan_request); + insert_metadata->append("execution_time_s", execution_time_s); + insert_metadata->append("planning_time_s", planning_time_s); + insert_metadata->append("fraction", fraction); + + if (!start_meta_ok || !goal_meta_ok) + { + RCLCPP_ERROR(logger_, "Skipping cartesian trajectory insert: " + "Could not construct insert metadata."); + return false; + } + + RCLCPP_DEBUG(logger_, + "Inserting cartesian trajectory: New trajectory execution_time (%es) " + "is better than best trajectory's execution_time (%es) at fraction (%es)", + execution_time_s, best_execution_time, fraction); + + coll.insert(trajectory, insert_metadata); + return true; + } + + RCLCPP_DEBUG(logger_, + "Skipping cartesian trajectory insert: New trajectory execution_time (%es) " + "is worse than best trajectory's execution_time (%es) at fraction (%es)", + execution_time_s, best_execution_time, fraction); + return false; +} + +// ============================================================================= +// MOTION PLAN TRAJECTORY QUERY AND METADATA CONSTRUCTION +// ============================================================================= + +// MOTION PLAN TRAJECTORY CACHING: QUERY CONSTRUCTION ========================== + +bool TrajectoryCache::extractAndAppendTrajectoryStartToQuery( + Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request, double match_tolerance) const +{ + std::string workspace_frame_id = getWorkspaceFrameId(move_group, plan_request.workspace_parameters); + match_tolerance += options_.exact_match_precision; + + // Make ignored members explicit + if (!plan_request.start_state.multi_dof_joint_state.joint_names.empty()) + { + RCLCPP_WARN(logger_, "Ignoring start_state.multi_dof_joint_states: Not supported."); + } + if (!plan_request.start_state.attached_collision_objects.empty()) + { + RCLCPP_WARN(logger_, "Ignoring start_state.attached_collision_objects: Not supported."); + } + + query.append("group_name", plan_request.group_name); + + // Workspace params + // Match anything within our specified workspace limits. + query.append("workspace_parameters.header.frame_id", workspace_frame_id); + query.appendGTE("workspace_parameters.min_corner.x", plan_request.workspace_parameters.min_corner.x); + query.appendGTE("workspace_parameters.min_corner.y", plan_request.workspace_parameters.min_corner.y); + query.appendGTE("workspace_parameters.min_corner.z", plan_request.workspace_parameters.min_corner.z); + query.appendLTE("workspace_parameters.max_corner.x", plan_request.workspace_parameters.max_corner.x); + query.appendLTE("workspace_parameters.max_corner.y", plan_request.workspace_parameters.max_corner.y); + query.appendLTE("workspace_parameters.max_corner.z", plan_request.workspace_parameters.max_corner.z); + + // Joint state + // Only accounts for joint_state position. Ignores velocity and effort. + if (plan_request.start_state.is_diff) + { + // If plan request states that the start_state is_diff, then we need to get + // the current state from the move_group. + + // NOTE: methyldragon - + // I think if is_diff is on, the joint states will not be populated in all + // of our motion plan requests? If this isn't the case we might need to + // apply the joint states as offsets as well. + // + // TODO: Since MoveIt also potentially does another getCurrentState() call + // when planning, there is a chance that the current state in the cache + // differs from the state used in MoveIt's plan. + // + // When upstreaming this class to MoveIt, this issue should go away once + // the class is used within the move group's Plan call. + moveit::core::RobotStatePtr current_state = move_group.getCurrentState(); + if (!current_state) + { + RCLCPP_WARN(logger_, "Skipping start query append: Could not get robot state."); + // NOTE: methyldragon - + // Ideally we would restore the original state here and undo our changes, however copy of the query is not + // supported. + return false; + } + + moveit_msgs::msg::RobotState current_state_msg; + robotStateToRobotStateMsg(*current_state, current_state_msg); + + for (size_t i = 0; i < current_state_msg.joint_state.name.size(); i++) + { + query.append("start_state.joint_state.name_" + std::to_string(i), current_state_msg.joint_state.name.at(i)); + queryAppendRangeInclusiveWithTolerance(query, "start_state.joint_state.position_" + std::to_string(i), + current_state_msg.joint_state.position.at(i), match_tolerance); + } + } + else + { + for (size_t i = 0; i < plan_request.start_state.joint_state.name.size(); i++) + { + query.append("start_state.joint_state.name_" + std::to_string(i), plan_request.start_state.joint_state.name.at(i)); + queryAppendRangeInclusiveWithTolerance(query, "start_state.joint_state.position_" + std::to_string(i), + plan_request.start_state.joint_state.position.at(i), match_tolerance); + } + } + return true; +} + +bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery( + Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request, double match_tolerance) const +{ + std::string workspace_frame_id = getWorkspaceFrameId(move_group, plan_request.workspace_parameters); + match_tolerance += options_.exact_match_precision; + + // Make ignored members explicit + bool emit_position_constraint_warning = false; + for (auto& constraint : plan_request.goal_constraints) + { + for (auto& position_constraint : constraint.position_constraints) + { + if (!position_constraint.constraint_region.primitives.empty()) + { + emit_position_constraint_warning = true; + break; + } + } + if (emit_position_constraint_warning) + { + break; + } + } + if (emit_position_constraint_warning) + { + RCLCPP_WARN(logger_, "Ignoring goal_constraints.position_constraints.constraint_region: " + "Not supported."); + } + + queryAppendRangeInclusiveWithTolerance(query, "max_velocity_scaling_factor", plan_request.max_velocity_scaling_factor, + match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, "max_acceleration_scaling_factor", + plan_request.max_acceleration_scaling_factor, match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, "max_cartesian_speed", plan_request.max_cartesian_speed, + match_tolerance); + + // Extract constraints (so we don't have cardinality on goal_constraint idx.) + std::vector joint_constraints; + std::vector position_constraints; + std::vector orientation_constraints; + for (auto& constraint : plan_request.goal_constraints) + { + for (auto& joint_constraint : constraint.joint_constraints) + { + joint_constraints.push_back(joint_constraint); + } + for (auto& position_constraint : constraint.position_constraints) + { + position_constraints.push_back(position_constraint); + } + for (auto& orientation_constraint : constraint.orientation_constraints) + { + orientation_constraints.push_back(orientation_constraint); + } + + // Also sort for even less cardinality. + sortConstraints(joint_constraints, position_constraints, orientation_constraints); + } + + // Joint constraints + size_t joint_idx = 0; + for (auto& constraint : joint_constraints) + { + std::string meta_name = "goal_constraints.joint_constraints_" + std::to_string(joint_idx++); + + query.append(meta_name + ".joint_name", constraint.joint_name); + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".position", constraint.position, match_tolerance); + query.appendGTE(meta_name + ".tolerance_above", constraint.tolerance_above); + query.appendLTE(meta_name + ".tolerance_below", constraint.tolerance_below); + } + + // Position constraints + // All offsets will be "frozen" and computed wrt. the workspace frame + // instead. + if (!position_constraints.empty()) + { + query.append("goal_constraints.position_constraints.header.frame_id", workspace_frame_id); + + size_t pos_idx = 0; + for (auto& constraint : position_constraints) + { + std::string meta_name = "goal_constraints.position_constraints_" + std::to_string(pos_idx++); + + // Compute offsets wrt. to workspace frame. + double x_offset = 0; + double y_offset = 0; + double z_offset = 0; + + if (workspace_frame_id != constraint.header.frame_id) + { + try + { + auto transform = + tf_buffer_->lookupTransform(constraint.header.frame_id, workspace_frame_id, tf2::TimePointZero); + + x_offset = transform.transform.translation.x; + y_offset = transform.transform.translation.y; + z_offset = transform.transform.translation.z; + } + catch (tf2::TransformException& ex) + { + RCLCPP_WARN(logger_, + "Skipping goal query append: " + "Could not get goal transform for translation %s to %s: %s", + workspace_frame_id.c_str(), constraint.header.frame_id.c_str(), ex.what()); + + // NOTE: methyldragon - + // Ideally we would restore the original state here and undo our changes, however copy of the query is not + // supported. + return false; + } + } + + query.append(meta_name + ".link_name", constraint.link_name); + + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.x", + x_offset + constraint.target_point_offset.x, match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.y", + y_offset + constraint.target_point_offset.y, match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.z", + z_offset + constraint.target_point_offset.z, match_tolerance); + } + } + + // Orientation constraints + // All offsets will be "frozen" and computed wrt. the workspace frame + // instead. + if (!orientation_constraints.empty()) + { + query.append("goal_constraints.orientation_constraints.header.frame_id", workspace_frame_id); + + size_t ori_idx = 0; + for (auto& constraint : orientation_constraints) + { + std::string meta_name = "goal_constraints.orientation_constraints_" + std::to_string(ori_idx++); + + // Compute offsets wrt. to workspace frame. + geometry_msgs::msg::Quaternion quat_offset; + quat_offset.x = 0; + quat_offset.y = 0; + quat_offset.z = 0; + quat_offset.w = 1; + + if (workspace_frame_id != constraint.header.frame_id) + { + try + { + auto transform = + tf_buffer_->lookupTransform(constraint.header.frame_id, workspace_frame_id, tf2::TimePointZero); + + quat_offset = transform.transform.rotation; + } + catch (tf2::TransformException& ex) + { + RCLCPP_WARN(logger_, + "Skipping goal query append: " + "Could not get goal transform for orientation %s to %s: %s", + workspace_frame_id.c_str(), constraint.header.frame_id.c_str(), ex.what()); + + // NOTE: methyldragon - + // Ideally we would restore the original state here and undo our changes, however copy of the query is not + // supported. + return false; + } + } + + query.append(meta_name + ".link_name", constraint.link_name); + + // Orientation of constraint frame wrt. workspace frame + tf2::Quaternion tf2_quat_frame_offset(quat_offset.x, quat_offset.y, quat_offset.z, quat_offset.w); + + // Added offset on top of the constraint frame's orientation stated in + // the constraint. + tf2::Quaternion tf2_quat_goal_offset(constraint.orientation.x, constraint.orientation.y, constraint.orientation.z, + constraint.orientation.w); + + tf2_quat_frame_offset.normalize(); + tf2_quat_goal_offset.normalize(); + + auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset; + final_quat.normalize(); + + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.x", final_quat.getX(), + match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.y", final_quat.getY(), + match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.z", final_quat.getZ(), + match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.w", final_quat.getW(), + match_tolerance); + } + } + + return true; +} + +// MOTION PLAN TRAJECTORY CACHING: METADATA CONSTRUCTION ======================= + +bool TrajectoryCache::extractAndAppendTrajectoryStartToMetadata( + Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request) const +{ + std::string workspace_frame_id = getWorkspaceFrameId(move_group, plan_request.workspace_parameters); + + // Make ignored members explicit + if (!plan_request.start_state.multi_dof_joint_state.joint_names.empty()) + { + RCLCPP_WARN(logger_, "Ignoring start_state.multi_dof_joint_states: Not supported."); + } + if (!plan_request.start_state.attached_collision_objects.empty()) + { + RCLCPP_WARN(logger_, "Ignoring start_state.attached_collision_objects: Not supported."); + } + + metadata.append("group_name", plan_request.group_name); + + // Workspace params + metadata.append("workspace_parameters.header.frame_id", workspace_frame_id); + metadata.append("workspace_parameters.min_corner.x", plan_request.workspace_parameters.min_corner.x); + metadata.append("workspace_parameters.min_corner.y", plan_request.workspace_parameters.min_corner.y); + metadata.append("workspace_parameters.min_corner.z", plan_request.workspace_parameters.min_corner.z); + metadata.append("workspace_parameters.max_corner.x", plan_request.workspace_parameters.max_corner.x); + metadata.append("workspace_parameters.max_corner.y", plan_request.workspace_parameters.max_corner.y); + metadata.append("workspace_parameters.max_corner.z", plan_request.workspace_parameters.max_corner.z); + + // Joint state + // Only accounts for joint_state position. Ignores velocity and effort. + if (plan_request.start_state.is_diff) + { + // If plan request states that the start_state is_diff, then we need to get + // the current state from the move_group. + + // NOTE: methyldragon - + // I think if is_diff is on, the joint states will not be populated in all + // of our motion plan requests? If this isn't the case we might need to + // apply the joint states as offsets as well. + // + // TODO: Since MoveIt also potentially does another getCurrentState() call + // when planning, there is a chance that the current state in the cache + // differs from the state used in MoveIt's plan. + // + // When upstreaming this class to MoveIt, this issue should go away once + // the class is used within the move group's Plan call. + moveit::core::RobotStatePtr current_state = move_group.getCurrentState(); + if (!current_state) + { + RCLCPP_WARN(logger_, "Skipping start metadata append: Could not get robot state."); + // NOTE: methyldragon - + // Ideally we would restore the original state here and undo our changes, however copy of the query is not + // supported. + return false; + } + + moveit_msgs::msg::RobotState current_state_msg; + robotStateToRobotStateMsg(*current_state, current_state_msg); + + for (size_t i = 0; i < current_state_msg.joint_state.name.size(); i++) + { + metadata.append("start_state.joint_state.name_" + std::to_string(i), current_state_msg.joint_state.name.at(i)); + metadata.append("start_state.joint_state.position_" + std::to_string(i), + current_state_msg.joint_state.position.at(i)); + } + } + else + { + for (size_t i = 0; i < plan_request.start_state.joint_state.name.size(); i++) + { + metadata.append("start_state.joint_state.name_" + std::to_string(i), + plan_request.start_state.joint_state.name.at(i)); + metadata.append("start_state.joint_state.position_" + std::to_string(i), + plan_request.start_state.joint_state.position.at(i)); + } + } + + return true; +} + +bool TrajectoryCache::extractAndAppendTrajectoryGoalToMetadata( + Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request) const +{ + std::string workspace_frame_id = getWorkspaceFrameId(move_group, plan_request.workspace_parameters); + + // Make ignored members explicit + bool emit_position_constraint_warning = false; + for (auto& constraint : plan_request.goal_constraints) + { + for (auto& position_constraint : constraint.position_constraints) + { + if (!position_constraint.constraint_region.primitives.empty()) + { + emit_position_constraint_warning = true; + break; + } + } + if (emit_position_constraint_warning) + { + break; + } + } + if (emit_position_constraint_warning) + { + RCLCPP_WARN(logger_, "Ignoring goal_constraints.position_constraints.constraint_region: " + "Not supported."); + } + + metadata.append("max_velocity_scaling_factor", plan_request.max_velocity_scaling_factor); + metadata.append("max_acceleration_scaling_factor", plan_request.max_acceleration_scaling_factor); + metadata.append("max_cartesian_speed", plan_request.max_cartesian_speed); + + // Extract constraints (so we don't have cardinality on goal_constraint idx.) + std::vector joint_constraints; + std::vector position_constraints; + std::vector orientation_constraints; + for (auto& constraint : plan_request.goal_constraints) + { + for (auto& joint_constraint : constraint.joint_constraints) + { + joint_constraints.push_back(joint_constraint); + } + for (auto& position_constraint : constraint.position_constraints) + { + position_constraints.push_back(position_constraint); + } + for (auto& orientation_constraint : constraint.orientation_constraints) + { + orientation_constraints.push_back(orientation_constraint); + } + + // Also sort for even less cardinality. + sortConstraints(joint_constraints, position_constraints, orientation_constraints); + } + + // Joint constraints + size_t joint_idx = 0; + for (auto& constraint : joint_constraints) + { + std::string meta_name = "goal_constraints.joint_constraints_" + std::to_string(joint_idx++); + + metadata.append(meta_name + ".joint_name", constraint.joint_name); + metadata.append(meta_name + ".position", constraint.position); + metadata.append(meta_name + ".tolerance_above", constraint.tolerance_above); + metadata.append(meta_name + ".tolerance_below", constraint.tolerance_below); + } + + // Position constraints + if (!position_constraints.empty()) + { + // All offsets will be "frozen" and computed wrt. the workspace frame + // instead. + metadata.append("goal_constraints.position_constraints.header.frame_id", workspace_frame_id); + + size_t position_idx = 0; + for (auto& constraint : position_constraints) + { + std::string meta_name = "goal_constraints.position_constraints_" + std::to_string(position_idx++); + + // Compute offsets wrt. to workspace frame. + double x_offset = 0; + double y_offset = 0; + double z_offset = 0; + + if (workspace_frame_id != constraint.header.frame_id) + { + try + { + auto transform = + tf_buffer_->lookupTransform(constraint.header.frame_id, workspace_frame_id, tf2::TimePointZero); + + x_offset = transform.transform.translation.x; + y_offset = transform.transform.translation.y; + z_offset = transform.transform.translation.z; + } + catch (tf2::TransformException& ex) + { + RCLCPP_WARN(logger_, + "Skipping goal metadata append: " + "Could not get goal transform for translation %s to %s: %s", + workspace_frame_id.c_str(), constraint.header.frame_id.c_str(), ex.what()); + + // NOTE: methyldragon - + // Ideally we would restore the original state here and undo our changes, however copy of the query is not + // supported. + return false; + } + } + + metadata.append(meta_name + ".link_name", constraint.link_name); + + metadata.append(meta_name + ".target_point_offset.x", x_offset + constraint.target_point_offset.x); + metadata.append(meta_name + ".target_point_offset.y", y_offset + constraint.target_point_offset.y); + metadata.append(meta_name + ".target_point_offset.z", z_offset + constraint.target_point_offset.z); + } + } + + // Orientation constraints + if (!orientation_constraints.empty()) + { + // All offsets will be "frozen" and computed wrt. the workspace frame + // instead. + metadata.append("goal_constraints.orientation_constraints.header.frame_id", workspace_frame_id); + + size_t ori_idx = 0; + for (auto& constraint : orientation_constraints) + { + std::string meta_name = "goal_constraints.orientation_constraints_" + std::to_string(ori_idx++); + + // Compute offsets wrt. to workspace frame. + geometry_msgs::msg::Quaternion quat_offset; + quat_offset.x = 0; + quat_offset.y = 0; + quat_offset.z = 0; + quat_offset.w = 1; + + if (workspace_frame_id != constraint.header.frame_id) + { + try + { + auto transform = + tf_buffer_->lookupTransform(constraint.header.frame_id, workspace_frame_id, tf2::TimePointZero); + + quat_offset = transform.transform.rotation; + } + catch (tf2::TransformException& ex) + { + RCLCPP_WARN(logger_, + "Skipping goal metadata append: " + "Could not get goal transform for orientation %s to %s: %s", + workspace_frame_id.c_str(), constraint.header.frame_id.c_str(), ex.what()); + + // NOTE: methyldragon - + // Ideally we would restore the original state here and undo our changes, however copy of the query is not + // supported. + return false; + } + } + + metadata.append(meta_name + ".link_name", constraint.link_name); + + // Orientation of constraint frame wrt. workspace frame + tf2::Quaternion tf2_quat_frame_offset(quat_offset.x, quat_offset.y, quat_offset.z, quat_offset.w); + + // Added offset on top of the constraint frame's orientation stated in + // the constraint. + tf2::Quaternion tf2_quat_goal_offset(constraint.orientation.x, constraint.orientation.y, constraint.orientation.z, + constraint.orientation.w); + + tf2_quat_frame_offset.normalize(); + tf2_quat_goal_offset.normalize(); + + auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset; + final_quat.normalize(); + + metadata.append(meta_name + ".target_point_offset.x", final_quat.getX()); + metadata.append(meta_name + ".target_point_offset.y", final_quat.getY()); + metadata.append(meta_name + ".target_point_offset.z", final_quat.getZ()); + metadata.append(meta_name + ".target_point_offset.w", final_quat.getW()); + } + } + + return true; +} + +// ============================================================================= +// CARTESIAN TRAJECTORY QUERY AND METADATA CONSTRUCTION +// ============================================================================= + +// CARTESIAN TRAJECTORY CACHING: QUERY CONSTRUCTION ============================ + +bool TrajectoryCache::extractAndAppendCartesianTrajectoryStartToQuery( + Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double match_tolerance) const +{ + std::string path_request_frame_id = getCartesianPathRequestFrameId(move_group, plan_request); + match_tolerance += options_.exact_match_precision; + + // Make ignored members explicit + if (!plan_request.start_state.multi_dof_joint_state.joint_names.empty()) + { + RCLCPP_WARN(logger_, "Ignoring start_state.multi_dof_joint_states: Not supported."); + } + if (!plan_request.start_state.attached_collision_objects.empty()) + { + RCLCPP_WARN(logger_, "Ignoring start_state.attached_collision_objects: Not supported."); + } + + query.append("group_name", plan_request.group_name); + query.append("path_request.header.frame_id", path_request_frame_id); + + // Joint state + // Only accounts for joint_state position. Ignores velocity and effort. + if (plan_request.start_state.is_diff) + { + // If plan request states that the start_state is_diff, then we need to get + // the current state from the move_group. + + // NOTE: methyldragon - + // I think if is_diff is on, the joint states will not be populated in all + // of our motion plan requests? If this isn't the case we might need to + // apply the joint states as offsets as well. + // + // TODO: Since MoveIt also potentially does another getCurrentState() call + // when planning, there is a chance that the current state in the cache + // differs from the state used in MoveIt's plan. + // + // When upstreaming this class to MoveIt, this issue should go away once + // the class is used within the move group's Plan call. + moveit::core::RobotStatePtr current_state = move_group.getCurrentState(); + if (!current_state) + { + RCLCPP_WARN(logger_, "Skipping start metadata append: Could not get robot state."); + // NOTE: methyldragon - + // Ideally we would restore the original state here and undo our changes, however copy of the query is not + // supported. + return false; + } + + moveit_msgs::msg::RobotState current_state_msg; + robotStateToRobotStateMsg(*current_state, current_state_msg); + + for (size_t i = 0; i < current_state_msg.joint_state.name.size(); i++) + { + query.append("start_state.joint_state.name_" + std::to_string(i), current_state_msg.joint_state.name.at(i)); + queryAppendRangeInclusiveWithTolerance(query, "start_state.joint_state.position_" + std::to_string(i), + current_state_msg.joint_state.position.at(i), match_tolerance); + } + } + else + { + for (size_t i = 0; i < plan_request.start_state.joint_state.name.size(); i++) + { + query.append("start_state.joint_state.name_" + std::to_string(i), plan_request.start_state.joint_state.name.at(i)); + queryAppendRangeInclusiveWithTolerance(query, "start_state.joint_state.position_" + std::to_string(i), + plan_request.start_state.joint_state.position.at(i), match_tolerance); + } + } + + return true; +} + +bool TrajectoryCache::extractAndAppendCartesianTrajectoryGoalToQuery( + Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double match_tolerance) const +{ + std::string path_request_frame_id = getCartesianPathRequestFrameId(move_group, plan_request); + match_tolerance += options_.exact_match_precision; + + // Make ignored members explicit + if (!plan_request.path_constraints.joint_constraints.empty() || + !plan_request.path_constraints.position_constraints.empty() || + !plan_request.path_constraints.orientation_constraints.empty() || + !plan_request.path_constraints.visibility_constraints.empty()) + { + RCLCPP_WARN(logger_, "Ignoring path_constraints: Not supported."); + } + if (plan_request.avoid_collisions) + { + RCLCPP_WARN(logger_, "Ignoring avoid_collisions: Not supported."); + } + + queryAppendRangeInclusiveWithTolerance(query, "max_velocity_scaling_factor", plan_request.max_velocity_scaling_factor, + match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, "max_acceleration_scaling_factor", + plan_request.max_acceleration_scaling_factor, match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, "max_step", plan_request.max_step, match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, "jump_threshold", plan_request.jump_threshold, match_tolerance); + + // Waypoints + // Restating them in terms of the robot model frame (usually base_link) + std::string base_frame = move_group.getRobotModel()->getModelFrame(); + + // Compute offsets. + double x_offset = 0; + double y_offset = 0; + double z_offset = 0; + + geometry_msgs::msg::Quaternion quat_offset; + quat_offset.x = 0; + quat_offset.y = 0; + quat_offset.z = 0; + quat_offset.w = 1; + + if (base_frame != path_request_frame_id) + { + try + { + auto transform = tf_buffer_->lookupTransform(path_request_frame_id, base_frame, tf2::TimePointZero); + + x_offset = transform.transform.translation.x; + y_offset = transform.transform.translation.y; + z_offset = transform.transform.translation.z; + quat_offset = transform.transform.rotation; + } + catch (tf2::TransformException& ex) + { + RCLCPP_WARN(logger_, + "Skipping goal metadata append: " + "Could not get goal transform for %s to %s: %s", + base_frame.c_str(), path_request_frame_id.c_str(), ex.what()); + + // NOTE: methyldragon - + // Ideally we would restore the original state here and undo our changes, however copy of the query is not + // supported. + return false; + } + } + + tf2::Quaternion tf2_quat_frame_offset(quat_offset.x, quat_offset.y, quat_offset.z, quat_offset.w); + tf2_quat_frame_offset.normalize(); + + size_t waypoint_idx = 0; + for (auto& waypoint : plan_request.waypoints) + { + std::string meta_name = "waypoints_" + std::to_string(waypoint_idx++); + + // Apply offsets + // Position + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".position.x", x_offset + waypoint.position.x, + match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".position.y", y_offset + waypoint.position.y, + match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".position.z", z_offset + waypoint.position.z, + match_tolerance); + + // Orientation + tf2::Quaternion tf2_quat_goal_offset(waypoint.orientation.x, waypoint.orientation.y, waypoint.orientation.z, + waypoint.orientation.w); + tf2_quat_goal_offset.normalize(); + + auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset; + final_quat.normalize(); + + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".orientation.x", final_quat.getX(), match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".orientation.y", final_quat.getY(), match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".orientation.z", final_quat.getZ(), match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".orientation.w", final_quat.getW(), match_tolerance); + } + + query.append("link_name", plan_request.link_name); + query.append("header.frame_id", base_frame); + + return true; +} + +// CARTESIAN TRAJECTORY CACHING: METADATA CONSTRUCTION ========================= + +bool TrajectoryCache::extractAndAppendCartesianTrajectoryStartToMetadata( + Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request) const +{ + std::string path_request_frame_id = getCartesianPathRequestFrameId(move_group, plan_request); + + // Make ignored members explicit + if (!plan_request.start_state.multi_dof_joint_state.joint_names.empty()) + { + RCLCPP_WARN(logger_, "Ignoring start_state.multi_dof_joint_states: Not supported."); + } + if (!plan_request.start_state.attached_collision_objects.empty()) + { + RCLCPP_WARN(logger_, "Ignoring start_state.attached_collision_objects: Not supported."); + } + + metadata.append("group_name", plan_request.group_name); + metadata.append("path_request.header.frame_id", path_request_frame_id); + + // Joint state + // Only accounts for joint_state position. Ignores velocity and effort. + if (plan_request.start_state.is_diff) + { + // If plan request states that the start_state is_diff, then we need to get + // the current state from the move_group. + + // NOTE: methyldragon - + // I think if is_diff is on, the joint states will not be populated in all + // of our motion plan requests? If this isn't the case we might need to + // apply the joint states as offsets as well. + // + // TODO: Since MoveIt also potentially does another getCurrentState() call + // when planning, there is a chance that the current state in the cache + // differs from the state used in MoveIt's plan. + // + // When upstreaming this class to MoveIt, this issue should go away once + // the class is used within the move group's Plan call. + moveit::core::RobotStatePtr current_state = move_group.getCurrentState(); + if (!current_state) + { + RCLCPP_WARN(logger_, "Skipping start metadata append: Could not get robot state."); + // NOTE: methyldragon - + // Ideally we would restore the original state here and undo our changes, however copy of the query is not + // supported. + return false; + } + + moveit_msgs::msg::RobotState current_state_msg; + robotStateToRobotStateMsg(*current_state, current_state_msg); + + for (size_t i = 0; i < current_state_msg.joint_state.name.size(); i++) + { + metadata.append("start_state.joint_state.name_" + std::to_string(i), current_state_msg.joint_state.name.at(i)); + metadata.append("start_state.joint_state.position_" + std::to_string(i), + current_state_msg.joint_state.position.at(i)); + } + } + else + { + for (size_t i = 0; i < plan_request.start_state.joint_state.name.size(); i++) + { + metadata.append("start_state.joint_state.name_" + std::to_string(i), + plan_request.start_state.joint_state.name.at(i)); + metadata.append("start_state.joint_state.position_" + std::to_string(i), + plan_request.start_state.joint_state.position.at(i)); + } + } + + return true; +} + +bool TrajectoryCache::extractAndAppendCartesianTrajectoryGoalToMetadata( + Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request) const +{ + std::string path_request_frame_id = getCartesianPathRequestFrameId(move_group, plan_request); + + // Make ignored members explicit + if (!plan_request.path_constraints.joint_constraints.empty() || + !plan_request.path_constraints.position_constraints.empty() || + !plan_request.path_constraints.orientation_constraints.empty() || + !plan_request.path_constraints.visibility_constraints.empty()) + { + RCLCPP_WARN(logger_, "Ignoring path_constraints: Not supported."); + } + if (plan_request.avoid_collisions) + { + RCLCPP_WARN(logger_, "Ignoring avoid_collisions: Not supported."); + } + + metadata.append("max_velocity_scaling_factor", plan_request.max_velocity_scaling_factor); + metadata.append("max_acceleration_scaling_factor", plan_request.max_acceleration_scaling_factor); + metadata.append("max_step", plan_request.max_step); + metadata.append("jump_threshold", plan_request.jump_threshold); + + // Waypoints + // Restating them in terms of the robot model frame (usually base_link) + std::string base_frame = move_group.getRobotModel()->getModelFrame(); + + // Compute offsets. + double x_offset = 0; + double y_offset = 0; + double z_offset = 0; + + geometry_msgs::msg::Quaternion quat_offset; + quat_offset.x = 0; + quat_offset.y = 0; + quat_offset.z = 0; + quat_offset.w = 1; + + if (base_frame != path_request_frame_id) + { + try + { + auto transform = tf_buffer_->lookupTransform(path_request_frame_id, base_frame, tf2::TimePointZero); + + x_offset = transform.transform.translation.x; + y_offset = transform.transform.translation.y; + z_offset = transform.transform.translation.z; + quat_offset = transform.transform.rotation; + } + catch (tf2::TransformException& ex) + { + RCLCPP_WARN(logger_, + "Skipping goal metadata append: " + "Could not get goal transform for %s to %s: %s", + base_frame.c_str(), path_request_frame_id.c_str(), ex.what()); + + // NOTE: methyldragon - + // Ideally we would restore the original state here and undo our changes, however copy of the query is not + // supported. + return false; + } + } + + tf2::Quaternion tf2_quat_frame_offset(quat_offset.x, quat_offset.y, quat_offset.z, quat_offset.w); + tf2_quat_frame_offset.normalize(); + + size_t waypoint_idx = 0; + for (auto& waypoint : plan_request.waypoints) + { + std::string meta_name = "waypoints_" + std::to_string(waypoint_idx++); + + // Apply offsets + // Position + metadata.append(meta_name + ".position.x", x_offset + waypoint.position.x); + metadata.append(meta_name + ".position.y", y_offset + waypoint.position.y); + metadata.append(meta_name + ".position.z", z_offset + waypoint.position.z); + + // Orientation + tf2::Quaternion tf2_quat_goal_offset(waypoint.orientation.x, waypoint.orientation.y, waypoint.orientation.z, + waypoint.orientation.w); + tf2_quat_goal_offset.normalize(); + + auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset; + final_quat.normalize(); + + metadata.append(meta_name + ".orientation.x", final_quat.getX()); + metadata.append(meta_name + ".orientation.y", final_quat.getY()); + metadata.append(meta_name + ".orientation.z", final_quat.getZ()); + metadata.append(meta_name + ".orientation.w", final_quat.getW()); + } + + metadata.append("link_name", plan_request.link_name); + metadata.append("header.frame_id", base_frame); + + return true; +} + +} // namespace trajectory_cache +} // namespace moveit_ros diff --git a/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp b/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp new file mode 100644 index 0000000000..dfe8ef7e47 --- /dev/null +++ b/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp @@ -0,0 +1,1091 @@ +// Copyright 2024 Intrinsic Innovation LLC. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/** @file + * @author methylDragon + */ + +#include + +#include +#include +#include + +#include +#include + +using moveit::planning_interface::MoveGroupInterface; +using moveit_ros::trajectory_cache::TrajectoryCache; + +const std::string ROBOT_NAME = "panda"; +const std::string ROBOT_FRAME = "world"; + +// UTILS ======================================================================= +// Utility function to emit a pass or fail statement. +void checkAndEmit(bool predicate, const std::string& test_case, const std::string& label) +{ + if (predicate) + { + std::cout << "[PASS] " << test_case << ": " << label << "\n"; + } + else + { + std::cout << "[FAIL] " << test_case << ": " << label << "\n"; + } +} + +moveit_msgs::msg::RobotTrajectory getDummyPandaTraj() +{ + static moveit_msgs::msg::RobotTrajectory out = []() { + moveit_msgs::msg::RobotTrajectory traj; + + auto trajectory = &traj.joint_trajectory; + trajectory->header.frame_id = ROBOT_FRAME; + + trajectory->joint_names.push_back(ROBOT_NAME + "_joint1"); + trajectory->joint_names.push_back(ROBOT_NAME + "_joint2"); + trajectory->joint_names.push_back(ROBOT_NAME + "_joint3"); + trajectory->joint_names.push_back(ROBOT_NAME + "_joint4"); + trajectory->joint_names.push_back(ROBOT_NAME + "_joint5"); + trajectory->joint_names.push_back(ROBOT_NAME + "_joint6"); + trajectory->joint_names.push_back(ROBOT_NAME + "_joint7"); + + trajectory->points.emplace_back(); + trajectory->points.at(0).positions = { 0, 0, 0, 0, 0, 0 }; + trajectory->points.at(0).velocities = { 0, 0, 0, 0, 0, 0 }; + trajectory->points.at(0).accelerations = { 0, 0, 0, 0, 0, 0 }; + trajectory->points.at(0).time_from_start.sec = 999999; + + return traj; + }(); + return out; +} + +std::vector getDummyWaypoints() +{ + static std::vector out = []() { + std::vector waypoints; + for (size_t i = 0; i < 3; i++) + { + waypoints.emplace_back(); + waypoints.at(i).position.x = i; + waypoints.at(i).position.y = i; + waypoints.at(i).position.z = i; + waypoints.at(i).orientation.w = i; + waypoints.at(i).orientation.x = i + 0.1; + waypoints.at(i).orientation.y = i + 0.1; + waypoints.at(i).orientation.z = i + 0.1; + } + return waypoints; + }(); + return out; +} + +void testGettersAndSetters(const std::shared_ptr& cache) +{ + std::string test_case = "getters_and_setters"; + + checkAndEmit(cache->getDbPath() == ":memory:", test_case, "getDbPath"); + checkAndEmit(cache->getDbPort() == 0, test_case, "getDbPort"); + + checkAndEmit(cache->getExactMatchPrecision() == 10, test_case, "getExactMatchPrecision"); + cache->setExactMatchPrecision(1); + checkAndEmit(cache->getExactMatchPrecision() == 1, test_case, "getExactMatchPrecisionAfterSet"); + + checkAndEmit(cache->getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse() == 10, test_case, + "getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse"); + cache->setNumAdditionalTrajectoriesToPreserveWhenDeletingWorse(1); + checkAndEmit(cache->getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse() == 1, test_case, + "getNumAdditionalTrajectoriesToPreserveWhenDeletingWorseAfterSet"); +} + +void testMotionTrajectories(const std::shared_ptr& move_group, + const std::shared_ptr& cache) +{ + // Setup ===================================================================== + // All variants are modified copies of `plan_req`. + + /// MotionPlanRequest + + // Plain start + moveit_msgs::msg::MotionPlanRequest plan_req; + move_group->constructMotionPlanRequest(plan_req); + plan_req.workspace_parameters.header.frame_id = ROBOT_FRAME; + plan_req.workspace_parameters.max_corner.x = 10; + plan_req.workspace_parameters.max_corner.y = 10; + plan_req.workspace_parameters.max_corner.z = 10; + plan_req.workspace_parameters.min_corner.x = -10; + plan_req.workspace_parameters.min_corner.y = -10; + plan_req.workspace_parameters.min_corner.z = -10; + plan_req.start_state.multi_dof_joint_state.joint_names.clear(); + plan_req.start_state.multi_dof_joint_state.transforms.clear(); + plan_req.start_state.multi_dof_joint_state.twist.clear(); + plan_req.start_state.multi_dof_joint_state.wrench.clear(); + + // Empty frame start + moveit_msgs::msg::MotionPlanRequest empty_frame_plan_req = plan_req; + empty_frame_plan_req.workspace_parameters.header.frame_id = ""; + + // is_diff = true + auto is_diff_plan_req = plan_req; + is_diff_plan_req.start_state.is_diff = true; + is_diff_plan_req.start_state.joint_state.header.frame_id = ""; + is_diff_plan_req.start_state.joint_state.name.clear(); + is_diff_plan_req.start_state.joint_state.position.clear(); + is_diff_plan_req.start_state.joint_state.velocity.clear(); + is_diff_plan_req.start_state.joint_state.effort.clear(); + + // Something close enough (mod 0.1 away) + auto close_matching_plan_req = plan_req; + close_matching_plan_req.workspace_parameters.max_corner.x += 0.05; + close_matching_plan_req.workspace_parameters.min_corner.x -= 0.05; + close_matching_plan_req.start_state.joint_state.position.at(0) -= 0.05; + close_matching_plan_req.start_state.joint_state.position.at(1) += 0.05; + close_matching_plan_req.start_state.joint_state.position.at(2) -= 0.05; + close_matching_plan_req.goal_constraints.at(0).joint_constraints.at(0).position -= 0.05; + close_matching_plan_req.goal_constraints.at(0).joint_constraints.at(1).position += 0.05; + close_matching_plan_req.goal_constraints.at(0).joint_constraints.at(2).position -= 0.05; + + // Close with swapped constraints (mod 0.1 away) + auto swapped_close_matching_plan_req = close_matching_plan_req; + std::swap(swapped_close_matching_plan_req.goal_constraints.at(0).joint_constraints.at(0), + swapped_close_matching_plan_req.goal_constraints.at(0).joint_constraints.at(1)); + + // Smaller workspace start + auto smaller_workspace_plan_req = plan_req; + smaller_workspace_plan_req.workspace_parameters.max_corner.x = 1; + smaller_workspace_plan_req.workspace_parameters.max_corner.y = 1; + smaller_workspace_plan_req.workspace_parameters.max_corner.z = 1; + smaller_workspace_plan_req.workspace_parameters.min_corner.x = -1; + smaller_workspace_plan_req.workspace_parameters.min_corner.y = -1; + smaller_workspace_plan_req.workspace_parameters.min_corner.z = -1; + + // Larger workspace start + auto larger_workspace_plan_req = plan_req; + larger_workspace_plan_req.workspace_parameters.max_corner.x = 50; + larger_workspace_plan_req.workspace_parameters.max_corner.y = 50; + larger_workspace_plan_req.workspace_parameters.max_corner.z = 50; + larger_workspace_plan_req.workspace_parameters.min_corner.x = -50; + larger_workspace_plan_req.workspace_parameters.min_corner.y = -50; + larger_workspace_plan_req.workspace_parameters.min_corner.z = -50; + + // Different + auto different_plan_req = plan_req; + different_plan_req.workspace_parameters.max_corner.x += 1.05; + different_plan_req.workspace_parameters.min_corner.x -= 2.05; + different_plan_req.start_state.joint_state.position.at(0) -= 3.05; + different_plan_req.start_state.joint_state.position.at(1) += 4.05; + different_plan_req.start_state.joint_state.position.at(2) -= 5.05; + different_plan_req.goal_constraints.at(0).joint_constraints.at(0).position -= 6.05; + different_plan_req.goal_constraints.at(0).joint_constraints.at(1).position += 7.05; + different_plan_req.goal_constraints.at(0).joint_constraints.at(2).position -= 8.05; + + /// RobotTrajectory + + // Trajectory + auto traj = getDummyPandaTraj(); + + // Trajectory with no frame_id in its trajectory header + auto empty_frame_traj = traj; + empty_frame_traj.joint_trajectory.header.frame_id = ""; + + auto different_traj = traj; + different_traj.joint_trajectory.points.at(0).positions.at(0) = 999; + different_traj.joint_trajectory.points.at(0).positions.at(1) = 999; + different_traj.joint_trajectory.points.at(0).positions.at(2) = 999; + + // Test Utils + + std::string test_case; + + // Checks ==================================================================== + + // Initially empty + test_case = "testMotionTrajectories.empty"; + + checkAndEmit(cache->countTrajectories(ROBOT_NAME) == 0, test_case, "Trajectory cache initially empty"); + + checkAndEmit(cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, plan_req, 999, 999).empty(), test_case, + "Fetch all trajectories on empty cache returns empty"); + + checkAndEmit(cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, plan_req, 999, 999) == nullptr, test_case, + "Fetch best trajectory on empty cache returns nullptr"); + + // Put trajectory empty frame + // + // Trajectory must have frame in joint trajectory, expect put fail + test_case = "testMotionTrajectories.insertTrajectory_empty_frame"; + double execution_time = 999; + double planning_time = 999; + + checkAndEmit(!cache->insertTrajectory(*move_group, ROBOT_NAME, plan_req, empty_frame_traj, execution_time, + planning_time, false), + test_case, "Put empty frame trajectory, no prune_worse_trajectories, not ok"); + + checkAndEmit(cache->countTrajectories(ROBOT_NAME) == 0, test_case, "No trajectories in cache"); + + // Put trajectory req empty frame + // + // Trajectory request having no frame in workspace should default to robot frame + test_case = "testMotionTrajectories.insertTrajectory_req_empty_frame"; + execution_time = 1000; + planning_time = 1000; + + checkAndEmit(cache->insertTrajectory(*move_group, ROBOT_NAME, empty_frame_plan_req, traj, execution_time, + planning_time, false), + test_case, "Put empty frame req trajectory, no prune_worse_trajectories, ok"); + + checkAndEmit(cache->countTrajectories(ROBOT_NAME) == 1, test_case, "One trajectory in cache"); + + // Put second, no prune_worse_trajectories + test_case = "testMotionTrajectories.put_second"; + execution_time = 999; + planning_time = 999; + + checkAndEmit(cache->insertTrajectory(*move_group, ROBOT_NAME, plan_req, traj, execution_time, planning_time, false), + test_case, "Put second valid trajectory, no prune_worse_trajectories, ok"); + + checkAndEmit(cache->countTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache"); + + // Fetch matching, no tolerance + // + // Exact key match should have cache hit + test_case = "testMotionTrajectories.put_second.fetch_matching_no_tolerance"; + + auto fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, plan_req, 0, 0); + + auto fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, plan_req, 0, 0); + + checkAndEmit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); + checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); + + checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); + + checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); + + checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, + "Fetched trajectory has correct execution time"); + + checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, + "Fetched trajectory has correct planning time"); + + // Fetch with is_diff + // + // is_diff key should be equivalent to exact match if robot state did not + // change, hence should have cache hit + test_case = "testMotionTrajectories.put_second.fetch_is_diff_no_tolerance"; + + fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, is_diff_plan_req, 0, 0); + + fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, is_diff_plan_req, 0, 0); + + checkAndEmit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); + checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); + + checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); + + checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); + + checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, + "Fetched trajectory has correct execution time"); + + checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, + "Fetched trajectory has correct planning time"); + + // Fetch non-matching, out of tolerance + // + // Non-matching key should not have cache hit + test_case = "testMotionTrajectories.put_second.fetch_non_matching_out_of_tolerance"; + + fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, close_matching_plan_req, 0, 0); + + fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, close_matching_plan_req, 0, 0); + + checkAndEmit(fetched_trajectories.empty(), test_case, "Fetch all returns empty"); + checkAndEmit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr"); + + // Fetch non-matching, only start in tolerance (but not goal) + // + // Non-matching key should not have cache hit + test_case = "testMotionTrajectories.put_second.fetch_non_matching_only_start_in_tolerance"; + + fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, close_matching_plan_req, 999, 0); + + fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, close_matching_plan_req, 999, 0); + + checkAndEmit(fetched_trajectories.empty(), test_case, "Fetch all returns empty"); + checkAndEmit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr"); + + // Fetch non-matching, only goal in tolerance (but not start) + // + // Non-matching key should not have cache hit + test_case = "testMotionTrajectories.put_second.fetch_non_matching_only_goal_in_tolerance"; + + fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, close_matching_plan_req, 0, 999); + + fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, close_matching_plan_req, 0, 999); + + checkAndEmit(fetched_trajectories.empty(), test_case, "Fetch all returns empty"); + checkAndEmit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr"); + + // Fetch non-matching, in tolerance + // + // Close key within tolerance limit should have cache hit + test_case = "testMotionTrajectories.put_second.fetch_non_matching_in_tolerance"; + + fetched_trajectories = + cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, close_matching_plan_req, 0.1, 0.1); + + fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, close_matching_plan_req, 0.1, 0.1); + + checkAndEmit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); + checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); + + checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); + + checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); + + checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, + "Fetched trajectory has correct execution time"); + + checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, + "Fetched trajectory has correct planning time"); + + // Fetch swapped + // + // Matches should be mostly invariant to constraint ordering + test_case = "testMotionTrajectories.put_second.fetch_swapped"; + + fetched_trajectories = + cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, swapped_close_matching_plan_req, 0.1, 0.1); + + fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, swapped_close_matching_plan_req, 0.1, 0.1); + + checkAndEmit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); + checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); + + checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); + + checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); + + checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, + "Fetched trajectory has correct execution time"); + + checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, + "Fetched trajectory has correct planning time"); + + // Fetch with smaller workspace + // + // Matching trajectories with more restrictive workspace requirements should not + // pull up trajectories cached for less restrictive workspace requirements + test_case = "testMotionTrajectories.put_second.fetch_smaller_workspace"; + + fetched_trajectories = + cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, smaller_workspace_plan_req, 999, 999); + + fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, smaller_workspace_plan_req, 999, 999); + + checkAndEmit(fetched_trajectories.empty(), test_case, "Fetch all returns empty"); + checkAndEmit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr"); + + // Fetch with larger workspace + // + // Matching trajectories with less restrictive workspace requirements should pull up + // trajectories cached for more restrictive workspace requirements + test_case = "testMotionTrajectories.put_second.fetch_larger_workspace"; + + fetched_trajectories = + cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, larger_workspace_plan_req, 999, 999); + + fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, larger_workspace_plan_req, 999, 999); + + checkAndEmit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); + checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); + + checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); + + checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); + + checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, + "Fetched trajectory has correct execution time"); + + checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, + "Fetched trajectory has correct planning time"); + + checkAndEmit(fetched_traj->lookupDouble("workspace_parameters.max_corner.x") <= + larger_workspace_plan_req.workspace_parameters.max_corner.x, + test_case, "Fetched trajectory has more restrictive workspace max_corner"); + + checkAndEmit(fetched_traj->lookupDouble("workspace_parameters.max_corner.x") >= + larger_workspace_plan_req.workspace_parameters.min_corner.x, + test_case, "Fetched trajectory has more restrictive workspace min_corner"); + + // Put worse, no prune_worse_trajectories + // + // Worse trajectories should not be inserted + test_case = "testMotionTrajectories.put_worse"; + double worse_execution_time = execution_time + 100; + + checkAndEmit(!cache->insertTrajectory(*move_group, ROBOT_NAME, plan_req, traj, worse_execution_time, planning_time, + false), + test_case, "Put worse trajectory, no prune_worse_trajectories, not ok"); + + checkAndEmit(cache->countTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache"); + + // Put better, no prune_worse_trajectories + // + // Better trajectories should be inserted + test_case = "testMotionTrajectories.put_better"; + double better_execution_time = execution_time - 100; + + checkAndEmit(cache->insertTrajectory(*move_group, ROBOT_NAME, plan_req, traj, better_execution_time, planning_time, + false), + test_case, "Put better trajectory, no prune_worse_trajectories, ok"); + + checkAndEmit(cache->countTrajectories(ROBOT_NAME) == 3, test_case, "Three trajectories in cache"); + + // Fetch sorted + // + // With multiple trajectories in cache, fetches should be sorted accordingly + test_case = "testMotionTrajectories.put_better.fetch_sorted"; + + fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, plan_req, 0, 0); + + fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, plan_req, 0, 0); + + checkAndEmit(fetched_trajectories.size() == 3, test_case, "Fetch all returns three"); + checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); + + checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); + + checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); + + checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == better_execution_time, test_case, + "Fetched trajectory has correct execution time"); + + checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, + "Fetched trajectory has correct planning time"); + + checkAndEmit(fetched_trajectories.at(0)->lookupDouble("execution_time_s") == better_execution_time && + fetched_trajectories.at(1)->lookupDouble("execution_time_s") == execution_time, + test_case, "Fetched trajectories are sorted correctly"); + + // Put better, prune_worse_trajectories + // + // Better, different, trajectories should be inserted + test_case = "testMotionTrajectories.put_better_prune_worse_trajectories"; + double even_better_execution_time = better_execution_time - 100; + + checkAndEmit(cache->insertTrajectory(*move_group, ROBOT_NAME, plan_req, different_traj, even_better_execution_time, + planning_time, true), + test_case, "Put better trajectory, prune_worse_trajectories, ok"); + + checkAndEmit(cache->countTrajectories(ROBOT_NAME) == 1, test_case, "One trajectory in cache"); + + // Fetch better plan + test_case = "testMotionTrajectories.put_better_prune_worse_trajectories.fetch"; + + fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, plan_req, 0, 0); + + fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, plan_req, 0, 0); + + checkAndEmit(fetched_trajectories.size() == 1, test_case, "Fetch all returns one"); + checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); + + checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); + + checkAndEmit(*fetched_traj == different_traj, test_case, "Fetched trajectory matches original"); + + checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == even_better_execution_time, test_case, + "Fetched trajectory has correct execution time"); + + checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, + "Fetched trajectory has correct planning time"); + + // Put different req, prune_worse_trajectories + // + // Unrelated trajectory requests should live alongside pre-existing plans + test_case = "testMotionTrajectories.put_different_req"; + + checkAndEmit(cache->insertTrajectory(*move_group, ROBOT_NAME, different_plan_req, different_traj, + better_execution_time, planning_time, true), + test_case, "Put different trajectory req, prune_worse_trajectories, ok"); + + checkAndEmit(cache->countTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache"); + + // Fetch with different trajectory req + // + // With multiple trajectories in cache, fetches should be sorted accordingly + test_case = "testMotionTrajectories.put_different_req.fetch"; + + fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, different_plan_req, 0, 0); + + fetched_traj = cache->fetchBestMatchingTrajectory(*move_group, ROBOT_NAME, different_plan_req, 0, 0); + + checkAndEmit(fetched_trajectories.size() == 1, test_case, "Fetch all returns one"); + checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); + + checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); + + checkAndEmit(*fetched_traj == different_traj, test_case, "Fetched trajectory matches original"); + + checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == better_execution_time, test_case, + "Fetched trajectory has correct execution time"); + + checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, + "Fetched trajectory has correct planning time"); + + // Fetch different robot + // + // Since we didn't populate anything, we should expect empty + test_case = "testMotionTrajectories.different_robot.empty"; + std::string different_robot_name = "different_robot"; + + checkAndEmit(cache->countCartesianTrajectories(different_robot_name) == 0, test_case, "No trajectories in cache"); + + // Put first for different robot, prune_worse_trajectories + // + // A different robot's cache should not interact with the original cache + test_case = "testMotionTrajectories.different_robot.put_first"; + checkAndEmit(cache->insertTrajectory(*move_group, different_robot_name, different_plan_req, different_traj, + better_execution_time, planning_time, true), + test_case, "Put different trajectory req, prune_worse_trajectories, ok"); + + checkAndEmit(cache->countTrajectories(different_robot_name) == 1, test_case, "One trajectory in cache"); + + checkAndEmit(cache->countTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in original robot's cache"); + + fetched_trajectories = cache->fetchAllMatchingTrajectories(*move_group, ROBOT_NAME, different_plan_req, 0, 0); + + checkAndEmit(fetched_trajectories.size() == 1, test_case, "Fetch all on original still returns one"); +} + +void testCartesianTrajectories(const std::shared_ptr& move_group, + const std::shared_ptr& cache) +{ + std::string test_case; + + /// First, test if construction even works... + + // Construct get cartesian trajectory request + test_case = "testCartesianTrajectories.constructGetCartesianPathRequest"; + + int test_step = 1; + int test_jump = 2; + auto test_waypoints = getDummyWaypoints(); + auto cartesian_plan_req_under_test = + cache->constructGetCartesianPathRequest(*move_group, test_waypoints, test_step, test_jump, false); + + checkAndEmit(cartesian_plan_req_under_test.waypoints == test_waypoints && + static_cast(cartesian_plan_req_under_test.max_step) == test_step && + static_cast(cartesian_plan_req_under_test.jump_threshold) == test_jump && + !cartesian_plan_req_under_test.avoid_collisions, + test_case, "Ok"); + + // Setup ===================================================================== + // All variants are modified copies of `cartesian_plan_req`. + + /// GetCartesianPath::Request + + // Plain start + auto waypoints = getDummyWaypoints(); + auto cartesian_plan_req = cache->constructGetCartesianPathRequest(*move_group, waypoints, 1, 1, false); + cartesian_plan_req.start_state.multi_dof_joint_state.joint_names.clear(); + cartesian_plan_req.start_state.multi_dof_joint_state.transforms.clear(); + cartesian_plan_req.start_state.multi_dof_joint_state.twist.clear(); + cartesian_plan_req.start_state.multi_dof_joint_state.wrench.clear(); + cartesian_plan_req.path_constraints.joint_constraints.clear(); + cartesian_plan_req.path_constraints.position_constraints.clear(); + cartesian_plan_req.path_constraints.orientation_constraints.clear(); + cartesian_plan_req.path_constraints.visibility_constraints.clear(); + + // Empty frame start + auto empty_frame_cartesian_plan_req = cartesian_plan_req; + empty_frame_cartesian_plan_req.header.frame_id = ""; + + // is_diff = true + auto is_diff_cartesian_plan_req = cartesian_plan_req; + is_diff_cartesian_plan_req.start_state.is_diff = true; + is_diff_cartesian_plan_req.start_state.joint_state.header.frame_id = ""; + is_diff_cartesian_plan_req.start_state.joint_state.name.clear(); + is_diff_cartesian_plan_req.start_state.joint_state.position.clear(); + is_diff_cartesian_plan_req.start_state.joint_state.velocity.clear(); + is_diff_cartesian_plan_req.start_state.joint_state.effort.clear(); + + // Something close enough (mod 0.1 away) + auto close_matching_cartesian_plan_req = cartesian_plan_req; + close_matching_cartesian_plan_req.start_state.joint_state.position.at(0) -= 0.05; + close_matching_cartesian_plan_req.start_state.joint_state.position.at(1) += 0.05; + close_matching_cartesian_plan_req.start_state.joint_state.position.at(2) -= 0.05; + close_matching_cartesian_plan_req.waypoints.at(0).position.x -= 0.05; + close_matching_cartesian_plan_req.waypoints.at(1).position.x += 0.05; + close_matching_cartesian_plan_req.waypoints.at(2).position.x -= 0.05; + + // Different + auto different_cartesian_plan_req = cartesian_plan_req; + different_cartesian_plan_req.start_state.joint_state.position.at(0) -= 1.05; + different_cartesian_plan_req.start_state.joint_state.position.at(1) += 2.05; + different_cartesian_plan_req.start_state.joint_state.position.at(2) -= 3.05; + different_cartesian_plan_req.waypoints.at(0).position.x -= 1.05; + different_cartesian_plan_req.waypoints.at(1).position.x += 2.05; + different_cartesian_plan_req.waypoints.at(2).position.x -= 3.05; + + /// RobotTrajectory + + // Trajectory + auto traj = getDummyPandaTraj(); + + // Trajectory with no frame_id in its trajectory header + auto empty_frame_traj = traj; + empty_frame_traj.joint_trajectory.header.frame_id = ""; + + auto different_traj = traj; + different_traj.joint_trajectory.points.at(0).positions.at(0) = 999; + different_traj.joint_trajectory.points.at(0).positions.at(1) = 999; + different_traj.joint_trajectory.points.at(0).positions.at(2) = 999; + + // Checks ==================================================================== + + // Initially empty + test_case = "testCartesianTrajectories.empty"; + + checkAndEmit(cache->countCartesianTrajectories(ROBOT_NAME) == 0, test_case, "Trajectory cache initially empty"); + + checkAndEmit( + cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, 0, 999, 999).empty(), + test_case, "Fetch all trajectories on empty cache returns empty"); + + checkAndEmit(cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, 0, 999, 999) == + nullptr, + test_case, "Fetch best trajectory on empty cache returns nullptr"); + + // Put trajectory empty frame + // + // Trajectory must have frame in joint trajectory, expect put fail + test_case = "testCartesianTrajectories.insertTrajectory_empty_frame"; + double execution_time = 999; + double planning_time = 999; + double fraction = 0.5; + + checkAndEmit(!cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, empty_frame_traj, + execution_time, planning_time, fraction, false), + test_case, "Put empty frame trajectory, no prune_worse_trajectories, not ok"); + + checkAndEmit(cache->countCartesianTrajectories(ROBOT_NAME) == 0, test_case, "No trajectories in cache"); + + // Put trajectory req empty frame + // + // Trajectory request having no frame in workspace should default to robot frame + test_case = "testCartesianTrajectories.insertTrajectory_req_empty_frame"; + execution_time = 1000; + planning_time = 1000; + + checkAndEmit(cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, empty_frame_cartesian_plan_req, traj, + execution_time, planning_time, fraction, false), + test_case, "Put empty frame req trajectory, no prune_worse_trajectories, ok"); + + checkAndEmit(cache->countCartesianTrajectories(ROBOT_NAME) == 1, test_case, "One trajectory in cache"); + + // Put second, no prune_worse_trajectories + test_case = "testCartesianTrajectories.put_second"; + execution_time = 999; + planning_time = 999; + + checkAndEmit(cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, traj, execution_time, + planning_time, fraction, false), + test_case, "Put second valid trajectory, no prune_worse_trajectories, ok"); + + checkAndEmit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache"); + + // Fetch matching, no tolerance + // + // Exact key match should have cache hit + test_case = "testCartesianTrajectories.put_second.fetch_matching_no_tolerance"; + + auto fetched_trajectories = + cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0); + + auto fetched_traj = + cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0); + + checkAndEmit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); + checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); + + checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); + + checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); + + checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, + "Fetched trajectory has correct execution time"); + + checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, + "Fetched trajectory has correct planning time"); + + checkAndEmit(fetched_traj->lookupDouble("fraction") == fraction, test_case, "Fetched trajectory has correct fraction"); + + // Fetch with is_diff + // + // is_diff key should be equivalent to exact match if robot state did not + // change, hence should have cache hit + test_case = "testCartesianTrajectories.put_second.fetch_is_diff_no_tolerance"; + + fetched_trajectories = + cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, is_diff_cartesian_plan_req, fraction, 0, 0); + + fetched_traj = + cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, is_diff_cartesian_plan_req, fraction, 0, 0); + + checkAndEmit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); + checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); + + checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); + + checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); + + checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, + "Fetched trajectory has correct execution time"); + + checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, + "Fetched trajectory has correct planning time"); + + checkAndEmit(fetched_traj->lookupDouble("fraction") == fraction, test_case, "Fetched trajectory has correct fraction"); + + // Fetch non-matching, out of tolerance + // + // Non-matching key should not have cache hit + test_case = "testCartesianTrajectories.put_second.fetch_non_matching_out_of_tolerance"; + + fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories( + *move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 0, 0); + + fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, close_matching_cartesian_plan_req, + fraction, 0, 0); + + checkAndEmit(fetched_trajectories.empty(), test_case, "Fetch all returns empty"); + checkAndEmit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr"); + + // Fetch non-matching, only start in tolerance (but not goal) + // + // Non-matching key should not have cache hit + test_case = "testMotionTrajectories.put_second.fetch_non_matching_only_start_in_tolerance"; + + fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories( + *move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 999, 0); + + fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, close_matching_cartesian_plan_req, + fraction, 999, 0); + + checkAndEmit(fetched_trajectories.empty(), test_case, "Fetch all returns empty"); + checkAndEmit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr"); + + // Fetch non-matching, only goal in tolerance (but not start) + // + // Non-matching key should not have cache hit + test_case = "testMotionTrajectories.put_second.fetch_non_matching_only_goal_in_tolerance"; + + fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories( + *move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 0, 999); + + fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, close_matching_cartesian_plan_req, + fraction, 0, 999); + + checkAndEmit(fetched_trajectories.empty(), test_case, "Fetch all returns empty"); + checkAndEmit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr"); + + // Fetch non-matching, in tolerance + // + // Close key within tolerance limit should have cache hit + test_case = "testCartesianTrajectories.put_second.fetch_non_matching_in_tolerance"; + + fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories( + *move_group, ROBOT_NAME, close_matching_cartesian_plan_req, fraction, 0.1, 0.1); + + fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, close_matching_cartesian_plan_req, + fraction, 0.1, 0.1); + + checkAndEmit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); + checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); + + checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); + + checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); + + checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, + "Fetched trajectory has correct execution time"); + + checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, + "Fetched trajectory has correct planning time"); + + checkAndEmit(fetched_traj->lookupDouble("fraction") == fraction, test_case, "Fetched trajectory has correct fraction"); + + // Fetch with higher fraction + // + // Matching trajectories with more restrictive fraction requirements should not + // pull up trajectories cached for less restrictive fraction requirements + test_case = "testCartesianTrajectories.put_second.fetch_higher_fraction"; + + fetched_trajectories = + cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, 1, 999, 999); + + fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, 1, 999, 999); + + checkAndEmit(fetched_trajectories.empty(), test_case, "Fetch all returns empty"); + checkAndEmit(fetched_traj == nullptr, test_case, "Fetch best trajectory is nullptr"); + + // Fetch with lower fraction + // + // Matching trajectories with less restrictive fraction requirements should pull up + // trajectories cached for more restrictive fraction requirements + test_case = "testCartesianTrajectories.put_second.fetch_lower_fraction"; + + fetched_trajectories = + cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, 0, 999, 999); + + fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, 0, 999, 999); + + checkAndEmit(fetched_trajectories.size() == 2, test_case, "Fetch all returns two"); + checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); + + checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); + + checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); + + checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == execution_time, test_case, + "Fetched trajectory has correct execution time"); + + checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, + "Fetched trajectory has correct planning time"); + + checkAndEmit(fetched_traj->lookupDouble("fraction") == fraction, test_case, "Fetched trajectory has correct fraction"); + + // Put worse, no prune_worse_trajectories + // + // Worse trajectories should not be inserted + test_case = "testCartesianTrajectories.put_worse"; + double worse_execution_time = execution_time + 100; + + checkAndEmit(!cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, traj, + worse_execution_time, planning_time, fraction, false), + test_case, "Put worse trajectory, no prune_worse_trajectories, not ok"); + + checkAndEmit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache"); + + // Put better, no prune_worse_trajectories + // + // Better trajectories should be inserted + test_case = "testCartesianTrajectories.put_better"; + double better_execution_time = execution_time - 100; + + checkAndEmit(cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, traj, + better_execution_time, planning_time, fraction, false), + test_case, "Put better trajectory, no prune_worse_trajectories, ok"); + + checkAndEmit(cache->countCartesianTrajectories(ROBOT_NAME) == 3, test_case, "Three trajectories in cache"); + + // Fetch sorted + // + // With multiple trajectories in cache, fetches should be sorted accordingly + test_case = "testCartesianTrajectories.put_better.fetch_sorted"; + + fetched_trajectories = + cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0); + + fetched_traj = + cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0); + + checkAndEmit(fetched_trajectories.size() == 3, test_case, "Fetch all returns three"); + checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); + + checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); + + checkAndEmit(*fetched_traj == traj, test_case, "Fetched trajectory matches original"); + + checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == better_execution_time, test_case, + "Fetched trajectory has correct execution time"); + + checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, + "Fetched trajectory has correct planning time"); + + checkAndEmit(fetched_traj->lookupDouble("fraction") == fraction, test_case, "Fetched trajectory has correct fraction"); + + checkAndEmit(fetched_trajectories.at(0)->lookupDouble("execution_time_s") == better_execution_time && + fetched_trajectories.at(1)->lookupDouble("execution_time_s") == execution_time, + test_case, "Fetched trajectories are sorted correctly"); + + // Put better, prune_worse_trajectories + // + // Better, different, trajectories should be inserted + test_case = "testCartesianTrajectories.put_better_prune_worse_trajectories"; + double even_better_execution_time = better_execution_time - 100; + + checkAndEmit(cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, different_traj, + even_better_execution_time, planning_time, fraction, true), + test_case, "Put better trajectory, prune_worse_trajectories, ok"); + + checkAndEmit(cache->countCartesianTrajectories(ROBOT_NAME) == 1, test_case, "One trajectory in cache"); + + // Fetch better plan + test_case = "testCartesianTrajectories.put_better_prune_worse_trajectories.fetch"; + + fetched_trajectories = + cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0); + + fetched_traj = + cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, cartesian_plan_req, fraction, 0, 0); + + checkAndEmit(fetched_trajectories.size() == 1, test_case, "Fetch all returns one"); + checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); + + checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); + + checkAndEmit(*fetched_traj == different_traj, test_case, "Fetched trajectory matches original"); + + checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == even_better_execution_time, test_case, + "Fetched trajectory has correct execution time"); + + checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, + "Fetched trajectory has correct planning time"); + + checkAndEmit(fetched_traj->lookupDouble("fraction") == fraction, test_case, "Fetched trajectory has correct fraction"); + + // Put different req, prune_worse_trajectories + // + // Unrelated trajectory requests should live alongside pre-existing plans + test_case = "testCartesianTrajectories.put_different_req"; + + checkAndEmit(cache->insertCartesianTrajectory(*move_group, ROBOT_NAME, different_cartesian_plan_req, different_traj, + better_execution_time, planning_time, fraction, true), + test_case, "Put different trajectory req, prune_worse_trajectories, ok"); + + checkAndEmit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, test_case, "Two trajectories in cache"); + + // Fetch with different trajectory req + // + // With multiple trajectories in cache, fetches should be sorted accordingly + test_case = "testCartesianTrajectories.put_different_req.fetch"; + + fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, + different_cartesian_plan_req, fraction, 0, 0); + + fetched_traj = cache->fetchBestMatchingCartesianTrajectory(*move_group, ROBOT_NAME, different_cartesian_plan_req, + fraction, 0, 0); + + checkAndEmit(fetched_trajectories.size() == 1, test_case, "Fetch all returns one"); + checkAndEmit(fetched_traj != nullptr, test_case, "Fetch best trajectory is not nullptr"); + + checkAndEmit(*fetched_trajectories.at(0) == *fetched_traj, test_case, "Fetched trajectory on both fetches match"); + + checkAndEmit(*fetched_traj == different_traj, test_case, "Fetched trajectory matches original"); + + checkAndEmit(fetched_traj->lookupDouble("execution_time_s") == better_execution_time, test_case, + "Fetched trajectory has correct execution time"); + + checkAndEmit(fetched_traj->lookupDouble("planning_time_s") == planning_time, test_case, + "Fetched trajectory has correct planning time"); + + checkAndEmit(fetched_traj->lookupDouble("fraction") == fraction, test_case, "Fetched trajectory has correct fraction"); + + // Fetch different robot + // + // Since we didn't populate anything, we should expect empty + test_case = "testCartesianTrajectories.different_robot.empty"; + std::string different_robot_name = "different_robot"; + + checkAndEmit(cache->countCartesianTrajectories(different_robot_name) == 0, test_case, "No trajectories in cache"); + + // Put first for different robot, prune_worse_trajectories + // + // A different robot's cache should not interact with the original cache + test_case = "testCartesianTrajectories.different_robot.put_first"; + checkAndEmit(cache->insertCartesianTrajectory(*move_group, different_robot_name, different_cartesian_plan_req, + different_traj, better_execution_time, planning_time, fraction, true), + test_case, "Put different trajectory req, prune_worse_trajectories, ok"); + + checkAndEmit(cache->countCartesianTrajectories(different_robot_name) == 1, test_case, "One trajectory in cache"); + + checkAndEmit(cache->countCartesianTrajectories(ROBOT_NAME) == 2, test_case, + "Two trajectories in original robot's cache"); + + fetched_trajectories = cache->fetchAllMatchingCartesianTrajectories(*move_group, ROBOT_NAME, + different_cartesian_plan_req, fraction, 0, 0); + + checkAndEmit(fetched_trajectories.size() == 1, test_case, "Fetch all on original still returns one"); +} + +int main(int argc, char** argv) +{ + // Setup + rclcpp::init(argc, argv); + + rclcpp::NodeOptions test_node_options; + test_node_options.automatically_declare_parameters_from_overrides(true); + test_node_options.arguments({ "--ros-args", "-r", "__node:=test_node" }); + + rclcpp::NodeOptions move_group_node_options; + move_group_node_options.automatically_declare_parameters_from_overrides(true); + move_group_node_options.arguments({ "--ros-args", "-r", "__node:=move_group_node" }); + + auto test_node = rclcpp::Node::make_shared("test_node", test_node_options); + auto move_group_node = rclcpp::Node::make_shared("move_group_node", move_group_node_options); + + std::atomic running = true; + + std::thread spin_thread([&]() { + while (rclcpp::ok() && running) + { + rclcpp::spin_some(test_node); + rclcpp::spin_some(move_group_node); + } + }); + + // This is necessary + test_node->declare_parameter("warehouse_plugin", "warehouse_ros_sqlite::DatabaseConnection"); + + { + // Init. + + auto move_group = std::make_shared(move_group_node, "panda_arm"); + auto curr_state = move_group->getCurrentState(60); + move_group->setStartState(*curr_state); + + auto cache = std::make_shared(test_node); + + TrajectoryCache::Options options; + options.db_path = ":memory:"; + options.db_port = 0; + options.exact_match_precision = 10; + options.num_additional_trajectories_to_preserve_when_deleting_worse = 10; + + // Tests. + + checkAndEmit(cache->init(options), "init", "Cache init"); + + testGettersAndSetters(cache); + + cache->setExactMatchPrecision(1e-4); + cache->setNumAdditionalTrajectoriesToPreserveWhenDeletingWorse(0); + + testMotionTrajectories(move_group, cache); + testCartesianTrajectories(move_group, cache); + } + + running = false; + spin_thread.join(); + + test_node.reset(); + move_group_node.reset(); + + rclcpp::shutdown(); + return 0; +} diff --git a/moveit_ros/trajectory_cache/test/test_trajectory_cache.py b/moveit_ros/trajectory_cache/test/test_trajectory_cache.py new file mode 100755 index 0000000000..05cfcabd25 --- /dev/null +++ b/moveit_ros/trajectory_cache/test/test_trajectory_cache.py @@ -0,0 +1,153 @@ +# Copyright 2024 Intrinsic Innovation LLC. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Author: methylDragon + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ExecuteProcess +from launch_pytest.tools import process as process_tools +from launch_ros.actions import Node + +from moveit_configs_utils import MoveItConfigsBuilder + +import launch_pytest + +import pytest +import os + +# This would have been a gtest, but we need a move_group instance, which +# requires some parameters loaded and a separate node started. + + +@pytest.fixture +def moveit_config(): + return ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .planning_pipelines("ompl", ["ompl"]) + .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") + .to_moveit_configs() + ) + + +# We need this so the move_group has something to interact with +@pytest.fixture +def robot_fixture(moveit_config): + run_move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="log", + parameters=[moveit_config.to_dict()], + ) + + static_tf = Node( + package="tf2_ros", + executable="static_transform_publisher", + output="log", + arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"], + ) + + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="log", + parameters=[moveit_config.robot_description], + ) + + # ros2_control using FakeSystem as hardware + ros2_controllers_path = os.path.join( + get_package_share_directory("moveit_resources_panda_moveit_config"), + "config", + "ros2_controllers.yaml", + ) + ros2_control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[moveit_config.robot_description, ros2_controllers_path], + output="log", + ) + + load_controllers = [] + for controller in [ + "panda_arm_controller", + "panda_hand_controller", + "joint_state_broadcaster", + ]: + load_controllers += [ + ExecuteProcess( + cmd=["ros2 run controller_manager spawner {}".format(controller)], + shell=True, + output="log", + ) + ] + + return [ + run_move_group_node, + static_tf, + robot_state_publisher, + ros2_control_node, + *load_controllers, + ] + + +@pytest.fixture +def trajectory_cache_test_runner_node(moveit_config): + return Node( + package="moveit_ros_trajectory_cache", + executable="test_trajectory_cache", + name="test_trajectory_cache_node", + output="screen", + cached_output=True, + parameters=[moveit_config.to_dict()], + ) + + +@launch_pytest.fixture +def launch_description(trajectory_cache_test_runner_node, robot_fixture): + return LaunchDescription( + robot_fixture + + [trajectory_cache_test_runner_node, launch_pytest.actions.ReadyToTest()] + ) + + +def validate_stream(expected): + def wrapped(output): + assert ( + expected in output.splitlines() + ), f"Did not get expected: {expected} in output:\n\n{output}" + + return wrapped + + +@pytest.mark.launch(fixture=launch_description) +def test_all_tests_pass(trajectory_cache_test_runner_node, launch_context): + # Check for occurrences of [PASS] in output + assert process_tools.wait_for_output_sync( + launch_context, + trajectory_cache_test_runner_node, + lambda x: x.count("[PASS]") == 169, # All test cases passed. + timeout=30, + ) + + # Check no occurrences of [FAIL] in output + assert not process_tools.wait_for_output_sync( + launch_context, + trajectory_cache_test_runner_node, + lambda x: "[FAIL]" in x, + timeout=10, + ) + + yield