diff --git a/modules/planning/math/piecewise_jerk/piecewise_jerk_problem.cc b/modules/planning/math/piecewise_jerk/piecewise_jerk_problem.cc index 23660d5f31f6ef3d06813b7e315e92083a7eab4f..d73bdfc31d56f68e1e83acb3c4c5aea9bb011fd7 100644 --- a/modules/planning/math/piecewise_jerk/piecewise_jerk_problem.cc +++ b/modules/planning/math/piecewise_jerk/piecewise_jerk_problem.cc @@ -199,62 +199,6 @@ bool PiecewiseJerkProblem::Optimize(const int max_iter) { return true; } -//void PiecewiseJerkProblem::CalculateKernel(std::vector* P_data, -// std::vector* P_indices, -// std::vector* P_indptr) { -// const size_t n = num_of_knots_; -// const size_t num_of_variables = 3 * n; -// std::vector>> columns; -// columns.resize(num_of_variables); -// size_t value_index = 0; -// -// // x(i)^2 * (w_x + w_ref) -// for (size_t i = 0; i < n; ++i) { -// columns[i].emplace_back(i, weight_x_ + weight_x_reference_); -// ++value_index; -// } -// -// // x(i)'^2 * w_dx -// for (size_t i = 0; i < n; ++i) { -// columns[n + i].emplace_back(n + i, weight_dx_); -// ++value_index; -// } -// -// auto delta_s_square = delta_s_ * delta_s_; -// // 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); -// ++value_index; -// for (size_t i = 1; i + 1 < n; ++i) { -// columns[2 * n + i].emplace_back( -// 2 * n + i, weight_ddx_ + 2.0 * weight_dddx_ / delta_s_square); -// ++value_index; -// } -// columns[3 * n - 1].emplace_back( -// 3 * n - 1, weight_ddx_ + weight_dddx_ / delta_s_square); -// ++value_index; -// -// // -2 * w_dddx / delta_s^2 * x(i)'' * x(i + 1)'' -// for (size_t i = 0; i + 1 < n; ++i) { -// columns[2 * n + i].emplace_back( -// 2 * n + i + 1, -2.0 * weight_dddx_ / delta_s_square); -// ++value_index; -// } -// -// CHECK_EQ(value_index, num_of_variables + n - 1); -// -// size_t ind_p = 0; -// for (size_t i = 0; i < num_of_variables; ++i) { -// P_indptr->push_back(ind_p); -// for (const auto& row_data_pair : columns[i]) { -// P_indices->push_back(row_data_pair.first); -// P_data->push_back(row_data_pair.second * 2.0); -// ++ind_p; -// } -// } -// P_indptr->push_back(ind_p); -//} - void PiecewiseJerkProblem::CalculateAffineConstraint( std::vector* A_data, std::vector* A_indices, std::vector* A_indptr, std::vector* lower_bounds, @@ -353,18 +297,5 @@ void PiecewiseJerkProblem::CalculateAffineConstraint( A_indptr->push_back(ind_p); } -//void PiecewiseJerkProblem::CalculateOffset(std::vector* q) { -// CHECK_NOTNULL(q); -// const size_t N = num_of_knots_; -// q->resize(3 * N, 0.0); -// -// for (size_t i = 0; i < N; ++i) { -// q->at(i) += -2.0 * weight_x_reference_ * x_ref_[i]; -// } -//// q->at(N - 1) += -2.0 * weight_x_ * x_end_[0]; -//// q->at(N * 2 - 1) += -2.0 * weight_dx_ * x_end_[1]; -//// q->at(N * 3 - 1) += -2.0 * weight_ddx_ * x_end_[2]; -//} - } // namespace planning } // namespace apollo diff --git a/modules/planning/math/piecewise_jerk/piecewise_jerk_problem.h b/modules/planning/math/piecewise_jerk/piecewise_jerk_problem.h index 717b22d4ca1c0ed8729051027e8649dd3ffed08b..dc93c04354acc4c46d6194dcb8dde3b477ddbb21 100644 --- a/modules/planning/math/piecewise_jerk/piecewise_jerk_problem.h +++ b/modules/planning/math/piecewise_jerk/piecewise_jerk_problem.h @@ -64,7 +64,7 @@ class PiecewiseJerkProblem { const double dx_upper_bound); - void set_ddx_bounds(std::vector> d2x_bounds); + void set_ddx_bounds(std::vector> ddx_bounds); void set_ddx_bounds(const double ddx_lower_bound, const double ddx_upper_bound);