Added support for MPPI Controller to adjust wz_std parameter based on linear speed

nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp

@@ -15,6 +15,10 @@
 #ifndef NAV2_MPPI_CONTROLLER__MODELS__CONSTRAINTS_HPP_
 #define NAV2_MPPI_CONTROLLER__MODELS__CONSTRAINTS_HPP_
 
+#include <memory>
+#include <string>
+#include <stdexcept>
+
 namespace mppi::models
 {
 
@@ -35,6 +39,30 @@ struct ControlConstraints
   float az_max;
 };
 
+struct AdvancedConstraints
+{
+  /**
+   * @brief Defines the strength of the reduction function
+   * Allows dynamic modification of wz_std (angular deviation) based on linear velocity of the robot.
+   * When a robot with high inertia (e.g. 500kg) is moving fast and if wz_std is above 0.3, oscillation behavior can be observed. Lowering wz_std stabilizes the robot but then the maneuvers take more time.
+   * Dynamically reducing wz_std as vx, vy increase (speed of the robot) solves both problems.
+   * Suggested values to start with: wz_std = 0.4, wz_std_decay_to = 0.05, wz_std_decay_strength = 3.0
+   * The following is used as the decay function
+   * <pre>f(x) = (wz_std - wz_std_decay_to) * e^(-wz_std_decay_strength * v_linear) + wz_std_decay_to</pre>
+   * <a href="https://www.wolframalpha.com/input?i=plot+%5B%28a-b%29+*+e%5E%28-c+*+x%29+%2B+b%5D+with+a%3D0.5%2C+b%3D0.05%2C+c%3D3">Playground</a>
+   * Default: -1.0 (disabled)
+   */
+  float wz_std_decay_strength;
+
+  /**
+   * @brief Target wz_std value while linear speed goes to infinity.
+   * Must be between 0 and wz_std.
+   * Has no effect if `advanced.wz_std_decay_strength` <= 0.0
+   * Default: 0.0
+   */
+  float wz_std_decay_to;
+};
+
 /**
  * @struct mppi::models::SamplingStd
  * @brief Noise parameters for sampling trajectories
@@ -44,6 +72,38 @@ struct SamplingStd
   float vx;
   float vy;
   float wz;
+
+  /**
+   * @brief Internal variable that holds wz_std after decay is applied.
+   * If decay is disabled, SamplingStd.wz == wz_std_adaptive
+  */
+  std::shared_ptr<float> wz_std_adaptive;
+
+  /**
+   * @param c Reference to the advanced constraints
+   * @param quiet Whether to throw reason or just return false if there's an invalid constraint config
+   * @return true if constraints are valid
+   * @throw std::runtime_error Throws the error reason only if quiet is false and constraint configuration is invalid
+   */
+  bool validateConstraints(const AdvancedConstraints & c, bool quiet)
+  {
+    // Assume valid if angular decay is disabled
+    if (c.wz_std_decay_strength <= 0.0f) {
+      return true;  // valid
+    }
+
+    if (c.wz_std_decay_to < 0.0f || c.wz_std_decay_to > wz) {
+      if (quiet) {
+        return false;
+      }
+      // else
+      const std::string msg =
+        "wz_std_decay_to must be between 0 and wz_std. "
+        "wz: " + std::to_string(wz) + ", wz_std_decay_to: " + std::to_string(c.wz_std_decay_to);
+      throw std::runtime_error(msg);
+    }
+    return true;
+  }
 };
 
 }  // namespace mppi::models

nav2_mppi_controller/include/nav2_mppi_controller/models/optimizer_settings.hpp

@@ -16,6 +16,7 @@
 #define NAV2_MPPI_CONTROLLER__MODELS__OPTIMIZER_SETTINGS_HPP_
 
 #include <cstddef>
+#include <memory>
 #include "nav2_mppi_controller/models/constraints.hpp"
 
 namespace mppi::models
