[tmp] only constrain before publish

Author: adivardi

nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp

@@ -89,21 +89,43 @@ 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)
+      //   .cwiseMax(lower_bound_vx)
+      //   .cwiseMin(upper_bound_vx);
+      // state.vx.col(i) = state.cvx.col(i - 1);
+
+      // state.u(i) = state.cu(i-1)
+      // => we start from current robot speed (state.u(0)) and then apply u_virt (state.cu(i-1))
+      // but we also need to constrain u_virt before applying it based on static limits (max vel & curvature) to compute
+      // u_app
+      // then state.u(i) = u_app(i-1)
+      // ==>  x_k = F(x_k-1, u_app_k-1) = F(x_k-1, g(u_virt_k-1))
+
+      // TODO 2 constrains state.u based on static limits (max vel & curvature) to compute u_app
+
+      // u_app(0) should be limited on acceleration relative to either feedback (state.u(0)) as done now
+      // TODO 3 or ideally based on last published command
+
       state.vx.col(i) = state.cvx.col(i - 1)
         .cwiseMax(lower_bound_vx)
         .cwiseMin(upper_bound_vx);
 
+      // state.cwz.col(i - 1) = 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);
+
       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);
 
       // 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);
+      // }
 
       if (is_holo) {
         auto lower_bound_vy = (state.vy.col(i - 1) >

nav2_mppi_controller/src/optimizer.cpp

@@ -237,13 +237,13 @@ void Optimizer::computeControlSequenceAccel(const models::ControlSequence& contr
     // Check if accelerations exceed constraints
     float ax = (control_sequence.vx(i) - control_sequence.vx(i - 1)) / s.model_dt;
     if (std::abs(ax) > s.constraints.ax_max + epsilon) {
-      std::cout << "Acceleration constraint violated at index " << i << ":\n";
+      std::cout << "****Acceleration constraint violated at index " << i << ":\n";
       std::cout << "vx[i-1]: " << control_sequence.vx(i - 1) << ", vx[i]: " << control_sequence.vx(i) << ", ax: " << ax << "\n";
     }
 
     float wz_accel = (control_sequence.wz(i) - control_sequence.wz(i - 1)) / s.model_dt;
     if (std::abs(wz_accel) > s.constraints.az_max + epsilon) {
-      std::cout << "Angular acceleration constraint violated at index " << i << ":\n";
+      std::cout << "***Angular acceleration constraint violated at index " << i << ":\n";
       std::cout << "wz[i-1]: " << control_sequence.wz(i - 1) << ", wz[i]: " << control_sequence.wz(i) << ", wz_accel: " << wz_accel << "\n";
     }
   }
@@ -308,7 +308,12 @@ void Optimizer::shiftControlSequence()
   control_sequence_.vx(size - 1) = control_sequence_.vx(size - 2);
   control_sequence_.wz(size - 1) = control_sequence_.wz(size - 2);
 
-  if (isHolonomic()) {
+  // std::cout << "\n\t control_sequence_ After:\n\t\t" << control_sequence_.vx(Eigen::seq(0, 5)).transpose() << "\n\t\t"
+  //           << control_sequence_.wz(Eigen::seq(0, 5)).transpose() << "\n\t\t"
+  //           /*<< control_sequence_.vx(Eigen::seq(Eigen::last -5, Eigen::last)).transpose()*/<< std::endl;
+
+  if (isHolonomic())
+  {
     utils::shiftColumnsByOnePlace(control_sequence_.vy, -1);
     control_sequence_.vy(size - 1) = control_sequence_.vy(size - 2);
   }
@@ -338,17 +343,35 @@ void Optimizer::applyControlSequenceConstraints()
   float max_delta_vy = s.model_dt * s.constraints.ay_max;
   float min_delta_vy = s.model_dt * s.constraints.ay_min;
   float max_delta_wz = s.model_dt * s.constraints.az_max;
+  // --tried 1 limit ctrl_seq_(0) based on accel_limit from current robot speed (= state.vx(0,0))  (instead of in predict -> see it still accel issue)
+  // TODO 1 only constrain the published controls (u0 & u1), but keep the unconstrained sequence for warm start
+  // 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));
+
   control_sequence_.vx(0) = vx_last;
   control_sequence_.wz(0) = wz_last;
   float vy_last = 0;
   if (isHolonomic()) {
     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);
+
+  float vy_last = 0;
+  if (isHolonomic()) {
+    vy_last = static_cast<float>(state_.speed.linear.y);
+  }
 
-  for (unsigned int i = 1; i != control_sequence_.vx.size(); 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) {
@@ -359,10 +382,18 @@ void Optimizer::applyControlSequenceConstraints()
     vx_last = vx_curr;
 
     float & wz_curr = control_sequence_.wz(i);
+    if (i==0)
+    {
+      std::cout << "control_sequence_.wz(0) BEFORE: " << control_sequence_.wz(i) << std::endl;
+    }
     wz_curr = utils::clamp(-s.constraints.wz, s.constraints.wz, wz_curr);
     wz_curr = utils::clamp(wz_last - max_delta_wz, wz_last + max_delta_wz, wz_curr);
     wz_last = wz_curr;
 
+        if (i==0)
+    {
+      std::cout << "control_sequence_.wz(0) AFTER: " << control_sequence_.wz(i) << std::endl;
+    }
     if (isHolonomic()) {
       float & vy_curr = control_sequence_.vy(i);
       vy_curr = utils::clamp(-s.constraints.vy, s.constraints.vy, vy_curr);