Skip to content

Commit

Permalink
refactor
Browse files Browse the repository at this point in the history 
Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
  • Loading branch information
@kosuke55
kosuke55 committed Jul 6, 2022
1 parent 8fb6f2d commit 217f395
Show file tree
Hide file tree
Showing 2 changed files with 68 additions and 61 deletions.
24 changes: 14 additions & 10 deletions control/trajectory_follower/include/trajectory_follower/mpc.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -227,41 +227,45 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC
* @param [in] reference_trajectory used for linearization around reference trajectory
*/
MPCMatrix generateMPCMatrix(
const trajectory_follower::MPCTrajectory & reference_trajectory, const float64_t max_dt);
const trajectory_follower::MPCTrajectory & reference_trajectory, const float64_t predition_dt);
/**
* @brief generate MPC matrix with trajectory and vehicle model
* @param [in] mpc_matrix parameters matrix to use for optimization
* @param [in] x0 initial state vector
* @param [in] precition_dt predition deleta time
* @param [out] Uex optimized input vector
*/
bool8_t executeOptimization(
const MPCMatrix & mpc_matrix, const Eigen::VectorXd & x0, Eigen::VectorXd * Uex,
const float64_t max_dt);
const MPCMatrix & mpc_matrix, const Eigen::VectorXd & x0, const float64_t predition_dt,
Eigen::VectorXd * Uex);
/**
* @brief resample trajectory with mpc resampling time
*/
bool8_t resampleMPCTrajectoryByTime(
float64_t start_time, const trajectory_follower::MPCTrajectory & input,
trajectory_follower::MPCTrajectory * output, float64_t & max_dt,
const geometry_msgs::msg::Pose & current_pose) const;
const float64_t start_time, const float64_t prediction_dt,
const trajectory_follower::MPCTrajectory & input,
trajectory_follower::MPCTrajectory * output) const;
/**
* @brief apply velocity dynamics filter with v0 from closest index
*/
trajectory_follower::MPCTrajectory applyVelocityDynamicsFilter(
const trajectory_follower::MPCTrajectory & trajectory,
const geometry_msgs::msg::Pose & current_pose, const float64_t v0) const;
/**
* @brief get total prediction time of mpc
* @brief get prediction delta time of mpc.
* If trajectory length is shorter than min_prediction length, adjust delta time.
*/
float64_t getPredictionTime(const float64_t max_dt) const;
float64_t getPredictionDeletaTime(
const float64_t start_time, const trajectory_follower::MPCTrajectory & input,
const geometry_msgs::msg::Pose & current_pose) const;
/**
* @brief add weights related to lateral_jerk, steering_rate, steering_acc into R
*/
void addSteerWeightR(Eigen::MatrixXd * R, const float64_t max_dt) const;
void addSteerWeightR(Eigen::MatrixXd * R, const float64_t predition_dt) const;
/**
* @brief add weights related to lateral_jerk, steering_rate, steering_acc into f
*/
void addSteerWeightF(Eigen::MatrixXd * f, const float64_t max_dt) const;
void addSteerWeightF(Eigen::MatrixXd * f, const float64_t predition_dt) const;
/**
* @brief check if the matrix has invalid value
*/
Expand Down
105 changes: 54 additions & 51 deletions control/trajectory_follower/src/mpc.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,19 +67,20 @@ bool8_t MPC::calculateMPC(
/* resample ref_traj with mpc sampling time */
trajectory_follower::MPCTrajectory mpc_resampled_ref_traj;
const float64_t mpc_start_time = mpc_data.nearest_time + m_param.input_delay;
float64_t max_dt;
const float64_t prediction_dt =
getPredictionDeletaTime(mpc_start_time, reference_trajectory, current_pose);
if (!resampleMPCTrajectoryByTime(
mpc_start_time, reference_trajectory, &mpc_resampled_ref_traj, max_dt, current_pose)) {
mpc_start_time, prediction_dt, reference_trajectory, &mpc_resampled_ref_traj)) {
RCLCPP_WARN_THROTTLE(m_logger, *m_clock, 1000 /*ms*/, "trajectory resampling failed.");
return false;
}

/* generate mpc matrix : predict equation Xec = Aex * x0 + Bex * Uex + Wex */
MPCMatrix mpc_matrix = generateMPCMatrix(mpc_resampled_ref_traj, max_dt);
MPCMatrix mpc_matrix = generateMPCMatrix(mpc_resampled_ref_traj, prediction_dt);

