[mppi] constrain velocity & curvature in predict

Author: adivardi

nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp

@@ -127,6 +127,17 @@ class MotionModel
       //   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);
+
+      state.wz.col(i) = state.wz.col(i)
+        .cwiseMax(-control_constraints_.wz)
+        .cwiseMin(control_constraints_.wz);
+
+
+      // also constrain wz base
       if (is_holo) {
         auto lower_bound_vy = (state.vy.col(i - 1) >
           0).select(state.vy.col(i - 1) + min_delta_vy,
@@ -138,8 +149,14 @@ class MotionModel
         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)
+        .cwiseMax(-control_constraints_.vy)
+        .cwiseMin(control_constraints_.vy);
       }
     }
+
+    applyConstraints(state);
   }
 
   /**
@@ -153,6 +170,7 @@ class MotionModel
    * @param control_sequence Control sequence to apply constraints to
    */
   virtual void applyConstraints(models::ControlSequence & /*control_sequence*/) {}
+  virtual void applyConstraints(models::State & /*state*/) {}
 
 protected:
   float model_dt_{0.0};
@@ -201,6 +219,28 @@ class AckermannMotionModel : public MotionModel
     }
   }
 
+  void applyConstraints(models::State & state) override
+  {
+    std::cout << "\n\t wz Before applyConstraints:\n\t\t" << state.wz(Eigen::seq(0,1000,300), Eigen::seq(0, 2)) << std::endl;
+    // auto tmp_state = state;
+    // for(unsigned int i = 0; i < state.vx.cols(); i++) {
+    //   auto wz_constrained = state.vx.col(i).abs() / min_turning_r_;
+    //   state.wz.col(i) = state.wz.col(i)
+    //     .cwiseMax(-1 * wz_constrained)
+    //     .cwiseMin(wz_constrained);
+    // }
+
+    std::cout << "\n\t wz with loop:\n\t\t" << state.wz(Eigen::seq(0,1000,300), Eigen::seq(0, 2)) << std::endl;
+
+    // vectorized version
+    const auto wz_constrained = tmp_state.vx.abs() / min_turning_r_;
+    tmp_state.wz = tmp_state.wz
+      .max((-wz_constrained))
+      .min(wz_constrained);
+
+    std::cout << "\n\t wz without loop:\n\t\t" << tmp_state.wz(Eigen::seq(0,1000,300), Eigen::seq(0, 2)) << std::endl;
+  }
+
   /**
    * @brief Get minimum turning radius of ackermann drive
    * @return Minimum turning radius