Reduced lookups in predict function

Signed-off-by: Ayush1285 <[email protected]>
ros-navigation · May 24, 2024 · 70fba9c · 70fba9c
1 parent 8de0e44
commit 70fba9c
Show file tree

Hide file tree

Showing 4 changed files with 17 additions and 17 deletions.
diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp
@@ -31,8 +31,7 @@ struct ControlConstraints
   float ax_max;
   float ax_min;
   float ay_max;
-  float ay_min;
-  float az;
+  float az_max;
 };
 
 /**

diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/optimizer_settings.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/optimizer_settings.hpp
@@ -27,8 +27,8 @@ namespace mppi::models
  */
 struct OptimizerSettings
 {
-  models::ControlConstraints base_constraints{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
-  models::ControlConstraints constraints{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
+  models::ControlConstraints base_constraints{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
+  models::ControlConstraints constraints{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
   models::SamplingStd sampling_std{0.0f, 0.0f, 0.0f};
   float model_dt{0.0f};
   float temperature{0.0f};

diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp
@@ -86,25 +86,27 @@ class MotionModel
     float max_delta_vx = model_dt_ * control_constraints_.ax_max;
     float min_delta_vx = model_dt_ * control_constraints_.ax_min;
     float max_delta_vy = model_dt_ * control_constraints_.ay_max;
-    float min_delta_vy = model_dt_ * control_constraints_.ay_min;
-    float max_delta_wz = model_dt_ * control_constraints_.az;
+    float max_delta_wz = model_dt_ * control_constraints_.az_max;
+    float vx_last = state.vx(0, 0);
+    float vy_last = state.vy(0, 0);
+    float wz_last = state.wz(0, 0);
     for (unsigned int i = 0; i != state.vx.shape(0); i++) {
       for (unsigned int j = 1; j != state.vx.shape(1); j++) {
-        float& vx_curr = state.vx(i, j - 1);
         float& cvx_curr = state.cvx(i, j - 1);        
-        cvx_curr = std::clamp(cvx_curr, vx_curr + min_delta_vx, vx_curr + max_delta_vx);
+        cvx_curr = std::clamp(cvx_curr, vx_last + min_delta_vx, vx_last + max_delta_vx);
         state.vx(i, j) = cvx_curr;
+        vx_last  = cvx_curr;
 
-        float& wz_curr = state.wz(i, j - 1);
         float& cwz_curr = state.cwz(i, j - 1);
-        cwz_curr = std::clamp(cwz_curr, wz_curr - max_delta_wz, wz_curr + max_delta_wz);
+        cwz_curr = std::clamp(cwz_curr, wz_last - max_delta_wz, wz_last + max_delta_wz);
         state.wz(i, j) = cwz_curr;
+        wz_last = cwz_curr;
 
         if (is_holo) {
-          float& vy_curr = state.vy(i, j - 1);
           float& cvy_curr = state.cvy(i, j - 1);
-          cvy_curr = std::clamp(cvy_curr, vy_curr + min_delta_vy, vy_curr + max_delta_vy);
+          cvy_curr = std::clamp(cvy_curr, vy_last - max_delta_vy, vy_last + max_delta_vy);
           state.vy(i, j) = cvy_curr;
+          vy_last = cvy_curr;
         }
       }
     }

diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp
@@ -77,11 +77,10 @@ void Optimizer::getParams()
   getParam(s.base_constraints.vx_min, "vx_min", -0.35f);
   getParam(s.base_constraints.vy, "vy_max", 0.5f);
   getParam(s.base_constraints.wz, "wz_max", 1.9f);
-  getParam(s.base_constraints.ax_max, "ax_max", 2.0);
-  getParam(s.base_constraints.ax_min, "ax_min", -2.0);
-  getParam(s.base_constraints.ay_max, "ay_max", 0.0);
-  getParam(s.base_constraints.ay_min, "ay_min", -0.0);
-  getParam(s.base_constraints.az, "az", 3.0);
+  getParam(s.base_constraints.ax_max, "ax_max", 2.0f);
+  getParam(s.base_constraints.ax_min, "ax_min", -2.0f);
+  getParam(s.base_constraints.ay_max, "ay_max", 0.0f);
+  getParam(s.base_constraints.az, "az_max", 3.0f);
   getParam(s.sampling_std.vx, "vx_std", 0.2f);
   getParam(s.sampling_std.vy, "vy_std", 0.2f);
   getParam(s.sampling_std.wz, "wz_std", 0.4f);