[MPPI] Reworked Path Align Critic; 70% faster + Tracks Paths Better! …

…Edit: strike that, now 80% (ros-navigation#3872) (ros-navigation#3882)

* adding regenerate noise param + adding docs

* fix tests

* remove unnecessary normalization

* Update optimizer.cpp

* adding refactored path alignment critic

* fix visualization bug

* speed up another 30%

* remove a little jitter

* a few more small optimizaitons

* fixing unit tests

* retain legacy critic

* adding tests for legacy

(cherry picked from commit 7009ffb)

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

nav2_mppi_controller/CMakeLists.txt

@@ -78,6 +78,7 @@ add_library(mppi_critics SHARED
   src/critics/goal_critic.cpp
   src/critics/goal_angle_critic.cpp
   src/critics/path_align_critic.cpp
+  src/critics/path_align_legacy_critic.cpp
   src/critics/path_follow_critic.cpp
   src/critics/path_angle_critic.cpp
   src/critics/prefer_forward_critic.cpp

nav2_mppi_controller/LICENSE.md

@@ -2,6 +2,7 @@ MIT License
 
 Copyright (c) 2021-2022 Fast Sense Studio
 Copyright (c) 2022-2023 Samsung Research America
+Copyright (c) 2023 Open Navigation LLC
 
 Permission is hereby granted, free of charge, to any person obtaining a copy
 of this software and associated documentation files (the "Software"), to deal

nav2_mppi_controller/README.md

@@ -125,6 +125,8 @@ This process is then repeated a number of times and returns a converged solution
  | 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.  |
 
+Note: There is a "Legacy" version of this critic also available with the same parameters of an old formulation pre-October 2023.
+
 #### Path Angle Critic
  | Parameter                 | Type   | Definition                                                                                                  |
  | ---------------           | ------ | ----------------------------------------------------------------------------------------------------------- |

nav2_mppi_controller/critics.xml

@@ -17,6 +17,10 @@
       <description>mppi critic for aligning to path</description>
     </class>
 
+    <class type="mppi::critics::PathAlignLegacyCritic" base_class_type="mppi::critics::CriticFunction">
+      <description>mppi critic for aligning to path (legacy)</description>
+    </class>
+
     <class type="mppi::critics::PathAngleCritic" base_class_type="mppi::critics::CriticFunction">
       <description>mppi critic for tracking the path in the correct heading</description>
     </class>

nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp

@@ -1,4 +1,4 @@
-// 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.

nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_legacy_critic.hpp

@@ -0,0 +1,60 @@
+// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
+//
+// 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 NAV2_MPPI_CONTROLLER__CRITICS__PATH_ALIGN_LEGACY_CRITIC_HPP_
+#define NAV2_MPPI_CONTROLLER__CRITICS__PATH_ALIGN_LEGACY_CRITIC_HPP_
+
+#include "nav2_mppi_controller/critic_function.hpp"
+#include "nav2_mppi_controller/models/state.hpp"
+#include "nav2_mppi_controller/tools/utils.hpp"
+
+namespace mppi::critics
+{
+
+/**
+ * @class mppi::critics::PathAlignLegacyCritic
+ * @brief Critic objective function for aligning to the path. Note:
+ * High settings of this will follow the path more precisely, but also makes it
+ * difficult (or impossible) to deviate in the presence of dynamic obstacles.
+ * This is an important critic to tune and consider in tandem with Obstacle.
+ * This is the initial 'Legacy' implementation before replacement Oct 2023.
+ */
+class PathAlignLegacyCritic : public CriticFunction
+{
+public:
+  /**
+    * @brief Initialize critic
+    */
+  void initialize() override;
+
+  /**
+   * @brief Evaluate cost related to trajectories path alignment
+   *
+   * @param costs [out] add reference cost values to this tensor
+   */
+  void score(CriticData & data) override;
+
+protected:
+  size_t offset_from_furthest_{0};
+  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};
+};
+
+}  // namespace mppi::critics
+
+#endif  // NAV2_MPPI_CONTROLLER__CRITICS__PATH_ALIGN_LEGACY_CRITIC_HPP_

nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp

@@ -308,14 +308,16 @@ inline size_t findPathFurthestReachedPoint(const CriticData & data)
 
   const auto dists = dx * dx + dy * dy;
 
-  size_t max_id_by_trajectories = 0;
+  size_t max_id_by_trajectories = 0, min_id_by_path = 0;
   float min_distance_by_path = std::numeric_limits<float>::max();
+  float cur_dist = 0.0f;
 
   for (size_t i = 0; i < dists.shape(0); i++) {
-    size_t min_id_by_path = 0;
+    min_id_by_path = 0;
     for (size_t j = 0; j < dists.shape(1); j++) {
-      if (dists(i, j) < min_distance_by_path) {
-        min_distance_by_path = dists(i, j);
+      cur_dist = dists(i, j);
+      if (cur_dist < min_distance_by_path) {
+        min_distance_by_path = cur_dist;
         min_id_by_path = j;
       }
     }
@@ -663,6 +665,23 @@ inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path)
   return first_after_inversion;
 }
 
+/**
+ * @brief Compare to trajectory points to find closest path point along integrated distances
+ * @param vec Vect to check
+ * @return dist Distance to look for
+ */
+inline size_t findClosestPathPt(const std::vector<float> & vec, float dist, size_t init = 0)
+{
+  auto iter = std::lower_bound(vec.begin() + init, vec.end(), dist);
+  if (iter == vec.begin()) {
+    return 0;
+  }
+  if (dist - *(iter - 1) < *iter - dist) {
+    return iter - 1 - vec.begin();
+  }
+  return iter - vec.begin();
+}
+
 }  // namespace mppi::utils
 
 #endif  // NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_

