Apollo 11.0
自动驾驶开放平台
Public 成员函数 | Protected 成员函数 | 所有成员列表
apollo::planning::PiecewiseJerkPathProblem类 参考

#include <piecewise_jerk_path_problem.h>

类 apollo::planning::PiecewiseJerkPathProblem 继承关系图:
apollo::planning::PiecewiseJerkPathProblem 的协作图:

Public 成员函数

 PiecewiseJerkPathProblem (const size_t num_of_knots, const double delta_s, const std::array< double, 3 > &x_init)
 
virtual ~PiecewiseJerkPathProblem ()=default
 
void set_extra_constraints (const ObsCornerConstraints &extra_constraints)
 
void set_vertex_constraints (const ADCVertexConstraints &vertexs)
 
- Public 成员函数 继承自 apollo::planning::PiecewiseJerkProblem
 PiecewiseJerkProblem (const size_t num_of_knots, const double delta_s, const std::array< double, 3 > &x_init)
 
virtual ~PiecewiseJerkProblem ()=default
 
void set_x_bounds (std::vector< std::pair< double, double > > x_bounds)
 
void set_x_bounds (const double x_lower_bound, const double x_upper_bound)
 
void set_dx_bounds (std::vector< std::pair< double, double > > dx_bounds)
 
void set_dx_bounds (const double dx_lower_bound, const double dx_upper_bound)
 
void set_ddx_bounds (std::vector< std::pair< double, double > > ddx_bounds)
 
void set_ddx_bounds (const double ddx_lower_bound, const double ddx_upper_bound)
 
void set_dddx_bound (const double dddx_bound)
 
void set_dddx_bound (const double dddx_lower_bound, const double dddx_upper_bound)
 
void set_weight_x (const double weight_x)
 
void set_weight_dx (const double weight_dx)
 
void set_weight_ddx (const double weight_ddx)
 
void set_weight_dddx (const double weight_dddx)
 
void set_scale_factor (const std::array< double, 3 > &scale_factor)
 
void set_x_ref (const double weight_x_ref, std::vector< double > x_ref)
 Set the x ref object and the uniform x_ref weighting
 
void set_x_ref (std::vector< double > weight_x_ref_vec, std::vector< double > x_ref)
 Set the x ref object and piecewised x_ref weightings
 
void set_towing_x_ref (const double weight_towing_x_ref, std::vector< double > towing_x_ref)
 Set towing x ref object and piecewised towing_x_ref weightings
 
void set_towing_x_ref (std::vector< double > weight_towing_x_ref_vec, std::vector< double > towing_x_ref)
 
void set_end_state_ref (const std::array< double, 3 > &weight_end_state, const std::array< double, 3 > &end_state_ref)
 
virtual bool Optimize (const int max_iter=4000)
 
const std::vector< double > & opt_x () const
 
const std::vector< double > & opt_dx () const
 
const std::vector< double > & opt_ddx () const
 

Protected 成员函数

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
 
void CalculateAffineConstraint (std::vector< c_float > *A_data, std::vector< c_int > *A_indices, std::vector< c_int > *A_indptr, std::vector< c_float > *lower_bounds, std::vector< c_float > *upper_bounds) override
 
OSQPSettings * SolverDefaultSettings () override
 
- Protected 成员函数 继承自 apollo::planning::PiecewiseJerkProblem
bool FormulateProblem (OSQPData *data)
 
void FreeData (OSQPData *data)
 
bool CheckLowUpperBound (const std::vector< c_float > &lower, const std::vector< c_float > &upper)
 
template<typename T >
T * CopyData (const std::vector< T > &vec)
 

额外继承的成员函数

- Protected 属性 继承自 apollo::planning::PiecewiseJerkProblem
size_t num_of_knots_ = 0
 
std::vector< double > x_
 
std::vector< double > dx_
 
std::vector< double > ddx_
 
std::array< double, 3 > x_init_
 
std::array< double, 3 > scale_factor_ = {{1.0, 1.0, 1.0}}
 
std::vector< std::pair< double, double > > x_bounds_
 
std::vector< std::pair< double, double > > dx_bounds_
 
std::vector< std::pair< double, double > > ddx_bounds_
 
std::pair< double, double > dddx_bound_
 
double weight_x_ = 0.0
 
double weight_dx_ = 0.0
 
double weight_ddx_ = 0.0
 
