[mppi] fix division by zero, clean raw pointers code (#5636)

* [mppi] fix division by zero

leading to cost values being NaNs, which then propagate through all the critics and results in NaN control_sequence. These NaNs were removed by the hard applyControlSequenceConstraints(), but replaced with ax_max & wz_max. These lead to high steering at the start of a run

Signed-off-by: Adi Vardi <adi.vardi@enway.ai>

* [mppi] clean ackermann constraints

Signed-off-by: Adi Vardi <adi.vardi@enway.ai>

* fix long line

Signed-off-by: Adi Vardi <adi.vardi@enway.ai>

---------

Signed-off-by: Adi Vardi <adi.vardi@enway.ai>

Author: adivardi

nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp

@@ -163,14 +163,10 @@ class AckermannMotionModel : public MotionModel
    */
   void applyConstraints(models::ControlSequence & control_sequence) override
   {
-    const auto vx_ptr = control_sequence.vx.data();
-    auto wz_ptr = control_sequence.wz.data();
-    int steps = control_sequence.vx.size();
-    for(int i = 0; i < steps; i++) {
-      float wz_constrained = fabs(*(vx_ptr + i) / min_turning_r_);
-      float & wz_curr = *(wz_ptr + i);
-      wz_curr = utils::clamp(-1 * wz_constrained, wz_constrained, wz_curr);
-    }
+    const auto wz_constrained = control_sequence.vx.abs() / min_turning_r_;
+    control_sequence.wz = control_sequence.wz
+      .max((-wz_constrained))
+      .min(wz_constrained);
   }
 
   /**

nav2_mppi_controller/src/critics/constraint_critic.cpp

@@ -82,8 +82,12 @@ void ConstraintCritic::score(CriticData & data)
   if (acker != nullptr) {
     auto & vx = data.state.vx;
     auto & wz = data.state.wz;
-    float min_turning_rad = acker->getMinTurningRadius();
-    auto out_of_turning_rad_motion = (min_turning_rad - (vx.abs() / wz.abs())).max(0.0f);
+    const float min_turning_rad = acker->getMinTurningRadius();
+
+    const float epsilon = 1e-6f;
+    auto wz_safe = wz.abs().max(epsilon);  // Replace small wz values to avoid division by 0
+    auto out_of_turning_rad_motion = (min_turning_rad - (vx.abs() / wz_safe)).max(0.0f);
+
     if (power_ > 1u) {
       data.costs += ((((vx - max_vel_).max(0.0f) + (min_vel_ - vx).max(0.0f) +
         out_of_turning_rad_motion) * data.model_dt).rowwise().sum().eval() *