From 37b7494e466dec9b63f494f879e89d7fdba33b7d Mon Sep 17 00:00:00 2001 From: methylDragon Date: Fri, 17 May 2024 23:08:51 -0700 Subject: [PATCH] Implement trajectory cache Signed-off-by: methylDragon --- moveit_ros/trajectory_cache/CHANGELOG.rst | 7 + moveit_ros/trajectory_cache/CMakeLists.txt | 63 + moveit_ros/trajectory_cache/README.md | 1 + moveit_ros/trajectory_cache/package.xml | 40 + .../src/test_trajectory_cache.cpp | 1049 +++++++++++++ .../trajectory_cache/src/trajectory_cache.cpp | 1340 +++++++++++++++++ .../trajectory_cache/src/trajectory_cache.hpp | 261 ++++ .../test/test_trajectory_cache.py | 148 ++ 8 files changed, 2909 insertions(+) create mode 100644 moveit_ros/trajectory_cache/CHANGELOG.rst create mode 100644 moveit_ros/trajectory_cache/CMakeLists.txt create mode 100644 moveit_ros/trajectory_cache/README.md create mode 100644 moveit_ros/trajectory_cache/package.xml create mode 100644 moveit_ros/trajectory_cache/src/test_trajectory_cache.cpp create mode 100644 moveit_ros/trajectory_cache/src/trajectory_cache.cpp create mode 100644 moveit_ros/trajectory_cache/src/trajectory_cache.hpp create mode 100755 moveit_ros/trajectory_cache/test/test_trajectory_cache.py diff --git a/moveit_ros/trajectory_cache/CHANGELOG.rst b/moveit_ros/trajectory_cache/CHANGELOG.rst new file mode 100644 index 00000000000..82ad0f2be94 --- /dev/null +++ b/moveit_ros/trajectory_cache/CHANGELOG.rst @@ -0,0 +1,7 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package nexus_motion_planner +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.0 (2024-05-17) +------------------ +* Add ``moveit_ros_trajectory_cache`` package for trajectory caching. \ No newline at end of file diff --git a/moveit_ros/trajectory_cache/CMakeLists.txt b/moveit_ros/trajectory_cache/CMakeLists.txt new file mode 100644 index 00000000000..4fabec17d70 --- /dev/null +++ b/moveit_ros/trajectory_cache/CMakeLists.txt @@ -0,0 +1,63 @@ +cmake_minimum_required(VERSION 3.8) +project(moveit_ros_trajectory_cache) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +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) +find_package(warehouse_ros_sqlite REQUIRED) + +include_directories(include) + +set (trajectory_cache_dependencies + geometry_msgs + moveit_ros_planning_interface + moveit_ros_warehouse + rclcpp + tf2 + tf2_ros + trajectory_msgs + warehouse_ros + warehouse_ros_sqlite +) + +#=============================================================================== +set(TRAJECTORY_CACHE_LIBRARY_NAME trajectory_cache) + +# Motion plan cache library +add_library(${TRAJECTORY_CACHE_LIBRARY_NAME} src/trajectory_cache.cpp) +ament_target_dependencies(${TRAJECTORY_CACHE_LIBRARY_NAME} ${trajectory_cache_dependencies}) + +#=============================================================================== + +if(BUILD_TESTING) + find_package(ament_cmake_pytest REQUIRED) + find_package(launch_testing_ament_cmake REQUIRED) + find_package(rmf_utils REQUIRED) + + add_executable(test_trajectory_cache src/test_trajectory_cache.cpp) + target_link_libraries(test_trajectory_cache ${TRAJECTORY_CACHE_LIBRARY_NAME}) + + 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_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 00000000000..d0ff4993652 --- /dev/null +++ b/moveit_ros/trajectory_cache/README.md @@ -0,0 +1 @@ +# Trajectory Cache diff --git a/moveit_ros/trajectory_cache/package.xml b/moveit_ros/trajectory_cache/package.xml new file mode 100644 index 00000000000..9a1edf74b31 --- /dev/null +++ b/moveit_ros/trajectory_cache/package.xml @@ -0,0 +1,40 @@ + + + + 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 + + geometry_msgs + moveit_ros_planning_interface + rclcpp + rclcpp_action + tf2_ros + trajectory_msgs + + moveit_ros + python3-yaml + warehouse_ros_sqlite + 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 + + + ament_cmake + + diff --git a/moveit_ros/trajectory_cache/src/test_trajectory_cache.cpp b/moveit_ros/trajectory_cache/src/test_trajectory_cache.cpp new file mode 100644 index 00000000000..51799661f15 --- /dev/null +++ b/moveit_ros/trajectory_cache/src/test_trajectory_cache.cpp @@ -0,0 +1,1049 @@ +// Copyright 2023 Johnson & Johnson +// +// 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. + +#include + +#include "moveit/robot_state/conversions.h" +#include "moveit/robot_state/robot_state.h" +#include "trajectory_cache.hpp" + +#include + +using moveit::planning_interface::MoveGroupInterface; +using moveit_ros::trajectory_cache::TrajectoryCache; + +const std::string g_robot_name = "panda"; +const std::string g_robot_frame = "world"; + +// UTILS ======================================================================= +// Utility function to emit a pass or fail statement. +void check_and_emit(bool predicate, const std::string& prefix, const std::string& label) +{ + if (predicate) + { + std::cout << "[PASS] " << prefix << ": " << label << std::endl; + } + else + { + std::cout << "[FAIL] " << prefix << ": " << label << std::endl; + exit(1); + } +} + +moveit_msgs::msg::RobotTrajectory get_dummy_panda_traj() +{ + static moveit_msgs::msg::RobotTrajectory out = []() { + moveit_msgs::msg::RobotTrajectory traj; + + auto trajectory = &traj.joint_trajectory; + trajectory->header.frame_id = g_robot_frame; + + trajectory->joint_names.push_back(g_robot_name + "_joint1"); + trajectory->joint_names.push_back(g_robot_name + "_joint2"); + trajectory->joint_names.push_back(g_robot_name + "_joint3"); + trajectory->joint_names.push_back(g_robot_name + "_joint4"); + trajectory->joint_names.push_back(g_robot_name + "_joint5"); + trajectory->joint_names.push_back(g_robot_name + "_joint6"); + trajectory->joint_names.push_back(g_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 get_dummy_waypoints() +{ + 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 test_motion_trajectories(std::shared_ptr move_group, std::shared_ptr cache) +{ + // Setup ===================================================================== + // All variants are copies. + + /// MotionPlanRequest + + // Plain start + moveit_msgs::msg::MotionPlanRequest plan_req; + move_group->constructMotionPlanRequest(plan_req); + plan_req.workspace_parameters.header.frame_id = g_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 = get_dummy_panda_traj(); + + // 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 prefix; + + // Checks ==================================================================== + + // Initially empty + prefix = "test_motion_trajectories.empty"; + + check_and_emit(cache->count_trajectories(g_robot_name) == 0, prefix, "Trajectory cache initially empty"); + + check_and_emit(cache->fetch_all_matching_trajectories(*move_group, g_robot_name, plan_req, 999, 999).empty(), prefix, + "Fetch all trajectories on empty cache returns empty"); + + check_and_emit(cache->fetch_best_matching_trajectory(*move_group, g_robot_name, plan_req, 999, 999) == nullptr, + prefix, "Fetch best trajectory on empty cache returns nullptr"); + + // Put trajectory empty frame + // + // Trajectory must have frame in joint trajectory, expect put fail + prefix = "test_motion_trajectories.put_trajectory_empty_frame"; + double execution_time = 999; + double planning_time = 999; + + check_and_emit(!cache->put_trajectory(*move_group, g_robot_name, plan_req, empty_frame_traj, execution_time, + planning_time, false), + prefix, "Put empty frame traj, no delete_worse_trajectories, not ok"); + + check_and_emit(cache->count_trajectories(g_robot_name) == 0, prefix, "No trajectories in cache"); + + // Put trajectory req empty frame + // + // Trajectory request must have frame in workspace, expect put fail + prefix = "test_motion_trajectories.put_trajectory_req_empty_frame"; + execution_time = 999; + planning_time = 999; + + check_and_emit(!cache->put_trajectory(*move_group, g_robot_name, empty_frame_plan_req, traj, execution_time, + planning_time, false), + prefix, "Put empty frame req traj, no delete_worse_trajectories, not ok"); + + check_and_emit(cache->count_trajectories(g_robot_name) == 0, prefix, "No trajectories in cache"); + + // Put first, no delete_worse_trajectories + prefix = "test_motion_trajectories.put_first"; + execution_time = 999; + planning_time = 999; + + check_and_emit(cache->put_trajectory(*move_group, g_robot_name, plan_req, traj, execution_time, planning_time, false), + prefix, "Put first valid traj, no delete_worse_trajectories, ok"); + + check_and_emit(cache->count_trajectories(g_robot_name) == 1, prefix, "One trajectory in cache"); + + // Fetch matching, no tolerance + // + // Exact key match should have cache hit + prefix = "test_motion_trajectories.put_first.fetch_matching_no_tolerance"; + + auto fetched_trajectories = cache->fetch_all_matching_trajectories(*move_group, g_robot_name, plan_req, 0, 0); + + auto fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, plan_req, 0, 0); + + check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + + check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, prefix, + "Fetched trajectory has correct execution time"); + + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + "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 + prefix = "test_motion_trajectories.put_first.fetch_is_diff_no_tolerance"; + + fetched_trajectories = cache->fetch_all_matching_trajectories(*move_group, g_robot_name, is_diff_plan_req, 0, 0); + + fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, is_diff_plan_req, 0, 0); + + check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + + check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, prefix, + "Fetched trajectory has correct execution time"); + + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + "Fetched trajectory has correct planning time"); + + // Fetch non-matching, out of tolerance + // + // Non-matching key should not have cache hit + prefix = "test_motion_trajectories.put_first.fetch_non_matching_out_of_tolerance"; + + fetched_trajectories = + cache->fetch_all_matching_trajectories(*move_group, g_robot_name, close_matching_plan_req, 0, 0); + + fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, close_matching_plan_req, 0, 0); + + check_and_emit(fetched_trajectories.size() == 0, prefix, "Fetch all returns empty"); + check_and_emit(fetched_traj == nullptr, prefix, "Fetch best trajectory is nullptr"); + + // Fetch non-matching, only start in tolerance (but not goal) + // + // Non-matching key should not have cache hit + prefix = "test_motion_trajectories.put_first.fetch_non_matching_only_start_in_tolerance"; + + fetched_trajectories = + cache->fetch_all_matching_trajectories(*move_group, g_robot_name, close_matching_plan_req, 999, 0); + + fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, close_matching_plan_req, 999, 0); + + check_and_emit(fetched_trajectories.size() == 0, prefix, "Fetch all returns empty"); + check_and_emit(fetched_traj == nullptr, prefix, "Fetch best trajectory is nullptr"); + + // Fetch non-matching, only goal in tolerance (but not start) + // + // Non-matching key should not have cache hit + prefix = "test_motion_trajectories.put_first.fetch_non_matching_only_goal_in_tolerance"; + + fetched_trajectories = + cache->fetch_all_matching_trajectories(*move_group, g_robot_name, close_matching_plan_req, 0, 999); + + fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, close_matching_plan_req, 0, 999); + + check_and_emit(fetched_trajectories.size() == 0, prefix, "Fetch all returns empty"); + check_and_emit(fetched_traj == nullptr, prefix, "Fetch best trajectory is nullptr"); + + // Fetch non-matching, in tolerance + // + // Close key within tolerance limit should have cache hit + prefix = "test_motion_trajectories.put_first.fetch_non_matching_in_tolerance"; + + fetched_trajectories = + cache->fetch_all_matching_trajectories(*move_group, g_robot_name, close_matching_plan_req, 0.1, 0.1); + + fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, close_matching_plan_req, 0.1, 0.1); + + check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + + check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, prefix, + "Fetched trajectory has correct execution time"); + + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + "Fetched trajectory has correct planning time"); + + // Fetch swapped + // + // Matches should be mostly invariant to constraint ordering + prefix = "test_motion_trajectories.put_first.fetch_swapped"; + + fetched_trajectories = + cache->fetch_all_matching_trajectories(*move_group, g_robot_name, swapped_close_matching_plan_req, 0.1, 0.1); + + fetched_traj = + cache->fetch_best_matching_trajectory(*move_group, g_robot_name, swapped_close_matching_plan_req, 0.1, 0.1); + + check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + + check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, prefix, + "Fetched trajectory has correct execution time"); + + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + "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 + prefix = "test_motion_trajectories.put_first.fetch_smaller_workspace"; + + fetched_trajectories = + cache->fetch_all_matching_trajectories(*move_group, g_robot_name, smaller_workspace_plan_req, 999, 999); + + fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, smaller_workspace_plan_req, 999, 999); + + check_and_emit(fetched_trajectories.size() == 0, prefix, "Fetch all returns empty"); + check_and_emit(fetched_traj == nullptr, prefix, "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 + prefix = "test_motion_trajectories.put_first.fetch_larger_workspace"; + + fetched_trajectories = + cache->fetch_all_matching_trajectories(*move_group, g_robot_name, larger_workspace_plan_req, 999, 999); + + fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, larger_workspace_plan_req, 999, 999); + + check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + + check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, prefix, + "Fetched trajectory has correct execution time"); + + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + "Fetched trajectory has correct planning time"); + + check_and_emit(fetched_traj->lookupDouble("workspace_parameters.max_corner.x") <= + larger_workspace_plan_req.workspace_parameters.max_corner.x, + prefix, "Fetched trajectory has more restrictive workspace max_corner"); + + check_and_emit(fetched_traj->lookupDouble("workspace_parameters.max_corner.x") >= + larger_workspace_plan_req.workspace_parameters.min_corner.x, + prefix, "Fetched trajectory has more restrictive workspace min_corner"); + + // Put worse, no delete_worse_trajectories + // + // Worse trajectories should not be inserted + prefix = "test_motion_trajectories.put_worse"; + double worse_execution_time = execution_time + 100; + + check_and_emit(!cache->put_trajectory(*move_group, g_robot_name, plan_req, traj, worse_execution_time, planning_time, + false), + prefix, "Put worse traj, no delete_worse_trajectories, not ok"); + + check_and_emit(cache->count_trajectories(g_robot_name) == 1, prefix, "One trajectory in cache"); + + // Put better, no delete_worse_trajectories + // + // Better trajectories should be inserted + prefix = "test_motion_trajectories.put_better"; + double better_execution_time = execution_time - 100; + + check_and_emit(cache->put_trajectory(*move_group, g_robot_name, plan_req, traj, better_execution_time, planning_time, + false), + prefix, "Put better traj, no delete_worse_trajectories, ok"); + + check_and_emit(cache->count_trajectories(g_robot_name) == 2, prefix, "Two trajectories in cache"); + + // Fetch sorted + // + // With multiple trajectories in cache, fetches should be sorted accordingly + prefix = "test_motion_trajectories.put_better.fetch_sorted"; + + fetched_trajectories = cache->fetch_all_matching_trajectories(*move_group, g_robot_name, plan_req, 0, 0); + + fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, plan_req, 0, 0); + + check_and_emit(fetched_trajectories.size() == 2, prefix, "Fetch all returns two"); + check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + + check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == better_execution_time, prefix, + "Fetched trajectory has correct execution time"); + + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + "Fetched trajectory has correct planning time"); + + check_and_emit(fetched_trajectories.at(0)->lookupDouble("execution_time_s") == better_execution_time && + fetched_trajectories.at(1)->lookupDouble("execution_time_s") == execution_time, + prefix, "Fetched trajectories are sorted correctly"); + + // Put better, delete_worse_trajectories + // + // Better, different, trajectories should be inserted + prefix = "test_motion_trajectories.put_better_delete_worse_trajectories"; + double even_better_execution_time = better_execution_time - 100; + + check_and_emit(cache->put_trajectory(*move_group, g_robot_name, plan_req, different_traj, even_better_execution_time, + planning_time, true), + prefix, "Put better traj, delete_worse_trajectories, ok"); + + check_and_emit(cache->count_trajectories(g_robot_name) == 1, prefix, "One trajectory in cache"); + + // Fetch better plan + prefix = "test_motion_trajectories.put_better_delete_worse_trajectories.fetch"; + + fetched_trajectories = cache->fetch_all_matching_trajectories(*move_group, g_robot_name, plan_req, 0, 0); + + fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, plan_req, 0, 0); + + check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + + check_and_emit(*fetched_traj == different_traj, prefix, "Fetched trajectory matches original"); + + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == even_better_execution_time, prefix, + "Fetched trajectory has correct execution time"); + + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + "Fetched trajectory has correct planning time"); + + // Put different req, delete_worse_trajectories + // + // Unrelated trajectory requests should live alongside pre-existing plans + prefix = "test_motion_trajectories.put_different_req"; + + check_and_emit(cache->put_trajectory(*move_group, g_robot_name, different_plan_req, different_traj, + better_execution_time, planning_time, true), + prefix, "Put different trajectory req, delete_worse_trajectories, ok"); + + check_and_emit(cache->count_trajectories(g_robot_name) == 2, prefix, "Two trajectories in cache"); + + // Fetch with different trajectory req + // + // With multiple trajectories in cache, fetches should be sorted accordingly + prefix = "test_motion_trajectories.put_different_req.fetch"; + + fetched_trajectories = cache->fetch_all_matching_trajectories(*move_group, g_robot_name, different_plan_req, 0, 0); + + fetched_traj = cache->fetch_best_matching_trajectory(*move_group, g_robot_name, different_plan_req, 0, 0); + + check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + + check_and_emit(*fetched_traj == different_traj, prefix, "Fetched trajectory matches original"); + + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == better_execution_time, prefix, + "Fetched trajectory has correct execution time"); + + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + "Fetched trajectory has correct planning time"); + + // Fetch different robot + // + // Since we didn't populate anything, we should expect empty + prefix = "test_motion_trajectories.different_robot.empty"; + std::string different_robot_name = "different_robot"; + + check_and_emit(cache->count_cartesian_trajectories(different_robot_name) == 0, prefix, "No trajectories in cache"); + + // Put first for different robot, delete_worse_trajectories + // + // A different robot's cache should not interact with the original cache + prefix = "test_motion_trajectories.different_robot.put_first"; + check_and_emit(cache->put_trajectory(*move_group, different_robot_name, different_plan_req, different_traj, + better_execution_time, planning_time, true), + prefix, "Put different trajectory req, delete_worse_trajectories, ok"); + + check_and_emit(cache->count_trajectories(different_robot_name) == 1, prefix, "One trajectories in cache"); + + check_and_emit(cache->count_trajectories(different_robot_name) == 1, prefix, "One trajectories in cache"); + + check_and_emit(cache->count_trajectories(g_robot_name) == 2, prefix, "Two trajectories in original robot's cache"); + + fetched_trajectories = cache->fetch_all_matching_trajectories(*move_group, g_robot_name, different_plan_req, 0, 0); + + check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all on original still returns one"); +} + +void test_cartesian_trajectories(std::shared_ptr move_group, std::shared_ptr cache) +{ + std::string prefix; + + /// First, test if construction even works... + + // Construct get cartesian trajectory request + prefix = "test_cartesian_trajectories.construct_get_cartesian_path_request"; + + int test_step = 1; + int test_jump = 2; + auto test_waypoints = get_dummy_waypoints(); + auto cartesian_plan_req_under_test = + cache->construct_get_cartesian_path_request(*move_group, test_waypoints, test_step, test_jump, false); + + check_and_emit(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, + prefix, "Ok"); + + // Setup ===================================================================== + // All variants are copies. + + /// GetCartesianPath::Request + + // Plain start + auto waypoints = get_dummy_waypoints(); + auto cartesian_plan_req = cache->construct_get_cartesian_path_request(*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 = get_dummy_panda_traj(); + + // 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 + prefix = "test_cartesian_trajectories.empty"; + + check_and_emit(cache->count_cartesian_trajectories(g_robot_name) == 0, prefix, "Trajectory cache initially empty"); + + check_and_emit( + cache->fetch_all_matching_cartesian_trajectories(*move_group, g_robot_name, cartesian_plan_req, 0, 999, 999) + .empty(), + prefix, "Fetch all trajectories on empty cache returns empty"); + + check_and_emit(cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, 0, 999, + 999) == nullptr, + prefix, "Fetch best trajectory on empty cache returns nullptr"); + + // Put trajectory empty frame + // + // Trajectory must have frame in joint trajectory, expect put fail + prefix = "test_cartesian_trajectories.put_trajectory_empty_frame"; + double execution_time = 999; + double planning_time = 999; + double fraction = 0.5; + + check_and_emit(!cache->put_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, empty_frame_traj, + execution_time, planning_time, fraction, false), + prefix, "Put empty frame traj, no delete_worse_trajectories, not ok"); + + check_and_emit(cache->count_cartesian_trajectories(g_robot_name) == 0, prefix, "No trajectories in cache"); + + // Put trajectory req empty frame + // + // Trajectory request must have frame in workspace, expect put fail + prefix = "test_cartesian_trajectories.put_trajectory_req_empty_frame"; + execution_time = 999; + planning_time = 999; + + check_and_emit(!cache->put_cartesian_trajectory(*move_group, g_robot_name, empty_frame_cartesian_plan_req, traj, + execution_time, planning_time, fraction, false), + prefix, "Put empty frame req traj, no delete_worse_trajectories, not ok"); + + check_and_emit(cache->count_cartesian_trajectories(g_robot_name) == 0, prefix, "No trajectories in cache"); + + // Put first, no delete_worse_trajectories + prefix = "test_cartesian_trajectories.put_first"; + execution_time = 999; + planning_time = 999; + + check_and_emit(cache->put_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, traj, execution_time, + planning_time, fraction, false), + prefix, "Put first valid traj, no delete_worse_trajectories, ok"); + + check_and_emit(cache->count_cartesian_trajectories(g_robot_name) == 1, prefix, "One trajectory in cache"); + + // Fetch matching, no tolerance + // + // Exact key match should have cache hit + prefix = "test_cartesian_trajectories.put_first.fetch_matching_no_tolerance"; + + auto fetched_trajectories = + cache->fetch_all_matching_cartesian_trajectories(*move_group, g_robot_name, cartesian_plan_req, fraction, 0, 0); + + auto fetched_traj = + cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, fraction, 0, 0); + + check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + + check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, prefix, + "Fetched trajectory has correct execution time"); + + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + "Fetched trajectory has correct planning time"); + + check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, prefix, "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 + prefix = "test_cartesian_trajectories.put_first.fetch_is_diff_no_tolerance"; + + fetched_trajectories = cache->fetch_all_matching_cartesian_trajectories(*move_group, g_robot_name, + is_diff_cartesian_plan_req, fraction, 0, 0); + + fetched_traj = cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, is_diff_cartesian_plan_req, + fraction, 0, 0); + + check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + + check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, prefix, + "Fetched trajectory has correct execution time"); + + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + "Fetched trajectory has correct planning time"); + + check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, prefix, "Fetched trajectory has correct fraction"); + + // Fetch non-matching, out of tolerance + // + // Non-matching key should not have cache hit + prefix = "test_cartesian_trajectories.put_first.fetch_non_matching_out_of_tolerance"; + + fetched_trajectories = cache->fetch_all_matching_cartesian_trajectories( + *move_group, g_robot_name, close_matching_cartesian_plan_req, fraction, 0, 0); + + fetched_traj = cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, + close_matching_cartesian_plan_req, fraction, 0, 0); + + check_and_emit(fetched_trajectories.size() == 0, prefix, "Fetch all returns empty"); + check_and_emit(fetched_traj == nullptr, prefix, "Fetch best trajectory is nullptr"); + + // Fetch non-matching, only start in tolerance (but not goal) + // + // Non-matching key should not have cache hit + prefix = "test_motion_trajectories.put_first.fetch_non_matching_only_start_in_tolerance"; + + fetched_trajectories = cache->fetch_all_matching_cartesian_trajectories( + *move_group, g_robot_name, close_matching_cartesian_plan_req, fraction, 999, 0); + + fetched_traj = cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, + close_matching_cartesian_plan_req, fraction, 999, 0); + + check_and_emit(fetched_trajectories.size() == 0, prefix, "Fetch all returns empty"); + check_and_emit(fetched_traj == nullptr, prefix, "Fetch best trajectory is nullptr"); + + // Fetch non-matching, only goal in tolerance (but not start) + // + // Non-matching key should not have cache hit + prefix = "test_motion_trajectories.put_first.fetch_non_matching_only_goal_in_tolerance"; + + fetched_trajectories = cache->fetch_all_matching_cartesian_trajectories( + *move_group, g_robot_name, close_matching_cartesian_plan_req, fraction, 0, 999); + + fetched_traj = cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, + close_matching_cartesian_plan_req, fraction, 0, 999); + + check_and_emit(fetched_trajectories.size() == 0, prefix, "Fetch all returns empty"); + check_and_emit(fetched_traj == nullptr, prefix, "Fetch best trajectory is nullptr"); + + // Fetch non-matching, in tolerance + // + // Close key within tolerance limit should have cache hit + prefix = "test_cartesian_trajectories.put_first.fetch_non_matching_in_tolerance"; + + fetched_trajectories = cache->fetch_all_matching_cartesian_trajectories( + *move_group, g_robot_name, close_matching_cartesian_plan_req, fraction, 0.1, 0.1); + + fetched_traj = cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, + close_matching_cartesian_plan_req, fraction, 0.1, 0.1); + + check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + + check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, prefix, + "Fetched trajectory has correct execution time"); + + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + "Fetched trajectory has correct planning time"); + + check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, prefix, "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 + prefix = "test_cartesian_trajectories.put_first.fetch_higher_fraction"; + + fetched_trajectories = + cache->fetch_all_matching_cartesian_trajectories(*move_group, g_robot_name, cartesian_plan_req, 1, 999, 999); + + fetched_traj = + cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, 1, 999, 999); + + check_and_emit(fetched_trajectories.size() == 0, prefix, "Fetch all returns empty"); + check_and_emit(fetched_traj == nullptr, prefix, "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 + prefix = "test_cartesian_trajectories.put_first.fetch_lower_fraction"; + + fetched_trajectories = + cache->fetch_all_matching_cartesian_trajectories(*move_group, g_robot_name, cartesian_plan_req, 0, 999, 999); + + fetched_traj = + cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, 0, 999, 999); + + check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + + check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == execution_time, prefix, + "Fetched trajectory has correct execution time"); + + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + "Fetched trajectory has correct planning time"); + + check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, prefix, "Fetched trajectory has correct fraction"); + + // Put worse, no delete_worse_trajectories + // + // Worse trajectories should not be inserted + prefix = "test_cartesian_trajectories.put_worse"; + double worse_execution_time = execution_time + 100; + + check_and_emit(!cache->put_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, traj, + worse_execution_time, planning_time, fraction, false), + prefix, "Put worse traj, no delete_worse_trajectories, not ok"); + + check_and_emit(cache->count_cartesian_trajectories(g_robot_name) == 1, prefix, "One trajectory in cache"); + + // Put better, no delete_worse_trajectories + // + // Better trajectories should be inserted + prefix = "test_cartesian_trajectories.put_better"; + double better_execution_time = execution_time - 100; + + check_and_emit(cache->put_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, traj, + better_execution_time, planning_time, fraction, false), + prefix, "Put better traj, no delete_worse_trajectories, ok"); + + check_and_emit(cache->count_cartesian_trajectories(g_robot_name) == 2, prefix, "Two trajectories in cache"); + + // Fetch sorted + // + // With multiple trajectories in cache, fetches should be sorted accordingly + prefix = "test_cartesian_trajectories.put_better.fetch_sorted"; + + fetched_trajectories = + cache->fetch_all_matching_cartesian_trajectories(*move_group, g_robot_name, cartesian_plan_req, fraction, 0, 0); + + fetched_traj = + cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, fraction, 0, 0); + + check_and_emit(fetched_trajectories.size() == 2, prefix, "Fetch all returns two"); + check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + + check_and_emit(*fetched_traj == traj, prefix, "Fetched trajectory matches original"); + + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == better_execution_time, prefix, + "Fetched trajectory has correct execution time"); + + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + "Fetched trajectory has correct planning time"); + + check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, prefix, "Fetched trajectory has correct fraction"); + + check_and_emit(fetched_trajectories.at(0)->lookupDouble("execution_time_s") == better_execution_time && + fetched_trajectories.at(1)->lookupDouble("execution_time_s") == execution_time, + prefix, "Fetched trajectories are sorted correctly"); + + // Put better, delete_worse_trajectories + // + // Better, different, trajectories should be inserted + prefix = "test_cartesian_trajectories.put_better_delete_worse_trajectories"; + double even_better_execution_time = better_execution_time - 100; + + check_and_emit(cache->put_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, different_traj, + even_better_execution_time, planning_time, fraction, true), + prefix, "Put better traj, delete_worse_trajectories, ok"); + + check_and_emit(cache->count_cartesian_trajectories(g_robot_name) == 1, prefix, "One trajectory in cache"); + + // Fetch better plan + prefix = "test_cartesian_trajectories.put_better_delete_worse_trajectories.fetch"; + + fetched_trajectories = + cache->fetch_all_matching_cartesian_trajectories(*move_group, g_robot_name, cartesian_plan_req, fraction, 0, 0); + + fetched_traj = + cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, cartesian_plan_req, fraction, 0, 0); + + check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + + check_and_emit(*fetched_traj == different_traj, prefix, "Fetched trajectory matches original"); + + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == even_better_execution_time, prefix, + "Fetched trajectory has correct execution time"); + + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + "Fetched trajectory has correct planning time"); + + check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, prefix, "Fetched trajectory has correct fraction"); + + // Put different req, delete_worse_trajectories + // + // Unrelated trajectory requests should live alongside pre-existing plans + prefix = "test_cartesian_trajectories.put_different_req"; + + check_and_emit(cache->put_cartesian_trajectory(*move_group, g_robot_name, different_cartesian_plan_req, + different_traj, better_execution_time, planning_time, fraction, true), + prefix, "Put different trajectory req, delete_worse_trajectories, ok"); + + check_and_emit(cache->count_cartesian_trajectories(g_robot_name) == 2, prefix, "Two trajectories in cache"); + + // Fetch with different trajectory req + // + // With multiple trajectories in cache, fetches should be sorted accordingly + prefix = "test_cartesian_trajectories.put_different_req.fetch"; + + fetched_trajectories = cache->fetch_all_matching_cartesian_trajectories(*move_group, g_robot_name, + different_cartesian_plan_req, fraction, 0, 0); + + fetched_traj = cache->fetch_best_matching_cartesian_trajectory(*move_group, g_robot_name, + different_cartesian_plan_req, fraction, 0, 0); + + check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all returns one"); + check_and_emit(fetched_traj != nullptr, prefix, "Fetch best trajectory is not nullptr"); + + check_and_emit(*fetched_trajectories.at(0) == *fetched_traj, prefix, "Fetched trajectory on both fetches match"); + + check_and_emit(*fetched_traj == different_traj, prefix, "Fetched trajectory matches original"); + + check_and_emit(fetched_traj->lookupDouble("execution_time_s") == better_execution_time, prefix, + "Fetched trajectory has correct execution time"); + + check_and_emit(fetched_traj->lookupDouble("planning_time_s") == planning_time, prefix, + "Fetched trajectory has correct planning time"); + + check_and_emit(fetched_traj->lookupDouble("fraction") == fraction, prefix, "Fetched trajectory has correct fraction"); + + // Fetch different robot + // + // Since we didn't populate anything, we should expect empty + prefix = "test_cartesian_trajectories.different_robot.empty"; + std::string different_robot_name = "different_robot"; + + check_and_emit(cache->count_cartesian_trajectories(different_robot_name) == 0, prefix, "No trajectories in cache"); + + // Put first for different robot, delete_worse_trajectories + // + // A different robot's cache should not interact with the original cache + prefix = "test_cartesian_trajectories.different_robot.put_first"; + check_and_emit(cache->put_cartesian_trajectory(*move_group, different_robot_name, different_cartesian_plan_req, + different_traj, better_execution_time, planning_time, fraction, true), + prefix, "Put different trajectory req, delete_worse_trajectories, ok"); + + check_and_emit(cache->count_cartesian_trajectories(different_robot_name) == 1, prefix, "One trajectories in cache"); + + check_and_emit(cache->count_cartesian_trajectories(different_robot_name) == 1, prefix, "One trajectories in cache"); + + check_and_emit(cache->count_cartesian_trajectories(g_robot_name) == 2, prefix, + "Two trajectories in original robot's cache"); + + fetched_trajectories = cache->fetch_all_matching_cartesian_trajectories(*move_group, g_robot_name, + different_cartesian_plan_req, fraction, 0, 0); + + check_and_emit(fetched_trajectories.size() == 1, prefix, "Fetch all on original still returns one"); +} + +int main(int argc, char** argv) +{ + // Setup + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + + auto test_node = rclcpp::Node::make_shared("test_node", node_options); + std::thread spin_thread([&]() { + while (rclcpp::ok()) + { + rclcpp::spin_some(test_node); + } + }); + + // This is necessary + test_node->declare_parameter("warehouse_plugin", "warehouse_ros_sqlite::DatabaseConnection"); + + // Test proper + { + auto cache = std::make_shared(test_node); + check_and_emit(cache->init(":memory:", 0, 1e-4), "init", "Cache init"); + + auto move_group = std::make_shared(test_node, "panda_arm"); + + auto curr_state = move_group->getCurrentState(10); + move_group->setStartState(*curr_state); + + test_motion_trajectories(move_group, cache); + test_cartesian_trajectories(move_group, cache); + } + + rclcpp::shutdown(); + spin_thread.join(); + + return 0; +} 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 00000000000..9127604c218 --- /dev/null +++ b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp @@ -0,0 +1,1340 @@ +// Copyright 2022 Johnson & Johnson +// +// 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. + +#include +#include + +#include "moveit/robot_state/conversions.h" +#include "moveit/robot_state/robot_state.h" +#include "moveit/warehouse/moveit_message_storage.h" +#include "warehouse_ros/message_collection.h" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/logging.hpp" +#include "trajectory_cache.hpp" + +namespace moveit_ros +{ +namespace trajectory_cache +{ + +using warehouse_ros::MessageWithMetadata; +using warehouse_ros::Metadata; +using warehouse_ros::Query; + +// Utils ======================================================================= + +// Append a range inclusive query with some tolerance about some center value. +void query_append_range_inclusive_with_tolerance(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 sort_constraints(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) +{ + tf_buffer_ = std::make_unique(node_->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); +} + +bool TrajectoryCache::init(const std::string& db_path, uint32_t db_port, double exact_match_precision) +{ + RCLCPP_INFO(node_->get_logger(), "Opening trajectory cache database at: %s (Port: %d, Precision: %f)", + db_path.c_str(), db_port, exact_match_precision); + + // If the `warehouse_plugin` parameter isn't set, defaults to warehouse_ros' + // default. + db_ = moveit_warehouse::loadDatabase(node_); + + exact_match_precision_ = exact_match_precision; + db_->setParams(db_path, db_port); + return db_->connect(); +} + +unsigned TrajectoryCache::count_trajectories(const std::string& move_group_namespace) +{ + auto coll = + db_->openCollection("move_group_trajectory_cache", move_group_namespace); + return coll.count(); +} + +unsigned TrajectoryCache::count_cartesian_trajectories(const std::string& move_group_namespace) +{ + auto coll = db_->openCollection("move_group_cartesian_trajectory_cache", + move_group_namespace); + return coll.count(); +} + +// ============================================================================= +// MOTION PLAN TRAJECTORY CACHING +// ============================================================================= +// MOTION PLAN TRAJECTORY CACHING: TOP LEVEL OPS +std::vector::ConstPtr> +TrajectoryCache::fetch_all_matching_trajectories(const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_namespace, + const moveit_msgs::msg::MotionPlanRequest& plan_request, + double start_tolerance, double goal_tolerance, bool metadata_only, + const std::string& sort_by) +{ + auto coll = + db_->openCollection("move_group_trajectory_cache", move_group_namespace); + + Query::Ptr query = coll.createQuery(); + + bool start_ok = this->extract_and_append_trajectory_start_to_query(*query, move_group, plan_request, start_tolerance); + bool goal_ok = this->extract_and_append_trajectory_goal_to_query(*query, move_group, plan_request, goal_tolerance); + + if (!start_ok || !goal_ok) + { + RCLCPP_ERROR(node_->get_logger(), "Could not construct trajectory query."); + return {}; + } + + return coll.queryList(query, metadata_only, sort_by, true); +} + +MessageWithMetadata::ConstPtr TrajectoryCache::fetch_best_matching_trajectory( + const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& move_group_namespace, + const moveit_msgs::msg::MotionPlanRequest& plan_request, double start_tolerance, double goal_tolerance, + bool metadata_only, const std::string& sort_by) +{ + // 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->fetch_all_matching_trajectories(move_group, move_group_namespace, plan_request, + start_tolerance, goal_tolerance, true, sort_by); + + if (matching_trajectories.empty()) + { + RCLCPP_INFO(node_->get_logger(), "No matching trajectories found."); + return nullptr; + } + + auto coll = + db_->openCollection("move_group_trajectory_cache", move_group_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::put_trajectory(const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_namespace, + const moveit_msgs::msg::MotionPlanRequest& plan_request, + const moveit_msgs::msg::RobotTrajectory& trajectory, double execution_time_s, + double planning_time_s, bool delete_worse_trajectories) +{ + // Check pre-conditions + if (!trajectory.multi_dof_joint_trajectory.points.empty()) + { + RCLCPP_ERROR(node_->get_logger(), "Skipping plan insert: Multi-DOF trajectory plans are not supported."); + return false; + } + if (plan_request.workspace_parameters.header.frame_id.empty() || trajectory.joint_trajectory.header.frame_id.empty()) + { + RCLCPP_ERROR(node_->get_logger(), "Skipping plan insert: Frame IDs cannot be empty."); + return false; + } + if (plan_request.workspace_parameters.header.frame_id != trajectory.joint_trajectory.header.frame_id) + { + RCLCPP_ERROR(node_->get_logger(), + "Skipping plan insert: " + "Plan request frame (%s) does not match plan frame (%s).", + plan_request.workspace_parameters.header.frame_id.c_str(), + trajectory.joint_trajectory.header.frame_id.c_str()); + return false; + } + + auto coll = + db_->openCollection("move_group_trajectory_cache", move_group_namespace); + + // Pull out trajectories "exactly" keyed by request in cache. + Query::Ptr exact_query = coll.createQuery(); + + bool start_query_ok = this->extract_and_append_trajectory_start_to_query(*exact_query, move_group, plan_request, 0); + bool goal_query_ok = this->extract_and_append_trajectory_goal_to_query(*exact_query, move_group, plan_request, 0); + + if (!start_query_ok || !goal_query_ok) + { + RCLCPP_ERROR(node_->get_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"); + + double best_execution_time = std::numeric_limits::infinity(); + if (!exact_matches.empty()) + { + best_execution_time = exact_matches.at(0)->lookupDouble("execution_time_s"); + + if (delete_worse_trajectories) + { + for (auto& match : exact_matches) + { + double match_execution_time_s = match->lookupDouble("execution_time_s"); + if (execution_time_s < match_execution_time_s) + { + int delete_id = match->lookupInt("id"); + RCLCPP_INFO(node_->get_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->extract_and_append_trajectory_start_to_metadata(*insert_metadata, move_group, plan_request); + bool goal_meta_ok = + this->extract_and_append_trajectory_goal_to_metadata(*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(node_->get_logger(), "Skipping plan insert: Could not construct insert metadata."); + return false; + } + + RCLCPP_INFO(node_->get_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_INFO(node_->get_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; +} + +// MOTION PLAN TRAJECTORY CACHING: QUERY CONSTRUCTION +bool TrajectoryCache::extract_and_append_trajectory_start_to_query( + Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request, double match_tolerance) +{ + match_tolerance += exact_match_precision_; + + // Make ignored members explicit + if (!plan_request.start_state.multi_dof_joint_state.joint_names.empty()) + { + RCLCPP_WARN(node_->get_logger(), "Ignoring start_state.multi_dof_joint_states: Not supported."); + } + if (!plan_request.start_state.attached_collision_objects.empty()) + { + RCLCPP_WARN(node_->get_logger(), "Ignoring start_state.attached_collision_objects: Not supported."); + } + + // auto original = *query; // Copy 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", plan_request.workspace_parameters.header.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(node_->get_logger(), "Skipping start query append: Could not get robot state."); + // *query = original; // Undo our changes. (Can't. Copy 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)); + query_append_range_inclusive_with_tolerance(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)); + query_append_range_inclusive_with_tolerance(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::extract_and_append_trajectory_goal_to_query( + Query& query, const moveit::planning_interface::MoveGroupInterface& /* move_group */, + const moveit_msgs::msg::MotionPlanRequest& plan_request, double match_tolerance) +{ + match_tolerance += 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(node_->get_logger(), "Ignoring goal_constraints.position_constraints.constraint_region: " + "Not supported."); + } + + // auto original = *query; // Copy not supported. + + query_append_range_inclusive_with_tolerance(query, "max_velocity_scaling_factor", + plan_request.max_velocity_scaling_factor, match_tolerance); + query_append_range_inclusive_with_tolerance(query, "max_acceleration_scaling_factor", + plan_request.max_acceleration_scaling_factor, match_tolerance); + query_append_range_inclusive_with_tolerance(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. + sort_constraints(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); + query_append_range_inclusive_with_tolerance(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", + plan_request.workspace_parameters.header.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 (plan_request.workspace_parameters.header.frame_id != constraint.header.frame_id) + { + try + { + auto transform = tf_buffer_->lookupTransform( + constraint.header.frame_id, plan_request.workspace_parameters.header.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(node_->get_logger(), + "Skipping goal query append: " + "Could not get goal transform for translation %s to %s: %s", + plan_request.workspace_parameters.header.frame_id.c_str(), constraint.header.frame_id.c_str(), + ex.what()); + + // (Can't. Copy not supported.) + // *query = original; // Undo our changes. + return false; + } + } + + query.append(meta_name + ".link_name", constraint.link_name); + + query_append_range_inclusive_with_tolerance(query, meta_name + ".target_point_offset.x", + x_offset + constraint.target_point_offset.x, match_tolerance); + query_append_range_inclusive_with_tolerance(query, meta_name + ".target_point_offset.y", + y_offset + constraint.target_point_offset.y, match_tolerance); + query_append_range_inclusive_with_tolerance(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", + plan_request.workspace_parameters.header.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 (plan_request.workspace_parameters.header.frame_id != constraint.header.frame_id) + { + try + { + auto transform = tf_buffer_->lookupTransform( + constraint.header.frame_id, plan_request.workspace_parameters.header.frame_id, tf2::TimePointZero); + + quat_offset = transform.transform.rotation; + } + catch (tf2::TransformException& ex) + { + RCLCPP_WARN(node_->get_logger(), + "Skipping goal query append: " + "Could not get goal transform for orientation %s to %s: %s", + plan_request.workspace_parameters.header.frame_id.c_str(), constraint.header.frame_id.c_str(), + ex.what()); + + // (Can't. Copy not supported.) + // *query = original; // Undo our changes. + 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(); + + query_append_range_inclusive_with_tolerance(query, meta_name + ".target_point_offset.x", final_quat.getX(), + match_tolerance); + query_append_range_inclusive_with_tolerance(query, meta_name + ".target_point_offset.y", final_quat.getY(), + match_tolerance); + query_append_range_inclusive_with_tolerance(query, meta_name + ".target_point_offset.z", final_quat.getZ(), + match_tolerance); + query_append_range_inclusive_with_tolerance(query, meta_name + ".target_point_offset.w", final_quat.getW(), + match_tolerance); + } + } + + return true; +} + +// MOTION PLAN TRAJECTORY CACHING: METADATA CONSTRUCTION +bool TrajectoryCache::extract_and_append_trajectory_start_to_metadata( + Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request) +{ + // Make ignored members explicit + if (!plan_request.start_state.multi_dof_joint_state.joint_names.empty()) + { + RCLCPP_WARN(node_->get_logger(), "Ignoring start_state.multi_dof_joint_states: Not supported."); + } + if (!plan_request.start_state.attached_collision_objects.empty()) + { + RCLCPP_WARN(node_->get_logger(), "Ignoring start_state.attached_collision_objects: Not supported."); + } + + // auto original = *metadata; // Copy not supported. + + metadata.append("group_name", plan_request.group_name); + + // Workspace params + metadata.append("workspace_parameters.header.frame_id", plan_request.workspace_parameters.header.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(node_->get_logger(), "Skipping start metadata append: Could not get robot state."); + // *metadata = original; // Undo our changes. // Copy 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::extract_and_append_trajectory_goal_to_metadata( + Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& /* move_group */, + const moveit_msgs::msg::MotionPlanRequest& plan_request) +{ + // 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(node_->get_logger(), "Ignoring goal_constraints.position_constraints.constraint_region: " + "Not supported."); + } + + // auto original = *metadata; // Copy 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. + sort_constraints(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", + plan_request.workspace_parameters.header.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 (plan_request.workspace_parameters.header.frame_id != constraint.header.frame_id) + { + try + { + auto transform = tf_buffer_->lookupTransform( + constraint.header.frame_id, plan_request.workspace_parameters.header.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(node_->get_logger(), + "Skipping goal metadata append: " + "Could not get goal transform for translation %s to %s: %s", + plan_request.workspace_parameters.header.frame_id.c_str(), constraint.header.frame_id.c_str(), + ex.what()); + + // (Can't. Copy not supported.) + // *metadata = original; // Undo our changes. + 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", + plan_request.workspace_parameters.header.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 (plan_request.workspace_parameters.header.frame_id != constraint.header.frame_id) + { + try + { + auto transform = tf_buffer_->lookupTransform( + constraint.header.frame_id, plan_request.workspace_parameters.header.frame_id, tf2::TimePointZero); + + quat_offset = transform.transform.rotation; + } + catch (tf2::TransformException& ex) + { + RCLCPP_WARN(node_->get_logger(), + "Skipping goal metadata append: " + "Could not get goal transform for orientation %s to %s: %s", + plan_request.workspace_parameters.header.frame_id.c_str(), constraint.header.frame_id.c_str(), + ex.what()); + + // (Can't. Copy not supported.) + // *metadata = original; // Undo our changes. + 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 CACHING +// ============================================================================= +// CARTESIAN TRAJECTORY CACHING: TOP LEVEL OPS +moveit_msgs::srv::GetCartesianPath::Request +TrajectoryCache::construct_get_cartesian_path_request(moveit::planning_interface::MoveGroupInterface& move_group, + const std::vector& waypoints, + double step, double jump_threshold, bool avoid_collisions) +{ + moveit_msgs::srv::GetCartesianPath::Request out; + + // Some of these parameters need us to pull PRIVATE values out of the + // move_group elsewhere... Yes, it is very cursed and I hate it. + // Fixing it requires fixing it in MoveIt. + moveit_msgs::msg::MotionPlanRequest tmp; + move_group.constructMotionPlanRequest(tmp); + + out.start_state = std::move(tmp.start_state); + out.group_name = std::move(tmp.group_name); + out.max_velocity_scaling_factor = tmp.max_velocity_scaling_factor; + out.max_acceleration_scaling_factor = tmp.max_acceleration_scaling_factor; + + out.header.frame_id = move_group.getPoseReferenceFrame(); + out.waypoints = waypoints; + out.max_step = step; + out.jump_threshold = jump_threshold; + out.path_constraints = moveit_msgs::msg::Constraints(); + out.avoid_collisions = avoid_collisions; + out.link_name = move_group.getEndEffectorLink(); + + // We were already cursed before, now we are double cursed... + // move_group.getNodeHandle() is UNIMPLEMENTED upstream. + out.header.stamp = node_->now(); + + return out; +} + +std::vector::ConstPtr> +TrajectoryCache::fetch_all_matching_cartesian_trajectories( + const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& move_group_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) +{ + auto coll = db_->openCollection("move_group_cartesian_trajectory_cache", + move_group_namespace); + + Query::Ptr query = coll.createQuery(); + + bool start_ok = + this->extract_and_append_cartesian_trajectory_start_to_query(*query, move_group, plan_request, start_tolerance); + bool goal_ok = + this->extract_and_append_cartesian_trajectory_goal_to_query(*query, move_group, plan_request, goal_tolerance); + + if (!start_ok || !goal_ok) + { + RCLCPP_ERROR(node_->get_logger(), "Could not construct cartesian trajectory query."); + return {}; + } + + query->appendGTE("fraction", min_fraction); + return coll.queryList(query, metadata_only, sort_by, true); +} + +MessageWithMetadata::ConstPtr +TrajectoryCache::fetch_best_matching_cartesian_trajectory( + const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& move_group_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) +{ + // 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->fetch_all_matching_cartesian_trajectories( + move_group, move_group_namespace, plan_request, min_fraction, start_tolerance, goal_tolerance, true, sort_by); + + if (matching_trajectories.empty()) + { + RCLCPP_INFO(node_->get_logger(), "No matching cartesian trajectories found."); + return nullptr; + } + + auto coll = db_->openCollection("move_group_cartesian_trajectory_cache", + move_group_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::put_cartesian_trajectory(const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_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 delete_worse_trajectories) +{ + // Check pre-conditions + if (!trajectory.multi_dof_joint_trajectory.points.empty()) + { + RCLCPP_ERROR(node_->get_logger(), "Skipping cartesian trajectory insert: " + "Multi-DOF trajectory plans are not supported."); + return false; + } + if (plan_request.header.frame_id.empty() || trajectory.joint_trajectory.header.frame_id.empty()) + { + RCLCPP_ERROR(node_->get_logger(), "Skipping cartesian trajectory insert: Frame IDs cannot be empty."); + return false; + } + + auto coll = db_->openCollection("move_group_cartesian_trajectory_cache", + move_group_namespace); + + // Pull out trajectories "exactly" keyed by request in cache. + Query::Ptr exact_query = coll.createQuery(); + + bool start_query_ok = + this->extract_and_append_cartesian_trajectory_start_to_query(*exact_query, move_group, plan_request, 0); + bool goal_query_ok = + this->extract_and_append_cartesian_trajectory_goal_to_query(*exact_query, move_group, plan_request, 0); + exact_query->append("fraction", fraction); + + if (!start_query_ok || !goal_query_ok) + { + RCLCPP_ERROR(node_->get_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"); + double best_execution_time = std::numeric_limits::infinity(); + if (!exact_matches.empty()) + { + best_execution_time = exact_matches.at(0)->lookupDouble("execution_time_s"); + + if (delete_worse_trajectories) + { + for (auto& match : exact_matches) + { + double match_execution_time_s = match->lookupDouble("execution_time_s"); + if (execution_time_s < match_execution_time_s) + { + int delete_id = match->lookupInt("id"); + RCLCPP_INFO(node_->get_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->extract_and_append_cartesian_trajectory_start_to_metadata(*insert_metadata, move_group, plan_request); + bool goal_meta_ok = + this->extract_and_append_cartesian_trajectory_goal_to_metadata(*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(node_->get_logger(), "Skipping cartesian trajectory insert: " + "Could not construct insert metadata."); + return false; + } + + RCLCPP_INFO(node_->get_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_INFO(node_->get_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; +} + +// CARTESIAN TRAJECTORY CACHING: QUERY CONSTRUCTION +bool TrajectoryCache::extract_and_append_cartesian_trajectory_start_to_query( + Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double match_tolerance) +{ + match_tolerance += exact_match_precision_; + + // Make ignored members explicit + if (!plan_request.start_state.multi_dof_joint_state.joint_names.empty()) + { + RCLCPP_WARN(node_->get_logger(), "Ignoring start_state.multi_dof_joint_states: Not supported."); + } + if (!plan_request.start_state.attached_collision_objects.empty()) + { + RCLCPP_WARN(node_->get_logger(), "Ignoring start_state.attached_collision_objects: Not supported."); + } + + // auto original = *metadata; // Copy not supported. + + query.append("group_name", plan_request.group_name); + + // 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(node_->get_logger(), "Skipping start metadata append: Could not get robot state."); + // *metadata = original; // Undo our changes. // Copy 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)); + query_append_range_inclusive_with_tolerance(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)); + query_append_range_inclusive_with_tolerance(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::extract_and_append_cartesian_trajectory_goal_to_query( + Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double match_tolerance) +{ + match_tolerance += 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(node_->get_logger(), "Ignoring path_constraints: Not supported."); + } + if (plan_request.avoid_collisions) + { + RCLCPP_WARN(node_->get_logger(), "Ignoring avoid_collisions: Not supported."); + } + + // auto original = *metadata; // Copy not supported. + + query_append_range_inclusive_with_tolerance(query, "max_velocity_scaling_factor", + plan_request.max_velocity_scaling_factor, match_tolerance); + query_append_range_inclusive_with_tolerance(query, "max_acceleration_scaling_factor", + plan_request.max_acceleration_scaling_factor, match_tolerance); + query_append_range_inclusive_with_tolerance(query, "max_step", plan_request.max_step, match_tolerance); + query_append_range_inclusive_with_tolerance(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 != plan_request.header.frame_id) + { + try + { + auto transform = tf_buffer_->lookupTransform(plan_request.header.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(node_->get_logger(), + "Skipping goal metadata append: " + "Could not get goal transform for %s to %s: %s", + base_frame.c_str(), plan_request.header.frame_id.c_str(), ex.what()); + + // (Can't. Copy not supported.) + // *metadata = original; // Undo our changes. + 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 + query_append_range_inclusive_with_tolerance(query, meta_name + ".position.x", x_offset + waypoint.position.x, + match_tolerance); + query_append_range_inclusive_with_tolerance(query, meta_name + ".position.y", y_offset + waypoint.position.y, + match_tolerance); + query_append_range_inclusive_with_tolerance(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(); + + query_append_range_inclusive_with_tolerance(query, meta_name + ".orientation.x", final_quat.getX(), match_tolerance); + query_append_range_inclusive_with_tolerance(query, meta_name + ".orientation.y", final_quat.getY(), match_tolerance); + query_append_range_inclusive_with_tolerance(query, meta_name + ".orientation.z", final_quat.getZ(), match_tolerance); + query_append_range_inclusive_with_tolerance(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::extract_and_append_cartesian_trajectory_start_to_metadata( + Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request) +{ + // Make ignored members explicit + if (!plan_request.start_state.multi_dof_joint_state.joint_names.empty()) + { + RCLCPP_WARN(node_->get_logger(), "Ignoring start_state.multi_dof_joint_states: Not supported."); + } + if (!plan_request.start_state.attached_collision_objects.empty()) + { + RCLCPP_WARN(node_->get_logger(), "Ignoring start_state.attached_collision_objects: Not supported."); + } + + // auto original = *metadata; // Copy not supported. + + metadata.append("group_name", plan_request.group_name); + + // 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(node_->get_logger(), "Skipping start metadata append: Could not get robot state."); + // *metadata = original; // Undo our changes. // Copy 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::extract_and_append_cartesian_trajectory_goal_to_metadata( + Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& 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(node_->get_logger(), "Ignoring path_constraints: Not supported."); + } + if (plan_request.avoid_collisions) + { + RCLCPP_WARN(node_->get_logger(), "Ignoring avoid_collisions: Not supported."); + } + + // auto original = *metadata; // Copy 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 != plan_request.header.frame_id) + { + try + { + auto transform = tf_buffer_->lookupTransform(plan_request.header.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(node_->get_logger(), + "Skipping goal metadata append: " + "Could not get goal transform for %s to %s: %s", + base_frame.c_str(), plan_request.header.frame_id.c_str(), ex.what()); + + // (Can't. Copy not supported.) + // *metadata = original; // Undo our changes. + 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/src/trajectory_cache.hpp b/moveit_ros/trajectory_cache/src/trajectory_cache.hpp new file mode 100644 index 00000000000..a1aab3410b5 --- /dev/null +++ b/moveit_ros/trajectory_cache/src/trajectory_cache.hpp @@ -0,0 +1,261 @@ +// Copyright 2022 Johnson & Johnson +// +// 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. + +#ifndef SRC__TRAJECTORY_CACHE_HPP +#define SRC__TRAJECTORY_CACHE_HPP + +#include +#include +#include + +#include + +#include +#include + +// TF2 +#include +#include + +// ROS2 Messages +#include +#include + +// moveit modules +#include +#include + +namespace moveit_ros +{ +namespace trajectory_cache +{ + +/** + * 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 + * - `warehouse_host`: Where the database should be created + * - `warehouse_port`: The port used for the database + * + * 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: + // We explicitly need a Node::SharedPtr because warehouse_ros ONLY supports + // it... + // + // TODO: methylDragon - + // Use rclcpp::node_interfaces::NodeInterfaces<> once warehouse_ros does. + explicit TrajectoryCache(const rclcpp::Node::SharedPtr& node); + + bool init(const std::string& db_path = ":memory:", uint32_t db_port = 0, double exact_match_precision = 1e-6); + + unsigned count_trajectories(const std::string& move_group_namespace); + + unsigned count_cartesian_trajectories(const std::string& move_group_namespace); + + // =========================================================================== + // MOTION PLAN TRAJECTORY CACHING + // =========================================================================== + // TOP LEVEL OPS + + // Fetches all plans that fit within the requested tolerances for start and + // goal conditions, returning them as a vector, sorted by some db column. + std::vector::ConstPtr> + fetch_all_matching_trajectories(const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_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"); + + // Fetches the best trajectory that fits within the requested tolerances for start + // and goal conditions, by some db column. + warehouse_ros::MessageWithMetadata::ConstPtr fetch_best_matching_trajectory( + const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& move_group_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"); + + // Put 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 have a smaller planned execution time + // than matching trajectories. + // + // Optionally deletes all worse trajectories by default to prune the cache. + bool put_trajectory(const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, + const moveit_msgs::msg::RobotTrajectory& trajectory, double execution_time_s, + double planning_time_s, bool delete_worse_trajectories = true); + + // QUERY CONSTRUCTION + bool extract_and_append_trajectory_start_to_query(warehouse_ros::Query& query, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request, + double match_tolerance); + + bool extract_and_append_trajectory_goal_to_query(warehouse_ros::Query& query, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request, + double match_tolerance); + + // METADATA CONSTRUCTION + bool extract_and_append_trajectory_start_to_metadata(warehouse_ros::Metadata& metadata, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request); + + bool extract_and_append_trajectory_goal_to_metadata(warehouse_ros::Metadata& metadata, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request); + + // =========================================================================== + // CARTESIAN TRAJECTORY CACHING + // =========================================================================== + // TOP LEVEL OPS + + // This mimics the move group computeCartesianPath signature (without path + // constraints). + moveit_msgs::srv::GetCartesianPath::Request + construct_get_cartesian_path_request(moveit::planning_interface::MoveGroupInterface& move_group, + const std::vector& waypoints, double step, + double jump_threshold, bool avoid_collisions = true); + + // Fetches all cartesian trajectories that fit within the requested tolerances + // for start and goal conditions, returning them as a vector, sorted by some + // db column. + std::vector::ConstPtr> + fetch_all_matching_cartesian_trajectories(const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_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"); + + // Fetches the best cartesian trajectory that fits within the requested tolerances + // for start and goal conditions, by some db column. + warehouse_ros::MessageWithMetadata::ConstPtr + fetch_best_matching_cartesian_trajectory(const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_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"); + + // Put 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 have a smaller planned execution time + // than matching cartesian trajectories. + // + // Optionally deletes all worse cartesian trajectories by default to prune the + // cache. + bool put_cartesian_trajectory(const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_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 delete_worse_trajectories = true); + + // QUERY CONSTRUCTION + bool extract_and_append_cartesian_trajectory_start_to_query( + warehouse_ros::Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double match_tolerance); + + bool extract_and_append_cartesian_trajectory_goal_to_query( + warehouse_ros::Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double match_tolerance); + + // METADATA CONSTRUCTION + bool extract_and_append_cartesian_trajectory_start_to_metadata( + warehouse_ros::Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request); + + bool extract_and_append_cartesian_trajectory_goal_to_metadata( + warehouse_ros::Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request); + +private: + rclcpp::Node::SharedPtr node_; + warehouse_ros::DatabaseConnection::Ptr db_; + + double exact_match_precision_ = 1e-6; + + std::unique_ptr tf_buffer_; + std::shared_ptr tf_listener_; +}; + +} // namespace trajectory_cache +} // namespace moveit_ros + +#endif // SRC__TRAJECTORY_CACHE_HPP 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 00000000000..81e599dd649 --- /dev/null +++ b/moveit_ros/trajectory_cache/test/test_trajectory_cache.py @@ -0,0 +1,148 @@ +# Copyright 2023 Johnson & Johnson +# +# 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. + +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 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 + ) + + # Wait for process to end and check for graceful exit + yield + assert trajectory_cache_test_runner_node.return_code == 0