Skip to content

A
apollo

Pinoxchio / apollo
与 Fork 源项目一致

从无法访问的项目Fork

通知 2
Star 0
Fork 0
A
apollo

体验新版 GitCode，发现更多精彩内容 >>

提交 83bed011 编写于 作者: Y Yajia Zhang 提交者： Qi Luo
浏览文件
操作

planning: code refactor for PiecewiseJerkProblem

上级 94c5fb09
隐藏空白更改
内联 并排
Showing with 377 addition and 418 deletion
modules/planning/common/BUILD
浏览文件 @ 83bed011
......@@ -252,7 +252,7 @@ cc_library(
"//modules/planning/math/curve1d:polynomial_curve1d",
"//modules/planning/math/curve1d:quartic_polynomial_curve1d",
"//modules/planning/math/curve1d:quintic_polynomial_curve1d",
"//modules/planning/math/piecewise_jerk:path_time_qp_problem",
"//modules/planning/math/piecewise_jerk:piecewise_jerk_speed_problem",
"//modules/planning/proto:planning_config_proto",
],
)
......
modules/planning/common/speed_profile_generator.cc
浏览文件 @ 83bed011
......@@ -24,11 +24,11 @@
#include <limits>
#include <memory>
#include "../math/piecewise_jerk/piecewise_jerk_speed_problem.h"
#include "cyber/common/log.h"
#include "modules/planning/common/ego_info.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/math/piecewise_jerk/path_time_qp_problem.h"
namespace apollo {
namespace planning {
......@@ -149,39 +149,45 @@ SpeedData SpeedProfileGenerator::GenerateFallbackSpeed(
// TODO(all): dt is too small;
double delta_t = FLAGS_fallback_time_unit;
double total_time = FLAGS_fallback_total_time;
int num_of_knots = static_cast<int>(total_time / delta_t) + 1;
// Start a PathTimeQpProblem
std::unique_ptr<PathTimeQpProblem> path_time_qp(new PathTimeQpProblem());
path_time_qp->InitProblem(num_of_knots, delta_t, w, init_s, end_s);
size_t num_of_knots = static_cast<size_t>(total_time / delta_t) + 1;
path_time_qp->SetZeroOrderBounds(0.0, std::fmax(stop_distance, 100.0));
path_time_qp->SetFirstOrderBounds(
PiecewiseJerkSpeedProblem piecewise_jerk_problem(
num_of_knots, delta_t, init_s, end_s);
piecewise_jerk_problem.set_weight_x(w[0]);
piecewise_jerk_problem.set_weight_dx(w[1]);
piecewise_jerk_problem.set_weight_ddx(w[2]);
piecewise_jerk_problem.set_weight_dddx(w[3]);
piecewise_jerk_problem.set_weight_x_reference(w[4]);
piecewise_jerk_problem.SetZeroOrderBounds(0.0, std::fmax(stop_distance, 100.0));
piecewise_jerk_problem.SetFirstOrderBounds(
0.0, std::fmax(FLAGS_planning_upper_speed_limit, init_v));
path_time_qp->SetSecondOrderBounds(veh_param.max_deceleration(),
piecewise_jerk_problem.SetSecondOrderBounds(veh_param.max_deceleration(),
veh_param.max_acceleration());
// TODO(Hongyi): Set back to vehicle_params when ready
path_time_qp->SetSecondOrderBounds(-4.4, 2.0);
path_time_qp->SetThirdOrderBound(FLAGS_longitudinal_jerk_bound);
piecewise_jerk_problem.SetSecondOrderBounds(-4.4, 2.0);
piecewise_jerk_problem.SetThirdOrderBound(FLAGS_longitudinal_jerk_bound);
// Solve the problem
if (!path_time_qp->Optimize()) {
if (!piecewise_jerk_problem.Optimize()) {
AERROR << "Piecewise jerk fallback speed optimizer failed!";
return GenerateStopProfile(init_v, init_a);
}
// Extract output
const std::vector<double>& s = path_time_qp->x();
const std::vector<double>& ds = path_time_qp->x_derivative();
const std::vector<double>& dds = path_time_qp->x_second_order_derivative();
const std::vector<double>& s = piecewise_jerk_problem.x();
const std::vector<double>& ds = piecewise_jerk_problem.x_derivative();
const std::vector<double>& dds = piecewise_jerk_problem.x_second_order_derivative();
SpeedData speed_data;
speed_data.AppendSpeedPoint(s[0], 0.0, ds[0], dds[0], 0.0);
for (int i = 1; i < num_of_knots; ++i) {
for (size_t i = 1; i < num_of_knots; ++i) {
// Avoid the very last points when already stopped
if (s[i] - s[i - 1] <= 0.0 || ds[i] <= 0.0) {
break;
}
speed_data.AppendSpeedPoint(s[i], delta_t * i, ds[i], dds[i],
speed_data.AppendSpeedPoint(s[i], delta_t * static_cast<double>(i), ds[i], dds[i],
(dds[i] - dds[i - 1]) / delta_t);
}
FillEnoughSpeedPoints(&speed_data);
......
modules/planning/math/piecewise_jerk/BUILD
浏览文件 @ 83bed011
......@@ -17,26 +17,13 @@ cc_library(
],
)
cc_test(
name = "piecewise_jerk_problem_test",
size = "small",
srcs = [
"piecewise_jerk_problem_test.cc",
],
deps = [
":piecewise_jerk_problem",
"//cyber/common:log",
"@gtest//:main",
],
)
cc_library(
name = "fem_1d_qp_problem",
name = "piecewise_jerk_path_problem",
srcs = [
"fem_1d_qp_problem.cc",
"piecewise_jerk_path_problem.cc",
],
hdrs = [
"fem_1d_qp_problem.h",
"piecewise_jerk_path_problem.h",
],
deps = [
":piecewise_jerk_problem",
......@@ -44,12 +31,12 @@ cc_library(
)
cc_library(
name = "path_time_qp_problem",
name = "piecewise_jerk_speed_problem",
srcs = [
"path_time_qp_problem.cc",
"piecewise_jerk_speed_problem.cc",
],
hdrs = [
"path_time_qp_problem.h",
"piecewise_jerk_speed_problem.h",
],
deps = [
":piecewise_jerk_problem",
......
modules/planning/math/piecewise_jerk/fem_1d_qp_problem.cc → modules/planning/math/piecewise_jerk/piecewise_jerk_path_problem.cc
浏览文件 @ 83bed011
......@@ -14,59 +14,68 @@
* limitations under the License.
*****************************************************************************/
#include "modules/planning/math/piecewise_jerk/fem_1d_qp_problem.h"
#include <algorithm>
#include "cyber/common/log.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/math/piecewise_jerk/piecewise_jerk_path_problem.h"
namespace apollo {
namespace planning {
void Fem1dQpProblem::CalculateKernel(std::vector<c_float>* P_data,
PiecewiseJerkPathProblem::PiecewiseJerkPathProblem(const size_t num_of_knots,
const double delta_s, const std::array<double, 3>& x_init) :
PiecewiseJerkProblem(num_of_knots, delta_s, x_init) {}
void PiecewiseJerkPathProblem::SetZeroOrderReference(std::vector<double> x_ref) {
CHECK_EQ(x_ref.size(), num_of_knots_);
x_ref_ = std::move(x_ref);
}
void PiecewiseJerkPathProblem::CalculateKernel(std::vector<c_float>* P_data,
std::vector<c_int>* P_indices,
std::vector<c_int>* P_indptr) {
const int N = static_cast<int>(num_of_knots_);
const int kNumParam = 3 * N;
const int kNumValue = kNumParam + (N - 1);
const int n = static_cast<int>(num_of_knots_);
const int kNumParam = 3 * n;
const int kNumValue = kNumParam + (n - 1);
std::vector<std::vector<std::pair<c_int, c_float>>> columns;
columns.resize(kNumParam);
int value_index = 0;
// x(i)^2 * (w_x + w_ref)
for (int i = 0; i < N; ++i) {
columns[i].emplace_back(i, (weight_.x_w + weight_.x_ref_w));
for (int i = 0; i < n; ++i) {
columns[i].emplace_back(i, (weight_x_ + weight_x_reference_));
++value_index;
}
// x(i)'^2 * w_dx
for (int i = 0; i < N; ++i) {
columns[N + i].emplace_back(N + i, weight_.x_derivative_w);
for (int 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_.x_second_order_derivative_w +
weight_.x_third_order_derivative_w / delta_s_sq_);
columns[2 * n].emplace_back(
2 * n, weight_ddx_ +
weight_dddx_ / delta_s_square);
++value_index;
for (int i = 1; i < N - 1; ++i) {
columns[2 * N + i].emplace_back(
2 * N + i, weight_.x_second_order_derivative_w +
2.0 * weight_.x_third_order_derivative_w / delta_s_sq_);
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);
++value_index;
}
columns[3 * N - 1].emplace_back(
3 * N - 1, weight_.x_second_order_derivative_w +
weight_.x_third_order_derivative_w / delta_s_sq_);
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 (int i = 0; i < N - 1; ++i) {
columns[2 * N + i].emplace_back(
2 * N + i + 1, -2.0 * weight_.x_third_order_derivative_w / delta_s_sq_);
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);
++value_index;
}
......@@ -84,13 +93,13 @@ void Fem1dQpProblem::CalculateKernel(std::vector<c_float>* P_data,
P_indptr->push_back(ind_p);
}
void Fem1dQpProblem::CalculateOffset(std::vector<c_float>* q) {
void PiecewiseJerkPathProblem::CalculateOffset(std::vector<c_float>* q) {
CHECK_NOTNULL(q);
const int N = static_cast<int>(num_of_knots_);
const int kNumParam = 3 * N;
q->resize(kNumParam);
for (int i = 0; i < N; ++i) {
q->at(i) += -2.0 * weight_.x_ref_w * x_ref_[i];
q->at(i) += -2.0 * weight_x_reference_ * x_ref_[i];
}
}
......
modules/planning/math/piecewise_jerk/fem_1d_qp_problem.h → modules/planning/math/piecewise_jerk/piecewise_jerk_path_problem.h
浏览文件 @ 83bed011
......@@ -47,19 +47,29 @@ namespace planning {
* which makes the line P(start), P0, P(1) ... P(k-1) "smooth".
*/
class Fem1dQpProblem : public PiecewiseJerkProblem {
class PiecewiseJerkPathProblem : public PiecewiseJerkProblem {
public:
Fem1dQpProblem() = default;
PiecewiseJerkPathProblem(const size_t num_of_knots, const double delta_s,
const std::array<double, 3>& x_init);
virtual ~Fem1dQpProblem() = default;
virtual ~PiecewiseJerkPathProblem() = default;
void SetZeroOrderReference(std::vector<double> x_ref);
void set_weight_x_reference(const double weight_x_reference) {
weight_x_reference_ = weight_x_reference;
}
protected:
// naming convention follows osqp solver.
void CalculateKernel(std::vector<c_float>* P_data,
virtual void CalculateKernel(std::vector<c_float>* P_data,
std::vector<c_int>* P_indices,
std::vector<c_int>* P_indptr) override;
void CalculateOffset(std::vector<c_float>* q) override;
virtual void CalculateOffset(std::vector<c_float>* q) override;
double weight_x_reference_ = 0.0;
std::vector<double> x_ref_;
};
} // namespace planning
......
modules/planning/math/piecewise_jerk/piecewise_jerk_problem.cc
浏览文件 @ 83bed011
......@@ -29,15 +29,12 @@ namespace {
constexpr double kMaxVariableRange = 1.0e10;
} // namespace
void PiecewiseJerkProblem::InitProblem(const size_t num_of_knots,
const double delta_s,
const std::array<double, 3>& x_init,
const std::array<double, 3>& x_end) {
PiecewiseJerkProblem::PiecewiseJerkProblem(const size_t num_of_knots, const double delta_s,
const std::array<double, 3>& x_init) {
CHECK_GE(num_of_knots, 2);
num_of_knots_ = num_of_knots;
x_init_ = x_init;
x_end_ = x_end;
delta_s_ = delta_s;
......@@ -60,6 +57,8 @@ bool PiecewiseJerkProblem::OptimizeWithOsqp(
std::vector<c_float>& upper_bounds, // NOLINT
std::vector<c_float>& q, OSQPData* data, OSQPWorkspace** work, // NOLINT
OSQPSettings* settings) {
CHECK_EQ(lower_bounds.size(), upper_bounds.size());
data->n = kernel_dim;
data->m = num_affine_constraint;
data->P = csc_matrix(data->n, data->n, P_data.size(), P_data.data(),
......@@ -70,8 +69,6 @@ bool PiecewiseJerkProblem::OptimizeWithOsqp(
data->l = lower_bounds.data();
data->u = upper_bounds.data();
CHECK_EQ(lower_bounds.size(), upper_bounds.size());
*work = osqp_setup(data, settings);
// Solve Problem
......@@ -92,17 +89,6 @@ bool PiecewiseJerkProblem::OptimizeWithOsqp(
return true;
}
void PiecewiseJerkProblem::SetZeroOrderReference(std::vector<double> x_ref) {
CHECK_EQ(x_ref.size(), num_of_knots_);
x_ref_ = std::move(x_ref);
}
void PiecewiseJerkProblem::SetFirstOrderPenalty(
std::vector<double> penalty_dx) {
CHECK_EQ(penalty_dx.size(), num_of_knots_);
penalty_dx_ = std::move(penalty_dx);
}
void PiecewiseJerkProblem::SetZeroOrderBounds(
std::vector<std::pair<double, double>> x_bounds) {
CHECK_EQ(x_bounds.size(), num_of_knots_);
......@@ -202,8 +188,6 @@ bool PiecewiseJerkProblem::Optimize(const int max_iter) {
dx_.at(i) = work->solution->x[i + num_of_knots_];
ddx_.at(i) = work->solution->x[i + 2 * num_of_knots_];
}
dx_.back() = work->solution->x[2 * num_of_knots_ - 1];
ddx_.back() = work->solution->x[3 * num_of_knots_ - 1];
// Cleanup
osqp_cleanup(work);
......@@ -215,63 +199,61 @@ bool PiecewiseJerkProblem::Optimize(const int max_iter) {
return true;
}
void PiecewiseJerkProblem::CalculateKernel(std::vector<c_float>* P_data,
std::vector<c_int>* P_indices,
std::vector<c_int>* P_indptr) {
const int N = static_cast<int>(num_of_knots_);
const int kNumParam = 3 * N;
const int kNumValue = kNumParam + (N - 1);
std::vector<std::vector<std::pair<c_int, c_float>>> columns;
columns.resize(kNumParam);
int value_index = 0;
// x(i)^2 * (w_x + w_ref)
for (int i = 0; i < N; ++i) {
columns[i].emplace_back(i, (weight_x_ + weight_x_reference_));
++value_index;
}
// x(i)'^2 * w_dx
for (int i = 0; i < N; ++i) {
columns[N + i].emplace_back(N + i, weight_dx_);
++value_index;
}
auto delta_s_sq_ = 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_sq_);
++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_sq_);
++value_index;
}
columns[3 * N - 1].emplace_back(
3 * N - 1, weight_ddx_ + weight_dddx_ / delta_s_sq_);
++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_sq_);
++value_index;
}
CHECK_EQ(value_index, kNumValue);
int ind_p = 0;
for (int i = 0; i < kNumParam; ++i) {
P_indptr->push_back(ind_p);
for (const auto& row_data_pair : columns[i]) {
P_data->push_back(row_data_pair.second * 2.0);
P_indices->push_back(row_data_pair.first);
++ind_p;
}
}
P_indptr->push_back(ind_p);
}
//void PiecewiseJerkProblem::CalculateKernel(std::vector<c_float>* P_data,
// std::vector<c_int>* P_indices,
// std::vector<c_int>* P_indptr) {
// const size_t n = num_of_knots_;
// const size_t num_of_variables = 3 * n;
// std::vector<std::vector<std::pair<c_int, c_float>>> 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<c_float>* A_data, std::vector<c_int>* A_indices,
......@@ -371,18 +353,18 @@ void PiecewiseJerkProblem::CalculateAffineConstraint(
A_indptr->push_back(ind_p);
}
void PiecewiseJerkProblem::CalculateOffset(std::vector<c_float>* q) {
CHECK_NOTNULL(q);
const int N = static_cast<int>(num_of_knots_);
const int kNumParam = 3 * N;
q->resize(kNumParam);
for (int 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];
}
//void PiecewiseJerkProblem::CalculateOffset(std::vector<c_float>* 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
modules/planning/math/piecewise_jerk/piecewise_jerk_problem.h
浏览文件 @ 83bed011
......@@ -48,40 +48,11 @@ namespace planning {
class PiecewiseJerkProblem {
public:
PiecewiseJerkProblem() = default;
PiecewiseJerkProblem(const size_t num_of_knots, const double delta_s,
const std::array<double, 3>& x_init);
virtual ~PiecewiseJerkProblem() = default;
/*
* @param
* x_init: the init status of x, x', x''
* delta_s: s(k) - s(k-1)
* x_bounds: x_bounds[i].first < x(i) < x_bounds[i].second
* w: weight array
* -- w[0]: x^2 term weight
* -- w[1]: (x')^2 term weight
* -- w[2]: (x'')^2 term weight
* -- w[3]: (x''')^2 term weight
* -- w[4]: default reference line weight, (x_bounds[k].first +
* x_bounds[k].second)/2
*/
virtual void InitProblem(const size_t num_of_knots, const double delta_s,
const std::array<double, 3>& x_init,
const std::array<double, 3>& x_end);
virtual void SetInitCondition(const std::array<double, 3>& x_init) {
x_init_ = x_init;
}
virtual void SetEndCondition(const std::array<double, 3>& x_end) {
x_end_ = x_end;
}
void SetZeroOrderReference(std::vector<double> x_ref);
void SetFirstOrderPenalty(std::vector<double> penalty_dx);
void SetZeroOrderBounds(std::vector<std::pair<double, double>> x_bounds);
void SetZeroOrderBounds(const double x_lower_bound,
......@@ -118,10 +89,6 @@ class PiecewiseJerkProblem {
weight_dddx_ = weight_dddx;
}
void set_weight_x_reference(const double weight_x_reference) {
weight_x_reference_ = weight_x_reference;
}
virtual bool Optimize(const int max_iter = 4000);
const std::vector<double>& x() const { return x_; }
......@@ -134,9 +101,9 @@ class PiecewiseJerkProblem {
// naming convention follows osqp solver.
virtual void CalculateKernel(std::vector<c_float>* P_data,
std::vector<c_int>* P_indices,
std::vector<c_int>* P_indptr);
std::vector<c_int>* P_indptr) = 0;
virtual void CalculateOffset(std::vector<c_float>* q);
virtual void CalculateOffset(std::vector<c_float>* q) = 0;
virtual void CalculateAffineConstraint(std::vector<c_float>* A_data,
std::vector<c_int>* A_indices,
......@@ -163,9 +130,6 @@ class PiecewiseJerkProblem {
std::vector<double> ddx_;
std::array<double, 3> x_init_;
std::array<double, 3> x_end_;
std::vector<double> x_ref_;
std::vector<double> penalty_dx_;
std::vector<std::pair<double, double>> x_bounds_;
std::vector<std::pair<double, double>> dx_bounds_;
......@@ -180,8 +144,6 @@ class PiecewiseJerkProblem {
double weight_dddx_ = 0.0;
double weight_x_reference_ = 0.0;
double delta_s_ = 1.0;
};
......
modules/planning/math/piecewise_jerk/piecewise_jerk_problem_test.cc 已删除 100644 → 0
浏览文件 @ 94c5fb09
/******************************************************************************
* Copyright 2017 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
/**
* @file
**/
#include <math.h>
#include <chrono>
#include <memory>
#include "cyber/common/log.h"
#include "gtest/gtest.h"
#include "modules/planning/common/planning_gflags.h"
#define private public
#define protected public
#include "modules/planning/math/piecewise_jerk/piecewise_jerk_problem.h"
namespace apollo {
namespace planning {
TEST(PiecewiseJerkProblemTest, basic_test) {
FLAGS_enable_osqp_debug = true;
std::array<double, 3> x_init = {1.5, 0.01, 0.001};
double delta_s = 0.5;
size_t n = 400;
std::vector<std::tuple<double, double, double>> x_bounds;
for (size_t i = 0; i < n; ++i) {
if (i != 2) {
x_bounds.emplace_back(
std::make_tuple(static_cast<double>(i), -1.81, 1.95));
} else {
x_bounds.emplace_back(
std::make_tuple(static_cast<double>(i), 0.81, 1.95));
}
}
std::array<double, 5> w = {1.0, 2.0, 3.0, 4.0, 1.45};
double max_x_third_order_derivative = 1.25;
std::unique_ptr<PiecewiseJerkProblem> fem_qp(new PiecewiseJerkProblem());
fem_qp->InitProblem(n, delta_s, w, x_init);
fem_qp->SetVariableBounds(x_bounds);
fem_qp->SetFirstOrderBounds(-FLAGS_lateral_derivative_bound_default,
FLAGS_lateral_derivative_bound_default);
fem_qp->SetSecondOrderBounds(-FLAGS_lateral_derivative_bound_default,
FLAGS_lateral_derivative_bound_default);
fem_qp->SetThirdOrderBound(max_x_third_order_derivative);
auto start_time = std::chrono::system_clock::now();
EXPECT_TRUE(fem_qp->Optimize());
auto end_time = std::chrono::system_clock::now();
std::chrono::duration<double> diff = end_time - start_time;
AINFO << "qp_optimizer used time: " << diff.count() * 1000 << " ms.";
const std::vector<double> x = fem_qp->x();
AINFO << "x.size() = " << x.size();
for (size_t i = 0; i < x.size(); ++i) {
EXPECT_LE(x[i], fem_qp->x_bounds_[i].second);
EXPECT_GE(x[i], fem_qp->x_bounds_[i].first);
}
}
TEST(PiecewiseJerkProblemTest, add_bounds_test) {
FLAGS_enable_osqp_debug = false;
std::array<double, 3> x_init = {1.5, 0.01, 0.001};
double delta_s = 0.5;
size_t n = 400;
std::array<double, 5> w = {1.0, 2.0, 3.0, 4.0, 1.45};
std::unique_ptr<PiecewiseJerkProblem> fem_qp(new PiecewiseJerkProblem());
fem_qp->InitProblem(n, delta_s, w, x_init);
std::vector<std::tuple<double, double, double>> x_bounds;
for (size_t i = 10; i < 20; ++i) {
x_bounds.emplace_back(std::make_tuple(static_cast<double>(i), -1.81, 1.95));
}
fem_qp->SetVariableBounds(x_bounds);
const auto& x = fem_qp->x_bounds_;
CHECK_EQ(n, x.size());
for (size_t i = 20; i < 40; i += 2) {
EXPECT_DOUBLE_EQ(std::get<0>(x[i]), -1.81);
EXPECT_DOUBLE_EQ(std::get<1>(x[i]), 1.95);
}
}
TEST(PiecewiseJerkProblemTest, derivative_constraint_test) {
FLAGS_enable_osqp_debug = true;
std::array<double, 3> x_init = {4.5, 0.00, 0.0};
double delta_s = 0.5;
size_t n = 200;
std::vector<std::tuple<double, double, double>> x_bounds;
for (size_t i = 0; i < n; ++i) {
x_bounds.emplace_back(std::make_tuple(static_cast<double>(i), -6.0, 6.0));
}
std::array<double, 5> w = {1.0, 100.0, 1000.0, 1000.0, 0.0};
double max_x_third_order_derivative = 2.0;
std::unique_ptr<PiecewiseJerkProblem> fem_qp(new PiecewiseJerkProblem());
fem_qp->InitProblem(n, delta_s, w, x_init);
fem_qp->SetVariableBounds(x_bounds);
fem_qp->SetFirstOrderBounds(-FLAGS_lateral_derivative_bound_default,
FLAGS_lateral_derivative_bound_default);
fem_qp->SetSecondOrderBounds(-FLAGS_lateral_derivative_bound_default,
FLAGS_lateral_derivative_bound_default);
fem_qp->SetThirdOrderBound(max_x_third_order_derivative);
const double dx_max = std::sqrt(0.5) / 15.0;
std::vector<std::tuple<double, double, double>> dx_bounds;
for (size_t i = 0; i < 20; ++i) {
dx_bounds.emplace_back(
std::make_tuple(static_cast<double>(i), -dx_max, dx_max));
}
fem_qp->SetVariableDerivativeBounds(dx_bounds);
auto start_time = std::chrono::system_clock::now();
EXPECT_TRUE(fem_qp->Optimize());
auto end_time = std::chrono::system_clock::now();
std::chrono::duration<double> diff = end_time - start_time;
AINFO << "qp_optimizer used time: " << diff.count() * 1000 << " ms.";
const std::vector<double>& x = fem_qp->x();
AINFO << "x.size() = " << x.size();
for (size_t i = 0; i < x.size(); ++i) {
EXPECT_LT(x[i], fem_qp->x_bounds_[i].second);
EXPECT_GT(x[i], fem_qp->x_bounds_[i].first);
}
const std::vector<double>& dx = fem_qp->x_derivative();
AINFO << "dx.size() = " << dx.size();
for (size_t i = 0; i < dx.size(); ++i) {
EXPECT_LT(dx[i] - 1e-12, fem_qp->dx_bounds_[i].second);
EXPECT_GT(dx[i] + 1e-12, fem_qp->dx_bounds_[i].first);
}
}
} // namespace planning
} // namespace apollo
modules/planning/math/piecewise_jerk/path_time_qp_problem.cc → modules/planning/math/piecewise_jerk/piecewise_jerk_speed_problem.cc
浏览文件 @ 83bed011
......@@ -14,63 +14,80 @@
* limitations under the License.
*****************************************************************************/
#include "modules/planning/math/piecewise_jerk/path_time_qp_problem.h"
#include <algorithm>
#include "cyber/common/log.h"
#include "modules/planning/common/planning_gflags.h"
#include "piecewise_jerk_speed_problem.h"
namespace apollo {
namespace planning {
void PathTimeQpProblem::CalculateKernel(std::vector<c_float>* P_data,
PiecewiseJerkSpeedProblem::PiecewiseJerkSpeedProblem(
const size_t num_of_knots, const double delta_s,
const std::array<double, 3>& x_init, const std::array<double, 3>& x_end) :
PiecewiseJerkProblem(num_of_knots, delta_s, x_init) {
end_state_target_ = x_end;
}
void PiecewiseJerkSpeedProblem::SetZeroOrderReference(std::vector<double> x_ref) {
CHECK_EQ(x_ref.size(), num_of_knots_);
x_reference_ = std::move(x_ref);
}
void PiecewiseJerkSpeedProblem::SetFirstOrderPenalty(
std::vector<double> penalty_dx) {
CHECK_EQ(penalty_dx.size(), num_of_knots_);
penalty_dx_ = std::move(penalty_dx);
}
void PiecewiseJerkSpeedProblem::CalculateKernel(std::vector<c_float>* P_data,
std::vector<c_int>* P_indices,
std::vector<c_int>* P_indptr) {
const int N = static_cast<int>(num_of_knots_);
const int kNumParam = 3 * N;
const int kNumValue = 4 * N - 1;
const int n = static_cast<int>(num_of_knots_);
const int kNumParam = 3 * n;
const int kNumValue = 4 * n - 1;
std::vector<std::vector<std::pair<c_int, c_float>>> columns;
columns.resize(kNumParam);
int value_index = 0;
// x(i)^2 * w_ref
for (int i = 0; i < N - 1; ++i) {
columns[i].emplace_back(i, weight_.x_ref_w);
for (int i = 0; i < n - 1; ++i) {
columns[i].emplace_back(i, weight_x_reference_);
++value_index;
}
// x(n-1)^2 * (w_ref + w_x)
columns[N - 1].emplace_back(N - 1, weight_.x_ref_w + weight_.x_w);
columns[n - 1].emplace_back(n - 1, weight_x_reference_ + weight_end_x_);
++value_index;
// x(i)'^2 * w_dx
for (int i = 0; i < N; ++i) {
columns[N + i].emplace_back(
N + i, weight_.x_derivative_w * (1.0 + penalty_dx_[i]));
for (int i = 0; i < n - 1; ++i) {
columns[n + i].emplace_back(
n + i, weight_dx_ * (1.0 + penalty_dx_[i]));
++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_.x_second_order_derivative_w +
weight_.x_third_order_derivative_w / delta_s_sq_);
columns[2 * n].emplace_back(
2 * n, weight_ddx_ + weight_dddx_ / delta_s_square);
++value_index;
for (int i = 1; i < N - 1; ++i) {
columns[2 * N + i].emplace_back(
2 * N + i, weight_.x_second_order_derivative_w +
2.0 * weight_.x_third_order_derivative_w / delta_s_sq_);
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);
++value_index;
}
columns[3 * N - 1].emplace_back(
3 * N - 1, weight_.x_second_order_derivative_w +
weight_.x_third_order_derivative_w / delta_s_sq_);
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 (int i = 0; i < N - 1; ++i) {
columns[2 * N + i].emplace_back(
2 * N + i + 1, -2.0 * weight_.x_third_order_derivative_w / delta_s_sq_);
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);
++value_index;
}
......@@ -88,18 +105,19 @@ void PathTimeQpProblem::CalculateKernel(std::vector<c_float>* P_data,
P_indptr->push_back(ind_p);
}
void PathTimeQpProblem::CalculateOffset(std::vector<c_float>* q) {
void PiecewiseJerkSpeedProblem::CalculateOffset(std::vector<c_float>* q) {
CHECK_NOTNULL(q);
const int N = static_cast<int>(num_of_knots_);
const int kNumParam = 3 * N;
const int n = static_cast<int>(num_of_knots_);
const int kNumParam = 3 * n;
q->resize(kNumParam);
for (int i = 0; i < N; ++i) {
q->at(i) += -2.0 * weight_.x_ref_w * x_ref_[i];
q->at(N + i) += -2.0 * weight_.x_derivative_w * x_derivative_desire;
for (int i = 0; i < n; ++i) {
q->at(i) += -2.0 * weight_x_reference_ * x_reference_[i];
q->at(n + i) += -2.0 * weight_dx_ * dx_reference_;
}
q->at(N - 1) += -2.0 * weight_.x_w * x_end_[0];
q->at(2 * N - 1) += -2.0 * weight_.x_derivative_w * x_end_[1];
q->at(3 * N - 1) += -2.0 * weight_.x_second_order_derivative_w * x_end_[2];
q->at(n - 1) += -2.0 * weight_end_x_ * end_state_target_[0];
q->at(2 * n - 1) += -2.0 * weight_end_dx_ * end_state_target_[1];
q->at(3 * n - 1) += -2.0 * weight_end_ddx_ * end_state_target_[2];
}
} // namespace planning
......
modules/planning/math/piecewise_jerk/path_time_qp_problem.h → modules/planning/math/piecewise_jerk/piecewise_jerk_speed_problem.h
浏览文件 @ 83bed011
......@@ -46,14 +46,23 @@ namespace planning {
* which makes the line P(start), P0, P(1) ... P(k-1) "smooth".
*/
class PathTimeQpProblem : public PiecewiseJerkProblem {
class PiecewiseJerkSpeedProblem : public PiecewiseJerkProblem {
public:
PathTimeQpProblem() = default;
PiecewiseJerkSpeedProblem(const size_t num_of_knots, const double delta_s,
const std::array<double, 3>& x_init, const std::array<double, 3>& x_end);
virtual ~PathTimeQpProblem() = default;
virtual ~PiecewiseJerkSpeedProblem() = default;
void SetDesireDerivative(const double dx_desire = 0.0) {
x_derivative_desire = dx_desire;
void SetZeroOrderReference(std::vector<double> x_ref);
void SetFirstOrderReference(const double dx_desire = 0.0) {
dx_reference_ = dx_desire;
}
void SetFirstOrderPenalty(std::vector<double> penalty_dx);
void set_weight_x_reference(const double weight_x_reference) {
weight_x_reference_ = weight_x_reference;
}
protected:
......@@ -64,7 +73,21 @@ class PathTimeQpProblem : public PiecewiseJerkProblem {
void CalculateOffset(std::vector<c_float>* q) override;
double x_derivative_desire = 0.0;
double dx_reference_ = 0.0;
double weight_x_reference_ = 0.0;
std::vector<double> penalty_dx_;
std::vector<double> x_reference_;
std::array<double, 3> end_state_target_;
double weight_end_x_ = 0.0;
double weight_end_dx_ = 0.0;
double weight_end_ddx_ = 0.0;
};
} // namespace planning
......
modules/planning/open_space/coarse_trajectory_generator/BUILD
浏览文件 @ 83bed011
......@@ -76,7 +76,7 @@ cc_library(
"//modules/common/configs:vehicle_config_helper",
"//modules/planning/common:obstacle",
"//modules/planning/common:planning_gflags",
"//modules/planning/math/piecewise_jerk:path_time_qp_problem",
"//modules/planning/math/piecewise_jerk:piecewise_jerk_speed_problem",
"//modules/planning/proto:planner_open_space_config_proto",
],
)
......
modules/planning/open_space/coarse_trajectory_generator/hybrid_a_star.cc
浏览文件 @ 83bed011
......@@ -20,7 +20,7 @@
#include "modules/planning/open_space/coarse_trajectory_generator/hybrid_a_star.h"
#include "modules/planning/math/piecewise_jerk/path_time_qp_problem.h"
#include "modules/planning/math/piecewise_jerk/piecewise_jerk_speed_problem.h"
namespace apollo {
namespace planning {
......@@ -406,28 +406,35 @@ bool HybridAStar::GenerateSCurveSpeedAcceleration(HybridAStartResult* result) {
ADEBUG << "end_s: " << result->accumulated_s[x_size - 1] << " " << 0.0 << " "
<< 0.0;
std::unique_ptr<PathTimeQpProblem> path_time_qp(new PathTimeQpProblem());
path_time_qp->InitProblem(x_size, delta_t_, w, init_s, end_s);
path_time_qp->SetZeroOrderBounds(
const size_t num_of_knots = x_size - 1;
PiecewiseJerkSpeedProblem path_time_qp(num_of_knots, delta_t_, init_s, end_s);
path_time_qp.set_weight_x(w[0]);
path_time_qp.set_weight_dx(w[1]);
path_time_qp.set_weight_ddx(w[2]);
path_time_qp.set_weight_dddx(w[3]);
path_time_qp.set_weight_x_reference(w[4]);
path_time_qp.SetZeroOrderBounds(
*(std::min_element(std::begin(result->accumulated_s),
std::end(result->accumulated_s))) -
10,
*(std::max_element(std::begin(result->accumulated_s),
std::end(result->accumulated_s))) +
10);
path_time_qp->SetFirstOrderBounds(
path_time_qp.SetFirstOrderBounds(
*(std::min_element(std::begin(result->v), std::end(result->v)) - 10),
*(std::max_element(std::begin(result->v), std::end(result->v))) + 10);
// TODO(QiL): load this from configs
path_time_qp->SetSecondOrderBounds(-4.4, 10.0);
path_time_qp->SetThirdOrderBound(FLAGS_longitudinal_jerk_bound);
path_time_qp->SetDesireDerivative(0.0);
path_time_qp.SetSecondOrderBounds(-4.4, 10.0);
path_time_qp.SetThirdOrderBound(FLAGS_longitudinal_jerk_bound);
path_time_qp.SetFirstOrderReference(0.0);
path_time_qp->SetZeroOrderReference(result->accumulated_s);
path_time_qp.SetZeroOrderReference(result->accumulated_s);
// Sovle the problem
if (!path_time_qp->Optimize()) {
// Solve the problem
if (!path_time_qp.Optimize()) {
std::string msg("Piecewise jerk speed optimizer failed!");
AERROR << msg;
return false;
......@@ -436,9 +443,9 @@ bool HybridAStar::GenerateSCurveSpeedAcceleration(HybridAStartResult* result) {
// Extract output
result->v.clear();
result->accumulated_s.clear();
result->accumulated_s = path_time_qp->x();
result->v = path_time_qp->x_derivative();
result->a = path_time_qp->x_second_order_derivative();
result->accumulated_s = path_time_qp.x();
result->v = path_time_qp.x_derivative();
result->a = path_time_qp.x_second_order_derivative();
result->a.pop_back();
// load steering from phi
......
modules/planning/tasks/deciders/BUILD
浏览文件 @ 83bed011
......@@ -23,7 +23,7 @@ cc_library(
"//modules/planning/common:st_graph_data",
"//modules/planning/common/util:common_lib",
"//modules/planning/common/util:util_lib",
"//modules/planning/math/piecewise_jerk:fem_1d_qp_problem",
"//modules/planning/math/piecewise_jerk:piecewise_jerk_path_problem",
"//modules/planning/scenarios/util:scenario_util_lib",
"//modules/planning/tasks/deciders/speed_bounds_decider",
"//modules/planning/tasks/deciders/speed_bounds_decider:st_boundary_mapper",
......
modules/planning/tasks/optimizers/piecewise_jerk_path/BUILD
浏览文件 @ 83bed011
......@@ -32,7 +32,7 @@ cc_library(
"//modules/planning/math:polynomial_xd",
"//modules/planning/math/curve1d:polynomial_curve1d",
"//modules/planning/math/curve1d:quintic_polynomial_curve1d",
"//modules/planning/math/piecewise_jerk:fem_1d_qp_problem",
"//modules/planning/math/piecewise_jerk:piecewise_jerk_path_problem",
"//modules/planning/proto:planning_proto",
"//modules/planning/reference_line",
"//modules/planning/tasks/optimizers:path_optimizer",
......
modules/planning/tasks/optimizers/piecewise_jerk_path/piecewise_jerk_path_optimizer.cc
浏览文件 @ 83bed011
......@@ -25,10 +25,10 @@
#include <utility>
#include <vector>
#include "../../../math/piecewise_jerk/piecewise_jerk_path_problem.h"
#include "modules/planning/common/planning_context.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/trajectory1d/piecewise_jerk_trajectory1d.h"
#include "modules/planning/math/piecewise_jerk/fem_1d_qp_problem.h"
namespace apollo {
namespace planning {
......@@ -128,14 +128,21 @@ bool PiecewiseJerkPathOptimizer::OptimizePath(
const std::vector<std::pair<double, double>>& lat_boundaries,
const std::array<double, 5>& w, std::vector<double>* x,
std::vector<double>* dx, std::vector<double>* ddx, const int max_iter) {
std::unique_ptr<Fem1dQpProblem> fem_1d_qp(new Fem1dQpProblem());
fem_1d_qp->InitProblem(lat_boundaries.size(), delta_s, w, init_state.second,
end_state);
fem_1d_qp->SetThirdOrderBound(FLAGS_lateral_jerk_bound);
PiecewiseJerkPathProblem piecewise_jerk_problem(
lat_boundaries.size(), delta_s, init_state.second);
piecewise_jerk_problem.set_weight_x(w[0]);
piecewise_jerk_problem.set_weight_dx(w[1]);
piecewise_jerk_problem.set_weight_ddx(w[2]);
piecewise_jerk_problem.set_weight_dddx(w[3]);
// TODO(all): fillin the correct weight
piecewise_jerk_problem.set_weight_x_reference(w[4]);
piecewise_jerk_problem.SetThirdOrderBound(FLAGS_lateral_jerk_bound);
auto start_time = std::chrono::system_clock::now();
fem_1d_qp->SetZeroOrderBounds(lat_boundaries);
piecewise_jerk_problem.SetZeroOrderBounds(lat_boundaries);
double first_order_bounds = AdjustLateralDerivativeBounds(
init_state.first[1], init_state.second[1], init_state.second[2],
......@@ -145,12 +152,12 @@ bool PiecewiseJerkPathOptimizer::OptimizePath(
<< first_order_bounds;
// TODO(all): temp. disable AdjustLateralDerivativeBounds, enable later
// fem_1d_qp->SetFirstOrderBounds(-first_order_bounds, first_order_bounds);
fem_1d_qp->SetFirstOrderBounds(-FLAGS_lateral_derivative_bound_default,
piecewise_jerk_problem.SetFirstOrderBounds(-FLAGS_lateral_derivative_bound_default,
FLAGS_lateral_derivative_bound_default);
fem_1d_qp->SetSecondOrderBounds(-FLAGS_lateral_derivative_bound_default,
piecewise_jerk_problem.SetSecondOrderBounds(-FLAGS_lateral_derivative_bound_default,
FLAGS_lateral_derivative_bound_default);
bool success = fem_1d_qp->Optimize(max_iter);
bool success = piecewise_jerk_problem.Optimize(max_iter);
auto end_time = std::chrono::system_clock::now();
std::chrono::duration<double> diff = end_time - start_time;
......@@ -161,9 +168,9 @@ bool PiecewiseJerkPathOptimizer::OptimizePath(
return false;
}
*x = fem_1d_qp->x();
*dx = fem_1d_qp->x_derivative();
*ddx = fem_1d_qp->x_second_order_derivative();
*x = piecewise_jerk_problem.x();
*dx = piecewise_jerk_problem.x_derivative();
*ddx = piecewise_jerk_problem.x_second_order_derivative();
return true;
}
......
modules/planning/tasks/optimizers/piecewise_jerk_speed/BUILD
浏览文件 @ 83bed011
......@@ -15,7 +15,7 @@ cc_library(
"//modules/common/proto:error_code_proto",
"//modules/common/proto:pnc_point_proto",
"//modules/planning/common:st_graph_data",
"//modules/planning/math/piecewise_jerk:path_time_qp_problem",
"//modules/planning/math/piecewise_jerk:piecewise_jerk_speed_problem",
"//modules/planning/tasks/optimizers:speed_optimizer",
],
)
......
modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_optimizer.cc
浏览文件 @ 83bed011
......@@ -31,7 +31,7 @@
#include "modules/common/vehicle_state/vehicle_state_provider.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/st_graph_data.h"
#include "modules/planning/math/piecewise_jerk/path_time_qp_problem.h"
#include "modules/planning/math/piecewise_jerk/piecewise_jerk_speed_problem.h"
namespace apollo {
namespace planning {
......@@ -76,31 +76,31 @@ Status PiecewiseJerkSpeedOptimizer::Process(
std::array<double, 3> init_s = {0.0, st_graph_data.init_point().v(),
st_graph_data.init_point().a()};
double delta_t = 0.1;
const auto& piecewise_jerk_speed_config =
config_.piecewise_jerk_speed_config();
std::array<double, 5> w = {piecewise_jerk_speed_config.s_weight(),
piecewise_jerk_speed_config.velocity_weight(),
piecewise_jerk_speed_config.acc_weight(),
piecewise_jerk_speed_config.jerk_weight(),
piecewise_jerk_speed_config.ref_weight()};
double total_length = st_graph_data.path_length_by_conf();
double total_time = st_graph_data.total_time_by_conf();
int num_of_knots = static_cast<int>(total_time / delta_t) + 1;
// Start a PathTimeQpProblem
std::unique_ptr<PathTimeQpProblem> path_time_qp(new PathTimeQpProblem());
path_time_qp->InitProblem(num_of_knots, delta_t, w, init_s);
path_time_qp->SetZeroOrderBounds(0.0, total_length);
path_time_qp->SetFirstOrderBounds(0.0,
PiecewiseJerkSpeedProblem piecewise_jerk_problem(num_of_knots, delta_t, init_s, {0.0, 0.0, 0.0});
const auto& piecewise_jerk_speed_config =
config_.piecewise_jerk_speed_config();
piecewise_jerk_problem.set_weight_x(piecewise_jerk_speed_config.s_weight());
piecewise_jerk_problem.set_weight_dx(piecewise_jerk_speed_config.velocity_weight());
piecewise_jerk_problem.set_weight_ddx(piecewise_jerk_speed_config.acc_weight());
piecewise_jerk_problem.set_weight_dddx(piecewise_jerk_speed_config.jerk_weight());
piecewise_jerk_problem.set_weight_x_reference(piecewise_jerk_speed_config.ref_weight());
piecewise_jerk_problem.SetZeroOrderBounds(0.0, total_length);
piecewise_jerk_problem.SetFirstOrderBounds(0.0,
std::fmax(FLAGS_planning_upper_speed_limit,
st_graph_data.init_point().v()));
path_time_qp->SetSecondOrderBounds(veh_param.max_deceleration(),
piecewise_jerk_problem.SetSecondOrderBounds(veh_param.max_deceleration(),
veh_param.max_acceleration());
path_time_qp->SetThirdOrderBound(FLAGS_longitudinal_jerk_bound);
piecewise_jerk_problem.SetThirdOrderBound(FLAGS_longitudinal_jerk_bound);
// TODO(Hongyi): delete this when ready to use vehicle_params
path_time_qp->SetSecondOrderBounds(-4.4, 2.0);
path_time_qp->SetDesireDerivative(FLAGS_default_cruise_speed);
piecewise_jerk_problem.SetSecondOrderBounds(-4.4, 2.0);
piecewise_jerk_problem.SetFirstOrderReference(FLAGS_default_cruise_speed);
// Update STBoundary
std::vector<std::pair<double, double>> s_bounds;
......@@ -138,7 +138,7 @@ Status PiecewiseJerkSpeedOptimizer::Process(
}
s_bounds.emplace_back(s_lower_bound, s_upper_bound);
}
path_time_qp->SetZeroOrderBounds(std::move(s_bounds));
piecewise_jerk_problem.SetZeroOrderBounds(std::move(s_bounds));
// Update SpeedBoundary and ref_s
std::vector<double> x_ref;
......@@ -163,12 +163,12 @@ Status PiecewiseJerkSpeedOptimizer::Process(
v_upper_bound = speed_limit.GetSpeedLimitByS(path_s);
s_dot_bounds.emplace_back(v_lower_bound, std::fmax(v_upper_bound, 0.0));
}
path_time_qp->SetZeroOrderReference(x_ref);
path_time_qp->SetFirstOrderPenalty(penalty_dx);
path_time_qp->SetFirstOrderBounds(s_dot_bounds);
piecewise_jerk_problem.SetZeroOrderReference(x_ref);
piecewise_jerk_problem.SetFirstOrderPenalty(penalty_dx);
piecewise_jerk_problem.SetFirstOrderBounds(s_dot_bounds);
// Sovle the problem
if (!path_time_qp->Optimize()) {
// Solve the problem
if (!piecewise_jerk_problem.Optimize()) {
std::string msg("Piecewise jerk speed optimizer failed!");
AERROR << msg;
speed_data->clear();
......@@ -176,9 +176,9 @@ Status PiecewiseJerkSpeedOptimizer::Process(
}
// Extract output
const std::vector<double>& s = path_time_qp->x();
const std::vector<double>& ds = path_time_qp->x_derivative();
const std::vector<double>& dds = path_time_qp->x_second_order_derivative();
const std::vector<double>& s = piecewise_jerk_problem.x();
const std::vector<double>& ds = piecewise_jerk_problem.x_derivative();
const std::vector<double>& dds = piecewise_jerk_problem.x_second_order_derivative();
for (int i = 0; i < num_of_knots; ++i) {
ADEBUG << "For t[" << i * delta_t << "], s = " << s[i] << ", v = " << ds[i]
<< ", a = " << dds[i];
......
Markdown is supported
0% 或 .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑！
想要评论请 注册 或