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

nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp

@@ -35,6 +35,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

nav2_mppi_controller/include/nav2_mppi_controller/models/optimizer_settings.hpp

@@ -30,6 +30,7 @@ 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::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/tools/noise_generator.hpp

@@ -15,8 +15,6 @@
 #ifndef NAV2_MPPI_CONTROLLER__TOOLS__NOISE_GENERATOR_HPP_
 #define NAV2_MPPI_CONTROLLER__TOOLS__NOISE_GENERATOR_HPP_
 
-#include <Eigen/Dense>
-
 #include <string>
 #include <memory>
 #include <thread>
@@ -47,14 +45,13 @@ class NoiseGenerator
 
   /**
    * @brief Initialize noise generator with settings and model types
-   * @param settings Settings of controller
-   * @param is_holonomic If base is holonomic
+   * @param logger Reference to the package logger
    * @param name Namespace for configs
    * @param param_handler Get parameters util
    */
   void initialize(
-    mppi::models::OptimizerSettings & settings,
-    bool is_holonomic, const std::string & name, ParametersHandler * param_handler);
+    const std::shared_ptr<nav2::LifecycleNode> & node, const std::string & name,
+    ParametersHandler * param_handler);
 
   /**
    * @brief Shutdown noise generator thread
@@ -73,12 +70,27 @@ class NoiseGenerator
    */
   void setNoisedControls(models::State & state, const models::ControlSequence & control_sequence);
 
+  /**
+   * 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);
+
+  /**
+   * Validates decay constraints and returns true if constraints are valid
+   * @return true if decay constraints are valid
+   */
+  bool validateWzStdDecayConstraints() const;
+
+  float getWzStdAdaptive() const;
+
   /**
    * @brief Reset noise generator with settings and model types
    * @param settings Settings of controller
    * @param is_holonomic If base is holonomic
    */
-  void reset(mppi::models::OptimizerSettings & settings, bool is_holonomic);
+  void reset(const mppi::models::OptimizerSettings & settings, bool is_holonomic);
 
 protected:
   /**
@@ -100,17 +112,21 @@ class NoiseGenerator
   Eigen::ArrayXXf noises_wz_;
 
   std::default_random_engine generator_;
-  std::normal_distribution<float> ndistribution_vx_;
-  std::normal_distribution<float> ndistribution_wz_;
-  std::normal_distribution<float> ndistribution_vy_;
 
-  mppi::models::OptimizerSettings settings_;
+  std::unique_ptr<rclcpp::Logger> logger_;
+  models::OptimizerSettings settings_;
   bool is_holonomic_;
 
-  std::thread noise_thread_;
+  std::unique_ptr<std::thread> noise_thread_;
   std::condition_variable noise_cond_;
   std::mutex noise_lock_;
-  bool active_{false}, ready_{false}, regenerate_noises_{false};
+  bool active_{false}, ready_{false};
+
+  /**
+   * @brief Internal variable that holds wz_std after decay is applied.
+   * If decay is disabled, SamplingStd.wz == wz_std_adaptive
+  */
+  float wz_std_adaptive;
 };
 
 }  // namespace mppi

nav2_mppi_controller/src/noise_generator.cpp

@@ -21,34 +21,34 @@ namespace mppi
 {
 
 void NoiseGenerator::initialize(
-  mppi::models::OptimizerSettings & settings, bool is_holonomic,
-  const std::string & name, ParametersHandler * param_handler)
+  const std::shared_ptr<nav2::LifecycleNode> & node,
+  const std::string & name,
+  ParametersHandler * param_handler)
 {
-  settings_ = settings;
-  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);
-
+  this->logger_ = std::make_unique<rclcpp::Logger>(node->get_logger());
+  bool regenerate_noises;
   auto getParam = param_handler->getParamGetter(name);
-  getParam(regenerate_noises_, "regenerate_noises", false);
+  getParam(regenerate_noises, "regenerate_noises", false);
 
