[tmp] undo changes to predict & applyControlSequenceConstraints

Author: adivardi

nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp

@@ -119,20 +119,20 @@ class MotionModel
 
       // constrain u_virt 0 & 1 to make sure accelerations are limited at the first step (state.vx(0) is from feedback)
       // also constrain u_virt_1 as this is published as command
-      if (i <= 2)
-      {
-        state.cvx.col(i - 1) = state.vx.col(i);
-        state.cwz.col(i - 1) = state.wz.col(i);
-      }
+      // if (i <= 2)
+      // {
+      //   state.cvx.col(i - 1) = state.vx.col(i);
+      //   state.cwz.col(i - 1) = state.wz.col(i);
+      // }
 
-      // also apply velocity limits constrains
-      state.vx.col(i) = state.vx.col(i)
-        .cwiseMax(control_constraints_.vx_min)
-        .cwiseMin(control_constraints_.vx_max);
+      // // also apply velocity limits constrains
+      // state.vx.col(i) = state.vx.col(i)
+      //   .cwiseMax(control_constraints_.vx_min)
+      //   .cwiseMin(control_constraints_.vx_max);
 
-      state.wz.col(i) = state.wz.col(i)
-        .cwiseMax(-control_constraints_.wz)
-        .cwiseMin(control_constraints_.wz);
+      // state.wz.col(i) = state.wz.col(i)
+      //   .cwiseMax(-control_constraints_.wz)
+      //   .cwiseMin(control_constraints_.wz);
 
 
       // also constrain wz base
@@ -148,13 +148,13 @@ class MotionModel
           .cwiseMax(lower_bound_vy)
           .cwiseMin(upper_bound_vy);
 
-        state.vy.col(i) = state.cvy.col(i - 1)
-        .cwiseMax(-control_constraints_.vy)
-        .cwiseMin(control_constraints_.vy);
+        // state.vy.col(i) = state.cvy.col(i - 1)
+        // .cwiseMax(-control_constraints_.vy)
+        // .cwiseMin(control_constraints_.vy);
       }
     }
 
-    applyConstraints(state);
+    // applyConstraints(state);
   }
 
   /**

nav2_mppi_controller/src/optimizer.cpp

@@ -402,7 +402,6 @@ void Optimizer::applyControlSequenceConstraints()
   // TODO 4 or ideally based on last published command
 
   // at this point, control_sequence_ contains the softmax mean of state_.cu (u_virt)]
-  /*
   float vx_last = utils::clamp(s.constraints.vx_min, s.constraints.vx_max, control_sequence_.vx(0));
   float wz_last = utils::clamp(-s.constraints.wz, s.constraints.wz, control_sequence_.wz(0));
 
@@ -413,8 +412,8 @@ void Optimizer::applyControlSequenceConstraints()
     vy_last = utils::clamp(-s.constraints.vy, s.constraints.vy, control_sequence_.vy(0));
     control_sequence_.vy(0) = vy_last;
   }
-  */
 
+  /*
   // limit acceleration between current feedback speed and first control in the sequence
   float vx_last = static_cast<float>(state_.speed.linear.x);
   float wz_last = static_cast<float>(state_.speed.angular.z);
@@ -423,8 +422,9 @@ void Optimizer::applyControlSequenceConstraints()
   if (isHolonomic()) {
     vy_last = static_cast<float>(state_.speed.linear.y);
   }
+  */
 
-  for (unsigned int i = 0; i != control_sequence_.vx.size(); i++) {
+  for (unsigned int i = 1; 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) {