double weight_dddx_ = 0.0
 
double delta_s_ = 1.0
 
bool has_x_ref_ = false
 
double weight_x_ref_ = 0.0
 
std::vector< double > x_ref_
 
std::vector< double > weight_x_ref_vec_
 
bool has_towing_x_ref_ = false
 
std::vector< double > towing_x_ref_
 
std::vector< double > weight_towing_x_ref_vec_
 
bool has_end_state_ref_ = false
 
std::array< double, 3 > weight_end_state_ = {{0.0, 0.0, 0.0}}
 
std::array< double, 3 > end_state_ref_
 

详细描述

在文件 piecewise_jerk_path_problem.h 第 49 行定义.

构造及析构函数说明

◆ PiecewiseJerkPathProblem()

apollo::planning::PiecewiseJerkPathProblem::PiecewiseJerkPathProblem ( const size_t  num_of_knots,
const double  delta_s,
const std::array< double, 3 > &  x_init 
)

在文件 piecewise_jerk_path_problem.cc 第 25 行定义.

28 : PiecewiseJerkProblem(num_of_knots, delta_s, x_init) {}
apollo::planning::PiecewiseJerkProblem::PiecewiseJerkProblem
PiecewiseJerkProblem(const size_t num_of_knots, const double delta_s, const std::array< double, 3 > &x_init)
Definition piecewise_jerk_problem.cc:29

◆ ~PiecewiseJerkPathProblem()

virtual apollo::planning::PiecewiseJerkPathProblem::~PiecewiseJerkPathProblem ( )
virtualdefault

成员函数说明

◆ CalculateAffineConstraint()

void apollo::planning::PiecewiseJerkPathProblem::CalculateAffineConstraint ( std::vector< c_float > *  A_data,
std::vector< c_int > *  A_indices,
std::vector< c_int > *  A_indptr,
std::vector< c_float > *  lower_bounds,
std::vector< c_float > *  upper_bounds 
)
overrideprotectedvirtual

重载 apollo::planning::PiecewiseJerkProblem .

在文件 piecewise_jerk_path_problem.cc 第 104 行定义.

