From 5ea887337f5e237131c667fdf1393856df2c52ad Mon Sep 17 00:00:00 2001 From: "V. Oguz TOKMAK" <5289529+vahapt@users.noreply.github.com> Date: Thu, 19 Jun 2025 21:49:12 +0300 Subject: [PATCH 1/6] Added support for MPPI Controller to adjust wz_std parameter based on linear speed Signed-off-by: V. Oguz TOKMAK <5289529+vahapt@users.noreply.github.com> --- nav2_mppi_controller/README.md | 43 ++++++++-------- .../models/constraints.hpp | 38 ++++++++++++++ .../models/optimizer_settings.hpp | 1 + .../nav2_mppi_controller/models/state.hpp | 10 +++- .../nav2_mppi_controller/optimizer.hpp | 7 +++ .../tools/noise_generator.hpp | 2 +- nav2_mppi_controller/src/noise_generator.cpp | 23 +++++++-- nav2_mppi_controller/src/optimizer.cpp | 50 +++++++++++++++++-- nav2_mppi_controller/test/critics_tests.cpp | 12 ++--- nav2_mppi_controller/test/models_test.cpp | 3 +- .../test/motion_model_tests.cpp | 8 +-- .../test/noise_generator_test.cpp | 10 ++-- .../test/optimizer_unit_tests.cpp | 13 +++-- 13 files changed, 170 insertions(+), 50 deletions(-) diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 68db5b73719..448dbb89282 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -38,26 +38,29 @@ This process is then repeated a number of times and returns a converged solution ## Configuration ### Controller - | Parameter | Type | Definition | - | --------------------- | ------ | -------------------------------------------------------------------------------------------------------- | - | motion_model | string | Default: DiffDrive. Type of model [DiffDrive, Omni, Ackermann]. | - | critics | string | Default: None. Critics (plugins) names | - | iteration_count | int | Default 1. Iteration count in MPPI algorithm. Recommend to keep as 1 and prefer more batches. | - | batch_size | int | Default 1000. Count of randomly sampled candidate trajectories | - | time_steps | int | Default 56. Number of time steps (points) in each sampled trajectory | - | model_dt | double | Default: 0.05. Time interval (s) between two sampled points in trajectories. | - | vx_std | double | Default 0.2. Sampling standard deviation for VX | - | vy_std | double | Default 0.2. Sampling standard deviation for VY | - | wz_std | double | Default 0.4. Sampling standard deviation for Wz | - | vx_max | double | Default 0.5. Max VX (m/s) | - | vy_max | double | Default 0.5. Max VY in either direction, if holonomic. (m/s) | - | vx_min | double | Default -0.35. Min VX (m/s) | - | wz_max | double | Default 1.9. Max WZ (rad/s) | - | temperature | double | Default: 0.3. Selectiveness of trajectories by their costs (The closer this value to 0, the "more" we take in consideration controls with less cost), 0 mean use control with best cost, huge value will lead to just taking mean of all trajectories without cost consideration | - | gamma | double | Default: 0.015. A trade-off between smoothness (high) and low energy (low). This is a complex parameter that likely won't need to be changed from the default of `0.1` which works well for a broad range of cases. See Section 3D-2 in "Information Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving" for detailed information. | - | visualize | bool | Default: false. Publish visualization of trajectories, which can slow down the controller significantly. Use only for debugging. | - | retry_attempt_limit | int | Default 1. Number of attempts to find feasible trajectory on failure for soft-resets before reporting failure. | - | regenerate_noises | bool | Default false. Whether to regenerate noises each iteration or use single noise distribution computed on initialization and reset. Practically, this is found to work fine since the trajectories are being sampled stochastically from a normal distribution and reduces compute jittering at run-time due to thread wake-ups to resample normal distribution. | + | Parameter | Type | Definition | + |----------------------------------|--------|----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| + | motion_model | string | Default: DiffDrive. Type of model [DiffDrive, Omni, Ackermann]. | + | critics | string | Default: None. Critics (plugins) names | + | iteration_count | int | Default 1. Iteration count in MPPI algorithm. Recommend to keep as 1 and prefer more batches. | + | batch_size | int | Default 1000. Count of randomly sampled candidate trajectories | + | time_steps | int | Default 56. Number of time steps (points) in each sampled trajectory | + | model_dt | double | Default: 0.05. Time interval (s) between two sampled points in trajectories. | + | vx_std | double | Default 0.2. Sampling standard deviation for VX | + | vy_std | double | Default 0.2. Sampling standard deviation for VY | + | wz_std | double | Default 0.4. Sampling standard deviation for Wz | + | vx_max | double | Default 0.5. Max VX (m/s) | + | vy_max | double | Default 0.5. Max VY in either direction, if holonomic. (m/s) | + | vx_min | double | Default -0.35. Min VX (m/s) | + | wz_max | double | Default 1.9. Max WZ (rad/s) | + | temperature | double | Default: 0.3. Selectiveness of trajectories by their costs (The closer this value to 0, the "more" we take in consideration controls with less cost), 0 mean use control with best cost, huge value will lead to just taking mean of all trajectories without cost consideration | + | gamma | double | Default: 0.015. A trade-off between smoothness (high) and low energy (low). This is a complex parameter that likely won't need to be changed from the default of `0.1` which works well for a broad range of cases. See Section 3D-2 in "Information Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving" for detailed information. | + | visualize | bool | Default: false. Publish visualization of trajectories, which can slow down the controller significantly. Use only for debugging. | + | retry_attempt_limit | int | Default 1. Number of attempts to find feasible trajectory on failure for soft-resets before reporting failure. | + | regenerate_noises | bool | Default false. Whether to regenerate noises each iteration or use single noise distribution computed on initialization and reset. Practically, this is found to work fine since the trajectories are being sampled stochastically from a normal distribution and reduces compute jittering at run-time due to thread wake-ups to resample normal distribution. | + | advanced.wz_std_decay_strength | double | Default: -1.0 (disabled). 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
`f(x) = (wz_std - wz_std_decay_to) * e^(-wz_std_decay_strength * v_linear) + wz_std_decay_to`
[Playground](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) | + | advanced.wz_std_decay_to | double | Default: 0.0. 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 | + | publish_optimal_trajectory | bool | Publishes the full optimal trajectory sequence each control iteration for downstream control systems, collision checkers, etc to have context beyond the next timestep. | diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp index bd8b972473d..ed9c434e92b 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp @@ -46,6 +46,44 @@ struct SamplingStd float wz; }; +struct AdvancedConstraints +{ + // TODO(vahapt): also add documentation to https://github.com/ros-navigation/docs.nav2.org/blob/master/configuration/packages/configuring-mppic.rst + /** + * @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 + * 

f(x) = (wz_std - wz_std_decay_to) * e^(-wz_std_decay_strength * v_linear) + wz_std_decay_to
+ * Playground + * 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; + + bool validateWzStdDecayTo(const float & initial_wz_std) + { + // Assume valid if angular decay is disabled + if (wz_std_decay_strength <= 0.0f) { + return true; + } + + if (wz_std_decay_to < 0.0f || wz_std_decay_to > initial_wz_std) { + return false; + } + return true; + } +}; + } // namespace mppi::models #endif // NAV2_MPPI_CONTROLLER__MODELS__CONSTRAINTS_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/optimizer_settings.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/optimizer_settings.hpp index 9416b53e1ba..dac1abd9166 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/optimizer_settings.hpp +++ b/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}; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp index a8c81c3b07d..3c2a4bce57c 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp @@ -41,10 +41,16 @@ struct State geometry_msgs::msg::PoseStamped pose; geometry_msgs::msg::Twist speed; + /** + * @brief Internal variable that holds wz_std after decay is applied. + * If decay is disabled, SamplingStd.wz == wz_std_adaptive + */ + float wz_std_adaptive; + /** * @brief Reset state data */ - void reset(unsigned int batch_size, unsigned int time_steps) + void reset(unsigned int batch_size, unsigned int time_steps, float wz_std) { vx.setZero(batch_size, time_steps); vy.setZero(batch_size, time_steps); @@ -53,6 +59,8 @@ struct State cvx.setZero(batch_size, time_steps); cvy.setZero(batch_size, time_steps); cwz.setZero(batch_size, time_steps); + + wz_std_adaptive = wz_std; // reset initial adaptive value to parameterized value } }; } // namespace mppi::models diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index 2ced1fe2fc4..fb878673227 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -217,6 +217,13 @@ class Optimizer Eigen::Array & trajectories, const Eigen::ArrayXXf & state) const; + /** + * Calculates wz_std parameter based on linear speed + * See also wz_std_decay_strength, wz_std_decay_to parameters for more information + * @return new wz_std to use + */ + float calculateDecayForAngularDeviation(); + /** * @brief Update control sequence with state controls weighted by costs * using softmax function diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp index 5823d648782..8e59ea2f005 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp @@ -64,7 +64,7 @@ class NoiseGenerator * @brief Signal to the noise thread the controller is ready to generate a new * noised control for the next iteration */ - void generateNextNoises(); + void generateNextNoises(const models::State & state); /** * @brief set noised control_sequence to state controls diff --git a/nav2_mppi_controller/src/noise_generator.cpp b/nav2_mppi_controller/src/noise_generator.cpp index a704ef8b5cf..30255e82d57 100644 --- a/nav2_mppi_controller/src/noise_generator.cpp +++ b/nav2_mppi_controller/src/noise_generator.cpp @@ -28,10 +28,6 @@ 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); - auto getParam = param_handler->getParamGetter(name); getParam(regenerate_noises_, "regenerate_noises", false); @@ -52,12 +48,21 @@ void NoiseGenerator::shutdown() } } -void NoiseGenerator::generateNextNoises() +void NoiseGenerator::generateNextNoises(const models::State & state) { // Trigger the thread to run in parallel to this iteration // to generate the next iteration's noises (if applicable). { std::unique_lock guard(noise_lock_); + + // Check if there's any change on adaptive std's and re-create relevant distribution if any + // vahapt: Instead of equality check we may compare it to an epsilon value + // on another note, construction of ndistribution on every step should be pretty fast, + // it's just a 16 byte object + if (ndistribution_wz_.stddev() != state.wz_std_adaptive) { + ndistribution_wz_ = std::normal_distribution(0.0f, state.wz_std_adaptive); + } + ready_ = true; } noise_cond_.notify_all(); @@ -82,9 +87,17 @@ void NoiseGenerator::reset(mppi::models::OptimizerSettings & settings, bool is_h // Recompute the noises on reset, initialization, and fallback { std::unique_lock guard(noise_lock_); + // vahapt: Moved construction of normal distributions from initialize() to here + // reset() is called during initialization and on parameter change + // See issue #5274 for more info + 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); + 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); + ready_ = true; } diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 6e63361ede1..f83486683b7 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -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); @@ -134,7 +136,7 @@ void Optimizer::setOffset(double controller_frequency) void Optimizer::reset(bool reset_dynamic_speed_limits) { - state_.reset(settings_.batch_size, settings_.time_steps); + state_.reset(settings_.batch_size, settings_.time_steps, settings_.sampling_std.wz); 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 +153,14 @@ void Optimizer::reset(bool reset_dynamic_speed_limits) 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 + if (settings_.advanced_constraints.validateWzStdDecayTo(settings_.sampling_std.wz) == false) { + RCLCPP_WARN_STREAM(logger_, + "SKIPPING decay function. advanced.wz_std_decay_to must be between 0 and wz_std." << + " Applying: wz_std = " << settings_.sampling_std.wz << + " Ignoring: wz_std_decay_to = " << settings_.advanced_constraints.wz_std_decay_to); + } + RCLCPP_INFO(logger_, "Optimizer reset"); } @@ -228,6 +238,8 @@ void Optimizer::prepare( critics_data_.motion_model = motion_model_; critics_data_.furthest_reached_path_point.reset(); critics_data_.path_pts_valid.reset(); + + state_.wz_std_adaptive = calculateDecayForAngularDeviation(); } void Optimizer::shiftControlSequence() @@ -247,7 +259,7 @@ void Optimizer::shiftControlSequence() void Optimizer::generateNoisedTrajectories() { noise_generator_.setNoisedControls(state_, control_sequence_); - noise_generator_.generateNextNoises(); + noise_generator_.generateNextNoises(state_); updateStateVelocities(state_); integrateStateVelocities(generated_trajectories_, state_); } @@ -439,6 +451,36 @@ const models::ControlSequence & Optimizer::getOptimalControlSequence() return control_sequence_; } +float Optimizer::calculateDecayForAngularDeviation() +{ + auto & s = settings_; + // Should we apply decay function? + if (s.advanced_constraints.wz_std_decay_strength <= 0.0f) { + return s.sampling_std.wz; // skip calculation + } + + // boundary check, if wz_std_decay_to is out of bounds, print a warning + if (s.advanced_constraints.validateWzStdDecayTo(s.sampling_std.wz) == false) { + return s.sampling_std.wz; // skip calculation + } + + float current_speed; + if (isHolonomic()) { + const auto vx = static_cast(state_.speed.linear.x); + const auto vy = static_cast(state_.speed.linear.y); + current_speed = hypotf(vx, vy); + } else { + current_speed = fabs(static_cast(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; + + return decayed_wz_std; +} + void Optimizer::updateControlSequence() { const bool is_holo = isHolonomic(); @@ -449,10 +491,10 @@ 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) { + if (state_.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 / (state_.wz_std_adaptive * state_.wz_std_adaptive); costs_ += (gamma_wz * (bounded_noises_wz.rowwise() * wz_T).rowwise().sum()).eval(); } diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index 9a7b5b8225d..eb0a4f63ed5 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/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, 0.0f); models::ControlSequence control_sequence; models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); @@ -450,7 +450,7 @@ TEST(CriticTests, PreferForwardCritic) costmap_ros->on_configure(lstate); models::State state; - state.reset(1000, 30); + state.reset(1000, 30, 0.0f); models::ControlSequence control_sequence; models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); @@ -507,7 +507,7 @@ TEST(CriticTests, TwirlingCritic) costmap_ros->on_configure(lstate); models::State state; - state.reset(1000, 30); + state.reset(1000, 30, 0.0f); models::ControlSequence control_sequence; models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); @@ -572,7 +572,7 @@ TEST(CriticTests, PathFollowCritic) costmap_ros->on_configure(lstate); models::State state; - state.reset(1000, 30); + state.reset(1000, 30, 0.0f); models::ControlSequence control_sequence; models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); @@ -624,7 +624,7 @@ TEST(CriticTests, PathAlignCritic) costmap_ros->on_configure(lstate); models::State state; - state.reset(1000, 30); + state.reset(1000, 30, 0.0f); models::ControlSequence control_sequence; models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); @@ -738,7 +738,7 @@ TEST(CriticTests, VelocityDeadbandCritic) costmap_ros->on_configure(lstate); models::State state; - state.reset(1000, 30); + state.reset(1000, 30, 0.0f); models::ControlSequence control_sequence; models::Trajectories generated_trajectories; models::Path path; diff --git a/nav2_mppi_controller/test/models_test.cpp b/nav2_mppi_controller/test/models_test.cpp index 90de9abb0fc..2c66c7f6b57 100644 --- a/nav2_mppi_controller/test/models_test.cpp +++ b/nav2_mppi_controller/test/models_test.cpp @@ -90,7 +90,7 @@ TEST(ModelsTest, StateTest) EXPECT_EQ(state.vy(4), 1); EXPECT_EQ(state.wz(4), 1); - state.reset(20, 40); + state.reset(20, 40, 0.5f); // Show contents are gone and new size EXPECT_EQ(state.cvx(4), 0); @@ -111,6 +111,7 @@ TEST(ModelsTest, StateTest) EXPECT_EQ(state.vx.cols(), 40); EXPECT_EQ(state.vy.cols(), 40); EXPECT_EQ(state.wz.cols(), 40); + EXPECT_NEAR(state.wz_std_adaptive, 0.5f, 1e-6f); } TEST(ModelsTest, TrajectoriesTest) diff --git a/nav2_mppi_controller/test/motion_model_tests.cpp b/nav2_mppi_controller/test/motion_model_tests.cpp index c473e97dbc9..263c22671ff 100644 --- a/nav2_mppi_controller/test/motion_model_tests.cpp +++ b/nav2_mppi_controller/test/motion_model_tests.cpp @@ -33,7 +33,7 @@ TEST(MotionModelTests, DiffDriveTest) int batches = 1000; int timesteps = 50; control_sequence.reset(timesteps); // populates with zeros - state.reset(batches, timesteps); // populates with zeros + state.reset(batches, timesteps, 0.0f); // populates with zeros std::unique_ptr model = std::make_unique(); @@ -78,7 +78,7 @@ TEST(MotionModelTests, OmniTest) int batches = 1000; int timesteps = 50; control_sequence.reset(timesteps); // populates with zeros - state.reset(batches, timesteps); // populates with zeros + state.reset(batches, timesteps, 0.0f); // populates with zeros std::unique_ptr model = std::make_unique(); @@ -125,7 +125,7 @@ TEST(MotionModelTests, AckermannTest) int batches = 1000; int timesteps = 50; control_sequence.reset(timesteps); // populates with zeros - state.reset(batches, timesteps); // populates with zeros + state.reset(batches, timesteps, 0.0f); // populates with zeros auto node = std::make_shared("my_node"); std::string name = "test"; ParametersHandler param_handler(node, name); @@ -184,7 +184,7 @@ TEST(MotionModelTests, AckermannReversingTest) int timesteps = 50; control_sequence.reset(timesteps); // populates with zeros control_sequence2.reset(timesteps); // populates with zeros - state.reset(batches, timesteps); // populates with zeros + state.reset(batches, timesteps, 0.0f); // populates with zeros auto node = std::make_shared("my_node"); std::string name = "test"; ParametersHandler param_handler(node, name); diff --git a/nav2_mppi_controller/test/noise_generator_test.cpp b/nav2_mppi_controller/test/noise_generator_test.cpp index 6e86f60363b..56c23dab0cc 100644 --- a/nav2_mppi_controller/test/noise_generator_test.cpp +++ b/nav2_mppi_controller/test/noise_generator_test.cpp @@ -71,7 +71,7 @@ ParametersHandler handler(node, name); } mppi::models::State state; - state.reset(settings.batch_size, settings.time_steps); + state.reset(settings.batch_size, settings.time_steps, settings.sampling_std.wz); // Request an update with no noise yet generated, should result in identical outputs generator.initialize(settings, false, "test_name", &handler); @@ -100,7 +100,7 @@ ParametersHandler handler(node, name); EXPECT_NEAR(state.cwz(0, 9), 9, 0.3); // Request an update with noise requested - generator.generateNextNoises(); + generator.generateNextNoises(state); std::this_thread::sleep_for(std::chrono::milliseconds(100)); generator.setNoisedControls(state, control_sequence); @@ -115,7 +115,7 @@ ParametersHandler handler(node, name); // Test holonomic setting generator.reset(settings, true); // Now holonomically - generator.generateNextNoises(); + generator.generateNextNoises(state); std::this_thread::sleep_for(std::chrono::milliseconds(100)); generator.setNoisedControls(state, control_sequence); EXPECT_NE(state.cvx(0), 0); @@ -160,7 +160,7 @@ ParametersHandler handler(node, name); } mppi::models::State state; - state.reset(settings.batch_size, settings.time_steps); + state.reset(settings.batch_size, settings.time_steps, settings.sampling_std.wz); // Request an update with no noise yet generated, should result in identical outputs generator.initialize(settings, false, "test_name", &handler); @@ -189,7 +189,7 @@ ParametersHandler handler(node, name); EXPECT_NEAR(state.cwz(0, 9), 9, 0.3); // this doesn't work if regenerate_noises is false - generator.generateNextNoises(); + generator.generateNextNoises(state); std::this_thread::sleep_for(std::chrono::milliseconds(100)); generator.setNoisedControls(state, control_sequence); diff --git a/nav2_mppi_controller/test/optimizer_unit_tests.cpp b/nav2_mppi_controller/test/optimizer_unit_tests.cpp index 1c3732399e6..68887d680d9 100644 --- a/nav2_mppi_controller/test/optimizer_unit_tests.cpp +++ b/nav2_mppi_controller/test/optimizer_unit_tests.cpp @@ -161,7 +161,7 @@ class OptimizerTester : public Optimizer { // updateInitialStateVelocities models::State state; - state.reset(1000, 50); + state.reset(1000, 50, settings_.sampling_std.wz); state.speed.linear.x = 5.0; state.speed.linear.y = 1.0; state.speed.angular.z = 6.0; @@ -188,7 +188,7 @@ class OptimizerTester : public Optimizer EXPECT_NEAR(state.wz(0, 1), std::clamp(0.1, 6.0 - max_delta_wz, 6.0 + max_delta_wz), 1e-6); // Putting them together: updateStateVelocities - state.reset(1000, 50); + state.reset(1000, 50, settings_.sampling_std.wz); state.speed.linear.x = -5.0; state.speed.linear.y = -1.0; state.speed.angular.z = -6.0; @@ -202,6 +202,13 @@ class OptimizerTester : public Optimizer EXPECT_NEAR(state.vx(0, 1), std::clamp(-0.75, -5.0 + min_delta_vx, -5.0 + max_delta_vx), 1e-6); EXPECT_NEAR(state.vy(0, 1), std::clamp(-0.5, -1.0 - max_delta_vy, -1.0 + max_delta_vy), 1e-6); EXPECT_NEAR(state.wz(0, 1), std::clamp(-0.1, -6.0 - max_delta_wz, -6.0 + max_delta_wz), 1e-6); + + // calculateDecayForAngularDeviation + state.reset(1000, 50, settings_.sampling_std.wz); + float result = calculateDecayForAngularDeviation(); + EXPECT_NEAR(result, settings_.sampling_std.wz, 1e-6); + + // TODO(vahapt): fill in this section } geometry_msgs::msg::TwistStamped getControlFromSequenceAsTwistWrapper() @@ -620,7 +627,7 @@ TEST(OptimizerTests, integrateStateVelocitiesTests) // Give it a couple of easy const traj and check rollout, start from 0 models::State state; - state.reset(1000, 50); + state.reset(1000, 50, optimizer_tester.getSettings().sampling_std.wz); models::Trajectories traj; traj.reset(1000, 50); state.vx = 0.1 * Eigen::ArrayXXf::Ones(1000, 50); From 1332a38202a8cfff64e53b8f36a3d22d0cb378a4 Mon Sep 17 00:00:00 2001 From: "V. Oguz TOKMAK" <5289529+vahapt@users.noreply.github.com> Date: Sun, 22 Jun 2025 19:59:59 +0300 Subject: [PATCH 2/6] Added support for MPPI Controller to adjust wz_std parameter based on linear speed Signed-off-by: Vahap Oguz TOKMAK <5289529+vahapt@users.noreply.github.com> --- nav2_mppi_controller/README.md | 44 ++++++------- .../models/constraints.hpp | 56 ++++++++++++----- .../models/optimizer_settings.hpp | 3 +- .../nav2_mppi_controller/models/state.hpp | 10 +-- .../nav2_mppi_controller/optimizer.hpp | 12 ++-- .../tools/noise_generator.hpp | 14 ++++- nav2_mppi_controller/src/noise_generator.cpp | 62 ++++++++++++++----- nav2_mppi_controller/src/optimizer.cpp | 56 +++++------------ nav2_mppi_controller/test/critics_tests.cpp | 12 ++-- nav2_mppi_controller/test/models_test.cpp | 3 +- .../test/motion_model_tests.cpp | 8 +-- .../test/noise_generator_test.cpp | 62 +++++++++++++++++-- .../test/optimizer_unit_tests.cpp | 13 +--- 13 files changed, 215 insertions(+), 140 deletions(-) diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 448dbb89282..1932a62ff19 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -38,28 +38,28 @@ This process is then repeated a number of times and returns a converged solution ## Configuration ### Controller - | Parameter | Type | Definition | - |----------------------------------|--------|----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| - | motion_model | string | Default: DiffDrive. Type of model [DiffDrive, Omni, Ackermann]. | - | critics | string | Default: None. Critics (plugins) names | - | iteration_count | int | Default 1. Iteration count in MPPI algorithm. Recommend to keep as 1 and prefer more batches. | - | batch_size | int | Default 1000. Count of randomly sampled candidate trajectories | - | time_steps | int | Default 56. Number of time steps (points) in each sampled trajectory | - | model_dt | double | Default: 0.05. Time interval (s) between two sampled points in trajectories. | - | vx_std | double | Default 0.2. Sampling standard deviation for VX | - | vy_std | double | Default 0.2. Sampling standard deviation for VY | - | wz_std | double | Default 0.4. Sampling standard deviation for Wz | - | vx_max | double | Default 0.5. Max VX (m/s) | - | vy_max | double | Default 0.5. Max VY in either direction, if holonomic. (m/s) | - | vx_min | double | Default -0.35. Min VX (m/s) | - | wz_max | double | Default 1.9. Max WZ (rad/s) | - | temperature | double | Default: 0.3. Selectiveness of trajectories by their costs (The closer this value to 0, the "more" we take in consideration controls with less cost), 0 mean use control with best cost, huge value will lead to just taking mean of all trajectories without cost consideration | - | gamma | double | Default: 0.015. A trade-off between smoothness (high) and low energy (low). This is a complex parameter that likely won't need to be changed from the default of `0.1` which works well for a broad range of cases. See Section 3D-2 in "Information Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving" for detailed information. | - | visualize | bool | Default: false. Publish visualization of trajectories, which can slow down the controller significantly. Use only for debugging. | - | retry_attempt_limit | int | Default 1. Number of attempts to find feasible trajectory on failure for soft-resets before reporting failure. | - | regenerate_noises | bool | Default false. Whether to regenerate noises each iteration or use single noise distribution computed on initialization and reset. Practically, this is found to work fine since the trajectories are being sampled stochastically from a normal distribution and reduces compute jittering at run-time due to thread wake-ups to resample normal distribution. | - | advanced.wz_std_decay_strength | double | Default: -1.0 (disabled). 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
`f(x) = (wz_std - wz_std_decay_to) * e^(-wz_std_decay_strength * v_linear) + wz_std_decay_to`
[Playground](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) | - | advanced.wz_std_decay_to | double | Default: 0.0. 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 | + | Parameter | Type | Definition | + |----------------------------------|--------|---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| + | motion_model | string | Default: DiffDrive. Type of model [DiffDrive, Omni, Ackermann]. | + | critics | string | Default: None. Critics (plugins) names | + | iteration_count | int | Default 1. Iteration count in MPPI algorithm. Recommend to keep as 1 and prefer more batches. | + | batch_size | int | Default 1000. Count of randomly sampled candidate trajectories | + | time_steps | int | Default 56. Number of time steps (points) in each sampled trajectory | + | model_dt | double | Default: 0.05. Time interval (s) between two sampled points in trajectories. | + | vx_std | double | Default 0.2. Sampling standard deviation for VX | + | vy_std | double | Default 0.2. Sampling standard deviation for VY | + | wz_std | double | Default 0.4. Sampling standard deviation for Wz | + | vx_max | double | Default 0.5. Max VX (m/s) | + | vy_max | double | Default 0.5. Max VY in either direction, if holonomic. (m/s) | + | vx_min | double | Default -0.35. Min VX (m/s) | + | wz_max | double | Default 1.9. Max WZ (rad/s) | + | temperature | double | Default: 0.3. Selectiveness of trajectories by their costs (The closer this value to 0, the "more" we take in consideration controls with less cost), 0 mean use control with best cost, huge value will lead to just taking mean of all trajectories without cost consideration | + | gamma | double | Default: 0.015. A trade-off between smoothness (high) and low energy (low). This is a complex parameter that likely won't need to be changed from the default of `0.1` which works well for a broad range of cases. See Section 3D-2 in "Information Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving" for detailed information. | + | visualize | bool | Default: false. Publish visualization of trajectories, which can slow down the controller significantly. Use only for debugging. | + | retry_attempt_limit | int | Default 1. Number of attempts to find feasible trajectory on failure for soft-resets before reporting failure. | + | regenerate_noises | bool | Default false. Whether to regenerate noises each iteration or use single noise distribution computed on initialization and reset. Practically, this is found to work fine since the trajectories are being sampled stochastically from a normal distribution and reduces compute jittering at run-time due to thread wake-ups to resample normal distribution. | + | advanced.wz_std_decay_strength | double | Default: -1.0 (disabled). Defines the strength of the reduction function.
`wz_std_decay_strength` and `wz_std_decay_to` defines a function that enables 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 are: `wz_std = 0.4, wz_std_decay_to = 0.05, wz_std_decay_strength = 3.0`

The following is used as the decay function
`f(x) = (wz_std - wz_std_decay_to) * e^(-wz_std_decay_strength * v_linear) + wz_std_decay_to`
[Visualize the decay function here](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) | + | advanced.wz_std_decay_to | double | Default: 0.0. 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 | | publish_optimal_trajectory | bool | Publishes the full optimal trajectory sequence each control iteration for downstream control systems, collision checkers, etc to have context beyond the next timestep. | diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp index ed9c434e92b..9296a8b38e7 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp +++ b/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 +#include +#include + namespace mppi::models { @@ -35,20 +39,8 @@ struct ControlConstraints float az_max; }; -/** - * @struct mppi::models::SamplingStd - * @brief Noise parameters for sampling trajectories - */ -struct SamplingStd -{ - float vx; - float vy; - float wz; -}; - struct AdvancedConstraints { - // TODO(vahapt): also add documentation to https://github.com/ros-navigation/docs.nav2.org/blob/master/configuration/packages/configuring-mppic.rst /** * @brief Defines the strength of the reduction function * Allows dynamic modification of wz_std (angular deviation) based on linear velocity of the robot. @@ -69,16 +61,46 @@ struct AdvancedConstraints * Default: 0.0 */ float wz_std_decay_to; +}; - bool validateWzStdDecayTo(const float & initial_wz_std) +/** + * @struct mppi::models::SamplingStd + * @brief Noise parameters for sampling trajectories + */ +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 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 (wz_std_decay_strength <= 0.0f) { - return true; + if (c.wz_std_decay_strength <= 0.0f) { + return true; // valid } - if (wz_std_decay_to < 0.0f || wz_std_decay_to > initial_wz_std) { - return false; + 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; } diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/optimizer_settings.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/optimizer_settings.hpp index dac1abd9166..c62c798f22d 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/optimizer_settings.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/optimizer_settings.hpp @@ -16,6 +16,7 @@ #define NAV2_MPPI_CONTROLLER__MODELS__OPTIMIZER_SETTINGS_HPP_ #include +#include #include "nav2_mppi_controller/models/constraints.hpp" namespace mppi::models @@ -29,7 +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::SamplingStd sampling_std{0.0f, 0.0f, 0.0f, std::make_shared(0.0f)}; models::AdvancedConstraints advanced_constraints{0.0f, 0.0f}; float model_dt{0.0f}; float temperature{0.0f}; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp index 3c2a4bce57c..a8c81c3b07d 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp @@ -41,16 +41,10 @@ struct State geometry_msgs::msg::PoseStamped pose; geometry_msgs::msg::Twist speed; - /** - * @brief Internal variable that holds wz_std after decay is applied. - * If decay is disabled, SamplingStd.wz == wz_std_adaptive - */ - float wz_std_adaptive; - /** * @brief Reset state data */ - void reset(unsigned int batch_size, unsigned int time_steps, float wz_std) + void reset(unsigned int batch_size, unsigned int time_steps) { vx.setZero(batch_size, time_steps); vy.setZero(batch_size, time_steps); @@ -59,8 +53,6 @@ struct State cvx.setZero(batch_size, time_steps); cvy.setZero(batch_size, time_steps); cwz.setZero(batch_size, time_steps); - - wz_std_adaptive = wz_std; // reset initial adaptive value to parameterized value } }; } // namespace mppi::models diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index fb878673227..fb15826e01f 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/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 @@ -217,13 +222,6 @@ class Optimizer Eigen::Array & trajectories, const Eigen::ArrayXXf & state) const; - /** - * Calculates wz_std parameter based on linear speed - * See also wz_std_decay_strength, wz_std_decay_to parameters for more information - * @return new wz_std to use - */ - float calculateDecayForAngularDeviation(); - /** * @brief Update control sequence with state controls weighted by costs * using softmax function diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp index 8e59ea2f005..d707f78cd86 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp +++ b/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 */ @@ -64,7 +69,14 @@ class NoiseGenerator * @brief Signal to the noise thread the controller is ready to generate a new * noised control for the next iteration */ - void generateNextNoises(const models::State & state); + 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 diff --git a/nav2_mppi_controller/src/noise_generator.cpp b/nav2_mppi_controller/src/noise_generator.cpp index 30255e82d57..7a21f7830b2 100644 --- a/nav2_mppi_controller/src/noise_generator.cpp +++ b/nav2_mppi_controller/src/noise_generator.cpp @@ -28,6 +28,7 @@ void NoiseGenerator::initialize( is_holonomic_ = is_holonomic; active_ = true; + initializeNormalDistributions(); auto getParam = param_handler->getParamGetter(name); getParam(regenerate_noises_, "regenerate_noises", false); @@ -38,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; @@ -48,24 +56,49 @@ void NoiseGenerator::shutdown() } } -void NoiseGenerator::generateNextNoises(const models::State & state) +void NoiseGenerator::generateNextNoises() { // Trigger the thread to run in parallel to this iteration // to generate the next iteration's noises (if applicable). { std::unique_lock guard(noise_lock_); + ready_ = true; + } + noise_cond_.notify_all(); +} + +void NoiseGenerator::computeAdaptiveStds(const models::State & state) +{ + auto & s = settings_; + float & wz_std_adaptive = *settings_.sampling_std.wz_std_adaptive; - // Check if there's any change on adaptive std's and re-create relevant distribution if any - // vahapt: Instead of equality check we may compare it to an epsilon value - // on another note, construction of ndistribution on every step should be pretty fast, - // it's just a 16 byte object - if (ndistribution_wz_.stddev() != state.wz_std_adaptive) { - ndistribution_wz_ = std::normal_distribution(0.0f, state.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(state.speed.linear.x); + const auto vy = static_cast(state.speed.linear.y); + current_speed = hypotf(vx, vy); + } else { + current_speed = fabs(static_cast(state.speed.linear.x)); } - ready_ = true; + 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); } - noise_cond_.notify_all(); } void NoiseGenerator::setNoisedControls( @@ -74,6 +107,8 @@ void NoiseGenerator::setNoisedControls( { std::unique_lock 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(); @@ -87,17 +122,12 @@ void NoiseGenerator::reset(mppi::models::OptimizerSettings & settings, bool is_h // Recompute the noises on reset, initialization, and fallback { std::unique_lock guard(noise_lock_); - // vahapt: Moved construction of normal distributions from initialize() to here - // reset() is called during initialization and on parameter change - // See issue #5274 for more info - 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(); 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); - ready_ = true; } diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index f83486683b7..1f1c21b1194 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -134,9 +134,16 @@ void Optimizer::setOffset(double controller_frequency) } } +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, settings_.sampling_std.wz); + 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}; @@ -154,11 +161,10 @@ void Optimizer::reset(bool reset_dynamic_speed_limits) motion_model_->initialize(settings_.constraints, settings_.model_dt); // Validate decay function, print warning message if decay_to is out of bounds - if (settings_.advanced_constraints.validateWzStdDecayTo(settings_.sampling_std.wz) == false) { - RCLCPP_WARN_STREAM(logger_, - "SKIPPING decay function. advanced.wz_std_decay_to must be between 0 and wz_std." << - " Applying: wz_std = " << settings_.sampling_std.wz << - " Ignoring: wz_std_decay_to = " << settings_.advanced_constraints.wz_std_decay_to); + 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"); @@ -238,8 +244,6 @@ void Optimizer::prepare( critics_data_.motion_model = motion_model_; critics_data_.furthest_reached_path_point.reset(); critics_data_.path_pts_valid.reset(); - - state_.wz_std_adaptive = calculateDecayForAngularDeviation(); } void Optimizer::shiftControlSequence() @@ -259,7 +263,7 @@ void Optimizer::shiftControlSequence() void Optimizer::generateNoisedTrajectories() { noise_generator_.setNoisedControls(state_, control_sequence_); - noise_generator_.generateNextNoises(state_); + noise_generator_.generateNextNoises(); updateStateVelocities(state_); integrateStateVelocities(generated_trajectories_, state_); } @@ -451,35 +455,6 @@ const models::ControlSequence & Optimizer::getOptimalControlSequence() return control_sequence_; } -float Optimizer::calculateDecayForAngularDeviation() -{ - auto & s = settings_; - // Should we apply decay function? - if (s.advanced_constraints.wz_std_decay_strength <= 0.0f) { - return s.sampling_std.wz; // skip calculation - } - - // boundary check, if wz_std_decay_to is out of bounds, print a warning - if (s.advanced_constraints.validateWzStdDecayTo(s.sampling_std.wz) == false) { - return s.sampling_std.wz; // skip calculation - } - - float current_speed; - if (isHolonomic()) { - const auto vx = static_cast(state_.speed.linear.x); - const auto vy = static_cast(state_.speed.linear.y); - current_speed = hypotf(vx, vy); - } else { - current_speed = fabs(static_cast(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; - - return decayed_wz_std; -} void Optimizer::updateControlSequence() { @@ -491,10 +466,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 (state_.wz_std_adaptive > 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 / (state_.wz_std_adaptive * state_.wz_std_adaptive); + const float gamma_wz = s.gamma / (wz_std_adaptive * wz_std_adaptive); costs_ += (gamma_wz * (bounded_noises_wz.rowwise() * wz_T).rowwise().sum()).eval(); } diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index eb0a4f63ed5..ba803f9095c 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/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, 0.0f); + state.reset(1000, 30); models::ControlSequence control_sequence; models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); @@ -450,7 +450,7 @@ TEST(CriticTests, PreferForwardCritic) costmap_ros->on_configure(lstate); models::State state; - state.reset(1000, 30, 0.0f); + state.reset(1000, 30); models::ControlSequence control_sequence; models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); @@ -507,7 +507,7 @@ TEST(CriticTests, TwirlingCritic) costmap_ros->on_configure(lstate); models::State state; - state.reset(1000, 30, 0.0f); + state.reset(1000, 30); models::ControlSequence control_sequence; models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); @@ -572,7 +572,7 @@ TEST(CriticTests, PathFollowCritic) costmap_ros->on_configure(lstate); models::State state; - state.reset(1000, 30, 0.0f); + state.reset(1000, 30); models::ControlSequence control_sequence; models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); @@ -624,7 +624,7 @@ TEST(CriticTests, PathAlignCritic) costmap_ros->on_configure(lstate); models::State state; - state.reset(1000, 30, 0.0f); + state.reset(1000, 30); models::ControlSequence control_sequence; models::Trajectories generated_trajectories; generated_trajectories.reset(1000, 30); @@ -738,7 +738,7 @@ TEST(CriticTests, VelocityDeadbandCritic) costmap_ros->on_configure(lstate); models::State state; - state.reset(1000, 30, 0.0f); + state.reset(1000, 30); models::ControlSequence control_sequence; models::Trajectories generated_trajectories; models::Path path; diff --git a/nav2_mppi_controller/test/models_test.cpp b/nav2_mppi_controller/test/models_test.cpp index 2c66c7f6b57..90de9abb0fc 100644 --- a/nav2_mppi_controller/test/models_test.cpp +++ b/nav2_mppi_controller/test/models_test.cpp @@ -90,7 +90,7 @@ TEST(ModelsTest, StateTest) EXPECT_EQ(state.vy(4), 1); EXPECT_EQ(state.wz(4), 1); - state.reset(20, 40, 0.5f); + state.reset(20, 40); // Show contents are gone and new size EXPECT_EQ(state.cvx(4), 0); @@ -111,7 +111,6 @@ TEST(ModelsTest, StateTest) EXPECT_EQ(state.vx.cols(), 40); EXPECT_EQ(state.vy.cols(), 40); EXPECT_EQ(state.wz.cols(), 40); - EXPECT_NEAR(state.wz_std_adaptive, 0.5f, 1e-6f); } TEST(ModelsTest, TrajectoriesTest) diff --git a/nav2_mppi_controller/test/motion_model_tests.cpp b/nav2_mppi_controller/test/motion_model_tests.cpp index 263c22671ff..c473e97dbc9 100644 --- a/nav2_mppi_controller/test/motion_model_tests.cpp +++ b/nav2_mppi_controller/test/motion_model_tests.cpp @@ -33,7 +33,7 @@ TEST(MotionModelTests, DiffDriveTest) int batches = 1000; int timesteps = 50; control_sequence.reset(timesteps); // populates with zeros - state.reset(batches, timesteps, 0.0f); // populates with zeros + state.reset(batches, timesteps); // populates with zeros std::unique_ptr model = std::make_unique(); @@ -78,7 +78,7 @@ TEST(MotionModelTests, OmniTest) int batches = 1000; int timesteps = 50; control_sequence.reset(timesteps); // populates with zeros - state.reset(batches, timesteps, 0.0f); // populates with zeros + state.reset(batches, timesteps); // populates with zeros std::unique_ptr model = std::make_unique(); @@ -125,7 +125,7 @@ TEST(MotionModelTests, AckermannTest) int batches = 1000; int timesteps = 50; control_sequence.reset(timesteps); // populates with zeros - state.reset(batches, timesteps, 0.0f); // populates with zeros + state.reset(batches, timesteps); // populates with zeros auto node = std::make_shared("my_node"); std::string name = "test"; ParametersHandler param_handler(node, name); @@ -184,7 +184,7 @@ TEST(MotionModelTests, AckermannReversingTest) int timesteps = 50; control_sequence.reset(timesteps); // populates with zeros control_sequence2.reset(timesteps); // populates with zeros - state.reset(batches, timesteps, 0.0f); // populates with zeros + state.reset(batches, timesteps); // populates with zeros auto node = std::make_shared("my_node"); std::string name = "test"; ParametersHandler param_handler(node, name); diff --git a/nav2_mppi_controller/test/noise_generator_test.cpp b/nav2_mppi_controller/test/noise_generator_test.cpp index 56c23dab0cc..17d4e2f8087 100644 --- a/nav2_mppi_controller/test/noise_generator_test.cpp +++ b/nav2_mppi_controller/test/noise_generator_test.cpp @@ -28,6 +28,8 @@ using namespace mppi; // NOLINT +static constexpr double EPSILON = std::numeric_limits::epsilon(); + TEST(NoiseGeneratorTest, NoiseGeneratorLifecycle) { // Tests shuts down internal thread cleanly @@ -71,7 +73,7 @@ ParametersHandler handler(node, name); } mppi::models::State state; - state.reset(settings.batch_size, settings.time_steps, settings.sampling_std.wz); + state.reset(settings.batch_size, settings.time_steps); // Request an update with no noise yet generated, should result in identical outputs generator.initialize(settings, false, "test_name", &handler); @@ -100,7 +102,7 @@ ParametersHandler handler(node, name); EXPECT_NEAR(state.cwz(0, 9), 9, 0.3); // Request an update with noise requested - generator.generateNextNoises(state); + generator.generateNextNoises(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); generator.setNoisedControls(state, control_sequence); @@ -115,7 +117,7 @@ ParametersHandler handler(node, name); // Test holonomic setting generator.reset(settings, true); // Now holonomically - generator.generateNextNoises(state); + generator.generateNextNoises(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); generator.setNoisedControls(state, control_sequence); EXPECT_NE(state.cvx(0), 0); @@ -160,7 +162,7 @@ ParametersHandler handler(node, name); } mppi::models::State state; - state.reset(settings.batch_size, settings.time_steps, settings.sampling_std.wz); + state.reset(settings.batch_size, settings.time_steps); // Request an update with no noise yet generated, should result in identical outputs generator.initialize(settings, false, "test_name", &handler); @@ -189,7 +191,7 @@ ParametersHandler handler(node, name); EXPECT_NEAR(state.cwz(0, 9), 9, 0.3); // this doesn't work if regenerate_noises is false - generator.generateNextNoises(state); + generator.generateNextNoises(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); generator.setNoisedControls(state, control_sequence); @@ -204,6 +206,56 @@ ParametersHandler handler(node, name); generator.shutdown(); } +TEST(NoiseGeneratorTest, AdaptiveStds) +{ + // Tests shuts down internal thread cleanly + auto node = std::make_shared("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 +} + int main(int argc, char **argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_mppi_controller/test/optimizer_unit_tests.cpp b/nav2_mppi_controller/test/optimizer_unit_tests.cpp index 68887d680d9..1c3732399e6 100644 --- a/nav2_mppi_controller/test/optimizer_unit_tests.cpp +++ b/nav2_mppi_controller/test/optimizer_unit_tests.cpp @@ -161,7 +161,7 @@ class OptimizerTester : public Optimizer { // updateInitialStateVelocities models::State state; - state.reset(1000, 50, settings_.sampling_std.wz); + state.reset(1000, 50); state.speed.linear.x = 5.0; state.speed.linear.y = 1.0; state.speed.angular.z = 6.0; @@ -188,7 +188,7 @@ class OptimizerTester : public Optimizer EXPECT_NEAR(state.wz(0, 1), std::clamp(0.1, 6.0 - max_delta_wz, 6.0 + max_delta_wz), 1e-6); // Putting them together: updateStateVelocities - state.reset(1000, 50, settings_.sampling_std.wz); + state.reset(1000, 50); state.speed.linear.x = -5.0; state.speed.linear.y = -1.0; state.speed.angular.z = -6.0; @@ -202,13 +202,6 @@ class OptimizerTester : public Optimizer EXPECT_NEAR(state.vx(0, 1), std::clamp(-0.75, -5.0 + min_delta_vx, -5.0 + max_delta_vx), 1e-6); EXPECT_NEAR(state.vy(0, 1), std::clamp(-0.5, -1.0 - max_delta_vy, -1.0 + max_delta_vy), 1e-6); EXPECT_NEAR(state.wz(0, 1), std::clamp(-0.1, -6.0 - max_delta_wz, -6.0 + max_delta_wz), 1e-6); - - // calculateDecayForAngularDeviation - state.reset(1000, 50, settings_.sampling_std.wz); - float result = calculateDecayForAngularDeviation(); - EXPECT_NEAR(result, settings_.sampling_std.wz, 1e-6); - - // TODO(vahapt): fill in this section } geometry_msgs::msg::TwistStamped getControlFromSequenceAsTwistWrapper() @@ -627,7 +620,7 @@ TEST(OptimizerTests, integrateStateVelocitiesTests) // Give it a couple of easy const traj and check rollout, start from 0 models::State state; - state.reset(1000, 50, optimizer_tester.getSettings().sampling_std.wz); + state.reset(1000, 50); models::Trajectories traj; traj.reset(1000, 50); state.vx = 0.1 * Eigen::ArrayXXf::Ones(1000, 50); From 3d042fee89d2d4cc15e7860a5b47c7609ea9c13e Mon Sep 17 00:00:00 2001 From: Vahap Oguz TOKMAK <5289529+vahapt@users.noreply.github.com> Date: Sun, 22 Jun 2025 20:39:39 +0300 Subject: [PATCH 3/6] Added support for MPPI Controller to adjust wz_std parameter based on linear speed Signed-off-by: Vahap Oguz TOKMAK <5289529+vahapt@users.noreply.github.com> --- nav2_mppi_controller/README.md | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 1932a62ff19..4fd16fb2e58 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -57,12 +57,10 @@ This process is then repeated a number of times and returns a converged solution | gamma | double | Default: 0.015. A trade-off between smoothness (high) and low energy (low). This is a complex parameter that likely won't need to be changed from the default of `0.1` which works well for a broad range of cases. See Section 3D-2 in "Information Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving" for detailed information. | | visualize | bool | Default: false. Publish visualization of trajectories, which can slow down the controller significantly. Use only for debugging. | | retry_attempt_limit | int | Default 1. Number of attempts to find feasible trajectory on failure for soft-resets before reporting failure. | - | regenerate_noises | bool | Default false. Whether to regenerate noises each iteration or use single noise distribution computed on initialization and reset. Practically, this is found to work fine since the trajectories are being sampled stochastically from a normal distribution and reduces compute jittering at run-time due to thread wake-ups to resample normal distribution. | + | regenerate_noises | bool | Default false. Whether to regenerate noises each iteration or use single noise distribution computed on initialization and reset. Practically, this is found to work fine since the trajectories are being sampled stochastically from a normal distribution and reduces compute jittering at run-time due to thread wake-ups to resample normal distribution. | + | publish_optimal_trajectory | bool | Publishes the full optimal trajectory sequence each control iteration for downstream control systems, collision checkers, etc to have context beyond the next timestep. | | advanced.wz_std_decay_strength | double | Default: -1.0 (disabled). Defines the strength of the reduction function.
`wz_std_decay_strength` and `wz_std_decay_to` defines a function that enables 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 are: `wz_std = 0.4, wz_std_decay_to = 0.05, wz_std_decay_strength = 3.0`

The following is used as the decay function
`f(x) = (wz_std - wz_std_decay_to) * e^(-wz_std_decay_strength * v_linear) + wz_std_decay_to`
[Visualize the decay function here](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) | | advanced.wz_std_decay_to | double | Default: 0.0. 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 | - - | publish_optimal_trajectory | bool | Publishes the full optimal trajectory sequence each control iteration for downstream control systems, collision checkers, etc to have context beyond the next timestep. | - #### Trajectory Visualizer | Parameter | Type | Definition | From aaeb6274d9939a996fed585920564a50cea7673f Mon Sep 17 00:00:00 2001 From: Vahap Oguz TOKMAK <5289529+vahapt@users.noreply.github.com> Date: Sun, 22 Jun 2025 21:33:21 +0300 Subject: [PATCH 4/6] Added support for MPPI Controller to adjust wz_std parameter based on linear speed Added test cases Signed-off-by: Vahap Oguz TOKMAK <5289529+vahapt@users.noreply.github.com> --- .../test/noise_generator_test.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/nav2_mppi_controller/test/noise_generator_test.cpp b/nav2_mppi_controller/test/noise_generator_test.cpp index 17d4e2f8087..ce95d661d74 100644 --- a/nav2_mppi_controller/test/noise_generator_test.cpp +++ b/nav2_mppi_controller/test/noise_generator_test.cpp @@ -254,6 +254,23 @@ TEST(NoiseGeneratorTest, AdaptiveStds) 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) From b9691c5e8904a0a02429ba75e8e960d3f348dfff Mon Sep 17 00:00:00 2001 From: Vahap Oguz TOKMAK <5289529+vahapt@users.noreply.github.com> Date: Mon, 30 Jun 2025 10:47:49 +0300 Subject: [PATCH 5/6] Added support for MPPI Controller to adjust wz_std parameter based on linear speed - Refactored the code to simplify flow - Merged to latest `main` branch Signed-off-by: Vahap Oguz TOKMAK <5289529+vahapt@users.noreply.github.com> --- .../models/constraints.hpp | 36 ----- .../models/optimizer_settings.hpp | 3 +- .../nav2_mppi_controller/optimizer.hpp | 5 - .../tools/noise_generator.hpp | 46 +++--- nav2_mppi_controller/src/noise_generator.cpp | 132 +++++++++++------- nav2_mppi_controller/src/optimizer.cpp | 19 +-- .../test/noise_generator_test.cpp | 34 ++--- 7 files changed, 123 insertions(+), 152 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp index 9296a8b38e7..5626b461776 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp @@ -15,10 +15,6 @@ #ifndef NAV2_MPPI_CONTROLLER__MODELS__CONSTRAINTS_HPP_ #define NAV2_MPPI_CONTROLLER__MODELS__CONSTRAINTS_HPP_ -#include -#include -#include - namespace mppi::models { @@ -72,38 +68,6 @@ 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 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 diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/optimizer_settings.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/optimizer_settings.hpp index c62c798f22d..dac1abd9166 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/models/optimizer_settings.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/optimizer_settings.hpp @@ -16,7 +16,6 @@ #define NAV2_MPPI_CONTROLLER__MODELS__OPTIMIZER_SETTINGS_HPP_ #include -#include #include "nav2_mppi_controller/models/constraints.hpp" namespace mppi::models @@ -30,7 +29,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, std::make_shared(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}; diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index d94297aeabc..34cd9a95276 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -118,11 +118,6 @@ 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 diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp index b40bea39731..f8bc72d4f1d 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp +++ b/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 - #include #include #include @@ -47,19 +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); - - /** - * @brief Initialize normal_distributions to their SamplingStd parameterized values (vx, vy, wz) - */ - void initializeNormalDistributions(); + const std::shared_ptr & node, const std::string & name, + ParametersHandler * param_handler); /** * @brief Shutdown noise generator thread @@ -72,6 +64,12 @@ class NoiseGenerator */ void generateNextNoises(); + /** + * @brief set noised control_sequence to state controls + * @return noises vx, vy, wz + */ + 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 @@ -80,17 +78,19 @@ class NoiseGenerator void computeAdaptiveStds(const models::State & state); /** - * @brief set noised control_sequence to state controls - * @return noises vx, vy, wz + * Validates decay constraints and returns true if constraints are valid + * @return true if decay constraints are valid */ - void setNoisedControls(models::State & state, const models::ControlSequence & control_sequence); + 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: /** @@ -112,17 +112,21 @@ class NoiseGenerator Eigen::ArrayXXf noises_wz_; std::default_random_engine generator_; - std::normal_distribution ndistribution_vx_; - std::normal_distribution ndistribution_wz_; - std::normal_distribution ndistribution_vy_; - mppi::models::OptimizerSettings settings_; + std::unique_ptr logger_; + models::OptimizerSettings settings_; bool is_holonomic_; - std::thread noise_thread_; + std::unique_ptr 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 diff --git a/nav2_mppi_controller/src/noise_generator.cpp b/nav2_mppi_controller/src/noise_generator.cpp index 7a21f7830b2..236e07a23df 100644 --- a/nav2_mppi_controller/src/noise_generator.cpp +++ b/nav2_mppi_controller/src/noise_generator.cpp @@ -21,29 +21,22 @@ namespace mppi { void NoiseGenerator::initialize( - mppi::models::OptimizerSettings & settings, bool is_holonomic, - const std::string & name, ParametersHandler * param_handler) + const std::shared_ptr & node, + const std::string & name, + ParametersHandler * param_handler) { - settings_ = settings; - is_holonomic_ = is_holonomic; - active_ = true; - - initializeNormalDistributions(); + this->logger_ = std::make_unique(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::bind(&NoiseGenerator::noiseThread, this)); } else { - generateNoisedControls(); + noise_thread_.reset(); } -} -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); + active_ = true; } void NoiseGenerator::shutdown() @@ -51,8 +44,11 @@ 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(); } } @@ -67,16 +63,33 @@ void NoiseGenerator::generateNextNoises() noise_cond_.notify_all(); } +void NoiseGenerator::setNoisedControls( + models::State & state, + const models::ControlSequence & control_sequence) +{ + std::unique_lock 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::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)) - { + if (s.advanced_constraints.wz_std_decay_strength <= 0.0f || !validateWzStdDecayConstraints()) { wz_std_adaptive = s.sampling_std.wz; // skip calculation + printf("haydaaa wz_std_decay_strength: %f," + "wz_std_decay_to: %f, " + "wz_std_adaptive: %f, " + "current_speed: %f\n", + s.advanced_constraints.wz_std_decay_strength, + s.advanced_constraints.wz_std_decay_to, + wz_std_adaptive, 0.0f); } else { float current_speed; if (is_holonomic_) { @@ -84,54 +97,67 @@ void NoiseGenerator::computeAdaptiveStds(const models::State & state) const auto vy = static_cast(state.speed.linear.y); current_speed = hypotf(vx, vy); } else { - current_speed = fabs(static_cast(state.speed.linear.x)); + current_speed = std::fabs(static_cast(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) * + 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); + printf("hopbaaa wz_std_adaptive: %f, " + "current_speed: %f\n", wz_std_adaptive, current_speed); } } -void NoiseGenerator::setNoisedControls( - models::State & state, - const models::ControlSequence & control_sequence) +bool NoiseGenerator::validateWzStdDecayConstraints() const { - std::unique_lock guard(noise_lock_); - - computeAdaptiveStds(state); + 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 + } - 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(); + if (c.wz_std_decay_to < 0.0f || c.wz_std_decay_to > settings_.sampling_std.wz) { + return false; + } + return true; } -void NoiseGenerator::reset(mppi::models::OptimizerSettings & settings, bool is_holonomic) +float NoiseGenerator::getWzStdAdaptive() const { - settings_ = settings; - is_holonomic_ = is_holonomic; + 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 guard(noise_lock_); - - initializeNormalDistributions(); - + // 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(); @@ -151,14 +177,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, settings_.sampling_std.vy); + auto ndistribution_vy = std::normal_distribution(0.0f, wz_std_adaptive); 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 diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index f68c4d70c34..606da29e211 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/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(); } @@ -134,16 +134,9 @@ void Optimizer::setOffset(double controller_frequency) } } -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}; @@ -160,13 +153,6 @@ void Optimizer::reset(bool reset_dynamic_speed_limits) 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"); } @@ -455,7 +441,6 @@ const models::ControlSequence & Optimizer::getOptimalControlSequence() return control_sequence_; } - void Optimizer::updateControlSequence() { const bool is_holo = isHolonomic(); @@ -466,7 +451,7 @@ 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(); - const float & wz_std_adaptive = *s.sampling_std.wz_std_adaptive; + 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; diff --git a/nav2_mppi_controller/test/noise_generator_test.cpp b/nav2_mppi_controller/test/noise_generator_test.cpp index 6e698077f87..139cdfeaaa4 100644 --- a/nav2_mppi_controller/test/noise_generator_test.cpp +++ b/nav2_mppi_controller/test/noise_generator_test.cpp @@ -41,9 +41,9 @@ TEST(NoiseGeneratorTest, NoiseGeneratorLifecycle) auto node = std::make_shared("node"); node->declare_parameter("test_name.regenerate_noises", rclcpp::ParameterValue(false)); std::string name = "test"; -ParametersHandler handler(node, name); + ParametersHandler handler(node, name); - generator.initialize(settings, false, "test_name", &handler); + generator.initialize(node, "test_name", &handler); generator.reset(settings, false); generator.shutdown(); } @@ -54,7 +54,7 @@ TEST(NoiseGeneratorTest, NoiseGeneratorMain) auto node = std::make_shared("node"); node->declare_parameter("test_name.regenerate_noises", rclcpp::ParameterValue(true)); std::string name = "test"; -ParametersHandler handler(node, name); + ParametersHandler handler(node, name); NoiseGenerator generator; mppi::models::OptimizerSettings settings; settings.batch_size = 100; @@ -76,7 +76,7 @@ ParametersHandler handler(node, name); state.reset(settings.batch_size, settings.time_steps); // Request an update with no noise yet generated, should result in identical outputs - generator.initialize(settings, false, "test_name", &handler); + generator.initialize(node, "test_name", &handler); generator.reset(settings, false); // sets initial sizing and zeros out noises std::this_thread::sleep_for(std::chrono::milliseconds(100)); generator.setNoisedControls(state, control_sequence); @@ -143,7 +143,7 @@ TEST(NoiseGeneratorTest, NoiseGeneratorMainNoRegenerate) auto node = std::make_shared("node"); node->declare_parameter("test_name.regenerate_noises", rclcpp::ParameterValue(false)); std::string name = "test"; -ParametersHandler handler(node, name); + ParametersHandler handler(node, name); NoiseGenerator generator; mppi::models::OptimizerSettings settings; settings.batch_size = 100; @@ -165,7 +165,7 @@ ParametersHandler handler(node, name); state.reset(settings.batch_size, settings.time_steps); // Request an update with no noise yet generated, should result in identical outputs - generator.initialize(settings, false, "test_name", &handler); + generator.initialize(node, "test_name", &handler); generator.reset(settings, false); // sets initial sizing and zeros out noises std::this_thread::sleep_for(std::chrono::milliseconds(100)); generator.setNoisedControls(state, control_sequence); @@ -209,7 +209,7 @@ ParametersHandler handler(node, name); TEST(NoiseGeneratorTest, AdaptiveStds) { // Tests shuts down internal thread cleanly - auto node = std::make_shared("node"); + auto node = std::make_shared("node"); std::string name = "test"; ParametersHandler handler(node, name); NoiseGenerator generator; @@ -224,17 +224,17 @@ TEST(NoiseGeneratorTest, AdaptiveStds) state.reset(settings.batch_size, settings.time_steps); // Test with AdaptiveStd off (default behavior) - generator.initialize(settings, false, "test_name", &handler); + generator.initialize(node, "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); + EXPECT_NEAR(settings.sampling_std.wz, generator.getWzStdAdaptive(), 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); + EXPECT_NEAR(settings.sampling_std.wz, generator.getWzStdAdaptive(), EPSILON); // Enable AdaptiveStd, non-holonomic with vx == 1.0 m/sec settings.advanced_constraints.wz_std_decay_strength = 3.0f; @@ -242,7 +242,7 @@ TEST(NoiseGeneratorTest, AdaptiveStds) 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.052489355206489563f, generator.getWzStdAdaptive(), 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 @@ -252,7 +252,8 @@ TEST(NoiseGeneratorTest, AdaptiveStds) 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_TRUE(generator.validateWzStdDecayConstraints()); + EXPECT_NEAR(0.052489355206489563f, generator.getWzStdAdaptive(), EPSILON); EXPECT_NEAR(0.1f, settings.sampling_std.wz, EPSILON); // wz_std should stay the same // Enable AdaptiveStd, with invalid input @@ -262,13 +263,8 @@ TEST(NoiseGeneratorTest, AdaptiveStds) 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()); - } + EXPECT_FALSE(generator.validateWzStdDecayConstraints()); + EXPECT_EQ(settings.sampling_std.wz, generator.getWzStdAdaptive()); generator.shutdown(); } From 74dc70448af55906afed623712bb39bc5f62e881 Mon Sep 17 00:00:00 2001 From: Vahap Oguz TOKMAK <5289529+vahapt@users.noreply.github.com> Date: Mon, 30 Jun 2025 13:35:36 +0300 Subject: [PATCH 6/6] Added support for MPPI Controller to adjust wz_std parameter based on linear speed - Code cleanup & bugfix Signed-off-by: Vahap Oguz TOKMAK <5289529+vahapt@users.noreply.github.com> --- nav2_mppi_controller/src/noise_generator.cpp | 13 ++----------- 1 file changed, 2 insertions(+), 11 deletions(-) diff --git a/nav2_mppi_controller/src/noise_generator.cpp b/nav2_mppi_controller/src/noise_generator.cpp index 236e07a23df..e89631cda12 100644 --- a/nav2_mppi_controller/src/noise_generator.cpp +++ b/nav2_mppi_controller/src/noise_generator.cpp @@ -83,13 +83,6 @@ void NoiseGenerator::computeAdaptiveStds(const models::State & state) // 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 - printf("haydaaa wz_std_decay_strength: %f," - "wz_std_decay_to: %f, " - "wz_std_adaptive: %f, " - "current_speed: %f\n", - s.advanced_constraints.wz_std_decay_strength, - s.advanced_constraints.wz_std_decay_to, - wz_std_adaptive, 0.0f); } else { float current_speed; if (is_holonomic_) { @@ -107,8 +100,6 @@ void NoiseGenerator::computeAdaptiveStds(const models::State & state) s.advanced_constraints.wz_std_decay_to; wz_std_adaptive = decayed_wz_std; - printf("hopbaaa wz_std_adaptive: %f, " - "current_speed: %f\n", wz_std_adaptive, current_speed); } } @@ -178,8 +169,8 @@ 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, settings_.sampling_std.vy); - auto ndistribution_vy = std::normal_distribution(0.0f, wz_std_adaptive); + 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_);}); noises_wz_ = Eigen::ArrayXXf::NullaryExpr(