Revert "[tmp] only constrain control_seq(0,1) in applyControlSequenceConstraints"

This reverts commit ad50f92.

Author: adivardi

nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp

@@ -211,7 +211,7 @@ class AckermannMotionModel : public MotionModel
   {
     const auto vx_ptr = control_sequence.vx.data();
     auto wz_ptr = control_sequence.wz.data();
-    int steps = 2;//control_sequence.vx.size();
+    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);

nav2_mppi_controller/src/optimizer.cpp

@@ -373,7 +373,7 @@ void Optimizer::applyControlSequenceConstraints()
     vy_last = static_cast<float>(state_.speed.linear.y);
   }
 
-  for (unsigned int i = 0; i < 2; i++) {
+  for (unsigned int i = 0; i != control_sequence_.vx.size(); i++) {
     float & vx_curr = control_sequence_.vx(i);
     vx_curr = utils::clamp(s.constraints.vx_min, s.constraints.vx_max, vx_curr);
     if(vx_last > 0) {