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 <guillaume@dexory.com>

* 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 <doisyg@users.noreply.github.com>
Co-authored-by: Guillaume Doisy <guillaume@dexory.com>

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

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};
 };

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,22 +126,35 @@ 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<nav2_costmap_2d::Costmap2DROS> costmap_;
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
   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
 

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_

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<float>::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<float>::max();
-      size_t min_s = 0;
+      min_dist_sq = std::numeric_limits<float>::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<float, xt::xshape<2>> 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;

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<nav_msgs::msg::Path, PathIterator>
@@ -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