@@ -29,7 +30,8 @@ struct OptimizerSettings
 {
   models::ControlConstraints base_constraints{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
   models::ControlConstraints constraints{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
-  models::SamplingStd sampling_std{0.0f, 0.0f, 0.0f};
+  models::SamplingStd sampling_std{0.0f, 0.0f, 0.0f, std::make_shared<float>(0.0f)};
+  models::AdvancedConstraints advanced_constraints{0.0f, 0.0f};
   float model_dt{0.0f};
   float temperature{0.0f};
   float gamma{0.0f};

nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp

@@ -118,6 +118,11 @@ class Optimizer
    */
   void setSpeedLimit(double speed_limit, bool percentage);
 
+  /**
+   * Resets adaptive std values to their parametric defaults
+   */
+  void resetAdaptiveStds();
+
   /**
    * @brief Reset the optimization problem to initial conditions
    * @param Whether to reset the constraints to its base values

nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp

@@ -55,6 +55,11 @@ class NoiseGenerator
     mppi::models::OptimizerSettings & settings,
     bool is_holonomic, const std::string & name, ParametersHandler * param_handler);
 
+  /**
+   * @brief Initialize normal_distributions to their SamplingStd parameterized values (vx, vy, wz)
+   */
+  void initializeNormalDistributions();
+
   /**
    * @brief Shutdown noise generator thread
    */
@@ -66,6 +71,13 @@ class NoiseGenerator
    */
   void generateNextNoises();
 
+  /**
+   * Computes adaptive values of the SamplingStd parameters and updates adaptive counterparts
+   * See also wz_std_decay_strength, wz_std_decay_to parameters for more information on how wz => wz_std_adaptive is computed
+   * @param state Current state of the robot
+   */
+  void computeAdaptiveStds(const models::State & state);
+
   /**
    * @brief set noised control_sequence to state controls
    * @return noises vx, vy, wz

nav2_mppi_controller/src/noise_generator.cpp

@@ -28,10 +28,7 @@ void NoiseGenerator::initialize(
   is_holonomic_ = is_holonomic;
   active_ = true;
 
-  ndistribution_vx_ = std::normal_distribution(0.0f, settings_.sampling_std.vx);
-  ndistribution_vy_ = std::normal_distribution(0.0f, settings_.sampling_std.vy);
-  ndistribution_wz_ = std::normal_distribution(0.0f, settings_.sampling_std.wz);
-
+  initializeNormalDistributions();
   auto getParam = param_handler->getParamGetter(name);
   getParam(regenerate_noises_, "regenerate_noises", false);
 
@@ -42,6 +39,13 @@ void NoiseGenerator::initialize(
   }
 }
 
+void NoiseGenerator::initializeNormalDistributions()
+{
+  ndistribution_vx_ = std::normal_distribution(0.0f, settings_.sampling_std.vx);
+  ndistribution_vy_ = std::normal_distribution(0.0f, settings_.sampling_std.vy);
+  ndistribution_wz_ = std::normal_distribution(0.0f, settings_.sampling_std.wz);
+}
+
 void NoiseGenerator::shutdown()
 {
   active_ = false;
@@ -63,12 +67,48 @@ void NoiseGenerator::generateNextNoises()
   noise_cond_.notify_all();
 }
 
+void NoiseGenerator::computeAdaptiveStds(const models::State & state)
+{
+  auto & s = settings_;
+  float & wz_std_adaptive = *settings_.sampling_std.wz_std_adaptive;
+
+  // Should we apply decay function? or Any constraint is invalid?
+  if (s.advanced_constraints.wz_std_decay_strength <= 0.0f ||
+    !s.sampling_std.validateConstraints(s.advanced_constraints, true))
+  {
+    wz_std_adaptive = s.sampling_std.wz;  // skip calculation
+  } else {
+    float current_speed;
+    if (is_holonomic_) {
+      const auto vx = static_cast<float>(state.speed.linear.x);
+      const auto vy = static_cast<float>(state.speed.linear.y);
+      current_speed = hypotf(vx, vy);
+    } else {
+      current_speed = fabs(static_cast<float>(state.speed.linear.x));
+    }
+
+    static const float e = std::exp(1.0f);
+    const float decayed_wz_std = (s.sampling_std.wz - s.advanced_constraints.wz_std_decay_to) *
+      powf(e, -1.0f * s.advanced_constraints.wz_std_decay_strength * current_speed) +
+      s.advanced_constraints.wz_std_decay_to;
+
+    wz_std_adaptive = decayed_wz_std;
+  }
+
+  // Check if there's any change on adaptive std's and re-create relevant distribution if any
+  if (ndistribution_wz_.stddev() != wz_std_adaptive) {
+    ndistribution_wz_ = std::normal_distribution(0.0f, wz_std_adaptive);
+  }
+}
+
 void NoiseGenerator::setNoisedControls(
   models::State & state,
   const models::ControlSequence & control_sequence)
 {
   std::unique_lock<std::mutex> guard(noise_lock_);
 
+  computeAdaptiveStds(state);
+
   state.cvx = noises_vx_.rowwise() + control_sequence.vx.transpose();
   state.cvy = noises_vy_.rowwise() + control_sequence.vy.transpose();
   state.cwz = noises_wz_.rowwise() + control_sequence.wz.transpose();
@@ -82,6 +122,9 @@ void NoiseGenerator::reset(mppi::models::OptimizerSettings & settings, bool is_h
   // Recompute the noises on reset, initialization, and fallback
   {
     std::unique_lock<std::mutex> guard(noise_lock_);
+
+    initializeNormalDistributions();
+
     noises_vx_.setZero(settings_.batch_size, settings_.time_steps);
     noises_vy_.setZero(settings_.batch_size, settings_.time_steps);
     noises_wz_.setZero(settings_.batch_size, settings_.time_steps);

nav2_mppi_controller/src/optimizer.cpp

@@ -81,6 +81,8 @@
   getParam(s.sampling_std.vx, "vx_std", 0.2f);
   getParam(s.sampling_std.vy, "vy_std", 0.2f);
   getParam(s.sampling_std.wz, "wz_std", 0.4f);
+  getParam(s.advanced_constraints.wz_std_decay_to, "advanced.wz_std_decay_to", 0.0f);
+  getParam(s.advanced_constraints.wz_std_decay_strength, "advanced.wz_std_decay_strength", -1.0f);
   getParam(s.retry_attempt_limit, "retry_attempt_limit", 1);
 
   s.base_constraints.ax_max = fabs(s.base_constraints.ax_max);
@@ -132,9 +134,16 @@
   }
 }
 
+void Optimizer::resetAdaptiveStds()
+{
+  // reset initial adaptive value to parameterized value
+  *settings_.sampling_std.wz_std_adaptive = settings_.sampling_std.wz;
+}
+
 void Optimizer::reset(bool reset_dynamic_speed_limits)
 {
   state_.reset(settings_.batch_size, settings_.time_steps);
+  resetAdaptiveStds();
   control_sequence_.reset(settings_.time_steps);
   control_history_[0] = {0.0f, 0.0f, 0.0f};
   control_history_[1] = {0.0f, 0.0f, 0.0f};
@@ -151,6 +160,13 @@
   noise_generator_.reset(settings_, isHolonomic());
   motion_model_->initialize(settings_.constraints, settings_.model_dt);
 
+  // Validate decay function, print warning message if decay_to is out of bounds
+  try {
+    settings_.sampling_std.validateConstraints(settings_.advanced_constraints, false);
+  } catch (const std::runtime_error & e) {
+    RCLCPP_WARN_STREAM(logger_, e.what());
+  }
+
   RCLCPP_INFO(logger_, "Optimizer reset");
 }
 
@@ -439,6 +455,7 @@
   return control_sequence_;
 }
 
+
 void Optimizer::updateControlSequence()
 {
   const bool is_holo = isHolonomic();
@@ -449,10 +466,11 @@
   const float gamma_vx = s.gamma / (s.sampling_std.vx * s.sampling_std.vx);
   costs_ += (gamma_vx * (bounded_noises_vx.rowwise() * vx_T).rowwise().sum()).eval();
 
-  if (s.sampling_std.wz > 0.0f) {
+  const float & wz_std_adaptive = *s.sampling_std.wz_std_adaptive;
+  if (wz_std_adaptive > 0.0f) {
     auto wz_T = control_sequence_.wz.transpose();
     auto bounded_noises_wz = state_.cwz.rowwise() - wz_T;
-    const float gamma_wz = s.gamma / (s.sampling_std.wz * s.sampling_std.wz);
+    const float gamma_wz = s.gamma / (wz_std_adaptive * wz_std_adaptive);
     costs_ += (gamma_wz * (bounded_noises_wz.rowwise() * wz_T).rowwise().sum()).eval();
   }
 

nav2_mppi_controller/test/critics_tests.cpp

@@ -331,7 +331,7 @@ TEST(CriticTests, PathAngleCritic)
   costmap_ros->on_configure(lstate);
 
   models::State state;
-  state.reset(1000, 30);
+   state.reset(1000, 30);
   models::ControlSequence control_sequence;
   models::Trajectories generated_trajectories;
   generated_trajectories.reset(1000, 30);

nav2_mppi_controller/test/noise_generator_test.cpp

@@ -28,6 +28,8 @@
 
 using namespace mppi;  // NOLINT
 
+static constexpr double EPSILON = std::numeric_limits<float>::epsilon();
+
 TEST(NoiseGeneratorTest, NoiseGeneratorLifecycle)
 {
   // Tests shuts down internal thread cleanly
@@ -204,6 +206,73 @@ ParametersHandler handler(node, name);
   generator.shutdown();
 }
 
+TEST(NoiseGeneratorTest, AdaptiveStds)
+{
+  // Tests shuts down internal thread cleanly
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("node");
+  std::string name = "test";
+  ParametersHandler handler(node, name);
+  NoiseGenerator generator;
+  mppi::models::OptimizerSettings settings;
+  settings.batch_size = 100;
+  settings.time_steps = 25;
+  settings.sampling_std.vx = 0.1;
+  settings.sampling_std.vy = 0.1;
+  settings.sampling_std.wz = 0.1;
+
+  mppi::models::State state;
+  state.reset(settings.batch_size, settings.time_steps);
+
+  // Test with AdaptiveStd off (default behavior)
+  generator.initialize(settings, false, "test_name", &handler);
+  generator.reset(settings, false);  // sets initial sizing and zeros out noises
+  generator.computeAdaptiveStds(state);
+  EXPECT_NEAR(settings.sampling_std.wz, *settings.sampling_std.wz_std_adaptive, EPSILON);
+
+  // Enable AdaptiveStd but keep velocity == 0
+  settings.advanced_constraints.wz_std_decay_strength = 3.0f;
+  settings.advanced_constraints.wz_std_decay_to = 0.05f;
+  generator.reset(settings, false);  // sets initial sizing and zeros out noises
+  generator.computeAdaptiveStds(state);
+  EXPECT_NEAR(settings.sampling_std.wz, *settings.sampling_std.wz_std_adaptive, EPSILON);
+
+  // Enable AdaptiveStd, non-holonomic with vx == 1.0 m/sec
+  settings.advanced_constraints.wz_std_decay_strength = 3.0f;
+  settings.advanced_constraints.wz_std_decay_to = 0.05f;
+  state.speed.linear.x = 1.0f;
+  generator.reset(settings, false);  // sets initial sizing and zeros out noises
+  generator.computeAdaptiveStds(state);
+  EXPECT_NEAR(0.052489355206489563f, *settings.sampling_std.wz_std_adaptive, EPSILON);
+  EXPECT_NEAR(0.1f, settings.sampling_std.wz, EPSILON);  // wz_std should stay the same
+
+  // Enable AdaptiveStd, holonomic with scalar velocity == 1.0 m/sec
+  settings.advanced_constraints.wz_std_decay_strength = 3.0f;
+  settings.advanced_constraints.wz_std_decay_to = 0.05f;
+  state.speed.linear.x = 0.70710678118655f;
+  state.speed.linear.y = 0.70710678118655f;
+  generator.reset(settings, true);  // sets initial sizing and zeros out noises
+  generator.computeAdaptiveStds(state);
+  EXPECT_NEAR(0.052489355206489563f, *settings.sampling_std.wz_std_adaptive, EPSILON);
+  EXPECT_NEAR(0.1f, settings.sampling_std.wz, EPSILON);  // wz_std should stay the same
+
+  // Enable AdaptiveStd, with invalid input
+  settings.advanced_constraints.wz_std_decay_strength = 3.0f;
+  settings.advanced_constraints.wz_std_decay_to = 0.2f;  // > than wz_std
+  state.speed.linear.x = 1.0f;
+  generator.reset(settings, false);  // sets initial sizing and zeros out noises
+  generator.computeAdaptiveStds(state);
+  // expect wz_std == wz_std_adaptive as adaptive std will be automatically disabled
+  EXPECT_EQ(settings.sampling_std.wz, *settings.sampling_std.wz_std_adaptive);
+  try {
+    settings.sampling_std.validateConstraints(settings.advanced_constraints, false);
+    FAIL() << "Expected to throw runtime error";
+  } catch (const std::runtime_error & e) {
+    EXPECT_TRUE(!std::string(e.what()).empty());
+  }
+
+  generator.shutdown();
+}
+
 int main(int argc, char **argv)
 {
   ::testing::InitGoogleTest(&argc, argv);