/* solve quadratic optimization */
Eigen::VectorXd Uex;
if (!executeOptimization(mpc_matrix, x0, &Uex, max_dt)) {
if (!executeOptimization(mpc_matrix, x0, prediction_dt, &Uex)) {
RCLCPP_WARN_THROTTLE(m_logger, *m_clock, 1000 /*ms*/, "optimization failed.");
return false;
}
Expand All @@ -90,9 +91,8 @@ bool8_t MPC::calculateMPC(

/* set control command */
{
const auto & dt = max_dt;
ctrl_cmd.steering_tire_angle = static_cast<float>(u_filtered);
ctrl_cmd.steering_tire_rotation_rate = static_cast<float>((Uex(1) - Uex(0)) / dt);
ctrl_cmd.steering_tire_rotation_rate = static_cast<float>((Uex(1) - Uex(0)) / prediction_dt);
}

storeSteerCmd(u_filtered);
Expand Down Expand Up @@ -396,40 +396,13 @@ void MPC::storeSteerCmd(const float64_t steer)
}

bool8_t MPC::resampleMPCTrajectoryByTime(
float64_t ts, const trajectory_follower::MPCTrajectory & input,
trajectory_follower::MPCTrajectory * output, float64_t & max_dt,
const geometry_msgs::msg::Pose & current_pose) const
const float64_t ts, const float64_t prediction_dt,
const trajectory_follower::MPCTrajectory & input,
trajectory_follower::MPCTrajectory * output) const
{
size_t nearest_idx =
static_cast<size_t>(trajectory_follower::MPCUtils::calcNearestIndex(input, current_pose));
float64_t sum_dist = 0;
const float64_t target_time = [&]() {
const float64_t t_ext =
100.0; // extra time to prevent mpc calculation failure due to short time
for (size_t i = nearest_idx + 1; i < input.relative_time.size(); i++) {
const float64_t segment_dist =
std::hypot(input.x.at(i) - input.x.at(i - 1), input.y.at(i) - input.y.at(i - 1));
sum_dist += segment_dist;
if (m_param.min_prediction_length < sum_dist) {
const float64_t prev_sum_dist = sum_dist - segment_dist;
const float64_t ratio = (m_param.min_prediction_length - prev_sum_dist) / segment_dist;
const float64_t relative_time_at_i = i == input.relative_time.size() - 1
? input.relative_time.at(i) - t_ext
: input.relative_time.at(i);
return input.relative_time.at(i - 1) +
(relative_time_at_i - input.relative_time.at(i - 1)) * ratio;
}
}
return input.relative_time.back() - t_ext;
}();

const float64_t dt =
(target_time - ts) / (static_cast<float64_t>(m_param.prediction_horizon) - 1.0);
max_dt = std::max(dt, m_param.prediction_dt);

std::vector<float64_t> mpc_time_v;
for (float64_t i = 0; i < static_cast<float64_t>(m_param.prediction_horizon); ++i) {
mpc_time_v.push_back(ts + i * max_dt);
mpc_time_v.push_back(ts + i * prediction_dt);
}
if (!trajectory_follower::MPCUtils::linearInterpMPCTrajectory(
input.relative_time, input, mpc_time_v, output)) {
Expand Down Expand Up @@ -540,12 +513,12 @@ trajectory_follower::MPCTrajectory MPC::applyVelocityDynamicsFilter(
* Qex = diag([Q,Q,...]), R1ex = diag([R,R,...])
*/
MPCMatrix MPC::generateMPCMatrix(
const trajectory_follower::MPCTrajectory & reference_trajectory, const float64_t max_dt)
const trajectory_follower::MPCTrajectory & reference_trajectory, const float64_t prediction_dt)
{
using Eigen::MatrixXd;

const int64_t N = m_param.prediction_horizon;
const float64_t DT = max_dt;
const float64_t DT = prediction_dt;
const int64_t DIM_X = m_vehicle_model_ptr->getDimX();
const int64_t DIM_U = m_vehicle_model_ptr->getDimU();
const int64_t DIM_Y = m_vehicle_model_ptr->getDimY();
Expand Down Expand Up @@ -646,7 +619,7 @@ MPCMatrix MPC::generateMPCMatrix(
m.R2ex.block(i, i, 2, 2) += J;
}

addSteerWeightR(&m.R1ex, max_dt);
addSteerWeightR(&m.R1ex, prediction_dt);

return m;
}
Expand Down Expand Up @@ -674,7 +647,8 @@ MPCMatrix MPC::generateMPCMatrix(
* [ -au_lim * dt ] < [uN-uN-1] < [ au_lim * dt ] (*N... DIM_U)
*/
bool8_t MPC::executeOptimization(
const MPCMatrix & m, const Eigen::VectorXd & x0, Eigen::VectorXd * Uex, const float64_t max_dt)
const MPCMatrix & m, const Eigen::VectorXd & x0, const float64_t prediction_dt,
Eigen::VectorXd * Uex)
{
using Eigen::MatrixXd;
using Eigen::VectorXd;
Expand All @@ -697,7 +671,7 @@ bool8_t MPC::executeOptimization(
H.triangularView<Eigen::Upper>() += m.R1ex + m.R2ex;
H.triangularView<Eigen::Lower>() = H.transpose();
MatrixXd f = (m.Cex * (m.Aex * x0 + m.Wex)).transpose() * QCB - m.Uref_ex.transpose() * m.R1ex;
addSteerWeightF(&f, max_dt);
addSteerWeightF(&f, prediction_dt);

MatrixXd A = MatrixXd::Identity(DIM_U_N, DIM_U_N);
for (int64_t i = 1; i < DIM_U_N; i++) {
Expand All @@ -706,8 +680,8 @@ bool8_t MPC::executeOptimization(

VectorXd lb = VectorXd::Constant(DIM_U_N, -m_steer_lim); // min steering angle
VectorXd ub = VectorXd::Constant(DIM_U_N, m_steer_lim); // max steering angle
VectorXd lbA = VectorXd::Constant(DIM_U_N, -m_steer_rate_lim * max_dt);
VectorXd ubA = VectorXd::Constant(DIM_U_N, m_steer_rate_lim * max_dt);
VectorXd lbA = VectorXd::Constant(DIM_U_N, -m_steer_rate_lim * prediction_dt);
VectorXd ubA = VectorXd::Constant(DIM_U_N, m_steer_rate_lim * prediction_dt);
lbA(0, 0) = m_raw_steer_cmd_prev - m_steer_rate_lim * m_ctrl_period;
ubA(0, 0) = m_raw_steer_cmd_prev + m_steer_rate_lim * m_ctrl_period;

Expand All @@ -732,10 +706,10 @@ bool8_t MPC::executeOptimization(
return true;
}

void MPC::addSteerWeightR(Eigen::MatrixXd * R_ptr, const float64_t max_dt) const
void MPC::addSteerWeightR(Eigen::MatrixXd * R_ptr, const float64_t prediction_dt) const
{
const int64_t N = m_param.prediction_horizon;
const float64_t DT = max_dt;
const float64_t DT = prediction_dt;

auto & R = *R_ptr;

Expand Down Expand Up @@ -777,13 +751,13 @@ void MPC::addSteerWeightR(Eigen::MatrixXd * R_ptr, const float64_t max_dt) const
}
}

void MPC::addSteerWeightF(Eigen::MatrixXd * f_ptr, const float64_t max_dt) const
void MPC::addSteerWeightF(Eigen::MatrixXd * f_ptr, const float64_t prediction_dt) const
{
if (f_ptr->rows() < 2) {
return;
}

const float64_t DT = max_dt;
const float64_t DT = prediction_dt;
auto & f = *f_ptr;

// steer rate for i = 0
Expand All @@ -803,10 +777,39 @@ void MPC::addSteerWeightF(Eigen::MatrixXd * f_ptr, const float64_t max_dt) const
f(0, 1) += (2.0 * m_raw_steer_cmd_prev * steer_acc_r_cp1) * 0.5;
}

[[maybe_unused]] float64_t MPC::getPredictionTime(const float64_t max_dt) const
float64_t MPC::getPredictionDeletaTime(
const float64_t start_time, const trajectory_follower::MPCTrajectory & input,
const geometry_msgs::msg::Pose & current_pose) const
{
return static_cast<float64_t>(m_param.prediction_horizon - 1) * max_dt + m_param.input_delay +
m_ctrl_period;
// Calculate the time min_prediction_length ahead from current_pose
const size_t nearest_idx =
static_cast<size_t>(trajectory_follower::MPCUtils::calcNearestIndex(input, current_pose));
float64_t sum_dist = 0;
const float64_t target_time = [&]() {
const float64_t t_ext =
100.0; // extra time to prevent mpc calculation failure due to short time
for (size_t i = nearest_idx + 1; i < input.relative_time.size(); i++) {
const float64_t segment_dist =
std::hypot(input.x.at(i) - input.x.at(i - 1), input.y.at(i) - input.y.at(i - 1));
sum_dist += segment_dist;
if (m_param.min_prediction_length < sum_dist) {
const float64_t prev_sum_dist = sum_dist - segment_dist;
const float64_t ratio = (m_param.min_prediction_length - prev_sum_dist) / segment_dist;
const float64_t relative_time_at_i = i == input.relative_time.size() - 1
? input.relative_time.at(i) - t_ext
: input.relative_time.at(i);
return input.relative_time.at(i - 1) +
(relative_time_at_i - input.relative_time.at(i - 1)) * ratio;
}
}
return input.relative_time.back() - t_ext;
}();

// Calculate deleta time for min_prediction_length
const float64_t dt =
(target_time - start_time) / (static_cast<float64_t>(m_param.prediction_horizon) - 1.0);

return std::max(dt, m_param.prediction_dt);
}

bool8_t MPC::isValid(const MPCMatrix & m) const
Expand Down

0 comments on commit 217f395

Please sign in to comment.