107 {
108 // 3N params bounds on x, x', x''
109 // 3(N-1) constraints on x, x', x''
110 // 3 constraints on x_init_
111 const int n = static_cast<int>(num_of_knots_);
112 const int num_of_extra_constraints =
113 static_cast<int>(extra_constraints_.size());
114 const int num_of_vertex_constraints =
115 static_cast<int>(vertex_constraints_.size());
116 const int num_of_variables = 3 * n;
117 const int num_of_constraints = num_of_variables + 3 * (n - 1) + 3 +
118 num_of_extra_constraints +
119 num_of_vertex_constraints;
120 lower_bounds->resize(num_of_constraints);
121 upper_bounds->resize(num_of_constraints);
122
123 std::vector<std::vector<std::pair<c_int, c_float>>> variables(
124 num_of_variables);
125
126 int constraint_index = 0;
127 // set x, x', x'' bounds
128 for (int i = 0; i < num_of_variables; ++i) {
129 if (i < n) {
130 variables[i].emplace_back(constraint_index, 1.0);
131 lower_bounds->at(constraint_index) =
132 x_bounds_[i].first * scale_factor_[0];
133 upper_bounds->at(constraint_index) =
134 x_bounds_[i].second * scale_factor_[0];
135 } else if (i < 2 * n) {
136 variables[i].emplace_back(constraint_index, 1.0);
137
138 lower_bounds->at(constraint_index) =
139 dx_bounds_[i - n].first * scale_factor_[1];
140 upper_bounds->at(constraint_index) =
141 dx_bounds_[i - n].second * scale_factor_[1];
142 } else {
143 variables[i].emplace_back(constraint_index, 1.0);
144 lower_bounds->at(constraint_index) =
145 ddx_bounds_[i - 2 * n].first * scale_factor_[2];
146 upper_bounds->at(constraint_index) =
147 ddx_bounds_[i - 2 * n].second * scale_factor_[2];
148 }
149 ++constraint_index;
150 }
151 CHECK_EQ(constraint_index, num_of_variables);
152 for (int i = 0; i < num_of_extra_constraints; ++i) {
153 auto& left_index = extra_constraints_[i].left_index;
154 auto& right_index = extra_constraints_[i].right_index;
155 auto& left_weight = extra_constraints_[i].left_weight;
156 auto& right_weight = extra_constraints_[i].right_weight;
157
158 variables[left_index].emplace_back(constraint_index,
159 left_weight / scale_factor_[0]);
160 variables[right_index].emplace_back(constraint_index,
161 right_weight / scale_factor_[0]);
162 lower_bounds->at(constraint_index) = extra_constraints_[i].lower_bound;
163 upper_bounds->at(constraint_index) = extra_constraints_[i].upper_bound;
164 ++constraint_index;
165 }
166 for (int i = 0; i < num_of_vertex_constraints; ++i) {
167 auto& left_index = vertex_constraints_[i].left_index;
168 auto& right_index = vertex_constraints_[i].right_index;
169 auto& left_weight = vertex_constraints_[i].left_weight;
170 auto& right_weight = vertex_constraints_[i].right_weight;
171
172 variables[left_index].emplace_back(constraint_index,
173 left_weight / scale_factor_[0]);
174 variables[right_index].emplace_back(constraint_index,
175 right_weight / scale_factor_[0]);
176 variables[n + left_index].emplace_back(
177 constraint_index, vertex_constraints_.front_edge_to_center *
178 left_weight / scale_factor_[1]);
179 variables[n + right_index].emplace_back(
180 constraint_index, vertex_constraints_.front_edge_to_center *
181 right_weight / scale_factor_[1]);
182 lower_bounds->at(constraint_index) = vertex_constraints_[i].lower_bound;
183 upper_bounds->at(constraint_index) = vertex_constraints_[i].upper_bound;
184 ++constraint_index;
185 }
186 // x(i->i+1)''' = (x(i+1)'' - x(i)'') / delta_s
187 for (int i = 0; i + 1 < n; ++i) {
188 variables[2 * n + i].emplace_back(constraint_index, -1.0);
189 variables[2 * n + i + 1].emplace_back(constraint_index, 1.0);
190 lower_bounds->at(constraint_index) =
191 dddx_bound_.first * delta_s_ * scale_factor_[2];
192 upper_bounds->at(constraint_index) =
193 dddx_bound_.second * delta_s_ * scale_factor_[2];
194 ++constraint_index;
195 }
196 // x(i+1)' - x(i)' - 0.5 * delta_s * x(i)'' - 0.5 * delta_s * x(i+1)'' = 0
197 for (int i = 0; i + 1 < n; ++i) {
198 variables[n + i].emplace_back(constraint_index, -1.0 * scale_factor_[2]);
199 variables[n + i + 1].emplace_back(constraint_index, 1.0 * scale_factor_[2]);
200 variables[2 * n + i].emplace_back(constraint_index,
201 -0.5 * delta_s_ * scale_factor_[1]);
202 variables[2 * n + i + 1].emplace_back(constraint_index,
203 -0.5 * delta_s_ * scale_factor_[1]);
204 lower_bounds->at(constraint_index) = 0.0;
205 upper_bounds->at(constraint_index) = 0.0;
206 ++constraint_index;
207 }
208 // x(i+1) - x(i) - delta_s * x(i)'
209 // - 1/3 * delta_s^2 * x(i)'' - 1/6 * delta_s^2 * x(i+1)''
210 auto delta_s_sq_ = delta_s_ * delta_s_;
211 for (int i = 0; i + 1 < n; ++i) {
212 variables[i].emplace_back(constraint_index,
213 -1.0 * scale_factor_[1] * scale_factor_[2]);
214 variables[i + 1].emplace_back(constraint_index,
215 1.0 * scale_factor_[1] * scale_factor_[2]);
216 variables[n + i].emplace_back(
217 constraint_index, -delta_s_ * scale_factor_[0] * scale_factor_[2]);
218 variables[2 * n + i].emplace_back(
219 constraint_index,
220 -delta_s_sq_ / 3.0 * scale_factor_[0] * scale_factor_[1]);
221 variables[2 * n + i + 1].emplace_back(
222 constraint_index,
223 -delta_s_sq_ / 6.0 * scale_factor_[0] * scale_factor_[1]);
224
225 lower_bounds->at(constraint_index) = 0.0;
226 upper_bounds->at(constraint_index) = 0.0;
227 ++constraint_index;
228 }
229 // constrain on x_init
230 variables[0].emplace_back(constraint_index, 1.0);
231 lower_bounds->at(constraint_index) = x_init_[0] * scale_factor_[0];
232 upper_bounds->at(constraint_index) = x_init_[0] * scale_factor_[0];
233 ++constraint_index;
234
235 variables[n].emplace_back(constraint_index, 1.0);
236 lower_bounds->at(constraint_index) = x_init_[1] * scale_factor_[1];
237 upper_bounds->at(constraint_index) = x_init_[1] * scale_factor_[1];
238 ++constraint_index;
239
240 variables[2 * n].emplace_back(constraint_index, 1.0);
241 lower_bounds->at(constraint_index) = x_init_[2] * scale_factor_[2];
242 upper_bounds->at(constraint_index) = x_init_[2] * scale_factor_[2];
243 ++constraint_index;
244
245 CHECK_EQ(constraint_index, num_of_constraints);
246 int ind_p = 0;
247 for (int i = 0; i < num_of_variables; ++i) {
248 A_indptr->push_back(ind_p);
249 for (const auto& variable_nz : variables[i]) {
250 // coefficient
251 A_data->push_back(variable_nz.second);
252
253 // constraint index
254 A_indices->push_back(variable_nz.first);
255 ++ind_p;
256 }
257 }
258 // We indeed need this line because of
259 // https://github.com/oxfordcontrol/osqp/blob/master/src/cs.c#L255
260 A_indptr->push_back(ind_p);
261}
apollo::planning::PiecewiseJerkProblem::x_bounds_
std::vector< std::pair< double, double > > x_bounds_
Definition piecewise_jerk_problem.h:172
apollo::planning::PiecewiseJerkProblem::ddx_bounds_
std::vector< std::pair< double, double > > ddx_bounds_
Definition piecewise_jerk_problem.h:174
apollo::planning::PiecewiseJerkProblem::dx_bounds_
std::vector< std::pair< double, double > > dx_bounds_
Definition piecewise_jerk_problem.h:173

