From 303b8cb0f3e11701a3bb16d06cf94db70dca1913 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Mon, 7 Oct 2024 22:07:57 +0900 Subject: [PATCH] fix(autoware_mpc_lateral_controller): fix calculation method of predicted trajectory (#9048) * fix(vehicle_model): fix calculation method of predicted trajectory Signed-off-by: Kyoichi Sugahara --------- Signed-off-by: Kyoichi Sugahara --- .../vehicle_model_bicycle_kinematics.cpp | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp index 32d97a783627c..c0052d4ff1b3e 100644 --- a/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp +++ b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp @@ -83,29 +83,25 @@ MPCTrajectory KinematicsBicycleModel::calculatePredictedTrajectoryInWorldCoordin // create initial state in the world coordinate Eigen::VectorXd state_w = [&]() { - Eigen::VectorXd state = Eigen::VectorXd::Zero(4); + Eigen::VectorXd state = Eigen::VectorXd::Zero(3); const auto lateral_error_0 = x0(0); const auto yaw_error_0 = x0(1); state(0, 0) = t.x.at(0) - std::sin(t.yaw.at(0)) * lateral_error_0; // world-x state(1, 0) = t.y.at(0) + std::cos(t.yaw.at(0)) * lateral_error_0; // world-y state(2, 0) = t.yaw.at(0) + yaw_error_0; // world-yaw - state(3, 0) = x0(2); // steering return state; }(); // update state in the world coordinate const auto updateState = [&]( - const Eigen::VectorXd & state_w, const Eigen::MatrixXd & input, - const double dt, const double velocity) { + const Eigen::VectorXd & state_w, const double & input, const double dt, + const double velocity) { const auto yaw = state_w(2); - const auto steer = state_w(3); - const auto desired_steer = input(0); Eigen::VectorXd dstate = Eigen::VectorXd::Zero(4); dstate(0) = velocity * std::cos(yaw); dstate(1) = velocity * std::sin(yaw); - dstate(2) = velocity * std::tan(steer) / m_wheelbase; - dstate(3) = -(steer - desired_steer) / m_steer_tau; + dstate(2) = velocity * std::tan(input) / m_wheelbase; // Note: don't do "return state_w + dstate * dt", which does not work due to the lazy evaluation // in Eigen. @@ -117,7 +113,7 @@ MPCTrajectory KinematicsBicycleModel::calculatePredictedTrajectoryInWorldCoordin const auto DIM_U = getDimU(); for (size_t i = 0; i < reference_trajectory.size(); ++i) { - state_w = updateState(state_w, Uex.block(i * DIM_U, 0, DIM_U, 1), dt, t.vx.at(i)); + state_w = updateState(state_w, Uex(i * DIM_U, 0), dt, t.vx.at(i)); mpc_predicted_trajectory.push_back( state_w(0), state_w(1), t.z.at(i), state_w(2), t.vx.at(i), t.k.at(i), t.smooth_k.at(i), t.relative_time.at(i));