From 924f167382080f3ccdd000ffc34b921cb64bcf95 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 10 Oct 2023 19:18:36 -0700 Subject: [PATCH] [MPPI Optimization] adding regenerate noise param + adding docs (#3868) * adding regenerate noise param + adding docs * fix tests * remove unnecessary normalization * Update optimizer.cpp --- nav2_mppi_controller/CMakeLists.txt | 5 +++- nav2_mppi_controller/README.md | 5 ++++ .../tools/noise_generator.hpp | 13 +++++++---- .../src/critics/goal_angle_critic.cpp | 6 +---- .../src/critics/goal_critic.cpp | 6 +---- .../src/critics/obstacles_critic.cpp | 2 +- .../src/critics/prefer_forward_critic.cpp | 9 +++----- .../src/critics/twirling_critic.cpp | 8 +++---- nav2_mppi_controller/src/noise_generator.cpp | 23 +++++++++++++++---- nav2_mppi_controller/src/optimizer.cpp | 12 +++++----- .../test/noise_generator_test.cpp | 12 ++++++++-- 11 files changed, 62 insertions(+), 39 deletions(-) diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index 5f0c5359db..af64e1b1ed 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -6,7 +6,10 @@ add_definitions(-DXTENSOR_USE_XSIMD) set(XTENSOR_USE_TBB 0) set(XTENSOR_USE_OPENMP 0) +set(XTENSOR_USE_XSIMD 1) +# set(XTENSOR_DEFAULT_LAYOUT column_major) # row_major, column_major +# set(XTENSOR_DEFAULT_TRAVERSAL row_major) # row_major, column_major find_package(ament_cmake REQUIRED) find_package(xtensor REQUIRED) @@ -86,7 +89,7 @@ set(libraries mppi_controller mppi_critics) foreach(lib IN LISTS libraries) target_compile_options(${lib} PUBLIC -fconcepts) - target_include_directories(${lib} PUBLIC include ${xsimd_INCLUDE_DIRS} ${OpenMP_INCLUDE_DIRS}) + target_include_directories(${lib} PUBLIC include ${xsimd_INCLUDE_DIRS}) # ${OpenMP_INCLUDE_DIRS} target_link_libraries(${lib} xtensor xtensor::optimize xtensor::use_xsimd) ament_target_dependencies(${lib} ${dependencies_pkgs}) endforeach() diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 684e428fa2..3872b72610 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -29,6 +29,7 @@ This process is then repeated a number of times and returns a converged solution - Includes fallback mechanisms to handle soft-failures before escalating to recovery behaviors - High-quality code implementation with Doxygen, high unit test coverage, documentation, and parameter guide - Easily extensible to support modern research variants of MPPI +- Comes pre-tuned for good out-of-the-box behavior ## Configuration @@ -52,6 +53,8 @@ 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. | + #### Trajectory Visualizer | Parameter | Type | Definition | | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | @@ -254,6 +257,8 @@ The most common parameters you might want to start off changing are the velocity If you don't require path following behavior (e.g. just want to follow a goal pose and let the model predictive elements decide the best way to accomplish that), you may easily remove the PathAlign, PathFollow and PathAngle critics. +By default, the controller is tuned and has the capabilities established in the PathAlign/Obstacle critics to generally follow the path closely when no obstacles prevent it, but able to deviate from the path when blocked. See `PathAlignCritic::score()` for details, but it is disabled when the local path is blocked so the obstacle critic takes over in that state. + ### Prediction Horizon, Costmap Sizing, and Offsets As this is a predictive planner, there is some relationship between maximum speed, prediction times, and costmap size that users should keep in mind while tuning for their application. If a controller server costmap is set to 3.0m in size, that means that with the robot in the center, there is 1.5m of information on either side of the robot. When your prediction horizon (time_steps * model_dt) at maximum speed (vx_max) is larger than this, then your robot will be artificially limited in its maximum speeds and behavior by the costmap limitation. For example, if you predict forward 3 seconds (60 steps @ 0.05s per step) at 0.5m/s maximum speed, the **minimum** required costmap radius is 1.5m - or 3m total width. 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 6eb1b36384..a811d998e4 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 @@ -25,8 +25,9 @@ #include #include "nav2_mppi_controller/models/optimizer_settings.hpp" -#include -#include +#include "nav2_mppi_controller/tools/parameters_handler.hpp" +#include "nav2_mppi_controller/models/control_sequence.hpp" +#include "nav2_mppi_controller/models/state.hpp" namespace mppi { @@ -47,8 +48,12 @@ class NoiseGenerator * @brief Initialize noise generator with settings and model types * @param settings Settings of controller * @param is_holonomic If base is holonomic + * @param name Namespace for configs + * @param param_handler Get parameters util */ - void initialize(mppi::models::OptimizerSettings & settings, bool is_holonomic); + void initialize( + mppi::models::OptimizerSettings & settings, + bool is_holonomic, const std::string & name, ParametersHandler * param_handler); /** * @brief Shutdown noise generator thread @@ -99,7 +104,7 @@ class NoiseGenerator std::thread noise_thread_; std::condition_variable noise_cond_; std::mutex noise_lock_; - bool active_{false}, ready_{false}; + bool active_{false}, ready_{false}, regenerate_noises_{false}; }; } // namespace mppi diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp index 62bfdf0676..604ee24eb1 100644 --- a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -35,11 +35,7 @@ void GoalAngleCritic::initialize() void GoalAngleCritic::score(CriticData & data) { - if (!enabled_) { - return; - } - - if (!utils::withinPositionGoalTolerance( + if (!enabled_ || !utils::withinPositionGoalTolerance( threshold_to_consider_, data.state.pose.pose, data.path)) { return; diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp index 852db11529..f61d6fd2b2 100644 --- a/nav2_mppi_controller/src/critics/goal_critic.cpp +++ b/nav2_mppi_controller/src/critics/goal_critic.cpp @@ -35,11 +35,7 @@ void GoalCritic::initialize() void GoalCritic::score(CriticData & data) { - if (!enabled_) { - return; - } - - if (!utils::withinPositionGoalTolerance( + if (!enabled_ || !utils::withinPositionGoalTolerance( threshold_to_consider_, data.state.pose.pose, data.path)) { return; diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index 80ac77e10f..90fa92a57a 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -152,7 +152,7 @@ void ObstaclesCritic::score(CriticData & data) } if (!trajectory_collide) {all_trajectories_collide = false;} - raw_cost[i] = static_cast(trajectory_collide ? collision_cost_ : traj_cost); + raw_cost[i] = trajectory_collide ? collision_cost_ : traj_cost; } data.costs += xt::pow( diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp index 18b6900177..08e755dd6a 100644 --- a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp +++ b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp @@ -33,12 +33,9 @@ void PreferForwardCritic::initialize() void PreferForwardCritic::score(CriticData & data) { using xt::evaluation_strategy::immediate; - - if (!enabled_) { - return; - } - - if (utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) { + if (!enabled_ || + utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) + { return; } diff --git a/nav2_mppi_controller/src/critics/twirling_critic.cpp b/nav2_mppi_controller/src/critics/twirling_critic.cpp index 28eb71b48b..6b492d13ba 100644 --- a/nav2_mppi_controller/src/critics/twirling_critic.cpp +++ b/nav2_mppi_controller/src/critics/twirling_critic.cpp @@ -31,11 +31,9 @@ void TwirlingCritic::initialize() void TwirlingCritic::score(CriticData & data) { using xt::evaluation_strategy::immediate; - if (!enabled_) { - return; - } - - if (utils::withinPositionGoalTolerance(data.goal_checker, data.state.pose.pose, data.path)) { + if (!enabled_ || + utils::withinPositionGoalTolerance(data.goal_checker, data.state.pose.pose, data.path)) + { return; } diff --git a/nav2_mppi_controller/src/noise_generator.cpp b/nav2_mppi_controller/src/noise_generator.cpp index 09ee4ab92d..b7c452aa6a 100644 --- a/nav2_mppi_controller/src/noise_generator.cpp +++ b/nav2_mppi_controller/src/noise_generator.cpp @@ -23,12 +23,22 @@ namespace mppi { -void NoiseGenerator::initialize(mppi::models::OptimizerSettings & settings, bool is_holonomic) +void NoiseGenerator::initialize( + mppi::models::OptimizerSettings & settings, bool is_holonomic, + const std::string & name, ParametersHandler * param_handler) { settings_ = settings; is_holonomic_ = is_holonomic; active_ = true; - noise_thread_ = std::thread(std::bind(&NoiseGenerator::noiseThread, this)); + + auto getParam = param_handler->getParamGetter(name); + getParam(regenerate_noises_, "regenerate_noises", false); + + if (regenerate_noises_) { + noise_thread_ = std::thread(std::bind(&NoiseGenerator::noiseThread, this)); + } else { + generateNoisedControls(); + } } void NoiseGenerator::shutdown() @@ -44,7 +54,7 @@ void NoiseGenerator::shutdown() void NoiseGenerator::generateNextNoises() { // Trigger the thread to run in parallel to this iteration - // to generate the next iteration's noises. + // to generate the next iteration's noises (if applicable). { std::unique_lock guard(noise_lock_); ready_ = true; @@ -76,7 +86,12 @@ void NoiseGenerator::reset(mppi::models::OptimizerSettings & settings, bool is_h xt::noalias(noises_wz_) = xt::zeros({settings_.batch_size, settings_.time_steps}); ready_ = true; } - noise_cond_.notify_all(); + + if (regenerate_noises_) { + noise_cond_.notify_all(); + } else { + generateNoisedControls(); + } } void NoiseGenerator::noiseThread() diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 6d0a3ceb6c..1dd8420230 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -49,7 +50,7 @@ void Optimizer::initialize( getParams(); critic_manager_.on_configure(parent_, name_, costmap_ros_, parameters_handler_); - noise_generator_.initialize(settings_, isHolonomic()); + noise_generator_.initialize(settings_, isHolonomic(), name_, parameters_handler_); reset(); } @@ -268,7 +269,7 @@ void Optimizer::integrateStateVelocities( xt::xtensor & trajectory, const xt::xtensor & sequence) const { - double initial_yaw = tf2::getYaw(state_.pose.pose.orientation); + float initial_yaw = tf2::getYaw(state_.pose.pose.orientation); const auto vx = xt::view(sequence, xt::all(), 0); const auto vy = xt::view(sequence, xt::all(), 2); @@ -278,8 +279,7 @@ void Optimizer::integrateStateVelocities( auto traj_y = xt::view(trajectory, xt::all(), 1); auto traj_yaws = xt::view(trajectory, xt::all(), 2); - xt::noalias(traj_yaws) = - utils::normalize_angles(xt::cumsum(wz * settings_.model_dt, 0) + initial_yaw); + xt::noalias(traj_yaws) = xt::cumsum(wz * settings_.model_dt, 0 + initial_yaw); auto && yaw_cos = xt::xtensor::from_shape(traj_yaws.shape()); auto && yaw_sin = xt::xtensor::from_shape(traj_yaws.shape()); @@ -307,10 +307,10 @@ void Optimizer::integrateStateVelocities( models::Trajectories & trajectories, const models::State & state) const { - const double initial_yaw = tf2::getYaw(state.pose.pose.orientation); + const float initial_yaw = tf2::getYaw(state.pose.pose.orientation); xt::noalias(trajectories.yaws) = - utils::normalize_angles(xt::cumsum(state.wz * settings_.model_dt, 1) + initial_yaw); + xt::cumsum(state.wz * settings_.model_dt, 1) + initial_yaw; const auto yaws_cutted = xt::view(trajectories.yaws, xt::all(), xt::range(0, -1)); diff --git a/nav2_mppi_controller/test/noise_generator_test.cpp b/nav2_mppi_controller/test/noise_generator_test.cpp index 3788f2b8a3..db8667e940 100644 --- a/nav2_mppi_controller/test/noise_generator_test.cpp +++ b/nav2_mppi_controller/test/noise_generator_test.cpp @@ -42,13 +42,21 @@ TEST(NoiseGeneratorTest, NoiseGeneratorLifecycle) settings.batch_size = 100; settings.time_steps = 25; - generator.initialize(settings, false); + auto node = std::make_shared("node"); + node->declare_parameter("test_name.regenerate_noises", rclcpp::ParameterValue(false)); + ParametersHandler handler(node); + + generator.initialize(settings, false, "test_name", &handler); + generator.reset(settings, false); generator.shutdown(); } TEST(NoiseGeneratorTest, NoiseGeneratorMain) { // Tests shuts down internal thread cleanly + auto node = std::make_shared("node"); + node->declare_parameter("test_name.regenerate_noises", rclcpp::ParameterValue(true)); + ParametersHandler handler(node); NoiseGenerator generator; mppi::models::OptimizerSettings settings; settings.batch_size = 100; @@ -70,7 +78,7 @@ TEST(NoiseGeneratorTest, NoiseGeneratorMain) 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); + generator.initialize(settings, false, "test_name", &handler); generator.reset(settings, false); // sets initial sizing and zeros out noises generator.setNoisedControls(state, control_sequence); EXPECT_EQ(state.cvx(0), 0);