Skip to content

Commit

Permalink
Revert "fix(autoware_mpc_lateral_controller): delete the zero speed c…
Browse files Browse the repository at this point in the history
…onstraint (#7673)"

This reverts commit 69258bd.
  • Loading branch information
takayuki5168 committed Jul 8, 2024
1 parent f28ccd8 commit b17e42e
Show file tree
Hide file tree
Showing 2 changed files with 18 additions and 7 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -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 MPCTrajectory & trajectory, const double current_velocity);

/**
* @brief Resample the trajectory with the MPC resampling time.
Expand Down Expand Up @@ -386,7 +386,8 @@ class MPC
* @param reference_trajectory The reference trajectory.
* @param current_velocity current velocity of ego.
*/
VectorXd calcSteerRateLimitOnTrajectory(const MPCTrajectory & trajectory) const;
VectorXd calcSteerRateLimitOnTrajectory(
const MPCTrajectory & trajectory, const double current_velocity) const;

//!< @brief logging with warn and return false
template <typename... Args>
Expand Down
20 changes: 15 additions & 5 deletions control/autoware_mpc_lateral_controller/src/mpc.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -76,8 +76,9 @@ 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);
const auto [success_opt, Uex] = executeOptimization(
mpc_matrix, x0_delayed, prediction_dt, mpc_resampled_ref_trajectory,
current_kinematics.twist.twist.linear.x);
if (!success_opt) {
return fail_warn_throttle("optimization failed. Stop MPC.");
}
Expand Down Expand Up @@ -543,7 +544,8 @@ 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 MPCMatrix & m, const VectorXd & x0, const double prediction_dt, const MPCTrajectory & traj,
const double current_velocity)
{
VectorXd Uex;

Expand Down Expand Up @@ -576,7 +578,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);
VectorXd steer_rate_limits = calcSteerRateLimitOnTrajectory(traj, current_velocity);
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;
Expand Down Expand Up @@ -728,7 +730,8 @@ double MPC::calcDesiredSteeringRate(
return steer_rate;
}

VectorXd MPC::calcSteerRateLimitOnTrajectory(const MPCTrajectory & trajectory) const
VectorXd MPC::calcSteerRateLimitOnTrajectory(
const MPCTrajectory & trajectory, const double current_velocity) const
{
const auto interp = [&](const auto & steer_rate_limit_map, const auto & current) {
std::vector<double> reference, limits;
Expand Down Expand Up @@ -762,6 +765,13 @@ VectorXd MPC::calcSteerRateLimitOnTrajectory(const MPCTrajectory & trajectory) c
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) {
Expand Down

0 comments on commit b17e42e

Please sign in to comment.