nav2_mppi_controller/src/critics/path_align_critic.cpp

@@ -1,4 +1,4 @@
-// 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.
@@ -54,7 +54,8 @@ void PathAlignCritic::score(CriticData & data)
 
   // Don't apply when first getting bearing w.r.t. the path
   utils::setPathFurthestPointIfNotSet(data);
-  if (*data.furthest_reached_path_point < offset_from_furthest_) {
+  const size_t path_segments_count = *data.furthest_reached_path_point;  // up to furthest only
+  if (path_segments_count < offset_from_furthest_) {
     return;
   }
 
@@ -70,59 +71,62 @@ 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);
-  const size_t traj_pts_eval = floor(time_steps / trajectory_point_step_);
-  const size_t path_segments_count = data.path.x.shape(0) - 1;
+  const size_t batch_size = data.trajectories.x.shape(0);
+  const size_t time_steps = data.trajectories.x.shape(1);
   auto && cost = xt::xtensor<float, 1>::from_shape({data.costs.shape(0)});
 
-  if (path_segments_count < 1) {
-    return;
+  // Find integrated distance in the path
+  std::vector<float> path_integrated_distances(path_segments_count, 0.0f);
+  float dx = 0.0f, dy = 0.0f;
+  for (unsigned int i = 1; i != path_segments_count; i++) {
+    dx = P_x(i) - P_x(i - 1);
+    dy = P_y(i) - P_y(i - 1);
+    float curr_dist = sqrtf(dx * dx + dy * dy);
+    path_integrated_distances[i] = path_integrated_distances[i - 1] + curr_dist;
   }
 
-  float dist_sq = 0.0f, dx = 0.0f, dy = 0.0f, dyaw = 0.0f, summed_dist = 0.0f;
-  float min_dist_sq = std::numeric_limits<float>::max();
-  size_t min_s = 0;
-
+  float traj_integrated_distance = 0.0f;
+  float summed_path_dist = 0.0f, dyaw = 0.0f;
+  float num_samples = 0.0f;
+  float Tx = 0.0f, Ty = 0.0f;
+  size_t path_pt = 0;
   for (size_t t = 0; t < batch_size; ++t) {
-    summed_dist = 0.0f;
+    traj_integrated_distance = 0.0f;
+    summed_path_dist = 0.0f;
+    num_samples = 0.0f;
+    const auto T_x = xt::view(data.trajectories.x, t, xt::all());
+    const auto T_y = xt::view(data.trajectories.y, t, xt::all());
     for (size_t p = trajectory_point_step_; p < time_steps; p += trajectory_point_step_) {
-      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;
-        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;
-        }
-      }
+      Tx = T_x(p);
+      Ty = T_y(p);
+      dx = Tx - T_x(p - trajectory_point_step_);
+      dy = Ty - T_y(p - trajectory_point_step_);
+      traj_integrated_distance += sqrtf(dx * dx + dy * dy);
+      path_pt = utils::findClosestPathPt(
+        path_integrated_distances, traj_integrated_distance, path_pt);
 
       // The nearest path point to align to needs to be not in collision, else
       // let the obstacle critic take over in this region due to dynamic obstacles
-      if (min_s != 0 && (*data.path_pts_valid)[min_s]) {
-        summed_dist += sqrtf(min_dist_sq);
+      if ((*data.path_pts_valid)[path_pt]) {
+        dx = P_x(path_pt) - Tx;
+        dy = P_y(path_pt) - Ty;
+        num_samples += 1.0f;
+        if (use_path_orientations_) {
+          const auto T_yaw = xt::view(data.trajectories.yaws, t, xt::all());
+          dyaw = angles::shortest_angular_distance(P_yaw(path_pt), T_yaw(p));
+          summed_path_dist += sqrtf(dx * dx + dy * dy + dyaw * dyaw);
+        } else {
+          summed_path_dist += sqrtf(dx * dx + dy * dy);
+        }
       }
     }
-
-    cost[t] = summed_dist / traj_pts_eval;
+    if (num_samples > 0) {
+      cost[t] = summed_path_dist / num_samples;
+    }
   }
 
   data.costs += xt::pow(std::move(cost) * weight_, power_);