fix(autoware_mpc_lateral_controller): delete the zero speed constraint (

autowarefoundation#7673) * delete steer rate limit when vel = 0 Signed-off-by: Zhe Shen <[email protected]> * delete unnecessary variable Signed-off-by: Zhe Shen <[email protected]> * pre-commit Signed-off-by: Zhe Shen <[email protected]> --------- Signed-off-by: Zhe Shen <[email protected]> Signed-off-by: palas21 <[email protected]>
palas21 · Jul 12, 2024 · e5d4b7f · e5d4b7f
1 parent bb92289
commit e5d4b7f
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 18 deletions.
diff --git a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp
@@ -271,7 +271,7 @@ class MPC
    */
   std::pair<bool, VectorXd> executeOptimization(
     const MPCMatrix & mpc_matrix, const VectorXd & x0, const double prediction_dt,
-    const MPCTrajectory & trajectory, const double current_velocity);
+    const MPCTrajectory & trajectory);
 
   /**
    * @brief Resample the trajectory with the MPC resampling time.
@@ -386,8 +386,7 @@ class MPC
    * @param reference_trajectory The reference trajectory.
    * @param current_velocity current velocity of ego.
    */
-  VectorXd calcSteerRateLimitOnTrajectory(
-    const MPCTrajectory & trajectory, const double current_velocity) const;
+  VectorXd calcSteerRateLimitOnTrajectory(const MPCTrajectory & trajectory) const;
 
   //!< @brief logging with warn and return false
   template <typename... Args>

diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp
@@ -76,9 +76,8 @@ bool MPC::calculateMPC(
   const auto mpc_matrix = generateMPCMatrix(mpc_resampled_ref_trajectory, prediction_dt);
 
   // solve Optimization problem
-  const auto [success_opt, Uex] = executeOptimization(
-    mpc_matrix, x0_delayed, prediction_dt, mpc_resampled_ref_trajectory,
-    current_kinematics.twist.twist.linear.x);
+  const auto [success_opt, Uex] =
+    executeOptimization(mpc_matrix, x0_delayed, prediction_dt, mpc_resampled_ref_trajectory);
   if (!success_opt) {
     return fail_warn_throttle("optimization failed. Stop MPC.");
   }
@@ -544,8 +543,7 @@ MPCMatrix MPC::generateMPCMatrix(
  * [    -au_lim * dt    ] < [uN-uN-1] < [     au_lim * dt    ] (*N... DIM_U)
  */
 std::pair<bool, VectorXd> MPC::executeOptimization(
-  const MPCMatrix & m, const VectorXd & x0, const double prediction_dt, const MPCTrajectory & traj,
-  const double current_velocity)
+  const MPCMatrix & m, const VectorXd & x0, const double prediction_dt, const MPCTrajectory & traj)
 {
   VectorXd Uex;
 
@@ -578,7 +576,7 @@ std::pair<bool, VectorXd> MPC::executeOptimization(
   VectorXd ub = VectorXd::Constant(DIM_U_N, m_steer_lim);   // max steering angle
 
   // steering angle rate limit
-  VectorXd steer_rate_limits = calcSteerRateLimitOnTrajectory(traj, current_velocity);
+  VectorXd steer_rate_limits = calcSteerRateLimitOnTrajectory(traj);
   VectorXd ubA = steer_rate_limits * prediction_dt;
   VectorXd lbA = -steer_rate_limits * prediction_dt;
   ubA(0) = m_raw_steer_cmd_prev + steer_rate_limits(0) * m_ctrl_period;
@@ -730,8 +728,7 @@ double MPC::calcDesiredSteeringRate(
   return steer_rate;
 }
 
-VectorXd MPC::calcSteerRateLimitOnTrajectory(
-  const MPCTrajectory & trajectory, const double current_velocity) const
+VectorXd MPC::calcSteerRateLimitOnTrajectory(const MPCTrajectory & trajectory) const
 {
   const auto interp = [&](const auto & steer_rate_limit_map, const auto & current) {
     std::vector<double> reference, limits;
@@ -765,13 +762,6 @@ VectorXd MPC::calcSteerRateLimitOnTrajectory(
     return reference.back();
   };
 
-  // when the vehicle is stopped, no steering rate limit.
-  constexpr double steer_rate_lim = 5.0;
-  const bool is_vehicle_stopped = std::fabs(current_velocity) < 0.01;
-  if (is_vehicle_stopped) {
-    return steer_rate_lim * VectorXd::Ones(m_param.prediction_horizon);
-  }
-
   // calculate steering rate limit
   VectorXd steer_rate_limits = VectorXd::Zero(m_param.prediction_horizon);
   for (int i = 0; i < m_param.prediction_horizon; ++i) {