Skip to content

Commit

Permalink
[MPPI Optimization] adding regenerate noise param + adding docs (ros-…
Browse files Browse the repository at this point in the history
…navigation#3868)

* adding regenerate noise param + adding docs

* fix tests

* remove unnecessary normalization

* Update optimizer.cpp
  • Loading branch information
SteveMacenski authored Oct 11, 2023
1 parent 57dd8d4 commit 924f167
Show file tree
Hide file tree
Showing 11 changed files with 62 additions and 39 deletions.
5 changes: 4 additions & 1 deletion nav2_mppi_controller/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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()
Expand Down
5 changes: 5 additions & 0 deletions nav2_mppi_controller/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand All @@ -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 |
| --------------- | ------ | ----------------------------------------------------------------------------------------------------------- |
Expand Down Expand Up @@ -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.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -25,8 +25,9 @@
#include <xtensor/xview.hpp>

#include "nav2_mppi_controller/models/optimizer_settings.hpp"
#include <nav2_mppi_controller/models/control_sequence.hpp>
#include <nav2_mppi_controller/models/state.hpp>
#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
{
Expand All @@ -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
Expand Down Expand Up @@ -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
Expand Down
6 changes: 1 addition & 5 deletions nav2_mppi_controller/src/critics/goal_angle_critic.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
6 changes: 1 addition & 5 deletions nav2_mppi_controller/src/critics/goal_critic.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
2 changes: 1 addition & 1 deletion nav2_mppi_controller/src/critics/obstacles_critic.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -152,7 +152,7 @@ void ObstaclesCritic::score(CriticData & data)
}

if (!trajectory_collide) {all_trajectories_collide = false;}
raw_cost[i] = static_cast<float>(trajectory_collide ? collision_cost_ : traj_cost);
raw_cost[i] = trajectory_collide ? collision_cost_ : traj_cost;
}

data.costs += xt::pow(
Expand Down
9 changes: 3 additions & 6 deletions nav2_mppi_controller/src/critics/prefer_forward_critic.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down
8 changes: 3 additions & 5 deletions nav2_mppi_controller/src/critics/twirling_critic.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down
23 changes: 19 additions & 4 deletions nav2_mppi_controller/src/noise_generator.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand All @@ -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<std::mutex> guard(noise_lock_);
ready_ = true;
Expand Down Expand Up @@ -76,7 +86,12 @@ void NoiseGenerator::reset(mppi::models::OptimizerSettings & settings, bool is_h
xt::noalias(noises_wz_) = xt::zeros<float>({settings_.batch_size, settings_.time_steps});
ready_ = true;
}
noise_cond_.notify_all();

if (regenerate_noises_) {
noise_cond_.notify_all();
} else {
generateNoisedControls();
}
}

void NoiseGenerator::noiseThread()
Expand Down
12 changes: 6 additions & 6 deletions nav2_mppi_controller/src/optimizer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
#include <stdexcept>
#include <string>
#include <vector>
#include <cmath>
#include <xtensor/xmath.hpp>
#include <xtensor/xrandom.hpp>
#include <xtensor/xnoalias.hpp>
Expand Down Expand Up @@ -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();
}
Expand Down Expand Up @@ -268,7 +269,7 @@ void Optimizer::integrateStateVelocities(
xt::xtensor<float, 2> & trajectory,
const xt::xtensor<float, 2> & 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);
Expand All @@ -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<float, 1>::from_shape(traj_yaws.shape());
auto && yaw_sin = xt::xtensor<float, 1>::from_shape(traj_yaws.shape());
Expand Down Expand Up @@ -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));

Expand Down
12 changes: 10 additions & 2 deletions nav2_mppi_controller/test/noise_generator_test.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -42,13 +42,21 @@ TEST(NoiseGeneratorTest, NoiseGeneratorLifecycle)
settings.batch_size = 100;
settings.time_steps = 25;

generator.initialize(settings, false);
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("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<rclcpp_lifecycle::LifecycleNode>("node");
node->declare_parameter("test_name.regenerate_noises", rclcpp::ParameterValue(true));
ParametersHandler handler(node);
NoiseGenerator generator;
mppi::models::OptimizerSettings settings;
settings.batch_size = 100;
Expand All @@ -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);
Expand Down

0 comments on commit 924f167

Please sign in to comment.