-  if (regenerate_noises_) {
-    noise_thread_ = std::thread(std::bind(&NoiseGenerator::noiseThread, this));
+  if (regenerate_noises) {
+    noise_thread_ = std::make_unique<std::thread>(std::bind(&NoiseGenerator::noiseThread, this));
   } else {
-    generateNoisedControls();
+    noise_thread_.reset();
   }
+
+  active_ = true;
 }
 
 void NoiseGenerator::shutdown()
 {
   active_ = false;
   ready_ = true;
   noise_cond_.notify_all();
-  if (noise_thread_.joinable()) {
-    noise_thread_.join();
+  if (noise_thread_ != nullptr) {
+    if (noise_thread_->joinable()) {
+      noise_thread_->join();
+    }
+    noise_thread_.reset();
   }
 }
 
@@ -69,26 +69,86 @@ void NoiseGenerator::setNoisedControls(
 {
   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();
 }
 
-void NoiseGenerator::reset(mppi::models::OptimizerSettings & settings, bool is_holonomic)
+void NoiseGenerator::computeAdaptiveStds(const models::State & state)
+{
+  auto & s = settings_;
+
+  // Should we apply decay function? or Any constraint is invalid?
+  if (s.advanced_constraints.wz_std_decay_strength <= 0.0f || !validateWzStdDecayConstraints()) {
+    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 = std::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;
+  }
+}
+
+bool NoiseGenerator::validateWzStdDecayConstraints() const
 {
-  settings_ = settings;
-  is_holonomic_ = is_holonomic;
+  const models::AdvancedConstraints & c = settings_.advanced_constraints;
+  // 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 > settings_.sampling_std.wz) {
+    return false;
+  }
+  return true;
+}
+
+float NoiseGenerator::getWzStdAdaptive() const
+{
+  return wz_std_adaptive;
+}
+
+void NoiseGenerator::reset(const mppi::models::OptimizerSettings & settings, bool is_holonomic)
+{
   // Recompute the noises on reset, initialization, and fallback
   {
     std::unique_lock<std::mutex> guard(noise_lock_);
+    // Copy settings and holonomic info after lock is acquired,
+    // otherwise we may encounter concurrent access errors
+    settings_ = settings;
+    is_holonomic_ = is_holonomic;
+    // reset initial adaptive value to parameterized value
+    wz_std_adaptive = settings_.sampling_std.wz;
     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);
+
+    // Validate decay function, print warning message if decay_to is out of bounds
+    if (!validateWzStdDecayConstraints()) {
+      RCLCPP_WARN_STREAM(
+        *logger_, "wz_std_decay_to must be between 0 and wz_std. wz: "
+                    << std::to_string(settings_.constraints.wz) << ", wz_std_decay_to: "
+                    << std::to_string(settings_.advanced_constraints.wz_std_decay_to));
+    }
     ready_ = true;
-  }
+  }  // release the lock
+
 
-  if (regenerate_noises_) {
+  if (noise_thread_ != nullptr) {  // if regenerate_noises_ == true, then noise_thread_ is non-null
     noise_cond_.notify_all();
   } else {
     generateNoisedControls();
@@ -108,14 +168,16 @@ void NoiseGenerator::noiseThread()
 void NoiseGenerator::generateNoisedControls()
 {
   auto & s = settings_;
+  auto ndistribution_vx = std::normal_distribution(0.0f, settings_.sampling_std.vx);
+  auto ndistribution_wz = std::normal_distribution(0.0f, wz_std_adaptive);
+  auto ndistribution_vy = std::normal_distribution(0.0f, settings_.sampling_std.vy);
   noises_vx_ = Eigen::ArrayXXf::NullaryExpr(
-    s.batch_size, s.time_steps, [&] () {return ndistribution_vx_(generator_);});
+    s.batch_size, s.time_steps, [&]() {return ndistribution_vx(generator_);});
   noises_wz_ = Eigen::ArrayXXf::NullaryExpr(
-    s.batch_size, s.time_steps, [&] () {return ndistribution_wz_(generator_);});
-  if(is_holonomic_) {
+    s.batch_size, s.time_steps, [&]() {return ndistribution_wz(generator_);});
+  if (is_holonomic_) {
     noises_vy_ = Eigen::ArrayXXf::NullaryExpr(
-      s.batch_size, s.time_steps, [&] () {return ndistribution_vy_(generator_);});
+      s.batch_size, s.time_steps, [&]() {return ndistribution_vy(generator_);});
   }
 }
-
 }  // namespace mppi

nav2_mppi_controller/src/optimizer.cpp

@@ -46,7 +46,7 @@ void Optimizer::initialize(
   getParams();
 
   critic_manager_.on_configure(parent_, name_, costmap_ros_, parameters_handler_);
-  noise_generator_.initialize(settings_, isHolonomic(), name_, parameters_handler_);
+  noise_generator_.initialize(node, name_, parameters_handler_);
 
   reset();
 }
@@ -81,6 +81,8 @@ void Optimizer::getParams()
   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);
@@ -449,10 +451,11 @@ void Optimizer::updateControlSequence()
   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 = noise_generator_.getWzStdAdaptive();
+  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);