diff --git a/moveit_ros/trajectory_cache/README.md b/moveit_ros/trajectory_cache/README.md index c3d7c526109..e208319dea1 100644 --- a/moveit_ros/trajectory_cache/README.md +++ b/moveit_ros/trajectory_cache/README.md @@ -69,7 +69,7 @@ else traj_cache->putTrajectory( *interface, robot_name, std::move(plan_req_msg), std::move(res->result.trajectory), rclcpp::Duration(res->result.trajectory.joint_trajectory.points.back().time_from_start).seconds(), - res->result.planning_time, /*overwrite=*/true); + res->result.planning_time, /*delete_worse_trajectories=*/true); } ``` @@ -131,4 +131,4 @@ Since this is an initial release, the following features are unsupported because - Including: path, constraint regions, everything related to collision. - The fuzzy lookup can't be configured on a per-joint basis. - Alternate ordinal lookup metrics for the cache - - Currently only execution time is explicitly supported as a way to compare cache entries. Ideally we should be able to inject lambdas to save custom cache DB metadata to represent and sort on custom cost functions (e.g. minimum jerk, path length, etc.). (e.g. https://github.com/moveit/moveit2/pull/2153) \ No newline at end of file + - Currently only execution time is explicitly supported as a way to compare cache entries. Ideally we should be able to inject lambdas to save custom cache DB metadata to represent and sort on custom cost functions (e.g. minimum jerk, path length, etc.). (e.g. https://github.com/moveit/moveit2/pull/2153) diff --git a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp index 1be586160e1..c7b327f4e78 100644 --- a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp @@ -204,7 +204,8 @@ class TrajectoryCache * \brief Put a trajectory into the database if it is the best matching trajectory seen so far. * * Trajectories are matched based off their start and goal states. - * And are considered "better" if they higher priority in the sorting order specified by `sort_by` than exactly matching trajectories. + * And are considered "better" if they higher priority in the sorting order specified by `sort_by` than exactly + * matching trajectories. * * A trajectory is "exactly matching" if its start and goal are close enough to another trajectory. * The tolerance for this depends on the `exact_match_tolerance` arg passed in init(). @@ -271,7 +272,8 @@ class TrajectoryCache const std::string& cache_namespace, const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double min_fraction, double start_tolerance, double goal_tolerance, - bool metadata_only = false, const std::string& sort_by = "execution_time_s", bool ascending = true); + bool metadata_only = false, const std::string& sort_by = "execution_time_s", + bool ascending = true); /** * \brief Fetch the best cartesian trajectory that fits within the requested tolerances for start and goal conditions, @@ -291,14 +293,15 @@ class TrajectoryCache warehouse_ros::MessageWithMetadata::ConstPtr fetchBestMatchingCartesianTrajectory( const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& cache_namespace, const moveit_msgs::srv::GetCartesianPath::Request& plan_request, double min_fraction, double start_tolerance, - double goal_tolerance, bool metadata_only = false, const std::string& sort_by = "execution_time_s", bool ascending = true); + double goal_tolerance, bool metadata_only = false, const std::string& sort_by = "execution_time_s", + bool ascending = true); /** * \brief Put a cartesian trajectory into the database if it is the best matching cartesian trajectory seen so far. * * Cartesian trajectories are matched based off their start and goal states. - * And are considered "better" if they higher priority in the sorting order specified by `sort_by` than exactly matching cartesian - * trajectories. + * And are considered "better" if they higher priority in the sorting order specified by `sort_by` than exactly + * matching cartesian trajectories. * * A trajectory is "exactly matching" if its start and goal (and fraction) are close enough to another trajectory. * The tolerance for this depends on the `exact_match_tolerance` arg passed in init(). @@ -373,7 +376,7 @@ class TrajectoryCache * * These parameters will be used key the cache element. * - * \param[out] metadata. The metadata to add paramters to. + * \param[out] metadata. The metadata to add parameters to. * \param[in] move_group. The manipulator move group, used to get its state. * \param[in] plan_request. The motion plan request to key the cache with. * \returns true if successfully added to. If false, the metadata might have been partially modified and should not be @@ -389,7 +392,7 @@ class TrajectoryCache * * These parameters will be used key the cache element. * - * \param[out] metadata. The metadata to add paramters to. + * \param[out] metadata. The metadata to add parameters to. * \param[in] move_group. The manipulator move group, used to get its state. * \param[in] plan_request. The motion plan request to key the cache with. * \returns true if successfully added to. If false, the metadata might have been partially modified and should not be @@ -448,7 +451,7 @@ class TrajectoryCache * * These parameters will be used key the cache element. * - * \param[out] metadata. The metadata to add paramters to. + * \param[out] metadata. The metadata to add parameters to. * \param[in] move_group. The manipulator move group, used to get its state. * \param[in] plan_request. The cartesian plan request to key the cache with. * \returns true if successfully added to. If false, the metadata might have been partially modified and should not be @@ -465,7 +468,7 @@ class TrajectoryCache * * These parameters will be used key the cache element. * - * \param[out] metadata. The metadata to add paramters to. + * \param[out] metadata. The metadata to add parameters to. * \param[in] move_group. The manipulator move group, used to get its state. * \param[in] plan_request. The cartesian plan request to key the cache with. * \returns true if successfully added to. If false, the metadata might have been partially modified and should not be diff --git a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp index b037673eac6..7b793031de8 100644 --- a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp @@ -36,15 +36,15 @@ using warehouse_ros::Query; // Utils ======================================================================= // Append a range inclusive query with some tolerance about some center value. -void query_append_range_inclusive_with_tolerance(Query& query, const std::string& name, double center, double tolerance) +void queryAppendRangeInclusiveWithTolerance(Query& query, const std::string& name, double center, double tolerance) { query.appendRangeInclusive(name, center - tolerance / 2, center + tolerance / 2); } // Sort constraint components by joint or link name. -void sort_constraints(std::vector& joint_constraints, - std::vector& position_constraints, - std::vector& orientation_constraints) +void sortConstraints(std::vector& joint_constraints, + std::vector& position_constraints, + std::vector& orientation_constraints) { std::sort(joint_constraints.begin(), joint_constraints.end(), [](const moveit_msgs::msg::JointConstraint& l, const moveit_msgs::msg::JointConstraint& r) { @@ -132,8 +132,8 @@ MessageWithMetadata::ConstPtr TrajectoryCache { // First find all matching, but metadata only. // Then use the ID metadata of the best plan to pull the actual message. - auto matching_trajectories = this->fetchAllMatchingTrajectories(move_group, cache_namespace, plan_request, - start_tolerance, goal_tolerance, true, sort_by, ascending); + auto matching_trajectories = this->fetchAllMatchingTrajectories( + move_group, cache_namespace, plan_request, start_tolerance, goal_tolerance, true, sort_by, ascending); if (matching_trajectories.empty()) { @@ -316,8 +316,9 @@ MessageWithMetadata::ConstPtr TrajectoryCache { // First find all matching, but metadata only. // Then use the ID metadata of the best plan to pull the actual message. - auto matching_trajectories = this->fetchAllMatchingCartesianTrajectories( - move_group, cache_namespace, plan_request, min_fraction, start_tolerance, goal_tolerance, true, sort_by, ascending); + auto matching_trajectories = + this->fetchAllMatchingCartesianTrajectories(move_group, cache_namespace, plan_request, min_fraction, + start_tolerance, goal_tolerance, true, sort_by, ascending); if (matching_trajectories.empty()) { @@ -504,8 +505,8 @@ bool TrajectoryCache::extractAndAppendTrajectoryStartToQuery( for (size_t i = 0; i < current_state_msg.joint_state.name.size(); i++) { query.append("start_state.joint_state.name_" + std::to_string(i), current_state_msg.joint_state.name.at(i)); - query_append_range_inclusive_with_tolerance(query, "start_state.joint_state.position_" + std::to_string(i), - current_state_msg.joint_state.position.at(i), match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, "start_state.joint_state.position_" + std::to_string(i), + current_state_msg.joint_state.position.at(i), match_tolerance); } } else @@ -513,8 +514,8 @@ bool TrajectoryCache::extractAndAppendTrajectoryStartToQuery( for (size_t i = 0; i < plan_request.start_state.joint_state.name.size(); i++) { query.append("start_state.joint_state.name_" + std::to_string(i), plan_request.start_state.joint_state.name.at(i)); - query_append_range_inclusive_with_tolerance(query, "start_state.joint_state.position_" + std::to_string(i), - plan_request.start_state.joint_state.position.at(i), match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, "start_state.joint_state.position_" + std::to_string(i), + plan_request.start_state.joint_state.position.at(i), match_tolerance); } } return true; @@ -549,12 +550,12 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery( "Not supported."); } - query_append_range_inclusive_with_tolerance(query, "max_velocity_scaling_factor", - plan_request.max_velocity_scaling_factor, match_tolerance); - query_append_range_inclusive_with_tolerance(query, "max_acceleration_scaling_factor", - plan_request.max_acceleration_scaling_factor, match_tolerance); - query_append_range_inclusive_with_tolerance(query, "max_cartesian_speed", plan_request.max_cartesian_speed, - match_tolerance); + 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; @@ -576,7 +577,7 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery( } // Also sort for even less cardinality. - sort_constraints(joint_constraints, position_constraints, orientation_constraints); + sortConstraints(joint_constraints, position_constraints, orientation_constraints); } // Joint constraints @@ -586,7 +587,7 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery( std::string meta_name = "goal_constraints.joint_constraints_" + std::to_string(joint_idx++); query.append(meta_name + ".joint_name", constraint.joint_name); - query_append_range_inclusive_with_tolerance(query, meta_name + ".position", constraint.position, match_tolerance); + 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); } @@ -636,12 +637,12 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery( query.append(meta_name + ".link_name", constraint.link_name); - query_append_range_inclusive_with_tolerance(query, meta_name + ".target_point_offset.x", - x_offset + constraint.target_point_offset.x, match_tolerance); - query_append_range_inclusive_with_tolerance(query, meta_name + ".target_point_offset.y", - y_offset + constraint.target_point_offset.y, match_tolerance); - query_append_range_inclusive_with_tolerance(query, meta_name + ".target_point_offset.z", - z_offset + constraint.target_point_offset.z, match_tolerance); + 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); } } @@ -704,14 +705,14 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery( auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset; final_quat.normalize(); - query_append_range_inclusive_with_tolerance(query, meta_name + ".target_point_offset.x", final_quat.getX(), - match_tolerance); - query_append_range_inclusive_with_tolerance(query, meta_name + ".target_point_offset.y", final_quat.getY(), - match_tolerance); - query_append_range_inclusive_with_tolerance(query, meta_name + ".target_point_offset.z", final_quat.getZ(), - match_tolerance); - query_append_range_inclusive_with_tolerance(query, meta_name + ".target_point_offset.w", final_quat.getW(), - match_tolerance); + 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); } } @@ -847,7 +848,7 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToMetadata( } // Also sort for even less cardinality. - sort_constraints(joint_constraints, position_constraints, orientation_constraints); + sortConstraints(joint_constraints, position_constraints, orientation_constraints); } // Joint constraints @@ -1039,8 +1040,8 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryStartToQuery( for (size_t i = 0; i < current_state_msg.joint_state.name.size(); i++) { query.append("start_state.joint_state.name_" + std::to_string(i), current_state_msg.joint_state.name.at(i)); - query_append_range_inclusive_with_tolerance(query, "start_state.joint_state.position_" + std::to_string(i), - current_state_msg.joint_state.position.at(i), match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, "start_state.joint_state.position_" + std::to_string(i), + current_state_msg.joint_state.position.at(i), match_tolerance); } } else @@ -1048,8 +1049,8 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryStartToQuery( for (size_t i = 0; i < plan_request.start_state.joint_state.name.size(); i++) { query.append("start_state.joint_state.name_" + std::to_string(i), plan_request.start_state.joint_state.name.at(i)); - query_append_range_inclusive_with_tolerance(query, "start_state.joint_state.position_" + std::to_string(i), - plan_request.start_state.joint_state.position.at(i), match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, "start_state.joint_state.position_" + std::to_string(i), + plan_request.start_state.joint_state.position.at(i), match_tolerance); } } @@ -1075,12 +1076,12 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryGoalToQuery( RCLCPP_WARN(logger_, "Ignoring avoid_collisions: Not supported."); } - query_append_range_inclusive_with_tolerance(query, "max_velocity_scaling_factor", - plan_request.max_velocity_scaling_factor, match_tolerance); - query_append_range_inclusive_with_tolerance(query, "max_acceleration_scaling_factor", - plan_request.max_acceleration_scaling_factor, match_tolerance); - query_append_range_inclusive_with_tolerance(query, "max_step", plan_request.max_step, match_tolerance); - query_append_range_inclusive_with_tolerance(query, "jump_threshold", plan_request.jump_threshold, match_tolerance); + 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) @@ -1131,12 +1132,12 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryGoalToQuery( // Apply offsets // Position - query_append_range_inclusive_with_tolerance(query, meta_name + ".position.x", x_offset + waypoint.position.x, - match_tolerance); - query_append_range_inclusive_with_tolerance(query, meta_name + ".position.y", y_offset + waypoint.position.y, - match_tolerance); - query_append_range_inclusive_with_tolerance(query, meta_name + ".position.z", z_offset + waypoint.position.z, - match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".position.x", x_offset + waypoint.position.x, + match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".position.y", y_offset + waypoint.position.y, + match_tolerance); + queryAppendRangeInclusiveWithTolerance(query, meta_name + ".position.z", z_offset + waypoint.position.z, + match_tolerance); // Orientation tf2::Quaternion tf2_quat_goal_offset(waypoint.orientation.x, waypoint.orientation.y, waypoint.orientation.z, @@ -1146,10 +1147,10 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryGoalToQuery( auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset; final_quat.normalize(); - query_append_range_inclusive_with_tolerance(query, meta_name + ".orientation.x", final_quat.getX(), match_tolerance); - query_append_range_inclusive_with_tolerance(query, meta_name + ".orientation.y", final_quat.getY(), match_tolerance); - query_append_range_inclusive_with_tolerance(query, meta_name + ".orientation.z", final_quat.getZ(), match_tolerance); - query_append_range_inclusive_with_tolerance(query, meta_name + ".orientation.w", final_quat.getW(), match_tolerance); + 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);