diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md
index 68db5b73719..4fd16fb2e58 100644
--- a/nav2_mppi_controller/README.md
+++ b/nav2_mppi_controller/README.md
@@ -38,28 +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. |
| 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 |
#### Trajectory Visualizer
| Parameter | Type | Definition |
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..5626b461776 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp
@@ -35,6 +35,30 @@ struct ControlConstraints
float az_max;
};
+struct AdvancedConstraints
+{
+ /**
+ * @brief Defines the strength of the reduction function
+ * Allows dynamic modification of wz_std (angular deviation) based on linear velocity of the robot.
+ * When a robot with high inertia (e.g. 500kg) is moving fast and if wz_std is above 0.3, oscillation behavior can be observed. Lowering wz_std stabilizes the robot but then the maneuvers take more time.
+ * Dynamically reducing wz_std as vx, vy increase (speed of the robot) solves both problems.
+ * Suggested values to start with: wz_std = 0.4, wz_std_decay_to = 0.05, wz_std_decay_strength = 3.0
+ * The following is used as the decay function
+ *
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;
+};
+
/**
* @struct mppi::models::SamplingStd
* @brief Noise parameters for sampling trajectories
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/tools/noise_generator.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp
index a201a1d4d1b..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,14 +45,13 @@ class NoiseGenerator
/**
* @brief Initialize noise generator with settings and model types
- * @param settings Settings of controller
- * @param is_holonomic If base is holonomic
+ * @param logger Reference to the package logger
* @param name Namespace for configs
* @param param_handler Get parameters util
*/
void initialize(
- mppi::models::OptimizerSettings & settings,
- bool is_holonomic, const std::string & name, ParametersHandler * param_handler);
+ const std::shared_ptr & node, const std::string & name,
+ ParametersHandler * param_handler);
/**
* @brief Shutdown noise generator thread
@@ -73,12 +70,27 @@ class NoiseGenerator
*/
void setNoisedControls(models::State & state, const models::ControlSequence & control_sequence);
+ /**
+ * Computes adaptive values of the SamplingStd parameters and updates adaptive counterparts
+ * See also wz_std_decay_strength, wz_std_decay_to parameters for more information on how wz => wz_std_adaptive is computed
+ * @param state Current state of the robot
+ */
+ void computeAdaptiveStds(const models::State & state);
+
+ /**
+ * Validates decay constraints and returns true if constraints are valid
+ * @return true if decay constraints are valid
+ */
+ bool validateWzStdDecayConstraints() const;
+
+ float getWzStdAdaptive() const;
+
/**
* @brief Reset noise generator with settings and model types
* @param settings Settings of controller
* @param is_holonomic If base is holonomic
*/
- void reset(mppi::models::OptimizerSettings & settings, bool is_holonomic);
+ void reset(const mppi::models::OptimizerSettings & settings, bool is_holonomic);
protected:
/**
@@ -100,17 +112,21 @@ class NoiseGenerator
Eigen::ArrayXXf noises_wz_;
std::default_random_engine generator_;
- std::normal_distribution 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 a704ef8b5cf..e89631cda12 100644
--- a/nav2_mppi_controller/src/noise_generator.cpp
+++ b/nav2_mppi_controller/src/noise_generator.cpp
@@ -21,25 +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;
-
- ndistribution_vx_ = std::normal_distribution(0.0f, settings_.sampling_std.vx);
- ndistribution_vy_ = std::normal_distribution(0.0f, settings_.sampling_std.vy);
- ndistribution_wz_ = std::normal_distribution(0.0f, settings_.sampling_std.wz);
-
+ this->logger_ = std::make_unique(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();
}
+
+ active_ = true;
}
void NoiseGenerator::shutdown()
@@ -47,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();
}
}
@@ -69,26 +69,86 @@ 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();
}
-void NoiseGenerator::reset(mppi::models::OptimizerSettings & settings, bool is_holonomic)
+void NoiseGenerator::computeAdaptiveStds(const models::State & state)
+{
+ auto & s = settings_;
+
+ // Should we apply decay function? or Any constraint is invalid?
+ if (s.advanced_constraints.wz_std_decay_strength <= 0.0f || !validateWzStdDecayConstraints()) {
+ wz_std_adaptive = s.sampling_std.wz; // skip calculation
+ } else {
+ float current_speed;
+ if (is_holonomic_) {
+ const auto vx = static_cast(state.speed.linear.x);
+ const auto vy = static_cast(state.speed.linear.y);
+ current_speed = hypotf(vx, vy);
+ } else {
+ 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) *
+ powf(e, -1.0f * s.advanced_constraints.wz_std_decay_strength * current_speed) +
+ s.advanced_constraints.wz_std_decay_to;
+
+ wz_std_adaptive = decayed_wz_std;
+ }
+}
+
+bool NoiseGenerator::validateWzStdDecayConstraints() const
{
- settings_ = settings;
- is_holonomic_ = is_holonomic;
+ const models::AdvancedConstraints & c = settings_.advanced_constraints;
+ // Assume valid if angular decay is disabled
+ if (c.wz_std_decay_strength <= 0.0f) {
+ return true; // valid
+ }
+ if (c.wz_std_decay_to < 0.0f || c.wz_std_decay_to > settings_.sampling_std.wz) {
+ return false;
+ }
+ return true;
+}
+
+float NoiseGenerator::getWzStdAdaptive() const
+{
+ return wz_std_adaptive;
+}
+
+void NoiseGenerator::reset(const mppi::models::OptimizerSettings & settings, bool is_holonomic)
+{
// Recompute the noises on reset, initialization, and fallback
{
std::unique_lock guard(noise_lock_);
+ // Copy settings and holonomic info after lock is acquired,
+ // otherwise we may encounter concurrent access errors
+ settings_ = settings;
+ is_holonomic_ = is_holonomic;
+ // reset initial adaptive value to parameterized value
+ wz_std_adaptive = settings_.sampling_std.wz;
noises_vx_.setZero(settings_.batch_size, settings_.time_steps);
noises_vy_.setZero(settings_.batch_size, settings_.time_steps);
noises_wz_.setZero(settings_.batch_size, settings_.time_steps);
+
+ // Validate decay function, print warning message if decay_to is out of bounds
+ if (!validateWzStdDecayConstraints()) {
+ RCLCPP_WARN_STREAM(
+ *logger_, "wz_std_decay_to must be between 0 and wz_std. wz: "
+ << std::to_string(settings_.constraints.wz) << ", wz_std_decay_to: "
+ << std::to_string(settings_.advanced_constraints.wz_std_decay_to));
+ }
ready_ = true;
- }
+ } // release the lock
+
- if (regenerate_noises_) {
+ if (noise_thread_ != nullptr) { // if regenerate_noises_ == true, then noise_thread_ is non-null
noise_cond_.notify_all();
} else {
generateNoisedControls();
@@ -108,14 +168,16 @@ void NoiseGenerator::noiseThread()
void NoiseGenerator::generateNoisedControls()
{
auto & s = settings_;
+ auto ndistribution_vx = std::normal_distribution(0.0f, settings_.sampling_std.vx);
+ auto ndistribution_wz = std::normal_distribution(0.0f, wz_std_adaptive);
+ auto ndistribution_vy = std::normal_distribution(0.0f, settings_.sampling_std.vy);
noises_vx_ = Eigen::ArrayXXf::NullaryExpr(
- s.batch_size, s.time_steps, [&] () {return ndistribution_vx_(generator_);});
+ s.batch_size, s.time_steps, [&]() {return ndistribution_vx(generator_);});
noises_wz_ = Eigen::ArrayXXf::NullaryExpr(
- s.batch_size, s.time_steps, [&] () {return ndistribution_wz_(generator_);});
- if(is_holonomic_) {
+ s.batch_size, s.time_steps, [&]() {return ndistribution_wz(generator_);});
+ if (is_holonomic_) {
noises_vy_ = Eigen::ArrayXXf::NullaryExpr(
- s.batch_size, s.time_steps, [&] () {return ndistribution_vy_(generator_);});
+ s.batch_size, s.time_steps, [&]() {return ndistribution_vy(generator_);});
}
}
-
} // namespace mppi
diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp
index 7f34a74e240..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();
}
@@ -81,6 +81,8 @@ void Optimizer::getParams()
getParam(s.sampling_std.vx, "vx_std", 0.2f);
getParam(s.sampling_std.vy, "vy_std", 0.2f);
getParam(s.sampling_std.wz, "wz_std", 0.4f);
+ getParam(s.advanced_constraints.wz_std_decay_to, "advanced.wz_std_decay_to", 0.0f);
+ getParam(s.advanced_constraints.wz_std_decay_strength, "advanced.wz_std_decay_strength", -1.0f);
getParam(s.retry_attempt_limit, "retry_attempt_limit", 1);
s.base_constraints.ax_max = fabs(s.base_constraints.ax_max);
@@ -449,10 +451,11 @@ void Optimizer::updateControlSequence()
const float gamma_vx = s.gamma / (s.sampling_std.vx * s.sampling_std.vx);
costs_ += (gamma_vx * (bounded_noises_vx.rowwise() * vx_T).rowwise().sum()).eval();
- if (s.sampling_std.wz > 0.0f) {
+ const float & wz_std_adaptive = noise_generator_.getWzStdAdaptive();
+ if (wz_std_adaptive > 0.0f) {
auto wz_T = control_sequence_.wz.transpose();
auto bounded_noises_wz = state_.cwz.rowwise() - wz_T;
- const float gamma_wz = s.gamma / (s.sampling_std.wz * s.sampling_std.wz);
+ const float gamma_wz = s.gamma / (wz_std_adaptive * wz_std_adaptive);
costs_ += (gamma_wz * (bounded_noises_wz.rowwise() * wz_T).rowwise().sum()).eval();
}
diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp
index 813cba14318..6a0b1ee6575 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);
models::ControlSequence control_sequence;
models::Trajectories generated_trajectories;
generated_trajectories.reset(1000, 30);
diff --git a/nav2_mppi_controller/test/noise_generator_test.cpp b/nav2_mppi_controller/test/noise_generator_test.cpp
index a051782d311..139cdfeaaa4 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
@@ -39,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();
}
@@ -52,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;
@@ -74,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);
@@ -141,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;
@@ -163,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);
@@ -204,6 +206,69 @@ 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(node, "test_name", &handler);
+ generator.reset(settings, false); // sets initial sizing and zeros out noises
+ generator.computeAdaptiveStds(state);
+ 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, generator.getWzStdAdaptive(), 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, 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
+ 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_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
+ 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_FALSE(generator.validateWzStdDecayConstraints());
+ EXPECT_EQ(settings.sampling_std.wz, generator.getWzStdAdaptive());
+
+ generator.shutdown();
+}
+
int main(int argc, char **argv)
{
::testing::InitGoogleTest(&argc, argv);