◆ CalculateKernel()

void apollo::planning::PiecewiseJerkPathProblem::CalculateKernel ( std::vector< c_float > *  P_data,
std::vector< c_int > *  P_indices,
std::vector< c_int > *  P_indptr 
)
overrideprotectedvirtual

实现了 apollo::planning::PiecewiseJerkProblem.

在文件 piecewise_jerk_path_problem.cc 第 30 行定义.

32 {
33 const int n = static_cast<int>(num_of_knots_);
34 const int num_of_variables = 3 * n;
35 const int num_of_nonzeros = num_of_variables + (n - 1);
36 std::vector<std::vector<std::pair<c_int, c_float>>> columns(num_of_variables);
37 int value_index = 0;
38
39 // x(i)^2 * (w_x + w_x_ref[i]), w_x_ref might be a uniform value for all x(i)
40 // or piecewise values for different x(i)
41 for (int i = 0; i < n - 1; ++i) {
42 columns[i].emplace_back(i, (weight_x_ + weight_x_ref_vec_[i]) /
43 (scale_factor_[0] * scale_factor_[0]));
44 ++value_index;
45 }
46 // x(n-1)^2 * (w_x + w_x_ref[n-1] + w_end_x)
47 columns[n - 1].emplace_back(
48 n - 1, (weight_x_ + weight_x_ref_vec_[n - 1] + weight_end_state_[0]) /
49 (scale_factor_[0] * scale_factor_[0]));
50 ++value_index;
51
52 // x(i)'^2 * w_dx
53 for (int i = 0; i < n - 1; ++i) {
54 columns[n + i].emplace_back(
55 n + i, weight_dx_ / (scale_factor_[1] * scale_factor_[1]));
56 ++value_index;
57 }
58 // x(n-1)'^2 * (w_dx + w_end_dx)
59 columns[2 * n - 1].emplace_back(2 * n - 1,
60 (weight_dx_ + weight_end_state_[1]) /
61 (scale_factor_[1] * scale_factor_[1]));
62 ++value_index;
63
64 auto delta_s_square = delta_s_ * delta_s_;
65 // x(i)''^2 * (w_ddx + 2 * w_dddx / delta_s^2)
66 columns[2 * n].emplace_back(2 * n,
67 (weight_ddx_ + weight_dddx_ / delta_s_square) /
68 (scale_factor_[2] * scale_factor_[2]));
69 ++value_index;
70
71 for (int i = 0; i < n - 1; ++i) {
72 columns[2 * n + i + 1].emplace_back(
73 2 * n + i, (-weight_dddx_ / delta_s_square) /
74 (scale_factor_[2] * scale_factor_[2]));
75 ++value_index;
76 }
77
78 for (int i = 1; i < n - 1; ++i) {
79 columns[2 * n + i].emplace_back(
80 2 * n + i, (weight_ddx_ + 2.0 * weight_dddx_ / delta_s_square) /
81 (scale_factor_[2] * scale_factor_[2]));
82 ++value_index;
83 }
84 columns[3 * n - 1].emplace_back(
85 3 * n - 1,
86 (weight_ddx_ + weight_dddx_ / delta_s_square + weight_end_state_[2]) /
87 (scale_factor_[2] * scale_factor_[2]));
88 ++value_index;
89
90 CHECK_EQ(value_index, num_of_nonzeros);
91
92 int ind_p = 0;
93 for (int i = 0; i < num_of_variables; ++i) {
94 P_indptr->push_back(ind_p);
95 for (const auto& row_data_pair : columns[i]) {
96 P_data->push_back(row_data_pair.second * 2.0);
97 P_indices->push_back(row_data_pair.first);
98 ++ind_p;
99 }
100 }
101 P_indptr->push_back(ind_p);
102}

