From af7e9b552f83cab9e97821a8d357031f7f73c814 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 26 Jun 2023 20:46:38 -0700 Subject: [PATCH] MPPI: Support Exact Path Following For Feasible Plans (#3659) * alternative to path align critic for inversion control * fix default behavior (enforce_path_inversion: false) (#3643) Co-authored-by: Guillaume Doisy * adding dyaw option for path alignment to incentivize following the path's intent where necessary * add docs for use path orientations * fix typo --------- Co-authored-by: Guillaume Doisy Co-authored-by: Guillaume Doisy --- nav2_mppi_controller/README.md | 5 ++ .../critics/path_align_critic.hpp | 1 + .../tools/path_handler.hpp | 19 ++++- .../nav2_mppi_controller/tools/utils.hpp | 54 ++++++++++++++ .../src/critics/path_align_critic.cpp | 24 +++++-- nav2_mppi_controller/src/path_handler.cpp | 52 +++++++++++--- .../test/path_handler_test.cpp | 72 +++++++++++++++++-- nav2_mppi_controller/test/utils_test.cpp | 63 ++++++++++++++++ 8 files changed, 267 insertions(+), 23 deletions(-) diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 276e24b058..2557bcd7d6 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -64,6 +64,9 @@ This process is then repeated a number of times and returns a converged solution | max_robot_pose_search_dist | double | Default: Costmap half-size. Max integrated distance ahead of robot pose to search for nearest path point in case of path looping. | | prune_distance | double | Default: 1.5. Distance ahead of nearest point on path to robot to prune path to. | | transform_tolerance | double | Default: 0.1. Time tolerance for data transformations with TF. | + | enforce_path_inversion | double | Default: False. If true, it will prune paths containing cusping points for segments changing directions (e.g. path inversions) such that the controller will be forced to change directions at or very near the planner's requested inversion point. This is targeting Smac Planner users with feasible paths who need their robots to switch directions where specifically requested. | + | inversion_xy_tolerance | double | Default: 0.2. Cartesian proximity (m) to path inversion point to be considered "achieved" to pass on the rest of the path after path inversion. | + | inversion_yaw_tolerance | double | Default: 0.4. Angular proximity (radians) to path inversion point to be considered "achieved" to pass on the rest of the path after path inversion. 0.4 rad = 23 deg. | #### Ackermann Motion Model | Parameter | Type | Definition | @@ -111,6 +114,7 @@ This process is then repeated a number of times and returns a converged solution | offset_from_furthest | double | Default 20. Checks that the candidate trajectories are sufficiently far along their way tracking the path to apply the alignment critic. This ensures that path alignment is only considered when actually tracking the path, preventing awkward initialization motions preventing the robot from leaving the path to achieve the appropriate heading. | | trajectory_point_step | double | Default 4. Step of trajectory points to evaluate for path distance to reduce compute time. Between 1-10 is typically reasonable. | | max_path_occupancy_ratio | double | Default 0.07 (7%). Maximum proportion of the path that can be occupied before this critic is not considered to allow the obstacle and path follow critics to avoid obstacles while following the path's intent in presence of dynamic objects in the scene. | + | use_path_orientations | bool | Default false. Whether to consider path's orientations in path alignment, which can be useful when paired with feasible smac planners to incentivize directional changes only where/when the smac planner requests them. If you want the robot to deviate and invert directions where the controller sees fit, keep as false. If your plans do not contain orientation information (e.g. navfn), keep as false. | #### Path Angle Critic | Parameter | Type | Definition | @@ -211,6 +215,7 @@ controller_server: trajectory_point_step: 3 threshold_to_consider: 0.5 offset_from_furthest: 20 + use_path_orientations: false PathFollowCritic: enabled: true cost_power: 1 diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp index 7618afb9a4..a549995557 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp @@ -49,6 +49,7 @@ class PathAlignCritic : public CriticFunction int trajectory_point_step_{0}; float threshold_to_consider_{0}; float max_path_occupancy_ratio_{0}; + bool use_path_orientations_{false}; unsigned int power_{0}; float weight_{0}; }; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp index c66d32257e..468e4964a4 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp @@ -1,4 +1,6 @@ // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Dexory +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -124,10 +126,18 @@ class PathHandler const geometry_msgs::msg::PoseStamped & global_pose); /** - * @brief Prune global path to only interesting portions + * @brief Prune a path to only interesting portions + * @param plan Plan to prune * @param end Final path iterator */ - void pruneGlobalPlan(const PathIterator end); + void prunePlan(nav_msgs::msg::Path & plan, const PathIterator end); + + /** + * @brief Check if the robot pose is within the set inversion tolerances + * @param robot_pose Robot's current pose to check + * @return bool If the robot pose is within the set inversion tolerances + */ + bool isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose); std::string name_; std::shared_ptr costmap_; @@ -135,11 +145,16 @@ class PathHandler ParametersHandler * parameters_handler_; nav_msgs::msg::Path global_plan_; + nav_msgs::msg::Path global_plan_up_to_inversion_; rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; double max_robot_pose_search_dist_{0}; double prune_distance_{0}; double transform_tolerance_{0}; + double inversion_xy_tolerance_{0.2}; + double inversion_yaw_tolerance{0.4}; + bool enforce_path_inversion_{false}; + unsigned int inversion_locale_{0u}; }; } // namespace mppi diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp index 77b801bf3e..ce2d0570c0 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -1,4 +1,5 @@ // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -634,6 +635,59 @@ inline void savitskyGolayFilter( control_sequence.wz(offset)}; } +/** + * @brief Find the iterator of the first pose at which there is an inversion on the path, + * @param path to check for inversion + * @return the first point after the inversion found in the path + */ +inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path) +{ + // At least 3 poses for a possible inversion + if (path.poses.size() < 3) { + return path.poses.size(); + } + + // Iterating through the path to determine the position of the path inversion + for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { + // We have two vectors for the dot product OA and AB. Determining the vectors. + double oa_x = path.poses[idx].pose.position.x - + path.poses[idx - 1].pose.position.x; + double oa_y = path.poses[idx].pose.position.y - + path.poses[idx - 1].pose.position.y; + double ab_x = path.poses[idx + 1].pose.position.x - + path.poses[idx].pose.position.x; + double ab_y = path.poses[idx + 1].pose.position.y - + path.poses[idx].pose.position.y; + + // Checking for the existance of cusp, in the path, using the dot product. + double dot_product = (oa_x * ab_x) + (oa_y * ab_y); + if (dot_product < 0.0) { + return idx + 1; + } + } + + return path.poses.size(); +} + +/** + * @brief Find and remove poses after the first inversion in the path + * @param path to check for inversion + * @return The location of the inversion, return 0 if none exist + */ +inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path) +{ + nav_msgs::msg::Path cropped_path = path; + const unsigned int first_after_inversion = findFirstPathInversion(cropped_path); + if (first_after_inversion == path.poses.size()) { + return 0u; + } + + cropped_path.poses.erase( + cropped_path.poses.begin() + first_after_inversion, cropped_path.poses.end()); + path = cropped_path; + return first_after_inversion; +} + } // namespace mppi::utils #endif // NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_ diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp index 2d7d93305b..2585193ade 100644 --- a/nav2_mppi_controller/src/critics/path_align_critic.cpp +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -35,6 +35,7 @@ void PathAlignCritic::initialize() getParam( threshold_to_consider_, "threshold_to_consider", 0.5); + getParam(use_path_orientations_, "use_path_orientations", false); RCLCPP_INFO( logger_, @@ -71,9 +72,11 @@ void PathAlignCritic::score(CriticData & data) const auto & T_x = data.trajectories.x; const auto & T_y = data.trajectories.y; + const auto & T_yaw = data.trajectories.yaws; const auto P_x = xt::view(data.path.x, xt::range(_, -1)); // path points const auto P_y = xt::view(data.path.y, xt::range(_, -1)); // path points + const auto P_yaw = xt::view(data.path.yaws, xt::range(_, -1)); // path points const size_t batch_size = T_x.shape(0); const size_t time_steps = T_x.shape(1); @@ -85,18 +88,27 @@ void PathAlignCritic::score(CriticData & data) return; } + float dist_sq = 0, dx = 0, dy = 0, dyaw = 0, summed_dist = 0; + double min_dist_sq = std::numeric_limits::max(); + size_t min_s = 0; + for (size_t t = 0; t < batch_size; ++t) { - float summed_dist = 0; + summed_dist = 0; for (size_t p = trajectory_point_step_; p < time_steps; p += trajectory_point_step_) { - double min_dist_sq = std::numeric_limits::max(); - size_t min_s = 0; + min_dist_sq = std::numeric_limits::max(); + min_s = 0; // Find closest path segment to the trajectory point for (size_t s = 0; s < path_segments_count - 1; s++) { xt::xtensor_fixed> P; - float dx = P_x(s) - T_x(t, p); - float dy = P_y(s) - T_y(t, p); - float dist_sq = dx * dx + dy * dy; + dx = P_x(s) - T_x(t, p); + dy = P_y(s) - T_y(t, p); + if (use_path_orientations_) { + dyaw = angles::shortest_angular_distance(P_yaw(s), T_yaw(t, p)); + dist_sq = dx * dx + dy * dy + dyaw * dyaw; + } else { + dist_sq = dx * dx + dy * dy; + } if (dist_sq < min_dist_sq) { min_dist_sq = dist_sq; min_s = s; diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp index 6fe71eada5..f1022513f2 100644 --- a/nav2_mppi_controller/src/path_handler.cpp +++ b/nav2_mppi_controller/src/path_handler.cpp @@ -1,4 +1,6 @@ // Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 Dexory +// Copyright (c) 2023 Open Navigation LLC // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -35,6 +37,12 @@ void PathHandler::initialize( getParam(max_robot_pose_search_dist_, "max_robot_pose_search_dist", getMaxCostmapDist()); getParam(prune_distance_, "prune_distance", 1.5); getParam(transform_tolerance_, "transform_tolerance", 0.1); + getParam(enforce_path_inversion_, "enforce_path_inversion", false); + if (enforce_path_inversion_) { + getParam(inversion_xy_tolerance_, "inversion_xy_tolerance", 0.2); + getParam(inversion_yaw_tolerance, "inversion_yaw_tolerance", 0.4); + inversion_locale_ = 0u; + } } std::pair @@ -43,12 +51,13 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( { using nav2_util::geometry_utils::euclidean_distance; - auto begin = global_plan_.poses.begin(); + auto begin = global_plan_up_to_inversion_.poses.begin(); // Limit the search for the closest pose up to max_robot_pose_search_dist on the path auto closest_pose_upper_bound = nav2_util::geometry_utils::first_after_integrated_distance( - global_plan_.poses.begin(), global_plan_.poses.end(), max_robot_pose_search_dist_); + global_plan_up_to_inversion_.poses.begin(), global_plan_up_to_inversion_.poses.end(), + max_robot_pose_search_dist_); // Find closest point to the robot auto closest_point = nav2_util::geometry_utils::min_by( @@ -63,7 +72,7 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( auto pruned_plan_end = nav2_util::geometry_utils::first_after_integrated_distance( - closest_point, global_plan_.poses.end(), prune_distance_); + closest_point, global_plan_up_to_inversion_.poses.end(), prune_distance_); unsigned int mx, my; // Find the furthest relevent pose on the path to consider within costmap @@ -95,12 +104,12 @@ PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( geometry_msgs::msg::PoseStamped PathHandler::transformToGlobalPlanFrame( const geometry_msgs::msg::PoseStamped & pose) { - if (global_plan_.poses.empty()) { + if (global_plan_up_to_inversion_.poses.empty()) { throw nav2_core::InvalidPath("Received plan with zero length"); } geometry_msgs::msg::PoseStamped robot_pose; - if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) { + if (!transformPose(global_plan_up_to_inversion_.header.frame_id, pose, robot_pose)) { throw nav2_core::ControllerTFError( "Unable to transform robot pose into global plan's frame"); } @@ -116,7 +125,15 @@ nav_msgs::msg::Path PathHandler::transformPath( transformToGlobalPlanFrame(robot_pose); auto [transformed_plan, lower_bound] = getGlobalPlanConsideringBoundsInCostmapFrame(global_pose); - pruneGlobalPlan(lower_bound); + prunePlan(global_plan_up_to_inversion_, lower_bound); + + if (enforce_path_inversion_ && inversion_locale_ != 0u) { + if (isWithinInversionTolerances(global_pose)) { + prunePlan(global_plan_, global_plan_.poses.begin() + inversion_locale_); + global_plan_up_to_inversion_ = global_plan_; + inversion_locale_ = utils::removePosesAfterFirstInversion(global_plan_up_to_inversion_); + } + } if (transformed_plan.poses.empty()) { throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); @@ -156,13 +173,32 @@ double PathHandler::getMaxCostmapDist() void PathHandler::setPath(const nav_msgs::msg::Path & plan) { global_plan_ = plan; + global_plan_up_to_inversion_ = global_plan_; + if (enforce_path_inversion_) { + inversion_locale_ = utils::removePosesAfterFirstInversion(global_plan_up_to_inversion_); + } } nav_msgs::msg::Path & PathHandler::getPath() {return global_plan_;} -void PathHandler::pruneGlobalPlan(const PathIterator end) +void PathHandler::prunePlan(nav_msgs::msg::Path & plan, const PathIterator end) { - global_plan_.poses.erase(global_plan_.poses.begin(), end); + plan.poses.erase(plan.poses.begin(), end); +} + +bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose) +{ + // Keep full path if we are within tolerance of the inversion pose + const auto last_pose = global_plan_up_to_inversion_.poses.back(); + double distance = std::hypot( + robot_pose.pose.position.x - last_pose.pose.position.x, + robot_pose.pose.position.y - last_pose.pose.position.y); + + double angle_distance = angles::shortest_angular_distance( + tf2::getYaw(robot_pose.pose.orientation), + tf2::getYaw(last_pose.pose.orientation)); + + return distance <= inversion_xy_tolerance_ && fabs(angle_distance) <= inversion_yaw_tolerance; } } // namespace mppi diff --git a/nav2_mppi_controller/test/path_handler_test.cpp b/nav2_mppi_controller/test/path_handler_test.cpp index 3eb737ed36..0bcf554392 100644 --- a/nav2_mppi_controller/test/path_handler_test.cpp +++ b/nav2_mppi_controller/test/path_handler_test.cpp @@ -38,9 +38,9 @@ class PathHandlerWrapper : public PathHandler PathHandlerWrapper() : PathHandler() {} - void pruneGlobalPlanWrapper(const PathIterator end) + void pruneGlobalPlanWrapper(nav_msgs::msg::Path & path, const PathIterator end) { - return pruneGlobalPlan(end); + return prunePlan(path, end); } double getMaxCostmapDistWrapper() @@ -66,6 +66,21 @@ class PathHandlerWrapper : public PathHandler { return transformToGlobalPlanFrame(pose); } + + void setGlobalPlanUpToInversion(const nav_msgs::msg::Path & path) + { + global_plan_up_to_inversion_ = path; + } + + bool isWithinInversionTolerancesWrapper(const geometry_msgs::msg::PoseStamped & robot_pose) + { + return isWithinInversionTolerances(robot_pose); + } + + nav_msgs::msg::Path & getInvertedPath() + { + return global_plan_up_to_inversion_; + } }; TEST(PathHandlerTests, GetAndPrunePath) @@ -82,7 +97,7 @@ TEST(PathHandlerTests, GetAndPrunePath) EXPECT_EQ(path.poses.size(), rtn_path.poses.size()); PathIterator it = rtn_path.poses.begin() + 5; - handler.pruneGlobalPlanWrapper(it); + handler.pruneGlobalPlanWrapper(rtn_path, it); auto rtn2_path = handler.getPath(); EXPECT_EQ(rtn2_path.poses.size(), 6u); } @@ -131,10 +146,10 @@ TEST(PathHandlerTests, TestBounds) handler.setPath(path); auto [transformed_plan, closest] = handler.getGlobalPlanConsideringBoundsInCostmapFrameWrapper(robot_pose); - auto & path_in = handler.getPath(); - EXPECT_EQ(closest - path_in.poses.begin(), 25); - handler.pruneGlobalPlanWrapper(closest); - auto & path_pruned = handler.getPath(); + auto & path_inverted = handler.getInvertedPath(); + EXPECT_EQ(closest - path_inverted.poses.begin(), 25); + handler.pruneGlobalPlanWrapper(path_inverted, closest); + auto & path_pruned = handler.getInvertedPath(); EXPECT_EQ(path_pruned.poses.size(), 75u); } @@ -189,3 +204,46 @@ TEST(PathHandlerTests, TestTransforms) auto final_path = handler.transformPath(robot_pose); EXPECT_EQ(final_path.poses.size(), path_out.poses.size()); } + +TEST(PathHandlerTests, TestInversionToleranceChecks) +{ + nav_msgs::msg::Path path; + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = static_cast(i); + path.poses.push_back(pose); + } + path.poses.back().pose.orientation.w = 1; + + PathHandlerWrapper handler; + handler.setGlobalPlanUpToInversion(path); + + // Not near (0,0) + geometry_msgs::msg::PoseStamped robot_pose; + EXPECT_FALSE(handler.isWithinInversionTolerancesWrapper(robot_pose)); + + // Exactly on top of it + robot_pose.pose.position.x = 9; + robot_pose.pose.orientation.w = 1.0; + EXPECT_TRUE(handler.isWithinInversionTolerancesWrapper(robot_pose)); + + // Laterally of it + robot_pose.pose.position.y = 9; + EXPECT_FALSE(handler.isWithinInversionTolerancesWrapper(robot_pose)); + + // On top but off angled + robot_pose.pose.position.y = 0; + robot_pose.pose.orientation.z = 0.8509035; + robot_pose.pose.orientation.w = 0.525322; + EXPECT_FALSE(handler.isWithinInversionTolerancesWrapper(robot_pose)); + + // On top but off angled within tolerances + robot_pose.pose.position.y = 0; + robot_pose.pose.orientation.w = 0.9961947; + robot_pose.pose.orientation.z = 0.0871558; + EXPECT_TRUE(handler.isWithinInversionTolerancesWrapper(robot_pose)); + + // Offset spatially + off angled but both within tolerances + robot_pose.pose.position.x = 9.10; + EXPECT_TRUE(handler.isWithinInversionTolerancesWrapper(robot_pose)); +} diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp index 4a77578fb0..51d94b4e2b 100644 --- a/nav2_mppi_controller/test/utils_test.cpp +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -380,3 +380,66 @@ TEST(UtilsTests, SmootherTest) EXPECT_LT(smoothed_val, original_val); } + +TEST(UtilsTests, FindPathInversionTest) +{ + // Straight path, no inversions to be found + nav_msgs::msg::Path path; + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + EXPECT_EQ(utils::findFirstPathInversion(path), 10u); + + // To short to process + path.poses.erase(path.poses.begin(), path.poses.begin() + 7); + EXPECT_EQ(utils::findFirstPathInversion(path), 3u); + + // Has inversion at index 10, so should return 11 for the first point afterwards + // 0 1 2 3 4 5 6 7 8 9 10 **9** 8 7 6 5 4 3 2 1 + path.poses.clear(); + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 10 - i; + path.poses.push_back(pose); + } + EXPECT_EQ(utils::findFirstPathInversion(path), 11u); +} + +TEST(UtilsTests, RemovePosesAfterPathInversionTest) +{ + nav_msgs::msg::Path path; + // straight path + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 0u); + + // try empty path + path.poses.clear(); + EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 0u); + + // cusping path + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = i; + path.poses.push_back(pose); + } + for (unsigned int i = 0; i != 10; i++) { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 10 - i; + path.poses.push_back(pose); + } + EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 11u); + // Check to see if removed + EXPECT_EQ(path.poses.size(), 11u); + EXPECT_EQ(path.poses.back().pose.position.x, 10); +}