From 15e7370a6d24e9f000462c83200079dfdfd8bf58 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Thu, 21 Sep 2023 15:28:11 -0700 Subject: [PATCH 01/34] Add motion plan cache Signed-off-by: methylDragon --- nexus_motion_planner/CMakeLists.txt | 4 +- .../src/motion_plan_cache.cpp | 918 ++++++++++++++++++ .../src/motion_plan_cache.hpp | 137 +++ .../src/motion_planner_server.cpp | 167 +++- .../src/motion_planner_server.hpp | 19 + 5 files changed, 1238 insertions(+), 7 deletions(-) create mode 100644 nexus_motion_planner/src/motion_plan_cache.cpp create mode 100644 nexus_motion_planner/src/motion_plan_cache.hpp diff --git a/nexus_motion_planner/CMakeLists.txt b/nexus_motion_planner/CMakeLists.txt index 2ed33c7..e9003bf 100644 --- a/nexus_motion_planner/CMakeLists.txt +++ b/nexus_motion_planner/CMakeLists.txt @@ -17,6 +17,7 @@ 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) @@ -28,6 +29,7 @@ set (motion_planner_server_dependencies rclcpp_action rclcpp_lifecycle warehouse_ros + warehouse_ros_sqlite tf2 tf2_ros ) @@ -45,7 +47,7 @@ set(LIBRARY_NAME motion_planner_server_core) set(EXECUTABLE_NAME motion_planner_server) # Server executable -add_executable(${EXECUTABLE_NAME} src/main.cpp src/motion_planner_server.cpp) +add_executable(${EXECUTABLE_NAME} src/main.cpp src/motion_planner_server.cpp src/motion_plan_cache.cpp) ament_target_dependencies(${EXECUTABLE_NAME} ${motion_planner_server_dependencies}) # Test executable diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp new file mode 100644 index 0000000..3723205 --- /dev/null +++ b/nexus_motion_planner/src/motion_plan_cache.cpp @@ -0,0 +1,918 @@ +#include +#include + +#include "moveit/robot_state/conversions.h" +#include "moveit/robot_state/robot_state.h" +#include "warehouse_ros/database_loader.h" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/logging.hpp" +#include "motion_plan_cache.hpp" + +namespace nexus { +namespace motion_planner { + +#define NEXUS_MATCH_RANGE(arg, tolerance) \ + arg - tolerance / 2, arg + tolerance / 2 + +using warehouse_ros::MessageWithMetadata; +using warehouse_ros::Metadata; +using warehouse_ros::Query; + +MotionPlanCache::MotionPlanCache(const rclcpp::Node::SharedPtr& node) +: node_(node) +{ + // TODO: methyldragon - + // Declare or pass in these in the motion_planner_server instead? + if (!node_->has_parameter("warehouse_plugin")) + { + node_->declare_parameter( + "warehouse_plugin", "warehouse_ros_sqlite::DatabaseConnection"); + } + + tf_buffer_ = + std::make_unique(node_->get_clock()); + tf_listener_ = + std::make_shared(*tf_buffer_); + + warehouse_ros::DatabaseLoader loader(node_); + db_ = loader.loadDatabase(); +} + +void MotionPlanCache::init( + const std::string& db_path, uint32_t db_port, double exact_match_precision) +{ + RCLCPP_INFO( + node_->get_logger(), + "Opening motion plan cache database at: %s (Port: %d, Precision: %f)", + db_path.c_str(), db_port, exact_match_precision); + + exact_match_precision_ = exact_match_precision; + db_->setParams(db_path, db_port); + db_->connect(); +} + +// TOP LEVEL OPS =============================================================== +std::vector::ConstPtr> +MotionPlanCache::fetchAllMatchingPlans( + 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) +{ + auto coll = db_->openCollection( + "move_group_plan_cache", move_group_namespace); + + Query::Ptr query = coll.createQuery(); + + bool start_ok = this->extractAndAppendStartToQuery( + *query, move_group, plan_request, start_tolerance); + bool goal_ok = this->extractAndAppendGoalToQuery( + *query, move_group, plan_request, goal_tolerance); + + if (!start_ok || !goal_ok) + { + RCLCPP_ERROR(node_->get_logger(), "Could not construct plan query."); + return {}; + } + + return coll.queryList( + query, metadata_only, /* sort_by */ "execution_time_s", true); +} + +MessageWithMetadata::ConstPtr +MotionPlanCache::fetchBestMatchingPlan( + 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) +{ + // First find all matching, but metadata only. + // Then use the ID metadata of the best plan to pull the actual message. + auto matching_plans = this->fetchAllMatchingPlans( + move_group, move_group_namespace, + plan_request, start_tolerance, goal_tolerance, + true); + + if (matching_plans.empty()) + { + RCLCPP_INFO(node_->get_logger(), "No matching plans found."); + return nullptr; + } + + auto coll = db_->openCollection( + "move_group_plan_cache", move_group_namespace); + + // Best plan is at first index, since the lookup query was sorted by + // execution_time. + int best_plan_id = matching_plans.at(0)->lookupInt("id"); + Query::Ptr best_query = coll.createQuery(); + best_query->append("id", best_plan_id); + + return coll.findOne(best_query, metadata_only); +} + +bool +MotionPlanCache::putPlan( + 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& plan, + double execution_time_s, + bool overwrite) +{ + // Check pre-conditions + if (!plan.multi_dof_joint_trajectory.points.empty()) + { + RCLCPP_ERROR( + node_->get_logger(), + "Skipping insert: Multi-DOF trajectory plans are not supported."); + return false; + } + if (plan_request.workspace_parameters.header.frame_id.empty() || + plan.joint_trajectory.header.frame_id.empty()) + { + RCLCPP_ERROR( + node_->get_logger(), "Skipping insert: Frame IDs cannot be empty."); + return false; + } + if (plan_request.workspace_parameters.header.frame_id != + plan.joint_trajectory.header.frame_id) + { + RCLCPP_ERROR( + node_->get_logger(), + "Skipping insert: " + "Plan request frame (%s) does not match plan frame (%s).", + plan_request.workspace_parameters.header.frame_id.c_str(), + plan.joint_trajectory.header.frame_id.c_str()); + return false; + } + + auto coll = db_->openCollection( + "move_group_plan_cache", move_group_namespace); + + // If start and goal are "exact" match, AND the candidate plan is better, + // overwrite. + Query::Ptr exact_query = coll.createQuery(); + + bool start_query_ok = this->extractAndAppendStartToQuery( + *exact_query, move_group, + plan_request, exact_match_precision_); + bool goal_query_ok = this->extractAndAppendGoalToQuery( + *exact_query, move_group, + plan_request, exact_match_precision_); + + if (!start_query_ok || !goal_query_ok) + { + RCLCPP_ERROR( + node_->get_logger(), + "Skipping insert: Could not construct overwrite query."); + return false; + } + + auto exact_matches = coll.queryList(exact_query, /* metadata_only */ true); + double best_execution_time_seen = std::numeric_limits::infinity(); + if (!exact_matches.empty()) + { + for (auto& match : exact_matches) + { + double match_execution_time_s = + match->lookupDouble("execution_time_s"); + if (match_execution_time_s < best_execution_time_seen) + { + best_execution_time_seen = match_execution_time_s; + } + + if (match_execution_time_s > execution_time_s) + { + // If we found any "exact" matches that are worse, delete them. + if (overwrite) + { + int delete_id = match->lookupInt("id"); + RCLCPP_INFO( + node_->get_logger(), + "Overwriting plan (id: %d): " + "execution_time (%es) > new plan'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_seen) + { + Metadata::Ptr insert_metadata = coll.createMetadata(); + + bool start_meta_ok = this->extractAndAppendStartToMetadata( + *insert_metadata, move_group, plan_request); + bool goal_meta_ok = this->extractAndAppendGoalToMetadata( + *insert_metadata, move_group, plan_request); + insert_metadata->append("execution_time_s", execution_time_s); + + if (!start_meta_ok || !goal_meta_ok) + { + RCLCPP_ERROR( + node_->get_logger(), + "Skipping insert: Could not construct insert metadata."); + return false; + } + + RCLCPP_INFO( + node_->get_logger(), + "Inserting plan: New plan execution_time (%es) " + "is better than best plan's execution_time (%es)", + execution_time_s, best_execution_time_seen); + + coll.insert(plan, insert_metadata); + return true; + } + + RCLCPP_INFO( + node_->get_logger(), + "Skipping insert: New plan execution_time (%es) " + "is worse than best plan's execution_time (%es)", + execution_time_s, best_execution_time_seen); + return false; +} + +// QUERY CONSTRUCTION ========================================================== +bool +MotionPlanCache::extractAndAppendStartToQuery( + 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 after as well. + 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.appendRangeInclusive( + "start_state.joint_state.position_" + std::to_string(i), + NEXUS_MATCH_RANGE( + 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.appendRangeInclusive( + "start_state.joint_state.position_" + std::to_string(i), + NEXUS_MATCH_RANGE( + plan_request.start_state.joint_state.position.at(i), match_tolerance) + ); + } + } + return true; +} + +bool +MotionPlanCache::extractAndAppendGoalToQuery( + 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.appendRangeInclusive( + "max_velocity_scaling_factor", + NEXUS_MATCH_RANGE( + plan_request.max_velocity_scaling_factor, match_tolerance) + ); + query.appendRangeInclusive( + "max_acceleration_scaling_factor", + NEXUS_MATCH_RANGE( + plan_request.max_acceleration_scaling_factor, match_tolerance) + ); + query.appendRangeInclusive( + "max_cartesian_speed", + NEXUS_MATCH_RANGE(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); + } + } + + // 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.appendRangeInclusive( + meta_name + ".position", + NEXUS_MATCH_RANGE(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.appendRangeInclusive( + meta_name + ".target_point_offset.x", + NEXUS_MATCH_RANGE( + x_offset + constraint.target_point_offset.x, match_tolerance)); + query.appendRangeInclusive( + meta_name + ".target_point_offset.y", + NEXUS_MATCH_RANGE( + y_offset + constraint.target_point_offset.y, match_tolerance)); + query.appendRangeInclusive( + meta_name + ".target_point_offset.z", + NEXUS_MATCH_RANGE( + 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.appendRangeInclusive( + meta_name + ".target_point_offset.x", + NEXUS_MATCH_RANGE(final_quat.getX(), match_tolerance)); + query.appendRangeInclusive( + meta_name + ".target_point_offset.y", + NEXUS_MATCH_RANGE(final_quat.getY(), match_tolerance)); + query.appendRangeInclusive( + meta_name + ".target_point_offset.z", + NEXUS_MATCH_RANGE(final_quat.getZ(), match_tolerance)); + query.appendRangeInclusive( + meta_name + ".target_point_offset.w", + NEXUS_MATCH_RANGE(final_quat.getW(), match_tolerance)); + } + } + + return true; +} + +// METADATA CONSTRUCTION ======================================================= +bool +MotionPlanCache::extractAndAppendStartToMetadata( + 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 after as well. + 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 +MotionPlanCache::extractAndAppendGoalToMetadata( + 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); + } + } + + // 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; +} + +#undef NEXUS_MATCH_RANGE + +} // namespace motion_planner +} // namespace nexus \ No newline at end of file diff --git a/nexus_motion_planner/src/motion_plan_cache.hpp b/nexus_motion_planner/src/motion_plan_cache.hpp new file mode 100644 index 0000000..a865506 --- /dev/null +++ b/nexus_motion_planner/src/motion_plan_cache.hpp @@ -0,0 +1,137 @@ +#ifndef SRC__MOTION_PLAN_CACHE_HPP +#define SRC__MOTION_PLAN_CACHE_HPP + +#include +#include +#include + +#include +#include + +#include + +// TF2 +#include +#include + +// ROS2 Messages +#include +#include +#include + +// moveit modules +#include + +// NEXUS messages +#include + +namespace nexus { +namespace motion_planner { + +/** + * Cache manager for the Nexus motion planner. + * + * This manager facilitates cache management for MoveIt 2's `MoveGroupInterface` + * by using `warehouse_ros` to manage a database of executed motion plans, and + * how long they took to execute. This allows for the lookup and reuse of the + * best performing plans found so far. + * + * 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 + * + * Motion plans are stored in the `move_group_plan_cache` database within the + * database file, with plans 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_plan_cache@my_move_group` + * + * Motion plans are keyed on: + * - Plan Start: robot joint state + * - Plan Goal (either of): + * - Final pose (wrt. `planning_frame` (usually `base_link`)) + * - Final robot joint states + * + * Motion plans may be looked up with some tolerance. + */ +class MotionPlanCache +{ +public: + // We explicitly need a Node::SharedPtr because warehouse_ros ONLY supports + // it... + explicit MotionPlanCache(const rclcpp::Node::SharedPtr& node); + + void init( + const std::string& db_path = ":memory:", + uint32_t db_port = 0, + double exact_match_precision = 0.00001); + + // TOP LEVEL OPS ============================================================= + std::vector< + warehouse_ros::MessageWithMetadata< + moveit_msgs::msg::RobotTrajectory + >::ConstPtr + > + fetchAllMatchingPlans( + 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); + + warehouse_ros::MessageWithMetadata< + moveit_msgs::msg::RobotTrajectory + >::ConstPtr + fetchBestMatchingPlan( + 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); + + bool putPlan( + 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& plan, + double execution_time_s, + bool overwrite = true); + + // QUERY CONSTRUCTION ======================================================== + bool extractAndAppendStartToQuery( + warehouse_ros::Query& query, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request, + double match_tolerance); + + bool extractAndAppendGoalToQuery( + warehouse_ros::Query& query, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request, + double match_tolerance); + + // METADATA CONSTRUCTION ===================================================== + bool extractAndAppendStartToMetadata( + warehouse_ros::Metadata& metadata, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request); + + bool extractAndAppendGoalToMetadata( + warehouse_ros::Metadata& metadata, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request); + +private: + rclcpp::Node::SharedPtr node_; + warehouse_ros::DatabaseConnection::Ptr db_; + + double exact_match_precision_ = 0; + + std::unique_ptr tf_buffer_; + std::shared_ptr tf_listener_; +}; + +} // namespace planning_interface +} // namespace moveit + +#endif // SRC__MOTION_PLAN_CACHE_HPP \ No newline at end of file diff --git a/nexus_motion_planner/src/motion_planner_server.cpp b/nexus_motion_planner/src/motion_planner_server.cpp index 9c53046..07abac6 100644 --- a/nexus_motion_planner/src/motion_planner_server.cpp +++ b/nexus_motion_planner/src/motion_planner_server.cpp @@ -162,6 +162,98 @@ MotionPlannerServer::MotionPlannerServer(const rclcpp::NodeOptions& options) "Setting parameter get_state_wait_seconds to [%.3f]", _get_state_wait_seconds ); + + // Motion plan cache params + _use_motion_plan_cache = this->declare_parameter( + "use_motion_plan_cache", true); + RCLCPP_INFO( + this->get_logger(), + "Setting parameter use_motion_plan_cache to [%s]", + _use_motion_plan_cache ? "True" : "False" + ); + + _only_use_cached_plans = this->declare_parameter( + "only_use_cached_plans", false); + RCLCPP_INFO( + this->get_logger(), + "Setting parameter only_use_cached_plans to [%s]", + _only_use_cached_plans ? "True" : "False" + ); + + _cache_db_plugin = this->declare_parameter( + "cache_db_plugin", "warehouse_ros_sqlite::DatabaseConnection"); + RCLCPP_INFO( + this->get_logger(), + "Setting parameter cache_db_plugin to [%s]", _cache_db_plugin.c_str() + ); + + _cache_db_host = this->declare_parameter("cache_db_host", ":memory:"); + RCLCPP_INFO( + this->get_logger(), + "Setting parameter cache_db_host to [%s]", _cache_db_host.c_str() + ); + + _cache_db_port = this->declare_parameter("cache_db_port", 0); + RCLCPP_INFO( + this->get_logger(), + "Setting parameter cache_db_port to [%d]", _cache_db_port + ); + + _cache_exact_match_tolerance = this->declare_parameter( + "cache_exact_match_tolerance", 0.025); + RCLCPP_INFO( + this->get_logger(), + "Setting parameter cache_exact_match_tolerance to [%.2e]", + _cache_exact_match_tolerance + ); + + _cache_start_match_tolerance = this->declare_parameter( + "cache_start_match_tolerance", 0.025); + RCLCPP_INFO( + this->get_logger(), + "Setting parameter cache_start_match_tolerance to [%.5f]", + _cache_start_match_tolerance + ); + + _cache_goal_match_tolerance = this->declare_parameter( + "cache_goal_match_tolerance", 0.001); + RCLCPP_INFO( + this->get_logger(), + "Setting parameter cache_goal_match_tolerance to [%.5f]", + _cache_goal_match_tolerance + ); + + if (_use_motion_plan_cache) + { + auto cache_node_options = rclcpp::NodeOptions(); + cache_node_options.automatically_declare_parameters_from_overrides(true); + cache_node_options.use_global_arguments(false); + _internal_cache_node = std::make_shared( + "motion_planner_server_internal_cache_node", cache_node_options); + _cache_spin_thread = std::thread( + [this]() + { + while (rclcpp::ok()) + { + rclcpp::spin_some(_internal_cache_node); + } + }); + + // Push warehouse_ros parameters to internal cache node + // This must happen BEFORE the MotionPlanCache is created! + _internal_cache_node->declare_parameter( + "warehouse_plugin", _cache_db_plugin); + + _internal_cache_node->declare_parameter( + "warehouse_host", _cache_db_host); + + _internal_cache_node->declare_parameter( + "warehouse_port", _cache_db_port); + + _motion_plan_cache = + std::make_shared( + _internal_cache_node); + } } //============================================================================== @@ -171,6 +263,11 @@ MotionPlannerServer::~MotionPlannerServer() { _spin_thread.join(); } + + if (_cache_spin_thread.joinable()) + { + _cache_spin_thread.join(); + } } //============================================================================== @@ -178,6 +275,21 @@ auto MotionPlannerServer::on_configure(const LifecycleState& /*state*/) -> CallbackReturn { RCLCPP_INFO(this->get_logger(), "Configuring..."); + if (!_use_motion_plan_cache && _only_use_cached_plans) + { + RCLCPP_ERROR( + this->get_logger(), + "use_motion_plan_cache must be true if only_use_cached_plans is true." + ); + return CallbackReturn::ERROR; + } + + if (_use_motion_plan_cache) + { + _motion_plan_cache->init( + _cache_db_host, _cache_db_port, _cache_exact_match_tolerance); + } + if (_use_move_group_interfaces) { auto ok = initialize_move_group_interfaces(); @@ -461,6 +573,7 @@ void MotionPlannerServer::plan_with_move_group( interface->setMaxVelocityScalingFactor(vel_scale); interface->setMaxAccelerationScalingFactor(acc_scale); moveit::core::MoveItErrorCode error; + moveit_msgs::msg::MotionPlanRequest plan_req_msg; // For caching if (req.cartesian) { std::vector waypoints; @@ -486,24 +599,66 @@ void MotionPlannerServer::plan_with_move_group( else { res->result.error_code.val = 1; - } } else { MoveGroupInterface::Plan plan; - res->result.error_code = interface->plan(plan); + interface->constructMotionPlanRequest(plan_req_msg); + + if (!_only_use_cached_plans) + { + res->result.error_code = interface->plan(plan); + } + else + { + auto fetched_plan = _motion_plan_cache->fetchBestMatchingPlan( + *interface, robot_name, plan_req_msg, + _cache_start_match_tolerance, _cache_goal_match_tolerance); + if (fetched_plan) + { + plan.start_state = plan_req_msg.start_state; + plan.trajectory = *fetched_plan; + plan.planning_time = 0; // ??? + res->result.error_code.val = + moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + } + else + { + RCLCPP_ERROR( + this->get_logger(), + "only_use_cached_plans was true, and could not find cached plan for " + "plan request: \n\n%s", + moveit_msgs::msg::to_yaml(plan_req_msg).c_str()); + res->result.error_code.val = + moveit_msgs::msg::MoveItErrorCodes::FAILURE; + return; + } + } res->result.trajectory_start = std::move(plan.start_state); res->result.trajectory = std::move(plan.trajectory); res->result.planning_time = std::move(plan.planning_time); } if (_execute_trajectory) { - RCLCPP_INFO( - this->get_logger(), - "Executing trajectory!"); + RCLCPP_INFO(this->get_logger(), "Executing trajectory!"); + + auto exec_start = this->now(); // This is a blocking call. - interface->execute(res->result.trajectory); + // interface->execute(res->result.trajectory); + auto exec_end = this->now(); + + if (_use_motion_plan_cache && !req.cartesian) + { + bool plan_put_ok = _motion_plan_cache->putPlan( + *interface, robot_name, + std::move(plan_req_msg), std::move(res->result.trajectory), + (exec_end - exec_start).seconds(), true); + if (!plan_put_ok) + { + RCLCPP_WARN(this->get_logger(), "Did not put plan into cache."); + } + } } return; } diff --git a/nexus_motion_planner/src/motion_planner_server.hpp b/nexus_motion_planner/src/motion_planner_server.hpp index 050929f..5ad8564 100644 --- a/nexus_motion_planner/src/motion_planner_server.hpp +++ b/nexus_motion_planner/src/motion_planner_server.hpp @@ -35,6 +35,9 @@ // NEXUS messages #include +// NEXUS motion plan cache +#include "motion_plan_cache.hpp" + //============================================================================== class MotionPlannerServer : public rclcpp_lifecycle::LifecycleNode { @@ -69,6 +72,10 @@ class MotionPlannerServer : public rclcpp_lifecycle::LifecycleNode rclcpp::Node::SharedPtr _internal_node; std::thread _spin_thread; + rclcpp::Node::SharedPtr _internal_cache_node; + std::thread _cache_spin_thread; + + // MoveIt planning std::vector _manipulators; bool _use_move_group_interfaces; bool _use_namespace; @@ -88,6 +95,18 @@ class MotionPlannerServer : public rclcpp_lifecycle::LifecycleNode double _workspace_max_y; double _workspace_max_z; + // Motion plan caching + std::shared_ptr _motion_plan_cache; + + bool _use_motion_plan_cache; + bool _only_use_cached_plans; + std::string _cache_db_plugin; + std::string _cache_db_host; + int _cache_db_port; + double _cache_exact_match_tolerance; + double _cache_start_match_tolerance; + double _cache_goal_match_tolerance; + rclcpp::Service::SharedPtr _plan_srv; std::unordered_map> From c715b29974b44e91a88b4aae6b1c3e4c613bdb8e Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 25 Sep 2023 17:57:33 -0700 Subject: [PATCH 02/34] Switch to snake case for function names Signed-off-by: methylDragon --- .../src/motion_plan_cache.cpp | 34 +++++++++---------- .../src/motion_plan_cache.hpp | 14 ++++---- .../src/motion_planner_server.cpp | 4 +-- 3 files changed, 26 insertions(+), 26 deletions(-) diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp index 3723205..8c5b372 100644 --- a/nexus_motion_planner/src/motion_plan_cache.cpp +++ b/nexus_motion_planner/src/motion_plan_cache.cpp @@ -54,7 +54,7 @@ void MotionPlanCache::init( // TOP LEVEL OPS =============================================================== std::vector::ConstPtr> -MotionPlanCache::fetchAllMatchingPlans( +MotionPlanCache::fetch_all_matching_plans( const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& move_group_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, @@ -65,9 +65,9 @@ MotionPlanCache::fetchAllMatchingPlans( Query::Ptr query = coll.createQuery(); - bool start_ok = this->extractAndAppendStartToQuery( + bool start_ok = this->extract_and_append_start_to_query( *query, move_group, plan_request, start_tolerance); - bool goal_ok = this->extractAndAppendGoalToQuery( + bool goal_ok = this->extract_and_append_goal_to_query( *query, move_group, plan_request, goal_tolerance); if (!start_ok || !goal_ok) @@ -81,7 +81,7 @@ MotionPlanCache::fetchAllMatchingPlans( } MessageWithMetadata::ConstPtr -MotionPlanCache::fetchBestMatchingPlan( +MotionPlanCache::fetch_best_matching_plan( const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& move_group_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, @@ -89,7 +89,7 @@ MotionPlanCache::fetchBestMatchingPlan( { // First find all matching, but metadata only. // Then use the ID metadata of the best plan to pull the actual message. - auto matching_plans = this->fetchAllMatchingPlans( + auto matching_plans = this->fetch_all_matching_plans( move_group, move_group_namespace, plan_request, start_tolerance, goal_tolerance, true); @@ -113,7 +113,7 @@ MotionPlanCache::fetchBestMatchingPlan( } bool -MotionPlanCache::putPlan( +MotionPlanCache::put_plan( const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& move_group_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, @@ -155,10 +155,10 @@ MotionPlanCache::putPlan( // overwrite. Query::Ptr exact_query = coll.createQuery(); - bool start_query_ok = this->extractAndAppendStartToQuery( + bool start_query_ok = this->extract_and_append_start_to_query( *exact_query, move_group, plan_request, exact_match_precision_); - bool goal_query_ok = this->extractAndAppendGoalToQuery( + bool goal_query_ok = this->extract_and_append_goal_to_query( *exact_query, move_group, plan_request, exact_match_precision_); @@ -208,9 +208,9 @@ MotionPlanCache::putPlan( { Metadata::Ptr insert_metadata = coll.createMetadata(); - bool start_meta_ok = this->extractAndAppendStartToMetadata( + bool start_meta_ok = this->extract_and_append_start_to_metadata( *insert_metadata, move_group, plan_request); - bool goal_meta_ok = this->extractAndAppendGoalToMetadata( + bool goal_meta_ok = this->extract_and_append_goal_to_metadata( *insert_metadata, move_group, plan_request); insert_metadata->append("execution_time_s", execution_time_s); @@ -242,7 +242,7 @@ MotionPlanCache::putPlan( // QUERY CONSTRUCTION ========================================================== bool -MotionPlanCache::extractAndAppendStartToQuery( +MotionPlanCache::extract_and_append_start_to_query( Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::msg::MotionPlanRequest& plan_request, @@ -302,7 +302,7 @@ MotionPlanCache::extractAndAppendStartToQuery( // 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 after as well. + // apply the joint states as offsets as well. moveit::core::RobotStatePtr current_state = move_group.getCurrentState(); if (!current_state) { @@ -348,7 +348,7 @@ MotionPlanCache::extractAndAppendStartToQuery( } bool -MotionPlanCache::extractAndAppendGoalToQuery( +MotionPlanCache::extract_and_append_goal_to_query( Query& query, const moveit::planning_interface::MoveGroupInterface& /* move_group */, const moveit_msgs::msg::MotionPlanRequest& plan_request, @@ -591,7 +591,7 @@ MotionPlanCache::extractAndAppendGoalToQuery( // METADATA CONSTRUCTION ======================================================= bool -MotionPlanCache::extractAndAppendStartToMetadata( +MotionPlanCache::extract_and_append_start_to_metadata( Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::msg::MotionPlanRequest& plan_request) @@ -647,7 +647,7 @@ MotionPlanCache::extractAndAppendStartToMetadata( // 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 after as well. + // apply the joint states as offsets as well. moveit::core::RobotStatePtr current_state = move_group.getCurrentState(); if (!current_state) { @@ -690,7 +690,7 @@ MotionPlanCache::extractAndAppendStartToMetadata( } bool -MotionPlanCache::extractAndAppendGoalToMetadata( +MotionPlanCache::extract_and_append_goal_to_metadata( Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& /* move_group */, const moveit_msgs::msg::MotionPlanRequest& plan_request) @@ -915,4 +915,4 @@ MotionPlanCache::extractAndAppendGoalToMetadata( #undef NEXUS_MATCH_RANGE } // namespace motion_planner -} // namespace nexus \ No newline at end of file +} // namespace nexus diff --git a/nexus_motion_planner/src/motion_plan_cache.hpp b/nexus_motion_planner/src/motion_plan_cache.hpp index a865506..f4c3362 100644 --- a/nexus_motion_planner/src/motion_plan_cache.hpp +++ b/nexus_motion_planner/src/motion_plan_cache.hpp @@ -74,7 +74,7 @@ class MotionPlanCache moveit_msgs::msg::RobotTrajectory >::ConstPtr > - fetchAllMatchingPlans( + fetch_all_matching_plans( const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& move_group_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, @@ -83,13 +83,13 @@ class MotionPlanCache warehouse_ros::MessageWithMetadata< moveit_msgs::msg::RobotTrajectory >::ConstPtr - fetchBestMatchingPlan( + fetch_best_matching_plan( 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); - bool putPlan( + bool put_plan( const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& move_group_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, @@ -98,25 +98,25 @@ class MotionPlanCache bool overwrite = true); // QUERY CONSTRUCTION ======================================================== - bool extractAndAppendStartToQuery( + bool extract_and_append_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 extractAndAppendGoalToQuery( + bool extract_and_append_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 extractAndAppendStartToMetadata( + bool extract_and_append_start_to_metadata( warehouse_ros::Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::msg::MotionPlanRequest& plan_request); - bool extractAndAppendGoalToMetadata( + bool extract_and_append_goal_to_metadata( warehouse_ros::Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::msg::MotionPlanRequest& plan_request); diff --git a/nexus_motion_planner/src/motion_planner_server.cpp b/nexus_motion_planner/src/motion_planner_server.cpp index 07abac6..8baf0d8 100644 --- a/nexus_motion_planner/src/motion_planner_server.cpp +++ b/nexus_motion_planner/src/motion_planner_server.cpp @@ -612,7 +612,7 @@ void MotionPlannerServer::plan_with_move_group( } else { - auto fetched_plan = _motion_plan_cache->fetchBestMatchingPlan( + auto fetched_plan = _motion_plan_cache->fetch_best_matching_plan( *interface, robot_name, plan_req_msg, _cache_start_match_tolerance, _cache_goal_match_tolerance); if (fetched_plan) @@ -650,7 +650,7 @@ void MotionPlannerServer::plan_with_move_group( if (_use_motion_plan_cache && !req.cartesian) { - bool plan_put_ok = _motion_plan_cache->putPlan( + bool plan_put_ok = _motion_plan_cache->put_plan( *interface, robot_name, std::move(plan_req_msg), std::move(res->result.trajectory), (exec_end - exec_start).seconds(), true); From 16784bb0ce685c53512d62186398154d1fbfe113 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 26 Sep 2023 20:06:14 -0700 Subject: [PATCH 03/34] Print cache fetch time and key plans on planned execution time Signed-off-by: methylDragon --- .../src/motion_planner_server.cpp | 46 +++++++++++++------ 1 file changed, 31 insertions(+), 15 deletions(-) diff --git a/nexus_motion_planner/src/motion_planner_server.cpp b/nexus_motion_planner/src/motion_planner_server.cpp index 8baf0d8..fd2491b 100644 --- a/nexus_motion_planner/src/motion_planner_server.cpp +++ b/nexus_motion_planner/src/motion_planner_server.cpp @@ -612,16 +612,22 @@ void MotionPlannerServer::plan_with_move_group( } else { + auto fetch_start = this->now(); auto fetched_plan = _motion_plan_cache->fetch_best_matching_plan( *interface, robot_name, plan_req_msg, _cache_start_match_tolerance, _cache_goal_match_tolerance); + auto fetch_end = this->now(); if (fetched_plan) { plan.start_state = plan_req_msg.start_state; plan.trajectory = *fetched_plan; - plan.planning_time = 0; // ??? + plan.planning_time = (fetch_end - fetch_start).seconds(); res->result.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + RCLCPP_INFO( + this->get_logger(), + "Cache fetch took %es, fetched plan planning time was: %es", + (fetch_end - fetch_start).seconds(), plan.planning_time); } else { @@ -635,30 +641,40 @@ void MotionPlannerServer::plan_with_move_group( return; } } + res->result.trajectory_start = std::move(plan.start_state); res->result.trajectory = std::move(plan.trajectory); res->result.planning_time = std::move(plan.planning_time); - } - if (_execute_trajectory) - { - RCLCPP_INFO(this->get_logger(), "Executing trajectory!"); - auto exec_start = this->now(); - // This is a blocking call. - // interface->execute(res->result.trajectory); - auto exec_end = this->now(); + if (res->result.error_code.val != + moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + { + RCLCPP_WARN( + this->get_logger(), + "Planning did not succeed: %d", res->result.error_code.val); + return; + } - if (_use_motion_plan_cache && !req.cartesian) + if (_use_motion_plan_cache) { - bool plan_put_ok = _motion_plan_cache->put_plan( - *interface, robot_name, - std::move(plan_req_msg), std::move(res->result.trajectory), - (exec_end - exec_start).seconds(), true); - if (!plan_put_ok) + if (!_motion_plan_cache->put_plan( + *interface, robot_name, + std::move(plan_req_msg), std::move(res->result.trajectory), + rclcpp::Duration( + res->result.trajectory.joint_trajectory.points.back() + .time_from_start + ).seconds(), + true)) { RCLCPP_WARN(this->get_logger(), "Did not put plan into cache."); } } } + if (_execute_trajectory) + { + RCLCPP_INFO(this->get_logger(), "Executing trajectory!"); + // This is a blocking call. + interface->execute(res->result.trajectory); + } return; } From 8889982c96bdce52d17ebda10ff63537826234c1 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 26 Sep 2023 21:36:05 -0700 Subject: [PATCH 04/34] Add overwrite_worse_plans param Signed-off-by: methylDragon --- nexus_motion_planner/src/motion_planner_server.cpp | 12 ++++++++++-- nexus_motion_planner/src/motion_planner_server.hpp | 1 + 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/nexus_motion_planner/src/motion_planner_server.cpp b/nexus_motion_planner/src/motion_planner_server.cpp index fd2491b..7da8563 100644 --- a/nexus_motion_planner/src/motion_planner_server.cpp +++ b/nexus_motion_planner/src/motion_planner_server.cpp @@ -180,6 +180,14 @@ MotionPlannerServer::MotionPlannerServer(const rclcpp::NodeOptions& options) _only_use_cached_plans ? "True" : "False" ); + _overwrite_worse_plans = this->declare_parameter( + "overwrite_worse_plans", true); + RCLCPP_INFO( + this->get_logger(), + "Setting parameter overwrite_worse_plans to [%s]", + _overwrite_worse_plans ? "True" : "False" + ); + _cache_db_plugin = this->declare_parameter( "cache_db_plugin", "warehouse_ros_sqlite::DatabaseConnection"); RCLCPP_INFO( @@ -200,7 +208,7 @@ MotionPlannerServer::MotionPlannerServer(const rclcpp::NodeOptions& options) ); _cache_exact_match_tolerance = this->declare_parameter( - "cache_exact_match_tolerance", 0.025); + "cache_exact_match_tolerance", 0.00001); RCLCPP_INFO( this->get_logger(), "Setting parameter cache_exact_match_tolerance to [%.2e]", @@ -664,7 +672,7 @@ void MotionPlannerServer::plan_with_move_group( res->result.trajectory.joint_trajectory.points.back() .time_from_start ).seconds(), - true)) + _overwrite_worse_plans)) { RCLCPP_WARN(this->get_logger(), "Did not put plan into cache."); } diff --git a/nexus_motion_planner/src/motion_planner_server.hpp b/nexus_motion_planner/src/motion_planner_server.hpp index 5ad8564..24e319a 100644 --- a/nexus_motion_planner/src/motion_planner_server.hpp +++ b/nexus_motion_planner/src/motion_planner_server.hpp @@ -100,6 +100,7 @@ class MotionPlannerServer : public rclcpp_lifecycle::LifecycleNode bool _use_motion_plan_cache; bool _only_use_cached_plans; + bool _overwrite_worse_plans; std::string _cache_db_plugin; std::string _cache_db_host; int _cache_db_port; From 32659fd4ea439d9d7c9837ba7e96467d278a47e2 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 26 Sep 2023 21:53:22 -0700 Subject: [PATCH 05/34] Fix plan fetching printout and don't recache fetched plans Signed-off-by: methylDragon --- nexus_motion_planner/src/motion_plan_cache.cpp | 2 ++ nexus_motion_planner/src/motion_plan_cache.hpp | 1 + .../src/motion_planner_server.cpp | 16 +++++++++++++--- 3 files changed, 16 insertions(+), 3 deletions(-) diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp index 8c5b372..a5b8d5b 100644 --- a/nexus_motion_planner/src/motion_plan_cache.cpp +++ b/nexus_motion_planner/src/motion_plan_cache.cpp @@ -119,6 +119,7 @@ MotionPlanCache::put_plan( const moveit_msgs::msg::MotionPlanRequest& plan_request, const moveit_msgs::msg::RobotTrajectory& plan, double execution_time_s, + double planning_time_s, bool overwrite) { // Check pre-conditions @@ -213,6 +214,7 @@ MotionPlanCache::put_plan( bool goal_meta_ok = this->extract_and_append_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) { diff --git a/nexus_motion_planner/src/motion_plan_cache.hpp b/nexus_motion_planner/src/motion_plan_cache.hpp index f4c3362..cbd9e16 100644 --- a/nexus_motion_planner/src/motion_plan_cache.hpp +++ b/nexus_motion_planner/src/motion_plan_cache.hpp @@ -95,6 +95,7 @@ class MotionPlanCache const moveit_msgs::msg::MotionPlanRequest& plan_request, const moveit_msgs::msg::RobotTrajectory& plan, double execution_time_s, + double planning_time_s, bool overwrite = true); // QUERY CONSTRUCTION ======================================================== diff --git a/nexus_motion_planner/src/motion_planner_server.cpp b/nexus_motion_planner/src/motion_planner_server.cpp index 7da8563..797708f 100644 --- a/nexus_motion_planner/src/motion_planner_server.cpp +++ b/nexus_motion_planner/src/motion_planner_server.cpp @@ -612,11 +612,16 @@ void MotionPlannerServer::plan_with_move_group( else { MoveGroupInterface::Plan plan; + bool plan_is_from_cache = false; interface->constructMotionPlanRequest(plan_req_msg); if (!_only_use_cached_plans) { res->result.error_code = interface->plan(plan); + RCLCPP_INFO( + this->get_logger(), + "Plan status: %d, planning time: %es", + res->result.error_code.val, plan.planning_time); } else { @@ -627,6 +632,7 @@ void MotionPlannerServer::plan_with_move_group( auto fetch_end = this->now(); if (fetched_plan) { + plan_is_from_cache = true; plan.start_state = plan_req_msg.start_state; plan.trajectory = *fetched_plan; plan.planning_time = (fetch_end - fetch_start).seconds(); @@ -634,8 +640,9 @@ void MotionPlannerServer::plan_with_move_group( moveit_msgs::msg::MoveItErrorCodes::SUCCESS; RCLCPP_INFO( this->get_logger(), - "Cache fetch took %es, fetched plan planning time was: %es", - (fetch_end - fetch_start).seconds(), plan.planning_time); + "Cache fetch took %es, planning time was: %es", + (fetch_end - fetch_start).seconds(), + fetched_plan->lookupDouble("planning_time_s")); } else { @@ -663,7 +670,9 @@ void MotionPlannerServer::plan_with_move_group( return; } - if (_use_motion_plan_cache) + // Make sure we check if the plan we have was fetched (so we don't have + // duplicate caches.) + if (_use_motion_plan_cache && !plan_is_from_cache) { if (!_motion_plan_cache->put_plan( *interface, robot_name, @@ -672,6 +681,7 @@ void MotionPlannerServer::plan_with_move_group( res->result.trajectory.joint_trajectory.points.back() .time_from_start ).seconds(), + plan.planning_time, _overwrite_worse_plans)) { RCLCPP_WARN(this->get_logger(), "Did not put plan into cache."); From b683eab613664407abcfb233596614bf0cf08149 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 26 Sep 2023 22:56:56 -0700 Subject: [PATCH 06/34] Update only use cached plans parameter name Signed-off-by: methylDragon --- .../src/motion_planner_server.cpp | 23 ++++++++++--------- .../src/motion_planner_server.hpp | 2 +- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/nexus_motion_planner/src/motion_planner_server.cpp b/nexus_motion_planner/src/motion_planner_server.cpp index 797708f..19c27e1 100644 --- a/nexus_motion_planner/src/motion_planner_server.cpp +++ b/nexus_motion_planner/src/motion_planner_server.cpp @@ -172,12 +172,12 @@ MotionPlannerServer::MotionPlannerServer(const rclcpp::NodeOptions& options) _use_motion_plan_cache ? "True" : "False" ); - _only_use_cached_plans = this->declare_parameter( - "only_use_cached_plans", false); + _use_cached_plans_instead_of_planning = this->declare_parameter( + "use_cached_plans_instead_of_planning", false); RCLCPP_INFO( this->get_logger(), - "Setting parameter only_use_cached_plans to [%s]", - _only_use_cached_plans ? "True" : "False" + "Setting parameter use_cached_plans_instead_of_planning to [%s]", + _use_cached_plans_instead_of_planning ? "True" : "False" ); _overwrite_worse_plans = this->declare_parameter( @@ -208,7 +208,7 @@ MotionPlannerServer::MotionPlannerServer(const rclcpp::NodeOptions& options) ); _cache_exact_match_tolerance = this->declare_parameter( - "cache_exact_match_tolerance", 0.00001); + "cache_exact_match_tolerance", 0.0005); // ~0.028 degrees per joint RCLCPP_INFO( this->get_logger(), "Setting parameter cache_exact_match_tolerance to [%.2e]", @@ -283,11 +283,12 @@ auto MotionPlannerServer::on_configure(const LifecycleState& /*state*/) -> CallbackReturn { RCLCPP_INFO(this->get_logger(), "Configuring..."); - if (!_use_motion_plan_cache && _only_use_cached_plans) + if (!_use_motion_plan_cache && _use_cached_plans_instead_of_planning) { RCLCPP_ERROR( this->get_logger(), - "use_motion_plan_cache must be true if only_use_cached_plans is true." + "use_motion_plan_cache must be true if " + "use_cached_plans_instead_of_planning is true." ); return CallbackReturn::ERROR; } @@ -615,7 +616,7 @@ void MotionPlannerServer::plan_with_move_group( bool plan_is_from_cache = false; interface->constructMotionPlanRequest(plan_req_msg); - if (!_only_use_cached_plans) + if (!_use_cached_plans_instead_of_planning) { res->result.error_code = interface->plan(plan); RCLCPP_INFO( @@ -640,7 +641,7 @@ void MotionPlannerServer::plan_with_move_group( moveit_msgs::msg::MoveItErrorCodes::SUCCESS; RCLCPP_INFO( this->get_logger(), - "Cache fetch took %es, planning time was: %es", + "Cache fetch took %es, planning time of fetched plan was: %es", (fetch_end - fetch_start).seconds(), fetched_plan->lookupDouble("planning_time_s")); } @@ -648,8 +649,8 @@ void MotionPlannerServer::plan_with_move_group( { RCLCPP_ERROR( this->get_logger(), - "only_use_cached_plans was true, and could not find cached plan for " - "plan request: \n\n%s", + "use_cached_plans_instead_of_planning was true, and could not find " + "cached plan for plan request: \n\n%s", moveit_msgs::msg::to_yaml(plan_req_msg).c_str()); res->result.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; diff --git a/nexus_motion_planner/src/motion_planner_server.hpp b/nexus_motion_planner/src/motion_planner_server.hpp index 24e319a..19dab7d 100644 --- a/nexus_motion_planner/src/motion_planner_server.hpp +++ b/nexus_motion_planner/src/motion_planner_server.hpp @@ -99,7 +99,7 @@ class MotionPlannerServer : public rclcpp_lifecycle::LifecycleNode std::shared_ptr _motion_plan_cache; bool _use_motion_plan_cache; - bool _only_use_cached_plans; + bool _use_cached_plans_instead_of_planning; bool _overwrite_worse_plans; std::string _cache_db_plugin; std::string _cache_db_host; From 5c94f4908d311a50c98f1616e84bbb4286f379c8 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 16 Oct 2023 19:34:34 -0700 Subject: [PATCH 07/34] Add copyright headers and motion_planner namespace Signed-off-by: methylDragon --- nexus_motion_planner/src/main.cpp | 2 +- nexus_motion_planner/src/motion_plan_cache.cpp | 14 ++++++++++++++ nexus_motion_planner/src/motion_plan_cache.hpp | 14 ++++++++++++++ nexus_motion_planner/src/motion_planner_server.cpp | 6 ++++++ nexus_motion_planner/src/motion_planner_server.hpp | 6 ++++++ 5 files changed, 41 insertions(+), 1 deletion(-) diff --git a/nexus_motion_planner/src/main.cpp b/nexus_motion_planner/src/main.cpp index 655af9f..c1d7353 100644 --- a/nexus_motion_planner/src/main.cpp +++ b/nexus_motion_planner/src/main.cpp @@ -27,7 +27,7 @@ int main(int argc, char** argv) setvbuf(stdout, NULL, _IONBF, BUFSIZ); rclcpp::init(argc, argv); - auto node = std::make_shared(); + auto node = std::make_shared(); rclcpp::spin(node->get_node_base_interface()); rclcpp::shutdown(); return 0; diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp index a5b8d5b..a63d148 100644 --- a/nexus_motion_planner/src/motion_plan_cache.cpp +++ b/nexus_motion_planner/src/motion_plan_cache.cpp @@ -1,3 +1,17 @@ +// 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 diff --git a/nexus_motion_planner/src/motion_plan_cache.hpp b/nexus_motion_planner/src/motion_plan_cache.hpp index cbd9e16..da4c54a 100644 --- a/nexus_motion_planner/src/motion_plan_cache.hpp +++ b/nexus_motion_planner/src/motion_plan_cache.hpp @@ -1,3 +1,17 @@ +// 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__MOTION_PLAN_CACHE_HPP #define SRC__MOTION_PLAN_CACHE_HPP diff --git a/nexus_motion_planner/src/motion_planner_server.cpp b/nexus_motion_planner/src/motion_planner_server.cpp index 19c27e1..5827f8e 100644 --- a/nexus_motion_planner/src/motion_planner_server.cpp +++ b/nexus_motion_planner/src/motion_planner_server.cpp @@ -14,6 +14,9 @@ #include "motion_planner_server.hpp" +namespace nexus { +namespace motion_planner { + //============================================================================== MotionPlannerServer::MotionPlannerServer(const rclcpp::NodeOptions& options) : rclcpp_lifecycle::LifecycleNode("motion_planner_server", options) @@ -697,3 +700,6 @@ void MotionPlannerServer::plan_with_move_group( } return; } + +} // namespace planning_interface +} // namespace moveit \ No newline at end of file diff --git a/nexus_motion_planner/src/motion_planner_server.hpp b/nexus_motion_planner/src/motion_planner_server.hpp index 19dab7d..8e19a84 100644 --- a/nexus_motion_planner/src/motion_planner_server.hpp +++ b/nexus_motion_planner/src/motion_planner_server.hpp @@ -38,6 +38,9 @@ // NEXUS motion plan cache #include "motion_plan_cache.hpp" +namespace nexus { +namespace motion_planner { + //============================================================================== class MotionPlannerServer : public rclcpp_lifecycle::LifecycleNode { @@ -122,4 +125,7 @@ class MotionPlannerServer : public rclcpp_lifecycle::LifecycleNode Response res); }; // class MotionPlannerServer +} // namespace planning_interface +} // namespace moveit + #endif // SRC__MOTION_PLANNER_SERVER_HPP From 856c00979fab0bc4a196d5369d7f5c8204d897ea Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 16 Oct 2023 19:44:13 -0700 Subject: [PATCH 08/34] Make motion_plan_cache a unique_ptr instead of shared_ptr Signed-off-by: methylDragon --- nexus_motion_planner/src/motion_planner_server.cpp | 2 +- nexus_motion_planner/src/motion_planner_server.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nexus_motion_planner/src/motion_planner_server.cpp b/nexus_motion_planner/src/motion_planner_server.cpp index 5827f8e..a353b3e 100644 --- a/nexus_motion_planner/src/motion_planner_server.cpp +++ b/nexus_motion_planner/src/motion_planner_server.cpp @@ -262,7 +262,7 @@ MotionPlannerServer::MotionPlannerServer(const rclcpp::NodeOptions& options) "warehouse_port", _cache_db_port); _motion_plan_cache = - std::make_shared( + std::make_unique( _internal_cache_node); } } diff --git a/nexus_motion_planner/src/motion_planner_server.hpp b/nexus_motion_planner/src/motion_planner_server.hpp index 8e19a84..504b131 100644 --- a/nexus_motion_planner/src/motion_planner_server.hpp +++ b/nexus_motion_planner/src/motion_planner_server.hpp @@ -99,7 +99,7 @@ class MotionPlannerServer : public rclcpp_lifecycle::LifecycleNode double _workspace_max_z; // Motion plan caching - std::shared_ptr _motion_plan_cache; + std::unique_ptr _motion_plan_cache; bool _use_motion_plan_cache; bool _use_cached_plans_instead_of_planning; From dd323a587c6090a1db4fca47f8db30661aec53fd Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 16 Oct 2023 19:51:35 -0700 Subject: [PATCH 09/34] Set numerical precision Signed-off-by: methylDragon --- nexus_motion_planner/src/motion_plan_cache.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nexus_motion_planner/src/motion_plan_cache.hpp b/nexus_motion_planner/src/motion_plan_cache.hpp index da4c54a..f3f3f3c 100644 --- a/nexus_motion_planner/src/motion_plan_cache.hpp +++ b/nexus_motion_planner/src/motion_plan_cache.hpp @@ -68,7 +68,7 @@ namespace motion_planner { * - Final pose (wrt. `planning_frame` (usually `base_link`)) * - Final robot joint states * - * Motion plans may be looked up with some tolerance. + * Motion plans may be looked up with some tolerance at call time. */ class MotionPlanCache { @@ -80,7 +80,7 @@ class MotionPlanCache void init( const std::string& db_path = ":memory:", uint32_t db_port = 0, - double exact_match_precision = 0.00001); + double exact_match_precision = 1e-6); // TOP LEVEL OPS ============================================================= std::vector< @@ -140,7 +140,7 @@ class MotionPlanCache rclcpp::Node::SharedPtr node_; warehouse_ros::DatabaseConnection::Ptr db_; - double exact_match_precision_ = 0; + double exact_match_precision_ = 1e-6; std::unique_ptr tf_buffer_; std::shared_ptr tf_listener_; From 52db5b5b2e1d9c1fe4fadd411840f38239df3dcf Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 16 Oct 2023 22:43:06 -0700 Subject: [PATCH 10/34] Use enum for planner database mode Signed-off-by: methylDragon --- .../src/motion_planner_server.cpp | 147 ++++++++++-------- .../src/motion_planner_server.hpp | 29 +++- 2 files changed, 109 insertions(+), 67 deletions(-) diff --git a/nexus_motion_planner/src/motion_planner_server.cpp b/nexus_motion_planner/src/motion_planner_server.cpp index a353b3e..8ff222b 100644 --- a/nexus_motion_planner/src/motion_planner_server.cpp +++ b/nexus_motion_planner/src/motion_planner_server.cpp @@ -14,9 +14,57 @@ #include "motion_planner_server.hpp" +std::string str_tolower(std::string s) +{ + std::transform(s.begin(), s.end(), s.begin(), + [](unsigned char c) { return std::tolower(c); } + ); + return s; +} + namespace nexus { namespace motion_planner { +constexpr bool cache_mode_is_execute(PlannerDatabaseMode mode) +{ + return mode == PlannerDatabaseMode::ExecuteBestEffort || + mode == PlannerDatabaseMode::ExecuteReadOnly; +} + +constexpr bool cache_mode_is_training(PlannerDatabaseMode mode) +{ + return mode == PlannerDatabaseMode::TrainingOverwrite || + mode == PlannerDatabaseMode::TrainingAppendOnly; +} + +// Convert planner database string param to PlannerDatabaseMode enum values. +PlannerDatabaseMode str_to_planner_database_mode(std::string s) +{ + std::string s_lower = str_tolower(s); + + // Using a switch-case here is... inconvenient (needs constexpr hashing or a map), so we don't. + if (s_lower == "training_overwrite" || s_lower == "trainingoverwrite") + { + return PlannerDatabaseMode::TrainingOverwrite; + } + else if (s_lower == "training_append_only" || s_lower == "trainingappendonly") + { + return PlannerDatabaseMode::TrainingAppendOnly; + } + else if (s_lower == "execute_best_effort" || s_lower == "executebesteffort") + { + return PlannerDatabaseMode::ExecuteBestEffort; + } + else if (s_lower == "execute_read_only" || s_lower == "executereadonly") + { + return PlannerDatabaseMode::ExecuteReadOnly; + } + else + { + return PlannerDatabaseMode::Unset; + } +} + //============================================================================== MotionPlannerServer::MotionPlannerServer(const rclcpp::NodeOptions& options) : rclcpp_lifecycle::LifecycleNode("motion_planner_server", options) @@ -167,29 +215,15 @@ MotionPlannerServer::MotionPlannerServer(const rclcpp::NodeOptions& options) ); // Motion plan cache params - _use_motion_plan_cache = this->declare_parameter( - "use_motion_plan_cache", true); - RCLCPP_INFO( - this->get_logger(), - "Setting parameter use_motion_plan_cache to [%s]", - _use_motion_plan_cache ? "True" : "False" - ); - - _use_cached_plans_instead_of_planning = this->declare_parameter( - "use_cached_plans_instead_of_planning", false); + // unset, training, training_append_only, execute_best_effort, execute_read_only + _planner_database_mode = this->declare_parameter( + "planner_database_mode", "unset"); RCLCPP_INFO( this->get_logger(), - "Setting parameter use_cached_plans_instead_of_planning to [%s]", - _use_cached_plans_instead_of_planning ? "True" : "False" - ); - - _overwrite_worse_plans = this->declare_parameter( - "overwrite_worse_plans", true); - RCLCPP_INFO( - this->get_logger(), - "Setting parameter overwrite_worse_plans to [%s]", - _overwrite_worse_plans ? "True" : "False" + "Setting parameter planner_database_mode to [%s]", + _planner_database_mode.c_str() ); + _cache_mode = str_to_planner_database_mode(_planner_database_mode); _cache_db_plugin = this->declare_parameter( "cache_db_plugin", "warehouse_ros_sqlite::DatabaseConnection"); @@ -234,36 +268,21 @@ MotionPlannerServer::MotionPlannerServer(const rclcpp::NodeOptions& options) _cache_goal_match_tolerance ); - if (_use_motion_plan_cache) + if (_cache_mode != PlannerDatabaseMode::Unset) { - auto cache_node_options = rclcpp::NodeOptions(); - cache_node_options.automatically_declare_parameters_from_overrides(true); - cache_node_options.use_global_arguments(false); - _internal_cache_node = std::make_shared( - "motion_planner_server_internal_cache_node", cache_node_options); - _cache_spin_thread = std::thread( - [this]() - { - while (rclcpp::ok()) - { - rclcpp::spin_some(_internal_cache_node); - } - }); - - // Push warehouse_ros parameters to internal cache node + // Push warehouse_ros parameters to internal node // This must happen BEFORE the MotionPlanCache is created! - _internal_cache_node->declare_parameter( + _internal_node->declare_parameter( "warehouse_plugin", _cache_db_plugin); - _internal_cache_node->declare_parameter( + _internal_node->declare_parameter( "warehouse_host", _cache_db_host); - _internal_cache_node->declare_parameter( + _internal_node->declare_parameter( "warehouse_port", _cache_db_port); _motion_plan_cache = - std::make_unique( - _internal_cache_node); + std::make_unique(_internal_node); } } @@ -286,17 +305,8 @@ auto MotionPlannerServer::on_configure(const LifecycleState& /*state*/) -> CallbackReturn { RCLCPP_INFO(this->get_logger(), "Configuring..."); - if (!_use_motion_plan_cache && _use_cached_plans_instead_of_planning) - { - RCLCPP_ERROR( - this->get_logger(), - "use_motion_plan_cache must be true if " - "use_cached_plans_instead_of_planning is true." - ); - return CallbackReturn::ERROR; - } - if (_use_motion_plan_cache) + if (_cache_mode != PlannerDatabaseMode::Unset) { _motion_plan_cache->init( _cache_db_host, _cache_db_port, _cache_exact_match_tolerance); @@ -619,21 +629,15 @@ void MotionPlannerServer::plan_with_move_group( bool plan_is_from_cache = false; interface->constructMotionPlanRequest(plan_req_msg); - if (!_use_cached_plans_instead_of_planning) - { - res->result.error_code = interface->plan(plan); - RCLCPP_INFO( - this->get_logger(), - "Plan status: %d, planning time: %es", - res->result.error_code.val, plan.planning_time); - } - else + // Fetch if in execute mode. + if (cache_mode_is_execute(_cache_mode)) { auto fetch_start = this->now(); auto fetched_plan = _motion_plan_cache->fetch_best_matching_plan( *interface, robot_name, plan_req_msg, _cache_start_match_tolerance, _cache_goal_match_tolerance); auto fetch_end = this->now(); + // Set plan if a cached plan was fetched. if (fetched_plan) { plan_is_from_cache = true; @@ -648,11 +652,12 @@ void MotionPlannerServer::plan_with_move_group( (fetch_end - fetch_start).seconds(), fetched_plan->lookupDouble("planning_time_s")); } - else + // Fail if ReadOnly mode and no cached plan was fetched. + else if (_cache_mode == PlannerDatabaseMode::ExecuteReadOnly) { RCLCPP_ERROR( this->get_logger(), - "use_cached_plans_instead_of_planning was true, and could not find " + "Cache mode was ExecuteReadOnly, and could not find " "cached plan for plan request: \n\n%s", moveit_msgs::msg::to_yaml(plan_req_msg).c_str()); res->result.error_code.val = @@ -661,6 +666,19 @@ void MotionPlannerServer::plan_with_move_group( } } + // Plan if needed. + // This is if we didn't fetch a plan from the cache. + // (In training or unset mode we never attempt to fetch, so it will always + // plan.) + if (!plan_is_from_cache) + { + res->result.error_code = interface->plan(plan); + RCLCPP_INFO( + this->get_logger(), + "Plan status: %d, planning time: %es", + res->result.error_code.val, plan.planning_time); + } + res->result.trajectory_start = std::move(plan.start_state); res->result.trajectory = std::move(plan.trajectory); res->result.planning_time = std::move(plan.planning_time); @@ -674,9 +692,10 @@ void MotionPlannerServer::plan_with_move_group( return; } + // Put plan if in training mode. // Make sure we check if the plan we have was fetched (so we don't have // duplicate caches.) - if (_use_motion_plan_cache && !plan_is_from_cache) + if (cache_mode_is_training(_cache_mode) && !plan_is_from_cache) { if (!_motion_plan_cache->put_plan( *interface, robot_name, @@ -686,7 +705,7 @@ void MotionPlannerServer::plan_with_move_group( .time_from_start ).seconds(), plan.planning_time, - _overwrite_worse_plans)) + _cache_mode == PlannerDatabaseMode::TrainingOverwrite)) { RCLCPP_WARN(this->get_logger(), "Did not put plan into cache."); } diff --git a/nexus_motion_planner/src/motion_planner_server.hpp b/nexus_motion_planner/src/motion_planner_server.hpp index 504b131..ee0e714 100644 --- a/nexus_motion_planner/src/motion_planner_server.hpp +++ b/nexus_motion_planner/src/motion_planner_server.hpp @@ -41,6 +41,30 @@ namespace nexus { namespace motion_planner { +enum class PlannerDatabaseMode : uint8_t +{ + // Unset. Planner will not use a cache. + Unset = 0, + + // Planner will always generate a plan. + // It will add plans to the database if they are the best seen so far. + // Will overwrite close-enough entries if a better plan was found. + TrainingOverwrite = 10, + + // Planner will always generate a plan. + // It will add plans to the database if they are the best seen so far. + // Will not overwrite any existing entries. + TrainingAppendOnly = 11, + + // Planner will prioritize returning plans in the cache. + // If no plan was found, it will instead attempt to generate a plan live, but will not update the + // cache. + ExecuteBestEffort = 20, + + // Planner will return a plan only if present in the database cache. + ExecuteReadOnly = 21, +}; + //============================================================================== class MotionPlannerServer : public rclcpp_lifecycle::LifecycleNode { @@ -100,10 +124,9 @@ class MotionPlannerServer : public rclcpp_lifecycle::LifecycleNode // Motion plan caching std::unique_ptr _motion_plan_cache; + std::string _planner_database_mode; + PlannerDatabaseMode _cache_mode = PlannerDatabaseMode::Unset; - bool _use_motion_plan_cache; - bool _use_cached_plans_instead_of_planning; - bool _overwrite_worse_plans; std::string _cache_db_plugin; std::string _cache_db_host; int _cache_db_port; From e0d4cbcd898b048ea125a1e30e25c194e29c3887 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 16 Oct 2023 23:27:17 -0700 Subject: [PATCH 11/34] Rename cache methods to prepare for cartesian caching Signed-off-by: methylDragon --- .../src/motion_plan_cache.cpp | 31 ++++++++++--------- .../src/motion_plan_cache.hpp | 19 +++++++----- 2 files changed, 28 insertions(+), 22 deletions(-) diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp index a63d148..113f397 100644 --- a/nexus_motion_planner/src/motion_plan_cache.cpp +++ b/nexus_motion_planner/src/motion_plan_cache.cpp @@ -36,8 +36,6 @@ using warehouse_ros::Query; MotionPlanCache::MotionPlanCache(const rclcpp::Node::SharedPtr& node) : node_(node) { - // TODO: methyldragon - - // Declare or pass in these in the motion_planner_server instead? if (!node_->has_parameter("warehouse_plugin")) { node_->declare_parameter( @@ -66,7 +64,10 @@ void MotionPlanCache::init( db_->connect(); } -// TOP LEVEL OPS =============================================================== +// ============================================================================= +// MOTION PLAN CACHING +// ============================================================================= +// MOTION PLAN CACHING: TOP LEVEL OPS std::vector::ConstPtr> MotionPlanCache::fetch_all_matching_plans( const moveit::planning_interface::MoveGroupInterface& move_group, @@ -79,9 +80,9 @@ MotionPlanCache::fetch_all_matching_plans( Query::Ptr query = coll.createQuery(); - bool start_ok = this->extract_and_append_start_to_query( + bool start_ok = this->extract_and_append_plan_start_to_query( *query, move_group, plan_request, start_tolerance); - bool goal_ok = this->extract_and_append_goal_to_query( + bool goal_ok = this->extract_and_append_plan_goal_to_query( *query, move_group, plan_request, goal_tolerance); if (!start_ok || !goal_ok) @@ -170,10 +171,10 @@ MotionPlanCache::put_plan( // overwrite. Query::Ptr exact_query = coll.createQuery(); - bool start_query_ok = this->extract_and_append_start_to_query( + bool start_query_ok = this->extract_and_append_plan_start_to_query( *exact_query, move_group, plan_request, exact_match_precision_); - bool goal_query_ok = this->extract_and_append_goal_to_query( + bool goal_query_ok = this->extract_and_append_plan_goal_to_query( *exact_query, move_group, plan_request, exact_match_precision_); @@ -223,9 +224,9 @@ MotionPlanCache::put_plan( { Metadata::Ptr insert_metadata = coll.createMetadata(); - bool start_meta_ok = this->extract_and_append_start_to_metadata( + bool start_meta_ok = this->extract_and_append_plan_start_to_metadata( *insert_metadata, move_group, plan_request); - bool goal_meta_ok = this->extract_and_append_goal_to_metadata( + bool goal_meta_ok = this->extract_and_append_plan_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); @@ -256,9 +257,9 @@ MotionPlanCache::put_plan( return false; } -// QUERY CONSTRUCTION ========================================================== +// MOTION PLAN CACHING: QUERY CONSTRUCTION bool -MotionPlanCache::extract_and_append_start_to_query( +MotionPlanCache::extract_and_append_plan_start_to_query( Query& query, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::msg::MotionPlanRequest& plan_request, @@ -364,7 +365,7 @@ MotionPlanCache::extract_and_append_start_to_query( } bool -MotionPlanCache::extract_and_append_goal_to_query( +MotionPlanCache::extract_and_append_plan_goal_to_query( Query& query, const moveit::planning_interface::MoveGroupInterface& /* move_group */, const moveit_msgs::msg::MotionPlanRequest& plan_request, @@ -605,9 +606,9 @@ MotionPlanCache::extract_and_append_goal_to_query( return true; } -// METADATA CONSTRUCTION ======================================================= +// MOTION PLAN CACHING: METADATA CONSTRUCTION bool -MotionPlanCache::extract_and_append_start_to_metadata( +MotionPlanCache::extract_and_append_plan_start_to_metadata( Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::msg::MotionPlanRequest& plan_request) @@ -706,7 +707,7 @@ MotionPlanCache::extract_and_append_start_to_metadata( } bool -MotionPlanCache::extract_and_append_goal_to_metadata( +MotionPlanCache::extract_and_append_plan_goal_to_metadata( Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& /* move_group */, const moveit_msgs::msg::MotionPlanRequest& plan_request) diff --git a/nexus_motion_planner/src/motion_plan_cache.hpp b/nexus_motion_planner/src/motion_plan_cache.hpp index f3f3f3c..c958757 100644 --- a/nexus_motion_planner/src/motion_plan_cache.hpp +++ b/nexus_motion_planner/src/motion_plan_cache.hpp @@ -50,6 +50,8 @@ namespace motion_planner { * how long they took to execute. This allows for the lookup and reuse of the * best performing plans found so far. * + * WARNING: This cache does NOT yet support collision detection! + * * Relevant ROS Parameters: * - `warehouse_plugin`: What database to use * - `warehouse_host`: Where the database should be created @@ -82,7 +84,10 @@ class MotionPlanCache uint32_t db_port = 0, double exact_match_precision = 1e-6); - // TOP LEVEL OPS ============================================================= + // =========================================================================== + // MOTION PLAN CACHING + // =========================================================================== + // TOP LEVEL OPS std::vector< warehouse_ros::MessageWithMetadata< moveit_msgs::msg::RobotTrajectory @@ -112,26 +117,26 @@ class MotionPlanCache double planning_time_s, bool overwrite = true); - // QUERY CONSTRUCTION ======================================================== - bool extract_and_append_start_to_query( + // QUERY CONSTRUCTION + bool extract_and_append_plan_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_goal_to_query( + bool extract_and_append_plan_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_start_to_metadata( + // METADATA CONSTRUCTION + bool extract_and_append_plan_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_goal_to_metadata( + bool extract_and_append_plan_goal_to_metadata( warehouse_ros::Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::msg::MotionPlanRequest& plan_request); From d7bd14285a98770fbda6b8d63c2375152506c4d1 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 17 Oct 2023 00:11:53 -0700 Subject: [PATCH 12/34] Add very important warning to cache class Signed-off-by: methylDragon --- nexus_motion_planner/src/motion_plan_cache.hpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/nexus_motion_planner/src/motion_plan_cache.hpp b/nexus_motion_planner/src/motion_plan_cache.hpp index c958757..d4cda80 100644 --- a/nexus_motion_planner/src/motion_plan_cache.hpp +++ b/nexus_motion_planner/src/motion_plan_cache.hpp @@ -50,7 +50,15 @@ namespace motion_planner { * how long they took to execute. This allows for the lookup and reuse of the * best performing plans found so far. * - * WARNING: This cache does NOT yet support collision detection! + * WARNING: + * This cache does NOT support collision detection! + * Plans will be put into and fetched from the cache IGNORING collision. + * If your planning scene is expected to change between cache lookups, do NOT + * use this cache, fetched plans are likely to result in collision then. + * + * To handle collisions this class will need to hash the planning scene world + * msg (after zeroing out std_msgs/Header timestamps and sequences) and do an + * appropriate lookup. * * Relevant ROS Parameters: * - `warehouse_plugin`: What database to use From f161f45f7b3fbeb91a8c2b8072ee4a1b7c8f9f2b Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 17 Oct 2023 20:22:30 -0700 Subject: [PATCH 13/34] Fix access after move bug Signed-off-by: methylDragon --- nexus_motion_planner/src/motion_planner_server.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nexus_motion_planner/src/motion_planner_server.cpp b/nexus_motion_planner/src/motion_planner_server.cpp index 8ff222b..08c9cf3 100644 --- a/nexus_motion_planner/src/motion_planner_server.cpp +++ b/nexus_motion_planner/src/motion_planner_server.cpp @@ -704,7 +704,7 @@ void MotionPlannerServer::plan_with_move_group( res->result.trajectory.joint_trajectory.points.back() .time_from_start ).seconds(), - plan.planning_time, + res->result.planning_time, _cache_mode == PlannerDatabaseMode::TrainingOverwrite)) { RCLCPP_WARN(this->get_logger(), "Did not put plan into cache."); From ca0f123ded4a46b63ccce28796710f3a20df218f Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 23 Oct 2023 17:01:02 -0700 Subject: [PATCH 14/34] Remove internal cache node Signed-off-by: methylDragon --- nexus_motion_planner/src/motion_planner_server.cpp | 5 ----- nexus_motion_planner/src/motion_planner_server.hpp | 3 --- 2 files changed, 8 deletions(-) diff --git a/nexus_motion_planner/src/motion_planner_server.cpp b/nexus_motion_planner/src/motion_planner_server.cpp index 08c9cf3..9355f57 100644 --- a/nexus_motion_planner/src/motion_planner_server.cpp +++ b/nexus_motion_planner/src/motion_planner_server.cpp @@ -293,11 +293,6 @@ MotionPlannerServer::~MotionPlannerServer() { _spin_thread.join(); } - - if (_cache_spin_thread.joinable()) - { - _cache_spin_thread.join(); - } } //============================================================================== diff --git a/nexus_motion_planner/src/motion_planner_server.hpp b/nexus_motion_planner/src/motion_planner_server.hpp index ee0e714..9032bf4 100644 --- a/nexus_motion_planner/src/motion_planner_server.hpp +++ b/nexus_motion_planner/src/motion_planner_server.hpp @@ -99,9 +99,6 @@ class MotionPlannerServer : public rclcpp_lifecycle::LifecycleNode rclcpp::Node::SharedPtr _internal_node; std::thread _spin_thread; - rclcpp::Node::SharedPtr _internal_cache_node; - std::thread _cache_spin_thread; - // MoveIt planning std::vector _manipulators; bool _use_move_group_interfaces; From e4ec917709e8f2be786f5afb1ff397433a454ba5 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 16 Oct 2023 23:28:58 -0700 Subject: [PATCH 15/34] Add cartesian plan caching interfaces Signed-off-by: methylDragon --- .../src/motion_plan_cache.hpp | 58 +++++++++++++++++++ 1 file changed, 58 insertions(+) diff --git a/nexus_motion_planner/src/motion_plan_cache.hpp b/nexus_motion_planner/src/motion_plan_cache.hpp index d4cda80..da5348a 100644 --- a/nexus_motion_planner/src/motion_plan_cache.hpp +++ b/nexus_motion_planner/src/motion_plan_cache.hpp @@ -35,6 +35,7 @@ // moveit modules #include +#include // NEXUS messages #include @@ -149,6 +150,63 @@ class MotionPlanCache const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::msg::MotionPlanRequest& plan_request); + // =========================================================================== + // CARTESIAN PLAN CACHING + // =========================================================================== + // TOP LEVEL OPS + std::vector< + warehouse_ros::MessageWithMetadata< + moveit_msgs::msg::RobotTrajectory + >::ConstPtr + > + fetch_all_matching_cartesian_plans( + const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_namespace, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, + double start_tolerance, double goal_tolerance, bool metadata_only = false); + + warehouse_ros::MessageWithMetadata< + moveit_msgs::msg::RobotTrajectory + >::ConstPtr + fetch_best_matching_cartesian_plan( + const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_namespace, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, + double start_tolerance, double goal_tolerance, bool metadata_only = false); + + bool put_cartesian_plan( + 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& plan, + double execution_time_s, + double planning_time_s, + bool overwrite = true); + + // QUERY CONSTRUCTION + bool extract_and_append_cartesian_plan_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_plan_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_plan_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_plan_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_; From caada7e24046c791984961f6f7228d0db23d6f23 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 17 Oct 2023 15:51:22 -0700 Subject: [PATCH 16/34] Add construct_get_cartesian_plan_request Signed-off-by: methylDragon --- .../src/motion_plan_cache.cpp | 889 ++++++++++++++++++ .../src/motion_plan_cache.hpp | 8 + 2 files changed, 897 insertions(+) diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp index 113f397..5948e72 100644 --- a/nexus_motion_planner/src/motion_plan_cache.cpp +++ b/nexus_motion_planner/src/motion_plan_cache.cpp @@ -929,6 +929,895 @@ MotionPlanCache::extract_and_append_plan_goal_to_metadata( return true; } +// ============================================================================= +// CARTESIAN PLAN CACHING +// ============================================================================= +// CARTESIAN PLAN CACHING: TOP LEVEL OPS +moveit_msgs::srv::GetCartesianPath::Request +MotionPlanCache::construct_get_cartesian_plan_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 = tmp.start_state; + out.group_name = 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> +// MotionPlanCache::fetch_all_matching_cartesian_plans( +// 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) +// { +// auto coll = db_->openCollection( +// "move_group_plan_cache", move_group_namespace); + +// Query::Ptr query = coll.createQuery(); + +// bool start_ok = this->extract_and_append_cartesian_plan_start_to_query( +// *query, move_group, plan_request, start_tolerance); +// bool goal_ok = this->extract_and_append_cartesian_plan_goal_to_query( +// *query, move_group, plan_request, goal_tolerance); + +// if (!start_ok || !goal_ok) +// { +// RCLCPP_ERROR(node_->get_logger(), "Could not construct plan query."); +// return {}; +// } + +// return coll.queryList( +// query, metadata_only, /* sort_by */ "execution_time_s", true); +// } + +// MessageWithMetadata::ConstPtr +// MotionPlanCache::fetch_best_matching_cartesian_plan( +// 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) +// { +// // First find all matching, but metadata only. +// // Then use the ID metadata of the best plan to pull the actual message. +// auto matching_plans = this->fetch_all_matching_plans( +// move_group, move_group_namespace, +// plan_request, start_tolerance, goal_tolerance, +// true); + +// if (matching_plans.empty()) +// { +// RCLCPP_INFO(node_->get_logger(), "No matching plans found."); +// return nullptr; +// } + +// auto coll = db_->openCollection( +// "move_group_plan_cache", move_group_namespace); + +// // Best plan is at first index, since the lookup query was sorted by +// // execution_time. +// int best_plan_id = matching_plans.at(0)->lookupInt("id"); +// Query::Ptr best_query = coll.createQuery(); +// best_query->append("id", best_plan_id); + +// return coll.findOne(best_query, metadata_only); +// } + +// bool +// MotionPlanCache::put_cartesian_plan( +// 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& plan, +// double execution_time_s, +// double planning_time_s, +// bool overwrite) +// { +// // Check pre-conditions +// if (!plan.multi_dof_joint_trajectory.points.empty()) +// { +// RCLCPP_ERROR( +// node_->get_logger(), +// "Skipping insert: Multi-DOF trajectory plans are not supported."); +// return false; +// } +// if (plan_request.workspace_parameters.header.frame_id.empty() || +// plan.joint_trajectory.header.frame_id.empty()) +// { +// RCLCPP_ERROR( +// node_->get_logger(), "Skipping insert: Frame IDs cannot be empty."); +// return false; +// } +// if (plan_request.workspace_parameters.header.frame_id != +// plan.joint_trajectory.header.frame_id) +// { +// RCLCPP_ERROR( +// node_->get_logger(), +// "Skipping insert: " +// "Plan request frame (%s) does not match plan frame (%s).", +// plan_request.workspace_parameters.header.frame_id.c_str(), +// plan.joint_trajectory.header.frame_id.c_str()); +// return false; +// } + +// auto coll = db_->openCollection( +// "move_group_plan_cache", move_group_namespace); + +// // If start and goal are "exact" match, AND the candidate plan is better, +// // overwrite. +// Query::Ptr exact_query = coll.createQuery(); + +// bool start_query_ok = this->extract_and_append_cartesian_plan_start_to_query( +// *exact_query, move_group, +// plan_request, exact_match_precision_); +// bool goal_query_ok = this->extract_and_append_cartesian_plan_goal_to_query( +// *exact_query, move_group, +// plan_request, exact_match_precision_); + +// if (!start_query_ok || !goal_query_ok) +// { +// RCLCPP_ERROR( +// node_->get_logger(), +// "Skipping insert: Could not construct overwrite query."); +// return false; +// } + +// auto exact_matches = coll.queryList(exact_query, /* metadata_only */ true); +// double best_execution_time_seen = std::numeric_limits::infinity(); +// if (!exact_matches.empty()) +// { +// for (auto& match : exact_matches) +// { +// double match_execution_time_s = +// match->lookupDouble("execution_time_s"); +// if (match_execution_time_s < best_execution_time_seen) +// { +// best_execution_time_seen = match_execution_time_s; +// } + +// if (match_execution_time_s > execution_time_s) +// { +// // If we found any "exact" matches that are worse, delete them. +// if (overwrite) +// { +// int delete_id = match->lookupInt("id"); +// RCLCPP_INFO( +// node_->get_logger(), +// "Overwriting plan (id: %d): " +// "execution_time (%es) > new plan'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_seen) +// { +// Metadata::Ptr insert_metadata = coll.createMetadata(); + +// bool start_meta_ok = this->extract_and_append_cartesian_plan_start_to_metadata( +// *insert_metadata, move_group, plan_request); +// bool goal_meta_ok = this->extract_and_append_cartesian_plan_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 insert: Could not construct insert metadata."); +// return false; +// } + +// RCLCPP_INFO( +// node_->get_logger(), +// "Inserting plan: New plan execution_time (%es) " +// "is better than best plan's execution_time (%es)", +// execution_time_s, best_execution_time_seen); + +// coll.insert(plan, insert_metadata); +// return true; +// } + +// RCLCPP_INFO( +// node_->get_logger(), +// "Skipping insert: New plan execution_time (%es) " +// "is worse than best plan's execution_time (%es)", +// execution_time_s, best_execution_time_seen); +// return false; +// } + +// // CARTESIAN PLAN CACHING: QUERY CONSTRUCTION +// bool +// MotionPlanCache::extract_and_append_cartesian_plan_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. +// 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.appendRangeInclusive( +// "start_state.joint_state.position_" + std::to_string(i), +// NEXUS_MATCH_RANGE( +// 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.appendRangeInclusive( +// "start_state.joint_state.position_" + std::to_string(i), +// NEXUS_MATCH_RANGE( +// plan_request.start_state.joint_state.position.at(i), match_tolerance) +// ); +// } +// } +// return true; +// } + +// bool +// MotionPlanCache::extract_and_append_cartesian_plan_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.appendRangeInclusive( +// "max_velocity_scaling_factor", +// NEXUS_MATCH_RANGE( +// plan_request.max_velocity_scaling_factor, match_tolerance) +// ); +// query.appendRangeInclusive( +// "max_acceleration_scaling_factor", +// NEXUS_MATCH_RANGE( +// plan_request.max_acceleration_scaling_factor, match_tolerance) +// ); +// query.appendRangeInclusive( +// "max_cartesian_speed", +// NEXUS_MATCH_RANGE(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); +// } +// } + +// // 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.appendRangeInclusive( +// meta_name + ".position", +// NEXUS_MATCH_RANGE(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.appendRangeInclusive( +// meta_name + ".target_point_offset.x", +// NEXUS_MATCH_RANGE( +// x_offset + constraint.target_point_offset.x, match_tolerance)); +// query.appendRangeInclusive( +// meta_name + ".target_point_offset.y", +// NEXUS_MATCH_RANGE( +// y_offset + constraint.target_point_offset.y, match_tolerance)); +// query.appendRangeInclusive( +// meta_name + ".target_point_offset.z", +// NEXUS_MATCH_RANGE( +// 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.appendRangeInclusive( +// meta_name + ".target_point_offset.x", +// NEXUS_MATCH_RANGE(final_quat.getX(), match_tolerance)); +// query.appendRangeInclusive( +// meta_name + ".target_point_offset.y", +// NEXUS_MATCH_RANGE(final_quat.getY(), match_tolerance)); +// query.appendRangeInclusive( +// meta_name + ".target_point_offset.z", +// NEXUS_MATCH_RANGE(final_quat.getZ(), match_tolerance)); +// query.appendRangeInclusive( +// meta_name + ".target_point_offset.w", +// NEXUS_MATCH_RANGE(final_quat.getW(), match_tolerance)); +// } +// } + +// return true; +// } + +// // CARTESIAN PLAN CACHING: METADATA CONSTRUCTION +// bool +// MotionPlanCache::extract_and_append_cartesian_plan_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. +// 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 +MotionPlanCache::extract_and_append_cartesian_plan_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_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); + // } + // } + + // // 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; +} + #undef NEXUS_MATCH_RANGE } // namespace motion_planner diff --git a/nexus_motion_planner/src/motion_plan_cache.hpp b/nexus_motion_planner/src/motion_plan_cache.hpp index da5348a..5426f2a 100644 --- a/nexus_motion_planner/src/motion_plan_cache.hpp +++ b/nexus_motion_planner/src/motion_plan_cache.hpp @@ -154,6 +154,14 @@ class MotionPlanCache // CARTESIAN PLAN CACHING // =========================================================================== // TOP LEVEL OPS + // This mimics the move group computeCartesianPath signature (without path + // constraints). + moveit_msgs::srv::GetCartesianPath::Request + construct_get_cartesian_plan_request( + moveit::planning_interface::MoveGroupInterface& move_group, + const std::vector& waypoints, double step, + double jump_threshold, bool avoid_collisions = true); + std::vector< warehouse_ros::MessageWithMetadata< moveit_msgs::msg::RobotTrajectory From 019dbfbd6d2ac0243fe0f85261978b6f488bf1f3 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 17 Oct 2023 16:53:32 -0700 Subject: [PATCH 17/34] Add goal query and metadata Signed-off-by: methylDragon --- .../src/motion_plan_cache.cpp | 634 ++++++------------ 1 file changed, 209 insertions(+), 425 deletions(-) diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp index 5948e72..695c611 100644 --- a/nexus_motion_planner/src/motion_plan_cache.cpp +++ b/nexus_motion_planner/src/motion_plan_cache.cpp @@ -172,11 +172,9 @@ MotionPlanCache::put_plan( Query::Ptr exact_query = coll.createQuery(); bool start_query_ok = this->extract_and_append_plan_start_to_query( - *exact_query, move_group, - plan_request, exact_match_precision_); + *exact_query, move_group, plan_request, 0); bool goal_query_ok = this->extract_and_append_plan_goal_to_query( - *exact_query, move_group, - plan_request, exact_match_precision_); + *exact_query, move_group, plan_request, 0); if (!start_query_ok || !goal_query_ok) { @@ -971,7 +969,7 @@ MotionPlanCache::construct_get_cartesian_plan_request( // MotionPlanCache::fetch_all_matching_cartesian_plans( // const moveit::planning_interface::MoveGroupInterface& move_group, // const std::string& move_group_namespace, -// const moveit_msgs::msg::MotionPlanRequest& plan_request, +// const moveit_msgs::srv::GetCartesianPath::Request& plan_request, // double start_tolerance, double goal_tolerance, bool metadata_only) // { // auto coll = db_->openCollection( @@ -998,7 +996,7 @@ MotionPlanCache::construct_get_cartesian_plan_request( // MotionPlanCache::fetch_best_matching_cartesian_plan( // const moveit::planning_interface::MoveGroupInterface& move_group, // const std::string& move_group_namespace, -// const moveit_msgs::msg::MotionPlanRequest& plan_request, +// const moveit_msgs::srv::GetCartesianPath::Request& plan_request, // double start_tolerance, double goal_tolerance, bool metadata_only) // { // // First find all matching, but metadata only. @@ -1030,7 +1028,7 @@ MotionPlanCache::construct_get_cartesian_plan_request( // MotionPlanCache::put_cartesian_plan( // const moveit::planning_interface::MoveGroupInterface& move_group, // const std::string& move_group_namespace, -// const moveit_msgs::msg::MotionPlanRequest& plan_request, +// const moveit_msgs::srv::GetCartesianPath::Request& plan_request, // const moveit_msgs::msg::RobotTrajectory& plan, // double execution_time_s, // double planning_time_s, @@ -1071,11 +1069,9 @@ MotionPlanCache::construct_get_cartesian_plan_request( // Query::Ptr exact_query = coll.createQuery(); // bool start_query_ok = this->extract_and_append_cartesian_plan_start_to_query( -// *exact_query, move_group, -// plan_request, exact_match_precision_); +// *exact_query, move_group, plan_request, 0); // bool goal_query_ok = this->extract_and_append_cartesian_plan_goal_to_query( -// *exact_query, move_group, -// plan_request, exact_match_precision_); +// *exact_query, move_group, plan_request, 0); // if (!start_query_ok || !goal_query_ok) // { @@ -1263,247 +1259,139 @@ MotionPlanCache::construct_get_cartesian_plan_request( // return true; // } -// bool -// MotionPlanCache::extract_and_append_cartesian_plan_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.appendRangeInclusive( -// "max_velocity_scaling_factor", -// NEXUS_MATCH_RANGE( -// plan_request.max_velocity_scaling_factor, match_tolerance) -// ); -// query.appendRangeInclusive( -// "max_acceleration_scaling_factor", -// NEXUS_MATCH_RANGE( -// plan_request.max_acceleration_scaling_factor, match_tolerance) -// ); -// query.appendRangeInclusive( -// "max_cartesian_speed", -// NEXUS_MATCH_RANGE(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); -// } -// } +bool +MotionPlanCache::extract_and_append_cartesian_plan_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_; -// // 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.appendRangeInclusive( -// meta_name + ".position", -// NEXUS_MATCH_RANGE(constraint.position, match_tolerance)); -// query.appendGTE( -// meta_name + ".tolerance_above", constraint.tolerance_above); -// query.appendLTE( -// meta_name + ".tolerance_below", constraint.tolerance_below); -// } + // 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."); + } -// // 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); + // auto original = *metadata; // Copy not supported. -// size_t pos_idx = 0; -// for (auto& constraint : position_constraints) -// { -// std::string meta_name = -// "goal_constraints.position_constraints_" + std::to_string(pos_idx++); + query.appendRangeInclusive( + "max_velocity_scaling_factor", + NEXUS_MATCH_RANGE( + plan_request.max_velocity_scaling_factor, match_tolerance)); + query.appendRangeInclusive( + "max_acceleration_scaling_factor", + NEXUS_MATCH_RANGE( + plan_request.max_acceleration_scaling_factor, match_tolerance)); + query.appendRangeInclusive( + "max_step", + NEXUS_MATCH_RANGE(plan_request.max_step, match_tolerance)); + query.appendRangeInclusive( + "jump_threshold", + NEXUS_MATCH_RANGE(plan_request.jump_threshold, match_tolerance)); -// // Compute offsets wrt. to workspace frame. -// double x_offset = 0; -// double y_offset = 0; -// double z_offset = 0; + // Waypoints + // Restating them in terms of the robot model frame (usually base_link) + std::string base_frame = move_group.getRobotModel()->getModelFrame(); -// 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; -// } -// } + // Compute offsets. + double x_offset = 0; + double y_offset = 0; + double z_offset = 0; -// query.append(meta_name + ".link_name", constraint.link_name); + geometry_msgs::msg::Quaternion quat_offset; + quat_offset.x = 0; + quat_offset.y = 0; + quat_offset.z = 0; + quat_offset.w = 1; -// query.appendRangeInclusive( -// meta_name + ".target_point_offset.x", -// NEXUS_MATCH_RANGE( -// x_offset + constraint.target_point_offset.x, match_tolerance)); -// query.appendRangeInclusive( -// meta_name + ".target_point_offset.y", -// NEXUS_MATCH_RANGE( -// y_offset + constraint.target_point_offset.y, match_tolerance)); -// query.appendRangeInclusive( -// meta_name + ".target_point_offset.z", -// NEXUS_MATCH_RANGE( -// z_offset + constraint.target_point_offset.z, match_tolerance)); -// } -// } + if (base_frame != plan_request.header.frame_id) + { + try + { + auto transform = tf_buffer_->lookupTransform( + plan_request.header.frame_id, base_frame, tf2::TimePointZero); -// // 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); + 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()); -// 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); + // (Can't. Copy not supported.) + // *metadata = original; // Undo our changes. + return false; + } + } -// 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; -// } -// } + tf2::Quaternion tf2_quat_frame_offset( + quat_offset.x, quat_offset.y, quat_offset.z, quat_offset.w); + tf2_quat_frame_offset.normalize(); -// query.append(meta_name + ".link_name", constraint.link_name); + size_t waypoint_idx = 0; + for (auto& waypoint : plan_request.waypoints) + { + std::string meta_name = "waypoints_" + std::to_string(waypoint_idx++); -// // 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); + // Apply offsets + // Position + query.appendRangeInclusive( + meta_name + ".position.x", + NEXUS_MATCH_RANGE(x_offset + waypoint.position.x, match_tolerance)); + query.appendRangeInclusive( + meta_name + ".position.y", + NEXUS_MATCH_RANGE(y_offset+ waypoint.position.y, match_tolerance)); + query.appendRangeInclusive( + meta_name + ".position.z", + NEXUS_MATCH_RANGE(z_offset+waypoint.position.z, match_tolerance)); -// // 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); + // Orientation + tf2::Quaternion tf2_quat_goal_offset( + waypoint.orientation.x, + waypoint.orientation.y, + waypoint.orientation.z, + waypoint.orientation.w); + tf2_quat_goal_offset.normalize(); -// tf2_quat_frame_offset.normalize(); -// tf2_quat_goal_offset.normalize(); + auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset; + final_quat.normalize(); -// auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset; -// final_quat.normalize(); + query.appendRangeInclusive( + meta_name + ".orientation.x", + NEXUS_MATCH_RANGE(final_quat.getX(), match_tolerance)); + query.appendRangeInclusive( + meta_name + ".orientation.y", + NEXUS_MATCH_RANGE(final_quat.getY(), match_tolerance)); + query.appendRangeInclusive( + meta_name + ".orientation.z", + NEXUS_MATCH_RANGE(final_quat.getZ(), match_tolerance)); + query.appendRangeInclusive( + meta_name + ".orientation.w", + NEXUS_MATCH_RANGE(final_quat.getW(), match_tolerance)); + } -// query.appendRangeInclusive( -// meta_name + ".target_point_offset.x", -// NEXUS_MATCH_RANGE(final_quat.getX(), match_tolerance)); -// query.appendRangeInclusive( -// meta_name + ".target_point_offset.y", -// NEXUS_MATCH_RANGE(final_quat.getY(), match_tolerance)); -// query.appendRangeInclusive( -// meta_name + ".target_point_offset.z", -// NEXUS_MATCH_RANGE(final_quat.getZ(), match_tolerance)); -// query.appendRangeInclusive( -// meta_name + ".target_point_offset.w", -// NEXUS_MATCH_RANGE(final_quat.getW(), match_tolerance)); -// } -// } + query.append("link_name", plan_request.link_name); -// return true; -// } + return true; +} // // CARTESIAN PLAN CACHING: METADATA CONSTRUCTION // bool @@ -1608,7 +1496,7 @@ MotionPlanCache::construct_get_cartesian_plan_request( bool MotionPlanCache::extract_and_append_cartesian_plan_goal_to_metadata( Metadata& metadata, - const moveit::planning_interface::MoveGroupInterface& /* move_group */, + const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::srv::GetCartesianPath::Request& plan_request) { // Make ignored members explicit @@ -1623,197 +1511,93 @@ MotionPlanCache::extract_and_append_cartesian_plan_goal_to_metadata( if (plan_request.avoid_collisions) { RCLCPP_WARN(node_->get_logger(), - "Ignoring avoid_collisions: Not supported.") + "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()); } - // // 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); - // } - // } - - // // 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()); - // } - // } + metadata.append("link_name", plan_request.link_name); return true; } From 3b70b6e3916255018aafc04355fee24c3152dc55 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 17 Oct 2023 19:29:47 -0700 Subject: [PATCH 18/34] Add start query and metadata Signed-off-by: methylDragon --- .../src/motion_plan_cache.cpp | 345 ++++++++---------- 1 file changed, 149 insertions(+), 196 deletions(-) diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp index 695c611..9821847 100644 --- a/nexus_motion_planner/src/motion_plan_cache.cpp +++ b/nexus_motion_planner/src/motion_plan_cache.cpp @@ -1153,111 +1153,87 @@ MotionPlanCache::construct_get_cartesian_plan_request( // } // // CARTESIAN PLAN CACHING: QUERY CONSTRUCTION -// bool -// MotionPlanCache::extract_and_append_cartesian_plan_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_; +bool +MotionPlanCache::extract_and_append_cartesian_plan_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."); -// } + // 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. -// 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; -// } + // auto original = *metadata; // Copy not supported. -// moveit_msgs::msg::RobotState current_state_msg; -// robotStateToRobotStateMsg(*current_state, current_state_msg); + query.append("group_name", plan_request.group_name); -// 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.appendRangeInclusive( -// "start_state.joint_state.position_" + std::to_string(i), -// NEXUS_MATCH_RANGE( -// 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.appendRangeInclusive( -// "start_state.joint_state.position_" + std::to_string(i), -// NEXUS_MATCH_RANGE( -// plan_request.start_state.joint_state.position.at(i), match_tolerance) -// ); -// } -// } -// return true; -// } + // 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. + 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.appendRangeInclusive( + "start_state.joint_state.position_" + std::to_string(i), + NEXUS_MATCH_RANGE( + 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.appendRangeInclusive( + "start_state.joint_state.position_" + std::to_string(i), + NEXUS_MATCH_RANGE( + plan_request.start_state.joint_state.position.at(i), + match_tolerance)); + } + } + + return true; +} bool MotionPlanCache::extract_and_append_cartesian_plan_goal_to_query( @@ -1393,105 +1369,82 @@ MotionPlanCache::extract_and_append_cartesian_plan_goal_to_query( return true; } -// // CARTESIAN PLAN CACHING: METADATA CONSTRUCTION -// bool -// MotionPlanCache::extract_and_append_cartesian_plan_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."); -// } +// CARTESIAN PLAN CACHING: METADATA CONSTRUCTION +bool +MotionPlanCache::extract_and_append_cartesian_plan_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); - -// // 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. -// 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; -// } + // auto original = *metadata; // Copy not supported. -// moveit_msgs::msg::RobotState current_state_msg; -// robotStateToRobotStateMsg(*current_state, current_state_msg); + metadata.append("group_name", plan_request.group_name); -// 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)); -// } -// } + // 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. -// return true; -// } + // 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. + 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 MotionPlanCache::extract_and_append_cartesian_plan_goal_to_metadata( From 87fda739441c795651f836af7460211d0f3268a5 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 17 Oct 2023 19:54:18 -0700 Subject: [PATCH 19/34] Implement top level cache ops Signed-off-by: methylDragon --- .../src/motion_plan_cache.cpp | 379 +++++++++--------- .../src/motion_plan_cache.hpp | 3 + 2 files changed, 196 insertions(+), 186 deletions(-) diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp index 9821847..0f57deb 100644 --- a/nexus_motion_planner/src/motion_plan_cache.cpp +++ b/nexus_motion_planner/src/motion_plan_cache.cpp @@ -965,192 +965,199 @@ MotionPlanCache::construct_get_cartesian_plan_request( return out; } -// std::vector::ConstPtr> -// MotionPlanCache::fetch_all_matching_cartesian_plans( -// const moveit::planning_interface::MoveGroupInterface& move_group, -// const std::string& move_group_namespace, -// const moveit_msgs::srv::GetCartesianPath::Request& plan_request, -// double start_tolerance, double goal_tolerance, bool metadata_only) -// { -// auto coll = db_->openCollection( -// "move_group_plan_cache", move_group_namespace); - -// Query::Ptr query = coll.createQuery(); - -// bool start_ok = this->extract_and_append_cartesian_plan_start_to_query( -// *query, move_group, plan_request, start_tolerance); -// bool goal_ok = this->extract_and_append_cartesian_plan_goal_to_query( -// *query, move_group, plan_request, goal_tolerance); - -// if (!start_ok || !goal_ok) -// { -// RCLCPP_ERROR(node_->get_logger(), "Could not construct plan query."); -// return {}; -// } - -// return coll.queryList( -// query, metadata_only, /* sort_by */ "execution_time_s", true); -// } - -// MessageWithMetadata::ConstPtr -// MotionPlanCache::fetch_best_matching_cartesian_plan( -// const moveit::planning_interface::MoveGroupInterface& move_group, -// const std::string& move_group_namespace, -// const moveit_msgs::srv::GetCartesianPath::Request& plan_request, -// double start_tolerance, double goal_tolerance, bool metadata_only) -// { -// // First find all matching, but metadata only. -// // Then use the ID metadata of the best plan to pull the actual message. -// auto matching_plans = this->fetch_all_matching_plans( -// move_group, move_group_namespace, -// plan_request, start_tolerance, goal_tolerance, -// true); - -// if (matching_plans.empty()) -// { -// RCLCPP_INFO(node_->get_logger(), "No matching plans found."); -// return nullptr; -// } - -// auto coll = db_->openCollection( -// "move_group_plan_cache", move_group_namespace); - -// // Best plan is at first index, since the lookup query was sorted by -// // execution_time. -// int best_plan_id = matching_plans.at(0)->lookupInt("id"); -// Query::Ptr best_query = coll.createQuery(); -// best_query->append("id", best_plan_id); - -// return coll.findOne(best_query, metadata_only); -// } - -// bool -// MotionPlanCache::put_cartesian_plan( -// 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& plan, -// double execution_time_s, -// double planning_time_s, -// bool overwrite) -// { -// // Check pre-conditions -// if (!plan.multi_dof_joint_trajectory.points.empty()) -// { -// RCLCPP_ERROR( -// node_->get_logger(), -// "Skipping insert: Multi-DOF trajectory plans are not supported."); -// return false; -// } -// if (plan_request.workspace_parameters.header.frame_id.empty() || -// plan.joint_trajectory.header.frame_id.empty()) -// { -// RCLCPP_ERROR( -// node_->get_logger(), "Skipping insert: Frame IDs cannot be empty."); -// return false; -// } -// if (plan_request.workspace_parameters.header.frame_id != -// plan.joint_trajectory.header.frame_id) -// { -// RCLCPP_ERROR( -// node_->get_logger(), -// "Skipping insert: " -// "Plan request frame (%s) does not match plan frame (%s).", -// plan_request.workspace_parameters.header.frame_id.c_str(), -// plan.joint_trajectory.header.frame_id.c_str()); -// return false; -// } - -// auto coll = db_->openCollection( -// "move_group_plan_cache", move_group_namespace); - -// // If start and goal are "exact" match, AND the candidate plan is better, -// // overwrite. -// Query::Ptr exact_query = coll.createQuery(); - -// bool start_query_ok = this->extract_and_append_cartesian_plan_start_to_query( -// *exact_query, move_group, plan_request, 0); -// bool goal_query_ok = this->extract_and_append_cartesian_plan_goal_to_query( -// *exact_query, move_group, plan_request, 0); - -// if (!start_query_ok || !goal_query_ok) -// { -// RCLCPP_ERROR( -// node_->get_logger(), -// "Skipping insert: Could not construct overwrite query."); -// return false; -// } - -// auto exact_matches = coll.queryList(exact_query, /* metadata_only */ true); -// double best_execution_time_seen = std::numeric_limits::infinity(); -// if (!exact_matches.empty()) -// { -// for (auto& match : exact_matches) -// { -// double match_execution_time_s = -// match->lookupDouble("execution_time_s"); -// if (match_execution_time_s < best_execution_time_seen) -// { -// best_execution_time_seen = match_execution_time_s; -// } - -// if (match_execution_time_s > execution_time_s) -// { -// // If we found any "exact" matches that are worse, delete them. -// if (overwrite) -// { -// int delete_id = match->lookupInt("id"); -// RCLCPP_INFO( -// node_->get_logger(), -// "Overwriting plan (id: %d): " -// "execution_time (%es) > new plan'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_seen) -// { -// Metadata::Ptr insert_metadata = coll.createMetadata(); - -// bool start_meta_ok = this->extract_and_append_cartesian_plan_start_to_metadata( -// *insert_metadata, move_group, plan_request); -// bool goal_meta_ok = this->extract_and_append_cartesian_plan_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 insert: Could not construct insert metadata."); -// return false; -// } - -// RCLCPP_INFO( -// node_->get_logger(), -// "Inserting plan: New plan execution_time (%es) " -// "is better than best plan's execution_time (%es)", -// execution_time_s, best_execution_time_seen); - -// coll.insert(plan, insert_metadata); -// return true; -// } - -// RCLCPP_INFO( -// node_->get_logger(), -// "Skipping insert: New plan execution_time (%es) " -// "is worse than best plan's execution_time (%es)", -// execution_time_s, best_execution_time_seen); -// return false; -// } +std::vector::ConstPtr> +MotionPlanCache::fetch_all_matching_cartesian_plans( + 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) +{ + auto coll = db_->openCollection( + "move_group_cartesian_plan_cache", move_group_namespace); + + Query::Ptr query = coll.createQuery(); + + bool start_ok = this->extract_and_append_cartesian_plan_start_to_query( + *query, move_group, plan_request, start_tolerance); + bool goal_ok = this->extract_and_append_cartesian_plan_goal_to_query( + *query, move_group, plan_request, goal_tolerance); + + if (!start_ok || !goal_ok) + { + RCLCPP_ERROR(node_->get_logger(), "Could not construct plan query."); + return {}; + } + + query->appendGTE("fraction", min_fraction); + + return coll.queryList( + query, metadata_only, /* sort_by */ "execution_time_s", true); +} + +MessageWithMetadata::ConstPtr +MotionPlanCache::fetch_best_matching_cartesian_plan( + 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) +{ + // First find all matching, but metadata only. + // Then use the ID metadata of the best plan to pull the actual message. + auto matching_plans = this->fetch_all_matching_cartesian_plans( + move_group, move_group_namespace, + plan_request, min_fraction, + start_tolerance, goal_tolerance, true); + + if (matching_plans.empty()) + { + RCLCPP_INFO(node_->get_logger(), "No matching plans found."); + return nullptr; + } + + auto coll = db_->openCollection( + "move_group_plan_cache", move_group_namespace); + + // Best plan is at first index, since the lookup query was sorted by + // execution_time. + int best_plan_id = matching_plans.at(0)->lookupInt("id"); + Query::Ptr best_query = coll.createQuery(); + best_query->append("id", best_plan_id); + + return coll.findOne(best_query, metadata_only); +} + +bool +MotionPlanCache::put_cartesian_plan( + 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& plan, + double execution_time_s, + double planning_time_s, + double fraction, + bool overwrite) +{ + // Check pre-conditions + if (!plan.multi_dof_joint_trajectory.points.empty()) + { + RCLCPP_ERROR( + node_->get_logger(), + "Skipping insert: Multi-DOF trajectory plans are not supported."); + return false; + } + if (plan_request.header.frame_id.empty() || + plan.joint_trajectory.header.frame_id.empty()) + { + RCLCPP_ERROR( + node_->get_logger(), "Skipping insert: Frame IDs cannot be empty."); + return false; + } + if (plan_request.header.frame_id != plan.joint_trajectory.header.frame_id) + { + RCLCPP_ERROR( + node_->get_logger(), + "Skipping insert: " + "Plan request frame (%s) does not match plan frame (%s).", + plan_request.header.frame_id.c_str(), + plan.joint_trajectory.header.frame_id.c_str()); + return false; + } + + auto coll = db_->openCollection( + "move_group_cartesian_plan_cache", move_group_namespace); + + // If start and goal are "exact" match, AND the candidate plan is better, + // overwrite. + Query::Ptr exact_query = coll.createQuery(); + + bool start_query_ok = this->extract_and_append_cartesian_plan_start_to_query( + *exact_query, move_group, plan_request, 0); + bool goal_query_ok = this->extract_and_append_cartesian_plan_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 insert: Could not construct overwrite query."); + return false; + } + + auto exact_matches = coll.queryList(exact_query, /* metadata_only */ true); + double best_execution_time_seen = std::numeric_limits::infinity(); + if (!exact_matches.empty()) + { + for (auto& match : exact_matches) + { + double match_execution_time_s = + match->lookupDouble("execution_time_s"); + if (match_execution_time_s < best_execution_time_seen) + { + best_execution_time_seen = match_execution_time_s; + } + + if (match_execution_time_s > execution_time_s) + { + // If we found any "exact" matches that are worse, delete them. + if (overwrite) + { + int delete_id = match->lookupInt("id"); + RCLCPP_INFO( + node_->get_logger(), + "Overwriting plan (id: %d): " + "execution_time (%es) > new plan'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_seen) + { + Metadata::Ptr insert_metadata = coll.createMetadata(); + + bool start_meta_ok = this->extract_and_append_cartesian_plan_start_to_metadata( + *insert_metadata, move_group, plan_request); + bool goal_meta_ok = this->extract_and_append_cartesian_plan_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 insert: Could not construct insert metadata."); + return false; + } + + RCLCPP_INFO( + node_->get_logger(), + "Inserting plan: New plan execution_time (%es) " + "is better than best plan's execution_time (%es) at fraction (%es)", + execution_time_s, best_execution_time_seen, fraction); + + coll.insert(plan, insert_metadata); + return true; + } + + RCLCPP_INFO( + node_->get_logger(), + "Skipping insert: New plan execution_time (%es) " + "is worse than best plan's execution_time (%es) at fraction (%es)", + execution_time_s, best_execution_time_seen, fraction); + return false; +} // // CARTESIAN PLAN CACHING: QUERY CONSTRUCTION bool diff --git a/nexus_motion_planner/src/motion_plan_cache.hpp b/nexus_motion_planner/src/motion_plan_cache.hpp index 5426f2a..4f916ad 100644 --- a/nexus_motion_planner/src/motion_plan_cache.hpp +++ b/nexus_motion_planner/src/motion_plan_cache.hpp @@ -171,6 +171,7 @@ class MotionPlanCache 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); warehouse_ros::MessageWithMetadata< @@ -180,6 +181,7 @@ class MotionPlanCache 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); bool put_cartesian_plan( @@ -189,6 +191,7 @@ class MotionPlanCache const moveit_msgs::msg::RobotTrajectory& plan, double execution_time_s, double planning_time_s, + double fraction, bool overwrite = true); // QUERY CONSTRUCTION From 7d705241df8f9ed2c91cef92f014adbef86587d2 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 17 Oct 2023 20:40:36 -0700 Subject: [PATCH 20/34] Use motion plan cache for cartesian plans Signed-off-by: methylDragon --- .../src/motion_planner_server.cpp | 144 +++++++++++++++--- 1 file changed, 127 insertions(+), 17 deletions(-) diff --git a/nexus_motion_planner/src/motion_planner_server.cpp b/nexus_motion_planner/src/motion_planner_server.cpp index 9355f57..055aae3 100644 --- a/nexus_motion_planner/src/motion_planner_server.cpp +++ b/nexus_motion_planner/src/motion_planner_server.cpp @@ -42,7 +42,8 @@ PlannerDatabaseMode str_to_planner_database_mode(std::string s) { std::string s_lower = str_tolower(s); - // Using a switch-case here is... inconvenient (needs constexpr hashing or a map), so we don't. + // Using a switch-case here is... inconvenient (needs constexpr hashing or a + // map), so we don't. if (s_lower == "training_overwrite" || s_lower == "trainingoverwrite") { return PlannerDatabaseMode::TrainingOverwrite; @@ -451,8 +452,7 @@ auto MotionPlannerServer::on_shutdown(const LifecycleState& /*state*/) //============================================================================== void MotionPlannerServer::plan_with_move_group( - const GetMotionPlanService::ServiceType::Request& req, - Response res) + const GetMotionPlanService::ServiceType::Request& req, Response res) { RCLCPP_INFO( this->get_logger(), @@ -598,24 +598,134 @@ void MotionPlannerServer::plan_with_move_group( // TODO(YV): For now we use simple cartesian interpolation. OMPL supports // constrained planning so we can consider adding orientation or line // constraints to the end-effector link instead and call plan(). - const double error = interface->computeCartesianPath( - waypoints, - _cartesian_max_step, - _cartesian_jump_threshold, - res->result.trajectory, - _collision_aware_cartesian_path - ); - RCLCPP_INFO( - this->get_logger(), - "Cartesian interpolation returned a fraction of [%.2f]", error - ); - if (error <= 0.0) + + moveit_msgs::msg::RobotTrajectory cartesian_plan; + double fraction; + bool cartesian_plan_is_from_cache = false; + auto cartesian_plan_req_msg = + _motion_plan_cache->construct_get_cartesian_plan_request( + *interface, waypoints, _cartesian_max_step, _cartesian_jump_threshold, + _collision_aware_cartesian_path); + + // Fetch if in execute mode. + if (cache_mode_is_execute(_cache_mode)) { - res->result.error_code.val = -1; + auto fetch_start = this->now(); + auto fetched_cartesian_plan = + _motion_plan_cache->fetch_best_matching_cartesian_plan( + *interface, robot_name, cartesian_plan_req_msg, + /* min_fraction */ 1.0, + _cache_start_match_tolerance, _cache_goal_match_tolerance); + auto fetch_end = this->now(); + // Set plan if a cached cartesian plan was fetched. + if (fetched_cartesian_plan) + { + fraction = fetched_cartesian_plan->lookupDouble("fraction"); + cartesian_plan_is_from_cache = true; + cartesian_plan = *fetched_cartesian_plan; + res->result.error_code.val = + moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + res->result.planning_time = (fetch_end - fetch_start).seconds(); + RCLCPP_INFO( + this->get_logger(), + "Cache fetch took %es, planning time of fetched plan was: %es", + (fetch_end - fetch_start).seconds(), + fetched_cartesian_plan->lookupDouble("planning_time_s")); + } + // Fail if ReadOnly mode and no cached cartesian plan was fetched. + else if (_cache_mode == PlannerDatabaseMode::ExecuteReadOnly) + { + RCLCPP_ERROR( + this->get_logger(), + "Cache mode was ExecuteReadOnly, and could not find " + "cached cartesian plan for cartesian plan request: \n\n%s", + moveit_msgs::srv::to_yaml(cartesian_plan_req_msg).c_str()); + res->result.error_code.val = + moveit_msgs::msg::MoveItErrorCodes::FAILURE; + return; + } + } + + // Plan if needed. + // This is if we didn't fetch a cartesian plan from the cache. + // (In training or unset mode we never attempt to fetch, so it will always + // plan.) + if (!cartesian_plan_is_from_cache) + { + auto cartesian_plan_start = this->now(); + fraction = interface->computeCartesianPath( + waypoints, + _cartesian_max_step, + _cartesian_jump_threshold, + cartesian_plan, + _collision_aware_cartesian_path + ); + auto cartesian_plan_end = this->now(); + + RCLCPP_INFO( + this->get_logger(), + "Cartesian interpolation returned a fraction of [%.2f]", fraction + ); + if (fraction <= 0.0) + { + res->result.error_code.val = -1; + } + else + { + res->result.error_code.val = 1; + } + + res->result.planning_time = + (cartesian_plan_end - cartesian_plan_start).seconds(); + + RCLCPP_INFO( + this->get_logger(), + "Plan status: %d, planning time: %es", + res->result.error_code.val, res->result.planning_time); } else { - res->result.error_code.val = 1; + if (fraction <= 0) + { + res->result.error_code.val = -1; + } + else + { + res->result.error_code.val = 1; + } + } + + res->result.trajectory_start = + std::move(cartesian_plan_req_msg.start_state); + res->result.trajectory = std::move(cartesian_plan); + + if (res->result.error_code.val != + moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + { + RCLCPP_WARN( + this->get_logger(), + "Cartesian planning did not succeed: %d", res->result.error_code.val); + return; + } + + // Put plan if in training mode. + // Make sure we check if the plan we have was fetched (so we don't have + // duplicate caches.) + if (cache_mode_is_training(_cache_mode) && !cartesian_plan_is_from_cache) + { + if (!_motion_plan_cache->put_cartesian_plan( + *interface, robot_name, + std::move(cartesian_plan_req_msg), std::move(res->result.trajectory), + rclcpp::Duration( + res->result.trajectory.joint_trajectory.points.back() + .time_from_start + ).seconds(), + res->result.planning_time, fraction, + _cache_mode == PlannerDatabaseMode::TrainingOverwrite)) + { + RCLCPP_WARN( + this->get_logger(), "Did not put cartesian plan into cache."); + } } } else From 188215b604c2dba0e12f3443557f766e66f3164b Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 17 Oct 2023 20:47:30 -0700 Subject: [PATCH 21/34] Allow mismatched plan frames since we coerce anyway Signed-off-by: methylDragon --- .../src/motion_plan_cache.cpp | 21 +++++++------------ 1 file changed, 7 insertions(+), 14 deletions(-) diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp index 0f57deb..e412812 100644 --- a/nexus_motion_planner/src/motion_plan_cache.cpp +++ b/nexus_motion_planner/src/motion_plan_cache.cpp @@ -1054,16 +1054,6 @@ MotionPlanCache::put_cartesian_plan( node_->get_logger(), "Skipping insert: Frame IDs cannot be empty."); return false; } - if (plan_request.header.frame_id != plan.joint_trajectory.header.frame_id) - { - RCLCPP_ERROR( - node_->get_logger(), - "Skipping insert: " - "Plan request frame (%s) does not match plan frame (%s).", - plan_request.header.frame_id.c_str(), - plan.joint_trajectory.header.frame_id.c_str()); - return false; - } auto coll = db_->openCollection( "move_group_cartesian_plan_cache", move_group_namespace); @@ -1125,9 +1115,11 @@ MotionPlanCache::put_cartesian_plan( { Metadata::Ptr insert_metadata = coll.createMetadata(); - bool start_meta_ok = this->extract_and_append_cartesian_plan_start_to_metadata( + bool start_meta_ok = + this->extract_and_append_cartesian_plan_start_to_metadata( *insert_metadata, move_group, plan_request); - bool goal_meta_ok = this->extract_and_append_cartesian_plan_goal_to_metadata( + bool goal_meta_ok = + this->extract_and_append_cartesian_plan_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); @@ -1537,8 +1529,8 @@ MotionPlanCache::extract_and_append_cartesian_plan_goal_to_metadata( // 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); + 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( @@ -1558,6 +1550,7 @@ MotionPlanCache::extract_and_append_cartesian_plan_goal_to_metadata( } metadata.append("link_name", plan_request.link_name); + metadata.append("header.frame_id", base_frame); return true; } From b2cda90f1604e0e83ef4bf117330161a545706c2 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 17 Oct 2023 21:08:16 -0700 Subject: [PATCH 22/34] Fix move bug Signed-off-by: methylDragon --- nexus_motion_planner/src/motion_plan_cache.cpp | 4 ++-- nexus_motion_planner/src/motion_planner_server.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp index e412812..3541f9c 100644 --- a/nexus_motion_planner/src/motion_plan_cache.cpp +++ b/nexus_motion_planner/src/motion_plan_cache.cpp @@ -945,8 +945,8 @@ MotionPlanCache::construct_get_cartesian_plan_request( moveit_msgs::msg::MotionPlanRequest tmp; move_group.constructMotionPlanRequest(tmp); - out.start_state = tmp.start_state; - out.group_name = tmp.group_name; + 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; diff --git a/nexus_motion_planner/src/motion_planner_server.cpp b/nexus_motion_planner/src/motion_planner_server.cpp index 055aae3..58d9208 100644 --- a/nexus_motion_planner/src/motion_planner_server.cpp +++ b/nexus_motion_planner/src/motion_planner_server.cpp @@ -695,8 +695,8 @@ void MotionPlannerServer::plan_with_move_group( } } - res->result.trajectory_start = - std::move(cartesian_plan_req_msg.start_state); + // Do NOT move this. We use the cartesian_plan_req_msg later. + res->result.trajectory_start = cartesian_plan_req_msg.start_state; res->result.trajectory = std::move(cartesian_plan); if (res->result.error_code.val != From 6ad592e61ec66219f2e7fe4b04cd0747a5d3d3ef Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 20 Nov 2023 11:37:13 -0800 Subject: [PATCH 23/34] Remove query appending macro Signed-off-by: methylDragon --- .../src/motion_plan_cache.cpp | 184 ++++++++---------- 1 file changed, 81 insertions(+), 103 deletions(-) diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp index 3541f9c..a0bdb20 100644 --- a/nexus_motion_planner/src/motion_plan_cache.cpp +++ b/nexus_motion_planner/src/motion_plan_cache.cpp @@ -26,13 +26,18 @@ namespace nexus { namespace motion_planner { -#define NEXUS_MATCH_RANGE(arg, tolerance) \ - arg - tolerance / 2, arg + tolerance / 2 - using warehouse_ros::MessageWithMetadata; using warehouse_ros::Metadata; using warehouse_ros::Query; +// Append a range inclusive query with some tolerance about some center value. +void queryAppendRangeInclusiveWithTolerance( + Query& query, const std::string& name, double center, double tolerance) +{ + query.appendRangeInclusive( + name, center - tolerance / 2, center + tolerance / 2); +} + MotionPlanCache::MotionPlanCache(const rclcpp::Node::SharedPtr& node) : node_(node) { @@ -336,11 +341,9 @@ MotionPlanCache::extract_and_append_plan_start_to_query( query.append( "start_state.joint_state.name_" + std::to_string(i), current_state_msg.joint_state.name.at(i)); - query.appendRangeInclusive( - "start_state.joint_state.position_" + std::to_string(i), - NEXUS_MATCH_RANGE( - current_state_msg.joint_state.position.at(i), match_tolerance) - ); + queryAppendRangeInclusiveWithTolerance( + query, "start_state.joint_state.position_" + std::to_string(i), + current_state_msg.joint_state.position.at(i), match_tolerance); } } else @@ -352,11 +355,9 @@ MotionPlanCache::extract_and_append_plan_start_to_query( query.append( "start_state.joint_state.name_" + std::to_string(i), plan_request.start_state.joint_state.name.at(i)); - query.appendRangeInclusive( - "start_state.joint_state.position_" + std::to_string(i), - NEXUS_MATCH_RANGE( - plan_request.start_state.joint_state.position.at(i), match_tolerance) - ); + queryAppendRangeInclusiveWithTolerance( + query, "start_state.joint_state.position_" + std::to_string(i), + plan_request.start_state.joint_state.position.at(i), match_tolerance); } } return true; @@ -398,19 +399,15 @@ MotionPlanCache::extract_and_append_plan_goal_to_query( // auto original = *query; // Copy not supported. - query.appendRangeInclusive( - "max_velocity_scaling_factor", - NEXUS_MATCH_RANGE( - plan_request.max_velocity_scaling_factor, match_tolerance) - ); - query.appendRangeInclusive( - "max_acceleration_scaling_factor", - NEXUS_MATCH_RANGE( - plan_request.max_acceleration_scaling_factor, match_tolerance) - ); - query.appendRangeInclusive( - "max_cartesian_speed", - NEXUS_MATCH_RANGE(plan_request.max_cartesian_speed, match_tolerance)); + queryAppendRangeInclusiveWithTolerance( + query, "max_velocity_scaling_factor", + plan_request.max_velocity_scaling_factor, match_tolerance); + queryAppendRangeInclusiveWithTolerance( + query, "max_acceleration_scaling_factor", + plan_request.max_acceleration_scaling_factor, match_tolerance); + queryAppendRangeInclusiveWithTolerance( + query, "max_cartesian_speed", + plan_request.max_cartesian_speed, match_tolerance); // Extract constraints (so we don't have cardinality on goal_constraint idx.) std::vector joint_constraints; @@ -440,13 +437,10 @@ MotionPlanCache::extract_and_append_plan_goal_to_query( "goal_constraints.joint_constraints_" + std::to_string(joint_idx++); query.append(meta_name + ".joint_name", constraint.joint_name); - query.appendRangeInclusive( - meta_name + ".position", - NEXUS_MATCH_RANGE(constraint.position, match_tolerance)); - query.appendGTE( - meta_name + ".tolerance_above", constraint.tolerance_above); - query.appendLTE( - meta_name + ".tolerance_below", constraint.tolerance_below); + queryAppendRangeInclusiveWithTolerance( + query, meta_name + ".position", constraint.position, match_tolerance); + query.appendGTE(meta_name + ".tolerance_above", constraint.tolerance_above); + query.appendLTE(meta_name + ".tolerance_below", constraint.tolerance_below); } // Position constraints @@ -501,18 +495,15 @@ MotionPlanCache::extract_and_append_plan_goal_to_query( query.append(meta_name + ".link_name", constraint.link_name); - query.appendRangeInclusive( - meta_name + ".target_point_offset.x", - NEXUS_MATCH_RANGE( - x_offset + constraint.target_point_offset.x, match_tolerance)); - query.appendRangeInclusive( - meta_name + ".target_point_offset.y", - NEXUS_MATCH_RANGE( - y_offset + constraint.target_point_offset.y, match_tolerance)); - query.appendRangeInclusive( - meta_name + ".target_point_offset.z", - NEXUS_MATCH_RANGE( - z_offset + constraint.target_point_offset.z, match_tolerance)); + queryAppendRangeInclusiveWithTolerance( + query, meta_name + ".target_point_offset.x", + x_offset + constraint.target_point_offset.x, match_tolerance); + queryAppendRangeInclusiveWithTolerance( + query, meta_name + ".target_point_offset.y", + y_offset + constraint.target_point_offset.y, match_tolerance); + queryAppendRangeInclusiveWithTolerance( + query, meta_name + ".target_point_offset.z", + z_offset + constraint.target_point_offset.z, match_tolerance); } } @@ -586,18 +577,18 @@ MotionPlanCache::extract_and_append_plan_goal_to_query( auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset; final_quat.normalize(); - query.appendRangeInclusive( - meta_name + ".target_point_offset.x", - NEXUS_MATCH_RANGE(final_quat.getX(), match_tolerance)); - query.appendRangeInclusive( - meta_name + ".target_point_offset.y", - NEXUS_MATCH_RANGE(final_quat.getY(), match_tolerance)); - query.appendRangeInclusive( - meta_name + ".target_point_offset.z", - NEXUS_MATCH_RANGE(final_quat.getZ(), match_tolerance)); - query.appendRangeInclusive( - meta_name + ".target_point_offset.w", - NEXUS_MATCH_RANGE(final_quat.getW(), match_tolerance)); + queryAppendRangeInclusiveWithTolerance( + query, meta_name + ".target_point_offset.x", + final_quat.getX(), match_tolerance); + queryAppendRangeInclusiveWithTolerance( + query, meta_name + ".target_point_offset.y", + final_quat.getY(), match_tolerance); + queryAppendRangeInclusiveWithTolerance( + query, meta_name + ".target_point_offset.z", + final_quat.getZ(), match_tolerance); + queryAppendRangeInclusiveWithTolerance( + query, meta_name + ".target_point_offset.w", + final_quat.getW(), match_tolerance); } } @@ -1208,10 +1199,9 @@ MotionPlanCache::extract_and_append_cartesian_plan_start_to_query( query.append( "start_state.joint_state.name_" + std::to_string(i), current_state_msg.joint_state.name.at(i)); - query.appendRangeInclusive( - "start_state.joint_state.position_" + std::to_string(i), - NEXUS_MATCH_RANGE( - current_state_msg.joint_state.position.at(i), match_tolerance)); + queryAppendRangeInclusiveWithTolerance( + query, "start_state.joint_state.position_" + std::to_string(i), + current_state_msg.joint_state.position.at(i), match_tolerance); } } else @@ -1223,11 +1213,9 @@ MotionPlanCache::extract_and_append_cartesian_plan_start_to_query( query.append( "start_state.joint_state.name_" + std::to_string(i), plan_request.start_state.joint_state.name.at(i)); - query.appendRangeInclusive( - "start_state.joint_state.position_" + std::to_string(i), - NEXUS_MATCH_RANGE( - plan_request.start_state.joint_state.position.at(i), - match_tolerance)); + queryAppendRangeInclusiveWithTolerance( + query, "start_state.joint_state.position_" + std::to_string(i), + plan_request.start_state.joint_state.position.at(i), match_tolerance); } } @@ -1260,20 +1248,16 @@ MotionPlanCache::extract_and_append_cartesian_plan_goal_to_query( // auto original = *metadata; // Copy not supported. - query.appendRangeInclusive( - "max_velocity_scaling_factor", - NEXUS_MATCH_RANGE( - plan_request.max_velocity_scaling_factor, match_tolerance)); - query.appendRangeInclusive( - "max_acceleration_scaling_factor", - NEXUS_MATCH_RANGE( - plan_request.max_acceleration_scaling_factor, match_tolerance)); - query.appendRangeInclusive( - "max_step", - NEXUS_MATCH_RANGE(plan_request.max_step, match_tolerance)); - query.appendRangeInclusive( - "jump_threshold", - NEXUS_MATCH_RANGE(plan_request.jump_threshold, match_tolerance)); + queryAppendRangeInclusiveWithTolerance( + query, "max_velocity_scaling_factor", + plan_request.max_velocity_scaling_factor, match_tolerance); + queryAppendRangeInclusiveWithTolerance( + query, "max_acceleration_scaling_factor", + plan_request.max_acceleration_scaling_factor, match_tolerance); + queryAppendRangeInclusiveWithTolerance( + query, "max_step", plan_request.max_step, match_tolerance); + queryAppendRangeInclusiveWithTolerance( + query, "jump_threshold", plan_request.jump_threshold, match_tolerance); // Waypoints // Restating them in terms of the robot model frame (usually base_link) @@ -1328,15 +1312,15 @@ MotionPlanCache::extract_and_append_cartesian_plan_goal_to_query( // Apply offsets // Position - query.appendRangeInclusive( - meta_name + ".position.x", - NEXUS_MATCH_RANGE(x_offset + waypoint.position.x, match_tolerance)); - query.appendRangeInclusive( - meta_name + ".position.y", - NEXUS_MATCH_RANGE(y_offset+ waypoint.position.y, match_tolerance)); - query.appendRangeInclusive( - meta_name + ".position.z", - NEXUS_MATCH_RANGE(z_offset+waypoint.position.z, match_tolerance)); + queryAppendRangeInclusiveWithTolerance( + query, meta_name + ".position.x", + x_offset + waypoint.position.x, match_tolerance); + queryAppendRangeInclusiveWithTolerance( + query, meta_name + ".position.y", + y_offset+ waypoint.position.y, match_tolerance); + queryAppendRangeInclusiveWithTolerance( + query, meta_name + ".position.z", + z_offset+waypoint.position.z, match_tolerance); // Orientation tf2::Quaternion tf2_quat_goal_offset( @@ -1349,18 +1333,14 @@ MotionPlanCache::extract_and_append_cartesian_plan_goal_to_query( auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset; final_quat.normalize(); - query.appendRangeInclusive( - meta_name + ".orientation.x", - NEXUS_MATCH_RANGE(final_quat.getX(), match_tolerance)); - query.appendRangeInclusive( - meta_name + ".orientation.y", - NEXUS_MATCH_RANGE(final_quat.getY(), match_tolerance)); - query.appendRangeInclusive( - meta_name + ".orientation.z", - NEXUS_MATCH_RANGE(final_quat.getZ(), match_tolerance)); - query.appendRangeInclusive( - meta_name + ".orientation.w", - NEXUS_MATCH_RANGE(final_quat.getW(), match_tolerance)); + queryAppendRangeInclusiveWithTolerance( + query, meta_name + ".orientation.x", final_quat.getX(), match_tolerance); + queryAppendRangeInclusiveWithTolerance( + query, meta_name + ".orientation.y", final_quat.getY(), match_tolerance); + queryAppendRangeInclusiveWithTolerance( + query, meta_name + ".orientation.z", final_quat.getZ(), match_tolerance); + queryAppendRangeInclusiveWithTolerance( + query, meta_name + ".orientation.w", final_quat.getW(), match_tolerance); } query.append("link_name", plan_request.link_name); @@ -1555,7 +1535,5 @@ MotionPlanCache::extract_and_append_cartesian_plan_goal_to_metadata( return true; } -#undef NEXUS_MATCH_RANGE - } // namespace motion_planner } // namespace nexus From 5563f3cd4064a1ada1c4899f605544af8ecbcf18 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 20 Nov 2023 11:49:17 -0800 Subject: [PATCH 24/34] Default to warehouse_ros plugin if warehouse plugin isn't set Signed-off-by: methylDragon --- nexus_motion_planner/src/motion_plan_cache.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp index a0bdb20..9ad674b 100644 --- a/nexus_motion_planner/src/motion_plan_cache.cpp +++ b/nexus_motion_planner/src/motion_plan_cache.cpp @@ -41,17 +41,13 @@ void queryAppendRangeInclusiveWithTolerance( MotionPlanCache::MotionPlanCache(const rclcpp::Node::SharedPtr& node) : node_(node) { - if (!node_->has_parameter("warehouse_plugin")) - { - node_->declare_parameter( - "warehouse_plugin", "warehouse_ros_sqlite::DatabaseConnection"); - } - tf_buffer_ = std::make_unique(node_->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); + // If the `warehouse_plugin` parameter isn't set, defaults to warehouse_ros' + // default. warehouse_ros::DatabaseLoader loader(node_); db_ = loader.loadDatabase(); } From 03afb8e898fa76ef41507727368769927de1d0ae Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 20 Nov 2023 11:58:02 -0800 Subject: [PATCH 25/34] Return and use init result Signed-off-by: methylDragon --- nexus_motion_planner/src/motion_plan_cache.cpp | 4 ++-- nexus_motion_planner/src/motion_plan_cache.hpp | 2 +- nexus_motion_planner/src/motion_planner_server.cpp | 8 ++++++-- 3 files changed, 9 insertions(+), 5 deletions(-) diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp index 9ad674b..7e0d3d8 100644 --- a/nexus_motion_planner/src/motion_plan_cache.cpp +++ b/nexus_motion_planner/src/motion_plan_cache.cpp @@ -52,7 +52,7 @@ MotionPlanCache::MotionPlanCache(const rclcpp::Node::SharedPtr& node) db_ = loader.loadDatabase(); } -void MotionPlanCache::init( +bool MotionPlanCache::init( const std::string& db_path, uint32_t db_port, double exact_match_precision) { RCLCPP_INFO( @@ -62,7 +62,7 @@ void MotionPlanCache::init( exact_match_precision_ = exact_match_precision; db_->setParams(db_path, db_port); - db_->connect(); + return db_->connect(); } // ============================================================================= diff --git a/nexus_motion_planner/src/motion_plan_cache.hpp b/nexus_motion_planner/src/motion_plan_cache.hpp index 4f916ad..e637e35 100644 --- a/nexus_motion_planner/src/motion_plan_cache.hpp +++ b/nexus_motion_planner/src/motion_plan_cache.hpp @@ -88,7 +88,7 @@ class MotionPlanCache // it... explicit MotionPlanCache(const rclcpp::Node::SharedPtr& node); - void init( + bool init( const std::string& db_path = ":memory:", uint32_t db_port = 0, double exact_match_precision = 1e-6); diff --git a/nexus_motion_planner/src/motion_planner_server.cpp b/nexus_motion_planner/src/motion_planner_server.cpp index 58d9208..4f7dc27 100644 --- a/nexus_motion_planner/src/motion_planner_server.cpp +++ b/nexus_motion_planner/src/motion_planner_server.cpp @@ -304,8 +304,12 @@ auto MotionPlannerServer::on_configure(const LifecycleState& /*state*/) if (_cache_mode != PlannerDatabaseMode::Unset) { - _motion_plan_cache->init( - _cache_db_host, _cache_db_port, _cache_exact_match_tolerance); + if (!_motion_plan_cache->init( + _cache_db_host, _cache_db_port, _cache_exact_match_tolerance)) + { + RCLCPP_ERROR(this->get_logger(), "Could not init motion plan cache."); + return CallbackReturn::ERROR; + } } if (_use_move_group_interfaces) From 0e84dd4576d070a1ed11d1cdac9d492ca5aedaf7 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 20 Nov 2023 12:13:25 -0800 Subject: [PATCH 26/34] Add todo for catching exceptions Signed-off-by: methylDragon --- nexus_motion_planner/src/motion_planner_server.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nexus_motion_planner/src/motion_planner_server.cpp b/nexus_motion_planner/src/motion_planner_server.cpp index 4f7dc27..3a26de4 100644 --- a/nexus_motion_planner/src/motion_planner_server.cpp +++ b/nexus_motion_planner/src/motion_planner_server.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +// TODO: Catch warehouse_ros exceptions if they are thrown. + #include "motion_planner_server.hpp" std::string str_tolower(std::string s) From 6004f96fc39d4b287ab5c142adb1fc220c83a4b8 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 20 Nov 2023 12:56:12 -0800 Subject: [PATCH 27/34] Implement plan fetching with configurable key Signed-off-by: methylDragon --- .../src/motion_plan_cache.cpp | 32 ++++++++-------- .../src/motion_plan_cache.hpp | 37 ++++++++++++++++--- 2 files changed, 46 insertions(+), 23 deletions(-) diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp index 7e0d3d8..39e0b78 100644 --- a/nexus_motion_planner/src/motion_plan_cache.cpp +++ b/nexus_motion_planner/src/motion_plan_cache.cpp @@ -74,7 +74,8 @@ MotionPlanCache::fetch_all_matching_plans( 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) + double start_tolerance, double goal_tolerance, bool metadata_only, + const std::string& sort_by) { auto coll = db_->openCollection( "move_group_plan_cache", move_group_namespace); @@ -92,8 +93,7 @@ MotionPlanCache::fetch_all_matching_plans( return {}; } - return coll.queryList( - query, metadata_only, /* sort_by */ "execution_time_s", true); + return coll.queryList(query, metadata_only, sort_by, true); } MessageWithMetadata::ConstPtr @@ -101,14 +101,14 @@ MotionPlanCache::fetch_best_matching_plan( 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) + 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_plans = this->fetch_all_matching_plans( move_group, move_group_namespace, - plan_request, start_tolerance, goal_tolerance, - true); + plan_request, start_tolerance, goal_tolerance, true, sort_by); if (matching_plans.empty()) { @@ -134,9 +134,7 @@ MotionPlanCache::put_plan( const std::string& move_group_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, const moveit_msgs::msg::RobotTrajectory& plan, - double execution_time_s, - double planning_time_s, - bool overwrite) + double execution_time_s, double planning_time_s, bool overwrite) { // Check pre-conditions if (!plan.multi_dof_joint_trajectory.points.empty()) @@ -921,8 +919,8 @@ MotionPlanCache::extract_and_append_plan_goal_to_metadata( moveit_msgs::srv::GetCartesianPath::Request MotionPlanCache::construct_get_cartesian_plan_request( moveit::planning_interface::MoveGroupInterface& move_group, - const std::vector& waypoints, double step, - double jump_threshold, bool avoid_collisions) + const std::vector& waypoints, + double step, double jump_threshold, bool avoid_collisions) { moveit_msgs::srv::GetCartesianPath::Request out; @@ -958,7 +956,8 @@ MotionPlanCache::fetch_all_matching_cartesian_plans( 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) + double start_tolerance, double goal_tolerance, bool metadata_only, + const std::string& sort_by) { auto coll = db_->openCollection( "move_group_cartesian_plan_cache", move_group_namespace); @@ -977,9 +976,7 @@ MotionPlanCache::fetch_all_matching_cartesian_plans( } query->appendGTE("fraction", min_fraction); - - return coll.queryList( - query, metadata_only, /* sort_by */ "execution_time_s", true); + return coll.queryList(query, metadata_only, sort_by, true); } MessageWithMetadata::ConstPtr @@ -988,14 +985,15 @@ MotionPlanCache::fetch_best_matching_cartesian_plan( 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) + 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_plans = this->fetch_all_matching_cartesian_plans( move_group, move_group_namespace, plan_request, min_fraction, - start_tolerance, goal_tolerance, true); + start_tolerance, goal_tolerance, true, sort_by); if (matching_plans.empty()) { diff --git a/nexus_motion_planner/src/motion_plan_cache.hpp b/nexus_motion_planner/src/motion_plan_cache.hpp index e637e35..89e06ce 100644 --- a/nexus_motion_planner/src/motion_plan_cache.hpp +++ b/nexus_motion_planner/src/motion_plan_cache.hpp @@ -97,6 +97,9 @@ class MotionPlanCache // MOTION PLAN 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< warehouse_ros::MessageWithMetadata< moveit_msgs::msg::RobotTrajectory @@ -106,8 +109,13 @@ class MotionPlanCache 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); + double start_tolerance, + double goal_tolerance, + bool metadata_only = false, + const std::string& sort_by = "execution_time_s"); + // Fetches the best plan that fits within the requested tolerances for start + // and goal conditions, by some db column. warehouse_ros::MessageWithMetadata< moveit_msgs::msg::RobotTrajectory >::ConstPtr @@ -115,7 +123,10 @@ class MotionPlanCache 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); + double start_tolerance, + double goal_tolerance, + bool metadata_only = false, + const std::string& sort_by = "execution_time_s"); bool put_plan( const moveit::planning_interface::MoveGroupInterface& move_group, @@ -154,14 +165,20 @@ class MotionPlanCache // CARTESIAN PLAN CACHING // =========================================================================== // TOP LEVEL OPS + // This mimics the move group computeCartesianPath signature (without path // constraints). moveit_msgs::srv::GetCartesianPath::Request construct_get_cartesian_plan_request( moveit::planning_interface::MoveGroupInterface& move_group, - const std::vector& waypoints, double step, - double jump_threshold, bool avoid_collisions = true); + const std::vector& waypoints, + double step, + double jump_threshold, + bool avoid_collisions = true); + // Fetches all cartesian plans that fit within the requested tolerances for + // start and goal conditions, returning them as a vector, sorted by some db + // column. std::vector< warehouse_ros::MessageWithMetadata< moveit_msgs::msg::RobotTrajectory @@ -172,8 +189,13 @@ class MotionPlanCache 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); + double start_tolerance, + double goal_tolerance, + bool metadata_only = false, + const std::string& sort_by = "execution_time_s"); + // Fetches the best cartesian plan that fits within the requested tolerances + // for start and goal conditions, by some db column. warehouse_ros::MessageWithMetadata< moveit_msgs::msg::RobotTrajectory >::ConstPtr @@ -182,7 +204,10 @@ class MotionPlanCache 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); + double start_tolerance, + double goal_tolerance, + bool metadata_only = false, + const std::string& sort_by = "execution_time_s"); bool put_cartesian_plan( const moveit::planning_interface::MoveGroupInterface& move_group, From ccbf91cd9f98e626bd1ba4a535b5b2aff10eba0c Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 20 Nov 2023 13:25:26 -0800 Subject: [PATCH 28/34] Add comments for exact match tolerance Signed-off-by: methylDragon --- nexus_motion_planner/src/motion_planner_server.cpp | 1 + nexus_motion_planner/src/motion_planner_server.hpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/nexus_motion_planner/src/motion_planner_server.cpp b/nexus_motion_planner/src/motion_planner_server.cpp index 3a26de4..2c3ce5e 100644 --- a/nexus_motion_planner/src/motion_planner_server.cpp +++ b/nexus_motion_planner/src/motion_planner_server.cpp @@ -247,6 +247,7 @@ MotionPlannerServer::MotionPlannerServer(const rclcpp::NodeOptions& options) "Setting parameter cache_db_port to [%d]", _cache_db_port ); + // For floating point comparison, what counts as an "exact" match. _cache_exact_match_tolerance = this->declare_parameter( "cache_exact_match_tolerance", 0.0005); // ~0.028 degrees per joint RCLCPP_INFO( diff --git a/nexus_motion_planner/src/motion_planner_server.hpp b/nexus_motion_planner/src/motion_planner_server.hpp index 9032bf4..7451d7a 100644 --- a/nexus_motion_planner/src/motion_planner_server.hpp +++ b/nexus_motion_planner/src/motion_planner_server.hpp @@ -127,7 +127,7 @@ class MotionPlannerServer : public rclcpp_lifecycle::LifecycleNode std::string _cache_db_plugin; std::string _cache_db_host; int _cache_db_port; - double _cache_exact_match_tolerance; + double _cache_exact_match_tolerance; // for floating point comparison double _cache_start_match_tolerance; double _cache_goal_match_tolerance; From 06cdcf90e5d44791f973f4ab3d900cf203089d87 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 20 Nov 2023 14:55:52 -0800 Subject: [PATCH 29/34] Slightly refactor put plan Signed-off-by: methylDragon --- .../src/motion_plan_cache.cpp | 100 +++++++++--------- 1 file changed, 48 insertions(+), 52 deletions(-) diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp index 39e0b78..10d2188 100644 --- a/nexus_motion_planner/src/motion_plan_cache.cpp +++ b/nexus_motion_planner/src/motion_plan_cache.cpp @@ -141,14 +141,14 @@ MotionPlanCache::put_plan( { RCLCPP_ERROR( node_->get_logger(), - "Skipping insert: Multi-DOF trajectory plans are not supported."); + "Skipping plan insert: Multi-DOF trajectory plans are not supported."); return false; } if (plan_request.workspace_parameters.header.frame_id.empty() || plan.joint_trajectory.header.frame_id.empty()) { RCLCPP_ERROR( - node_->get_logger(), "Skipping insert: Frame IDs cannot be empty."); + node_->get_logger(), "Skipping plan insert: Frame IDs cannot be empty."); return false; } if (plan_request.workspace_parameters.header.frame_id != @@ -156,7 +156,7 @@ MotionPlanCache::put_plan( { RCLCPP_ERROR( node_->get_logger(), - "Skipping insert: " + "Skipping plan insert: " "Plan request frame (%s) does not match plan frame (%s).", plan_request.workspace_parameters.header.frame_id.c_str(), plan.joint_trajectory.header.frame_id.c_str()); @@ -166,8 +166,7 @@ MotionPlanCache::put_plan( auto coll = db_->openCollection( "move_group_plan_cache", move_group_namespace); - // If start and goal are "exact" match, AND the candidate plan is better, - // overwrite. + // Pull out "exact" matching plans in cache. Query::Ptr exact_query = coll.createQuery(); bool start_query_ok = this->extract_and_append_plan_start_to_query( @@ -179,27 +178,25 @@ MotionPlanCache::put_plan( { RCLCPP_ERROR( node_->get_logger(), - "Skipping insert: Could not construct overwrite query."); + "Skipping plan insert: Could not construct lookup query."); return false; } - auto exact_matches = coll.queryList(exact_query, /* metadata_only */ true); - double best_execution_time_seen = std::numeric_limits::infinity(); + 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()) { - for (auto& match : exact_matches) - { - double match_execution_time_s = - match->lookupDouble("execution_time_s"); - if (match_execution_time_s < best_execution_time_seen) - { - best_execution_time_seen = match_execution_time_s; - } + best_execution_time = exact_matches.at(0)->lookupDouble("execution_time_s"); - if (match_execution_time_s > execution_time_s) + // Delete "worse" plans if overwrite requested. + if (overwrite) + { + for (auto& match : exact_matches) { - // If we found any "exact" matches that are worse, delete them. - if (overwrite) + 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( @@ -217,7 +214,7 @@ MotionPlanCache::put_plan( } // Insert if candidate is best seen. - if (execution_time_s < best_execution_time_seen) + if (execution_time_s < best_execution_time) { Metadata::Ptr insert_metadata = coll.createMetadata(); @@ -232,7 +229,7 @@ MotionPlanCache::put_plan( { RCLCPP_ERROR( node_->get_logger(), - "Skipping insert: Could not construct insert metadata."); + "Skipping plan insert: Could not construct insert metadata."); return false; } @@ -240,7 +237,7 @@ MotionPlanCache::put_plan( node_->get_logger(), "Inserting plan: New plan execution_time (%es) " "is better than best plan's execution_time (%es)", - execution_time_s, best_execution_time_seen); + execution_time_s, best_execution_time); coll.insert(plan, insert_metadata); return true; @@ -248,9 +245,9 @@ MotionPlanCache::put_plan( RCLCPP_INFO( node_->get_logger(), - "Skipping insert: New plan execution_time (%es) " + "Skipping plan insert: New plan execution_time (%es) " "is worse than best plan's execution_time (%es)", - execution_time_s, best_execution_time_seen); + execution_time_s, best_execution_time); return false; } @@ -971,7 +968,8 @@ MotionPlanCache::fetch_all_matching_cartesian_plans( if (!start_ok || !goal_ok) { - RCLCPP_ERROR(node_->get_logger(), "Could not construct plan query."); + RCLCPP_ERROR( + node_->get_logger(), "Could not construct cartesian plan query."); return {}; } @@ -997,7 +995,7 @@ MotionPlanCache::fetch_best_matching_cartesian_plan( if (matching_plans.empty()) { - RCLCPP_INFO(node_->get_logger(), "No matching plans found."); + RCLCPP_INFO(node_->get_logger(), "No matching cartesian plans found."); return nullptr; } @@ -1029,61 +1027,59 @@ MotionPlanCache::put_cartesian_plan( { RCLCPP_ERROR( node_->get_logger(), - "Skipping insert: Multi-DOF trajectory plans are not supported."); + "Skipping cartesian plan insert: " + "Multi-DOF trajectory plans are not supported."); return false; } if (plan_request.header.frame_id.empty() || plan.joint_trajectory.header.frame_id.empty()) { RCLCPP_ERROR( - node_->get_logger(), "Skipping insert: Frame IDs cannot be empty."); + node_->get_logger(), + "Skipping cartesian plan insert: " + "Frame IDs cannot be empty."); return false; } auto coll = db_->openCollection( "move_group_cartesian_plan_cache", move_group_namespace); - // If start and goal are "exact" match, AND the candidate plan is better, - // overwrite. + // Pull out "exact" matching plans in cache. Query::Ptr exact_query = coll.createQuery(); bool start_query_ok = this->extract_and_append_cartesian_plan_start_to_query( *exact_query, move_group, plan_request, 0); bool goal_query_ok = this->extract_and_append_cartesian_plan_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 insert: Could not construct overwrite query."); + "Skipping cartesian plan insert: Could not construct lookup query."); return false; } - auto exact_matches = coll.queryList(exact_query, /* metadata_only */ true); - double best_execution_time_seen = std::numeric_limits::infinity(); + 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()) { - for (auto& match : exact_matches) - { - double match_execution_time_s = - match->lookupDouble("execution_time_s"); - if (match_execution_time_s < best_execution_time_seen) - { - best_execution_time_seen = match_execution_time_s; - } + best_execution_time = exact_matches.at(0)->lookupDouble("execution_time_s"); - if (match_execution_time_s > execution_time_s) + // Delete "worse" plans if overwrite requested. + if (overwrite) + { + for (auto& match : exact_matches) { - // If we found any "exact" matches that are worse, delete them. - if (overwrite) + 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): " + "Overwriting cartesian plan (id: %d): " "execution_time (%es) > new plan's execution_time (%es)", delete_id, match_execution_time_s, execution_time_s); @@ -1096,7 +1092,7 @@ MotionPlanCache::put_cartesian_plan( } // Insert if candidate is best seen. - if (execution_time_s < best_execution_time_seen) + if (execution_time_s < best_execution_time) { Metadata::Ptr insert_metadata = coll.createMetadata(); @@ -1114,15 +1110,15 @@ MotionPlanCache::put_cartesian_plan( { RCLCPP_ERROR( node_->get_logger(), - "Skipping insert: Could not construct insert metadata."); + "Skipping cartesian plan insert: Could not construct insert metadata."); return false; } RCLCPP_INFO( node_->get_logger(), - "Inserting plan: New plan execution_time (%es) " + "Inserting cartesian plan: New plan execution_time (%es) " "is better than best plan's execution_time (%es) at fraction (%es)", - execution_time_s, best_execution_time_seen, fraction); + execution_time_s, best_execution_time, fraction); coll.insert(plan, insert_metadata); return true; @@ -1130,9 +1126,9 @@ MotionPlanCache::put_cartesian_plan( RCLCPP_INFO( node_->get_logger(), - "Skipping insert: New plan execution_time (%es) " + "Skipping cartesian plan insert: New plan execution_time (%es) " "is worse than best plan's execution_time (%es) at fraction (%es)", - execution_time_s, best_execution_time_seen, fraction); + execution_time_s, best_execution_time, fraction); return false; } From d1847baa85525f2c4bdabcf5eed631246e4c1acf Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 20 Nov 2023 14:59:29 -0800 Subject: [PATCH 30/34] Rename overwrite to delete_worse_plans Signed-off-by: methylDragon --- .../src/motion_plan_cache.cpp | 10 ++++------ .../src/motion_plan_cache.hpp | 19 +++++++++++++++++-- 2 files changed, 21 insertions(+), 8 deletions(-) diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp index 10d2188..8470182 100644 --- a/nexus_motion_planner/src/motion_plan_cache.cpp +++ b/nexus_motion_planner/src/motion_plan_cache.cpp @@ -134,7 +134,7 @@ MotionPlanCache::put_plan( const std::string& move_group_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request, const moveit_msgs::msg::RobotTrajectory& plan, - double execution_time_s, double planning_time_s, bool overwrite) + double execution_time_s, double planning_time_s, bool delete_worse_plans) { // Check pre-conditions if (!plan.multi_dof_joint_trajectory.points.empty()) @@ -190,8 +190,7 @@ MotionPlanCache::put_plan( { best_execution_time = exact_matches.at(0)->lookupDouble("execution_time_s"); - // Delete "worse" plans if overwrite requested. - if (overwrite) + if (delete_worse_plans) { for (auto& match : exact_matches) { @@ -1020,7 +1019,7 @@ MotionPlanCache::put_cartesian_plan( double execution_time_s, double planning_time_s, double fraction, - bool overwrite) + bool delete_worse_plans) { // Check pre-conditions if (!plan.multi_dof_joint_trajectory.points.empty()) @@ -1068,8 +1067,7 @@ MotionPlanCache::put_cartesian_plan( { best_execution_time = exact_matches.at(0)->lookupDouble("execution_time_s"); - // Delete "worse" plans if overwrite requested. - if (overwrite) + if (delete_worse_plans) { for (auto& match : exact_matches) { diff --git a/nexus_motion_planner/src/motion_plan_cache.hpp b/nexus_motion_planner/src/motion_plan_cache.hpp index 89e06ce..fc7bbff 100644 --- a/nexus_motion_planner/src/motion_plan_cache.hpp +++ b/nexus_motion_planner/src/motion_plan_cache.hpp @@ -128,6 +128,13 @@ class MotionPlanCache bool metadata_only = false, const std::string& sort_by = "execution_time_s"); + // Put a plan into the database if it is the best matching plan seen so far. + // + // Plans are matched based off their start and goal states. + // And are considered "better" if they have a smaller planned execution time + // than matching plans. + // + // Optionally deletes all worse plans by default to prune the cache. bool put_plan( const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& move_group_namespace, @@ -135,7 +142,7 @@ class MotionPlanCache const moveit_msgs::msg::RobotTrajectory& plan, double execution_time_s, double planning_time_s, - bool overwrite = true); + bool delete_worse_plans = true); // QUERY CONSTRUCTION bool extract_and_append_plan_start_to_query( @@ -209,6 +216,14 @@ class MotionPlanCache bool metadata_only = false, const std::string& sort_by = "execution_time_s"); + // Put a cartesian plan into the database if it is the best matching cartesian + // plan seen so far. + // + // Cartesian plans are matched based off their start and goal states. + // And are considered "better" if they have a smaller planned execution time + // than matching cartesian plans. + // + // Optionally deletes all worse cartesian plans by default to prune the cache. bool put_cartesian_plan( const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& move_group_namespace, @@ -217,7 +232,7 @@ class MotionPlanCache double execution_time_s, double planning_time_s, double fraction, - bool overwrite = true); + bool delete_worse_plans = true); // QUERY CONSTRUCTION bool extract_and_append_cartesian_plan_start_to_query( From 6e5b9634d55f0b5ea8e8a24018a1f5afeff55f91 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 20 Nov 2023 15:58:51 -0800 Subject: [PATCH 31/34] Split out motion plan cache into its own library Signed-off-by: methylDragon --- nexus_motion_planner/CMakeLists.txt | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/nexus_motion_planner/CMakeLists.txt b/nexus_motion_planner/CMakeLists.txt index e9003bf..ca94c1e 100644 --- a/nexus_motion_planner/CMakeLists.txt +++ b/nexus_motion_planner/CMakeLists.txt @@ -43,12 +43,17 @@ set (test_request_dependencies ) #=============================================================================== -set(LIBRARY_NAME motion_planner_server_core) +set(MOTION_PLAN_CACHE_LIBRARY_NAME motion_planner_server_core) set(EXECUTABLE_NAME motion_planner_server) +# Motion plan cache library +add_library(${MOTION_PLAN_CACHE_LIBRARY_NAME} src/motion_plan_cache.cpp) +ament_target_dependencies(${MOTION_PLAN_CACHE_LIBRARY_NAME} ${motion_planner_server_dependencies}) + # Server executable -add_executable(${EXECUTABLE_NAME} src/main.cpp src/motion_planner_server.cpp src/motion_plan_cache.cpp) +add_executable(${EXECUTABLE_NAME} src/main.cpp src/motion_planner_server.cpp) ament_target_dependencies(${EXECUTABLE_NAME} ${motion_planner_server_dependencies}) +target_link_libraries(${EXECUTABLE_NAME} ${MOTION_PLAN_CACHE_LIBRARY_NAME}) # Test executable add_executable(test_request src/test_request.cpp) From 636b4d504da1adc6e46dc8dadc13d8b5603bbde8 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 20 Nov 2023 19:32:26 -0800 Subject: [PATCH 32/34] Sort constraints for reduced cardinality Signed-off-by: methylDragon --- .../src/motion_plan_cache.cpp | 46 +++++++++++++++++++ 1 file changed, 46 insertions(+) diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp index 8470182..2a77d7e 100644 --- a/nexus_motion_planner/src/motion_plan_cache.cpp +++ b/nexus_motion_planner/src/motion_plan_cache.cpp @@ -30,6 +30,8 @@ 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 queryAppendRangeInclusiveWithTolerance( Query& query, const std::string& name, double center, double tolerance) @@ -38,6 +40,42 @@ void queryAppendRangeInclusiveWithTolerance( 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; + }); +} + +// Motion Plan Cache =========================================================== + MotionPlanCache::MotionPlanCache(const rclcpp::Node::SharedPtr& node) : node_(node) { @@ -417,6 +455,10 @@ MotionPlanCache::extract_and_append_plan_goal_to_query( { orientation_constraints.push_back(orientation_constraint); } + + // Also sort for even less cardinality. + sort_constraints( + joint_constraints, position_constraints, orientation_constraints); } // Joint constraints @@ -742,6 +784,10 @@ MotionPlanCache::extract_and_append_plan_goal_to_metadata( { orientation_constraints.push_back(orientation_constraint); } + + // Also sort for even less cardinality. + sort_constraints( + joint_constraints, position_constraints, orientation_constraints); } // Joint constraints From 5ea5ad5909e10c38336ce6463163420e885e13a3 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 20 Nov 2023 19:42:22 -0800 Subject: [PATCH 33/34] Rename util function Signed-off-by: methylDragon --- .../src/motion_plan_cache.cpp | 54 +++++++++---------- 1 file changed, 27 insertions(+), 27 deletions(-) diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp index 2a77d7e..b3655cb 100644 --- a/nexus_motion_planner/src/motion_plan_cache.cpp +++ b/nexus_motion_planner/src/motion_plan_cache.cpp @@ -33,7 +33,7 @@ using warehouse_ros::Query; // Utils ======================================================================= // Append a range inclusive query with some tolerance about some center value. -void queryAppendRangeInclusiveWithTolerance( +void query_append_range_inclusive_with_tolerance( Query& query, const std::string& name, double center, double tolerance) { query.appendRangeInclusive( @@ -369,7 +369,7 @@ MotionPlanCache::extract_and_append_plan_start_to_query( query.append( "start_state.joint_state.name_" + std::to_string(i), current_state_msg.joint_state.name.at(i)); - queryAppendRangeInclusiveWithTolerance( + 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); } @@ -383,7 +383,7 @@ MotionPlanCache::extract_and_append_plan_start_to_query( query.append( "start_state.joint_state.name_" + std::to_string(i), plan_request.start_state.joint_state.name.at(i)); - queryAppendRangeInclusiveWithTolerance( + 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); } @@ -427,13 +427,13 @@ MotionPlanCache::extract_and_append_plan_goal_to_query( // auto original = *query; // Copy not supported. - queryAppendRangeInclusiveWithTolerance( + query_append_range_inclusive_with_tolerance( query, "max_velocity_scaling_factor", plan_request.max_velocity_scaling_factor, match_tolerance); - queryAppendRangeInclusiveWithTolerance( + query_append_range_inclusive_with_tolerance( query, "max_acceleration_scaling_factor", plan_request.max_acceleration_scaling_factor, match_tolerance); - queryAppendRangeInclusiveWithTolerance( + query_append_range_inclusive_with_tolerance( query, "max_cartesian_speed", plan_request.max_cartesian_speed, match_tolerance); @@ -469,7 +469,7 @@ MotionPlanCache::extract_and_append_plan_goal_to_query( "goal_constraints.joint_constraints_" + std::to_string(joint_idx++); query.append(meta_name + ".joint_name", constraint.joint_name); - queryAppendRangeInclusiveWithTolerance( + 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); @@ -527,13 +527,13 @@ MotionPlanCache::extract_and_append_plan_goal_to_query( query.append(meta_name + ".link_name", constraint.link_name); - queryAppendRangeInclusiveWithTolerance( + query_append_range_inclusive_with_tolerance( query, meta_name + ".target_point_offset.x", x_offset + constraint.target_point_offset.x, match_tolerance); - queryAppendRangeInclusiveWithTolerance( + query_append_range_inclusive_with_tolerance( query, meta_name + ".target_point_offset.y", y_offset + constraint.target_point_offset.y, match_tolerance); - queryAppendRangeInclusiveWithTolerance( + query_append_range_inclusive_with_tolerance( query, meta_name + ".target_point_offset.z", z_offset + constraint.target_point_offset.z, match_tolerance); } @@ -609,16 +609,16 @@ MotionPlanCache::extract_and_append_plan_goal_to_query( auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset; final_quat.normalize(); - queryAppendRangeInclusiveWithTolerance( + query_append_range_inclusive_with_tolerance( query, meta_name + ".target_point_offset.x", final_quat.getX(), match_tolerance); - queryAppendRangeInclusiveWithTolerance( + query_append_range_inclusive_with_tolerance( query, meta_name + ".target_point_offset.y", final_quat.getY(), match_tolerance); - queryAppendRangeInclusiveWithTolerance( + query_append_range_inclusive_with_tolerance( query, meta_name + ".target_point_offset.z", final_quat.getZ(), match_tolerance); - queryAppendRangeInclusiveWithTolerance( + query_append_range_inclusive_with_tolerance( query, meta_name + ".target_point_offset.w", final_quat.getW(), match_tolerance); } @@ -1233,7 +1233,7 @@ MotionPlanCache::extract_and_append_cartesian_plan_start_to_query( query.append( "start_state.joint_state.name_" + std::to_string(i), current_state_msg.joint_state.name.at(i)); - queryAppendRangeInclusiveWithTolerance( + 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); } @@ -1247,7 +1247,7 @@ MotionPlanCache::extract_and_append_cartesian_plan_start_to_query( query.append( "start_state.joint_state.name_" + std::to_string(i), plan_request.start_state.joint_state.name.at(i)); - queryAppendRangeInclusiveWithTolerance( + 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); } @@ -1282,15 +1282,15 @@ MotionPlanCache::extract_and_append_cartesian_plan_goal_to_query( // auto original = *metadata; // Copy not supported. - queryAppendRangeInclusiveWithTolerance( + query_append_range_inclusive_with_tolerance( query, "max_velocity_scaling_factor", plan_request.max_velocity_scaling_factor, match_tolerance); - queryAppendRangeInclusiveWithTolerance( + query_append_range_inclusive_with_tolerance( query, "max_acceleration_scaling_factor", plan_request.max_acceleration_scaling_factor, match_tolerance); - queryAppendRangeInclusiveWithTolerance( + query_append_range_inclusive_with_tolerance( query, "max_step", plan_request.max_step, match_tolerance); - queryAppendRangeInclusiveWithTolerance( + query_append_range_inclusive_with_tolerance( query, "jump_threshold", plan_request.jump_threshold, match_tolerance); // Waypoints @@ -1346,13 +1346,13 @@ MotionPlanCache::extract_and_append_cartesian_plan_goal_to_query( // Apply offsets // Position - queryAppendRangeInclusiveWithTolerance( + query_append_range_inclusive_with_tolerance( query, meta_name + ".position.x", x_offset + waypoint.position.x, match_tolerance); - queryAppendRangeInclusiveWithTolerance( + query_append_range_inclusive_with_tolerance( query, meta_name + ".position.y", y_offset+ waypoint.position.y, match_tolerance); - queryAppendRangeInclusiveWithTolerance( + query_append_range_inclusive_with_tolerance( query, meta_name + ".position.z", z_offset+waypoint.position.z, match_tolerance); @@ -1367,13 +1367,13 @@ MotionPlanCache::extract_and_append_cartesian_plan_goal_to_query( auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset; final_quat.normalize(); - queryAppendRangeInclusiveWithTolerance( + query_append_range_inclusive_with_tolerance( query, meta_name + ".orientation.x", final_quat.getX(), match_tolerance); - queryAppendRangeInclusiveWithTolerance( + query_append_range_inclusive_with_tolerance( query, meta_name + ".orientation.y", final_quat.getY(), match_tolerance); - queryAppendRangeInclusiveWithTolerance( + query_append_range_inclusive_with_tolerance( query, meta_name + ".orientation.z", final_quat.getZ(), match_tolerance); - queryAppendRangeInclusiveWithTolerance( + query_append_range_inclusive_with_tolerance( query, meta_name + ".orientation.w", final_quat.getW(), match_tolerance); } From bbc07cec581f5b127ec7f86227da69b078994a38 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 20 Nov 2023 19:55:09 -0800 Subject: [PATCH 34/34] Add todo for is_diff Signed-off-by: methylDragon --- .../src/motion_plan_cache.cpp | 28 +++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp index b3655cb..b2e8096 100644 --- a/nexus_motion_planner/src/motion_plan_cache.cpp +++ b/nexus_motion_planner/src/motion_plan_cache.cpp @@ -351,6 +351,13 @@ MotionPlanCache::extract_and_append_plan_start_to_query( // 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) { @@ -686,6 +693,13 @@ MotionPlanCache::extract_and_append_plan_start_to_metadata( // 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) { @@ -1215,6 +1229,13 @@ MotionPlanCache::extract_and_append_cartesian_plan_start_to_query( // 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) { @@ -1418,6 +1439,13 @@ MotionPlanCache::extract_and_append_cartesian_plan_start_to_metadata( // 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) {