◆ CalculateOffset()

void apollo::planning::PiecewiseJerkPathProblem::CalculateOffset ( std::vector< c_float > *  q)
overrideprotectedvirtual

实现了 apollo::planning::PiecewiseJerkProblem.

在文件 piecewise_jerk_path_problem.cc 第 263 行定义.

263 {
264 CHECK_NOTNULL(q);
265 const int n = static_cast<int>(num_of_knots_);
266 const int kNumParam = 3 * n;
267 q->resize(kNumParam, 0.0);
268
269 if (has_x_ref_) {
270 for (int i = 0; i < n; ++i) {
271 q->at(i) += -2.0 * weight_x_ref_vec_.at(i) * x_ref_[i] / scale_factor_[0];
272 }
273 }
274
275 if (has_end_state_ref_) {
276 q->at(n - 1) +=
277 -2.0 * weight_end_state_[0] * end_state_ref_[0] / scale_factor_[0];
278 q->at(2 * n - 1) +=
279 -2.0 * weight_end_state_[1] * end_state_ref_[1] / scale_factor_[1];
280 q->at(3 * n - 1) +=
281 -2.0 * weight_end_state_[2] * end_state_ref_[2] / scale_factor_[2];
282 }
283
284 if (has_towing_x_ref_) {
285 for (int i = 0; i < n; ++i) {
286 q->at(i) += -2.0 * weight_towing_x_ref_vec_.at(i) * towing_x_ref_[i] /
287 scale_factor_[0];
288 }
289 }
290}

◆ set_extra_constraints()

void apollo::planning::PiecewiseJerkPathProblem::set_extra_constraints ( const ObsCornerConstraints &  extra_constraints)
inline

在文件 piecewise_jerk_path_problem.h 第 55 行定义.

55 {
56 extra_constraints_ = extra_constraints;
57 }

◆ set_vertex_constraints()

void apollo::planning::PiecewiseJerkPathProblem::set_vertex_constraints ( const ADCVertexConstraints &  vertexs)
inline

在文件 piecewise_jerk_path_problem.h 第 59 行定义.

59 {
60 vertex_constraints_ = vertexs;
61 }

◆ SolverDefaultSettings()

OSQPSettings * apollo::planning::PiecewiseJerkPathProblem::SolverDefaultSettings ( )
overrideprotectedvirtual

重载 apollo::planning::PiecewiseJerkProblem .

在文件 piecewise_jerk_path_problem.cc 第 292 行定义.

292 {
293 // Define Solver default settings
294 OSQPSettings* settings =
295 reinterpret_cast<OSQPSettings*>(c_malloc(sizeof(OSQPSettings)));
296 osqp_set_default_settings(settings);
297 settings->polish = true;
298 settings->verbose = FLAGS_enable_osqp_debug;
299 settings->scaled_termination = true;
300 settings->time_limit = FLAGS_path_speed_osqp_setting_time_limit;
301 return settings;
302}
该类的文档由以下文件生成: