diff --git a/modules/planning/planning_base/math/piecewise_jerk/piecewise_jerk_problem.h b/modules/planning/planning_base/math/piecewise_jerk/piecewise_jerk_problem.h index 11653ec7a35..ef52af345f2 100644 --- a/modules/planning/planning_base/math/piecewise_jerk/piecewise_jerk_problem.h +++ b/modules/planning/planning_base/math/piecewise_jerk/piecewise_jerk_problem.h @@ -180,6 +180,7 @@ class PiecewiseJerkProblem { double weight_dddx_ = 0.0; double delta_s_ = 1.0; + double delta_t_ = 1.0; bool has_x_ref_ = false; double weight_x_ref_ = 0.0; diff --git a/modules/planning/planning_base/math/piecewise_jerk/piecewise_jerk_speed_problem.cc b/modules/planning/planning_base/math/piecewise_jerk/piecewise_jerk_speed_problem.cc index 146b4561826..8ce64149752 100644 --- a/modules/planning/planning_base/math/piecewise_jerk/piecewise_jerk_speed_problem.cc +++ b/modules/planning/planning_base/math/piecewise_jerk/piecewise_jerk_speed_problem.cc @@ -86,34 +86,33 @@ void PiecewiseJerkSpeedProblem::CalculateKernel(std::vector* P_data, (scale_factor_[1] * scale_factor_[1])); ++value_index; - auto delta_s_square = delta_s_ * delta_s_; + auto delta_t_square = delta_t_ * delta_t_; // x(i)''^2 * (w_ddx + 2 * w_dddx / delta_s^2) columns[2 * n].emplace_back(2 * n, - (weight_ddx_ + weight_dddx_ / delta_s_square) / + (weight_ddx_ + weight_dddx_ / delta_t_square) / (scale_factor_[2] * scale_factor_[2])); ++value_index; + for (int i = 0; i < n - 1; ++i) + { + columns[2 * n + i + 1].emplace_back(2 * n + i, (-weight_dddx_ / delta_t_square) / + (scale_factor_[2] * scale_factor_[2])); + ++value_index; + } + for (int i = 1; i < n - 1; ++i) { columns[2 * n + i].emplace_back( - 2 * n + i, (weight_ddx_ + 2.0 * weight_dddx_ / delta_s_square) / + 2 * n + i, (weight_ddx_ + 2.0 * weight_dddx_ / delta_t_square) / (scale_factor_[2] * scale_factor_[2])); ++value_index; } columns[3 * n - 1].emplace_back( 3 * n - 1, - (weight_ddx_ + weight_dddx_ / delta_s_square + weight_end_state_[2]) / + (weight_ddx_ + weight_dddx_ / delta_t_square + weight_end_state_[2]) / (scale_factor_[2] * scale_factor_[2])); ++value_index; - // -2 * w_dddx / delta_s^2 * x(i)'' * x(i + 1)'' - for (int i = 0; i < n - 1; ++i) { - columns[2 * n + i].emplace_back(2 * n + i + 1, - -2.0 * weight_dddx_ / delta_s_square / - (scale_factor_[2] * scale_factor_[2])); - ++value_index; - } - CHECK_EQ(value_index, kNumValue); int ind_p = 0;