diff --git a/README.md b/README.md index 1dd80a3..d1ae2dc 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 3359901..3dca63a 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 a0a273b..44144fa 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,23 @@ 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 = [division](auto & state, const auto & noise, const auto & control) { + 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, _), 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; + }; + + 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) diff --git a/src/optimizer.cpp b/src/optimizer.cpp index 214c67b..7c68269 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);