Reapply "Unclamp noise velocity. (ros-navigation#5266)"

This reverts commit 52a4315.

Author: adivardi

nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp

@@ -89,15 +89,13 @@ class MotionModel
         0).select(state.vx.col(i - 1) + max_delta_vx,
         state.vx.col(i - 1) - min_delta_vx);
 
-      state.cvx.col(i - 1) = state.cvx.col(i - 1)
+      state.vx.col(i) = state.cvx.col(i - 1)
         .cwiseMax(lower_bound_vx)
         .cwiseMin(upper_bound_vx);
-      state.vx.col(i) = state.cvx.col(i - 1);
 
-      state.cwz.col(i - 1) = state.cwz.col(i - 1)
+      state.wz.col(i) = state.cwz.col(i - 1)
         .cwiseMax(state.wz.col(i - 1) - max_delta_wz)
         .cwiseMin(state.wz.col(i - 1) + max_delta_wz);
-      state.wz.col(i) = state.cwz.col(i - 1);
 
       if (is_holo) {
         auto lower_bound_vy = (state.vy.col(i - 1) >
@@ -106,10 +104,10 @@ class MotionModel
         auto upper_bound_vy = (state.vy.col(i - 1) >
           0).select(state.vy.col(i - 1) + max_delta_vy,
           state.vy.col(i - 1) - min_delta_vy);
-        state.cvy.col(i - 1) = state.cvy.col(i - 1)
+
+        state.vy.col(i) = state.cvy.col(i - 1)
           .cwiseMax(lower_bound_vy)
           .cwiseMin(upper_bound_vy);
-        state.vy.col(i) = state.cvy.col(i - 1);
       }
     }
   }