From 40804bcd26a978b8b3616c41c4ada3688befc159 Mon Sep 17 00:00:00 2001 From: stevemacenski Date: Wed, 5 Oct 2022 17:02:17 -0700 Subject: [PATCH 1/4] adding N% zero-mean trajectories for soft recovery --- README.md | 2 ++ include/mppic/models/optimizer_settings.hpp | 1 + src/noise_generator.cpp | 20 +++++++++++++++++--- src/optimizer.cpp | 1 + 4 files changed, 21 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 1dd80a39..d1ae2dc5 100644 --- a/README.md +++ b/README.md @@ -70,6 +70,7 @@ sudo make install | vx_min | double | Min VX | | wz_max | double | Max WZ | | temperature | double | Selectiveness of trajectories by their costs (The closer this value to 0, the "more" we take in considiration 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 | + | zero_mean_percentage | double | A percentage (0-1) of trajectory samples that should be around a zero-velocity mean to allow for some implicit recovery if path solutions change or dynamic obstacles result in collisions at full speed with a "soft reset". Under or near 1% (e.g. 0.01) is reasonable. | | visualize | bool | Use visualization | | retry_attempt_limit | int | Number of attempts to find feasible trajectory before failure | @@ -149,6 +150,7 @@ controller_server: prune_distance: 1.7 transform_tolerance: 0.1 temperature: 0.35 + zero_mean_percentage: 0.01 motion_model: "DiffDrive" visualize: false TrajectoryVisualizer: diff --git a/include/mppic/models/optimizer_settings.hpp b/include/mppic/models/optimizer_settings.hpp index 3359901f..3dca63af 100644 --- a/include/mppic/models/optimizer_settings.hpp +++ b/include/mppic/models/optimizer_settings.hpp @@ -14,6 +14,7 @@ struct OptimizerSettings models::SamplingStd sampling_std{0, 0, 0}; float model_dt{0}; float temperature{0}; + float zero_mean_percentage{0.0}; unsigned int batch_size{0}; unsigned int time_steps{0}; unsigned int iteration_count{0}; diff --git a/src/noise_generator.cpp b/src/noise_generator.cpp index a0a273bd..dec54eb9 100644 --- a/src/noise_generator.cpp +++ b/src/noise_generator.cpp @@ -10,6 +10,8 @@ namespace mppi { +using xt::placeholders::_; + void NoiseGenerator::initialize(mppi::models::OptimizerSettings & settings, bool is_holonomic) { settings_ = settings; @@ -43,9 +45,21 @@ void NoiseGenerator::setNoisedControls( { std::unique_lock guard(noise_lock_); - xt::noalias(state.cvx) = control_sequence.vx + noises_vx_; - xt::noalias(state.cvy) = control_sequence.vy + noises_vy_; - xt::noalias(state.cwz) = control_sequence.wz + noises_wz_; + // Where to divide set of noises between zero- and U-meaned controls + const int division = ceil((1.0 - settings_.zero_mean_percentage) * settings_.batch_size); + + auto applyNoises = [&](auto & axis_noises, auto & axis_state, auto & axis_control) -> void { + auto noises = xt::view(axis_noises, xt::range(0, division), xt::all()); + auto zero_mean_noises = xt::view(axis_noises, xt::range(division, _), xt::all()); + auto noised_controls = xt::view(axis_state, xt::range(0, division), xt::all()); + auto zero_mean_controls = xt::view(axis_state, xt::range(division, _), xt::all()); + xt::noalias(noised_controls) = axis_control + noises; + xt::noalias(zero_mean_controls) = zero_mean_noises; + }; + + applyNoises(noises_vx_, state.cvx, control_sequence.vx); + applyNoises(noises_vy_, state.cvy, control_sequence.vy); + applyNoises(noises_wz_, state.cwz, control_sequence.wz); } void NoiseGenerator::reset(mppi::models::OptimizerSettings & settings, bool is_holonomic) diff --git a/src/optimizer.cpp b/src/optimizer.cpp index 214c67b0..7c682690 100644 --- a/src/optimizer.cpp +++ b/src/optimizer.cpp @@ -58,6 +58,7 @@ void Optimizer::getParams() getParam(s.batch_size, "batch_size", 400); getParam(s.iteration_count, "iteration_count", 1); getParam(s.temperature, "temperature", 0.25f); + getParam(s.zero_mean_percentage, "zero_mean_percentage", 0.01f); getParam(s.base_constraints.vx_max, "vx_max", 0.5); getParam(s.base_constraints.vx_min, "vx_min", -0.35); getParam(s.base_constraints.vy, "vy_max", 0.5); From 9a85fcb0b401f815548b05e8c2b2cad4d7cb2120 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 6 Oct 2022 10:49:55 -0700 Subject: [PATCH 2/4] Update src/noise_generator.cpp Co-authored-by: Alex <48126993+artofnothingness@users.noreply.github.com> --- src/noise_generator.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/noise_generator.cpp b/src/noise_generator.cpp index dec54eb9..ca951c96 100644 --- a/src/noise_generator.cpp +++ b/src/noise_generator.cpp @@ -48,13 +48,15 @@ void NoiseGenerator::setNoisedControls( // Where to divide set of noises between zero- and U-meaned controls const int division = ceil((1.0 - settings_.zero_mean_percentage) * settings_.batch_size); - auto applyNoises = [&](auto & axis_noises, auto & axis_state, auto & axis_control) -> void { - auto noises = xt::view(axis_noises, xt::range(0, division), xt::all()); - auto zero_mean_noises = xt::view(axis_noises, xt::range(division, _), xt::all()); - auto noised_controls = xt::view(axis_state, xt::range(0, division), xt::all()); - auto zero_mean_controls = xt::view(axis_state, xt::range(division, _), xt::all()); - xt::noalias(noised_controls) = axis_control + noises; - xt::noalias(zero_mean_controls) = zero_mean_noises; + auto applyNoises = [division](auto & state, const auto & noise, const auto & control) { + auto lhs_state = xt::view(state, xt::range(0, division)); + const auto lhs_noise = xt::view(noise, xt::range(0, division)); + xt::noalias(lhs_state) = lhs_noise; + + auto rhs_state = xt::view(state, xt::range(division, _)); + const auto rhs_noise = xt::view(noise, xt::range(division, _)); + const auto rhs_control = xt::view(control, xt::range(division, _)); + xt::noalias(rhs_state) = rhs_control + rhs_noise; }; applyNoises(noises_vx_, state.cvx, control_sequence.vx); From 0ad7394e54e86acef2fd4f3a1772196b9f41eb0e Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 6 Oct 2022 10:50:01 -0700 Subject: [PATCH 3/4] Update src/noise_generator.cpp Co-authored-by: Alex <48126993+artofnothingness@users.noreply.github.com> --- src/noise_generator.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/noise_generator.cpp b/src/noise_generator.cpp index ca951c96..5934f28d 100644 --- a/src/noise_generator.cpp +++ b/src/noise_generator.cpp @@ -59,9 +59,9 @@ void NoiseGenerator::setNoisedControls( xt::noalias(rhs_state) = rhs_control + rhs_noise; }; - applyNoises(noises_vx_, state.cvx, control_sequence.vx); - applyNoises(noises_vy_, state.cvy, control_sequence.vy); - applyNoises(noises_wz_, state.cwz, control_sequence.wz); + applyNoises(state.cvx, noises_vx_, control_sequence.vx); + applyNoises(state.cvy, noises_vy_, control_sequence.vy); + applyNoises(state.cwz, noises_wz_, control_sequence.wz); } void NoiseGenerator::reset(mppi::models::OptimizerSettings & settings, bool is_holonomic) From 3d2d372da43efd4579dc160ba17e0f5aaeeccbf8 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 6 Oct 2022 11:25:50 -0700 Subject: [PATCH 4/4] Update src/noise_generator.cpp Co-authored-by: Alex <48126993+artofnothingness@users.noreply.github.com> --- src/noise_generator.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/noise_generator.cpp b/src/noise_generator.cpp index 5934f28d..44144fa3 100644 --- a/src/noise_generator.cpp +++ b/src/noise_generator.cpp @@ -49,13 +49,13 @@ void NoiseGenerator::setNoisedControls( const int division = ceil((1.0 - settings_.zero_mean_percentage) * settings_.batch_size); auto applyNoises = [division](auto & state, const auto & noise, const auto & control) { - auto lhs_state = xt::view(state, xt::range(0, division)); - const auto lhs_noise = xt::view(noise, xt::range(0, division)); + auto lhs_state = xt::view(state, xt::range(0, division), xt::all()); + const auto lhs_noise = xt::view(noise, xt::range(0, division), xt::all()); xt::noalias(lhs_state) = lhs_noise; - auto rhs_state = xt::view(state, xt::range(division, _)); - const auto rhs_noise = xt::view(noise, xt::range(division, _)); - const auto rhs_control = xt::view(control, xt::range(division, _)); + auto rhs_state = xt::view(state, xt::range(division, _), xt::all()); + const auto rhs_noise = xt::view(noise, xt::range(division, _), xt::all()); + const auto rhs_control = xt::view(control, xt::range(division, _), xt::all()); xt::noalias(rhs_state) = rhs_control + rhs_noise; };