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 index 058eb45bfaaff..902790f260e5e 100644 --- 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 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 diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp index c8e2da6daf7f4..ea97e9e6d5f39 100644 --- 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 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 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 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) {