adding N% zero-mean trajectories for soft recovery

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:

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};

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<std::mutex> 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)

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);