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

#include <dual_variable_warm_start_problem.h>

apollo::planning::DualVariableWarmStartProblem 的协作图:

Public 成员函数

 DualVariableWarmStartProblem (const PlannerOpenSpaceConfig &planner_open_space_config)
 
virtual ~DualVariableWarmStartProblem ()=default
 
bool Solve (const size_t horizon, const double ts, const Eigen::MatrixXd &ego, const size_t obstacles_num, const Eigen::MatrixXi &obstacles_edges_num, const Eigen::MatrixXd &obstacles_A, const Eigen::MatrixXd &obstacles_b, const Eigen::MatrixXd &xWS, Eigen::MatrixXd *l_warm_up, Eigen::MatrixXd *n_warm_up, Eigen::MatrixXd *s_warm_up)
 
 DualVariableWarmStartProblem (const DualVariableWarmStartConfig &dual_variable_warm_start_config)
 

详细描述

在文件 dual_variable_warm_start_problem.h 第 36 行定义.

构造及析构函数说明

◆ DualVariableWarmStartProblem() [1/2]

apollo::planning::DualVariableWarmStartProblem::DualVariableWarmStartProblem ( const PlannerOpenSpaceConfig &  planner_open_space_config)
explicit

在文件 dual_variable_warm_start_problem.cc 第 33 行定义.

34 {
35 planner_open_space_config_ = planner_open_space_config;
36}

◆ ~DualVariableWarmStartProblem()

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

◆ DualVariableWarmStartProblem() [2/2]

apollo::planning::DualVariableWarmStartProblem::DualVariableWarmStartProblem ( const DualVariableWarmStartConfig &  dual_variable_warm_start_config)
explicit

成员函数说明

◆ Solve()

bool apollo::planning::DualVariableWarmStartProblem::Solve ( const size_t  horizon,
const double  ts,
const Eigen::MatrixXd &  ego,
const size_t  obstacles_num,
const Eigen::MatrixXi &  obstacles_edges_num,
const Eigen::MatrixXd &  obstacles_A,
const Eigen::MatrixXd &  obstacles_b,
const Eigen::MatrixXd &  xWS,
Eigen::MatrixXd *  l_warm_up,
Eigen::MatrixXd *  n_warm_up,
Eigen::MatrixXd *  s_warm_up 
)

在文件 dual_variable_warm_start_problem.cc 第 38 行定义.

43 {
44 PERF_BLOCK_START()
45 bool solver_flag = false;
46
47 if (planner_open_space_config_.dual_variable_warm_start_config()
48 .qp_format() == OSQP) {
49 DualVariableWarmStartOSQPInterface ptop =
50 DualVariableWarmStartOSQPInterface(
51 horizon, ts, ego, obstacles_edges_num, obstacles_num, obstacles_A,
52 obstacles_b, xWS, planner_open_space_config_);
53
54 if (ptop.optimize()) {
55 ADEBUG << "dual warm up done.";
56 ptop.get_optimization_results(l_warm_up, n_warm_up);
57
58 PERF_BLOCK_END("DualVariableWarmStartSolving");
59 solver_flag = true;
60 } else {
61 AWARN << "dual warm up fail.";
62 ptop.get_optimization_results(l_warm_up, n_warm_up);
63 solver_flag = false;
64 }
65 } else if (planner_open_space_config_.dual_variable_warm_start_config()
66 .qp_format() == SLACKQP) {
67 DualVariableWarmStartSlackOSQPInterface ptop =
68 DualVariableWarmStartSlackOSQPInterface(
69 horizon, ts, ego, obstacles_edges_num, obstacles_num, obstacles_A,
70 obstacles_b, xWS, planner_open_space_config_);
71
72 if (ptop.optimize()) {
73 ADEBUG << "dual warm up done.";
74 ptop.get_optimization_results(l_warm_up, n_warm_up, s_warm_up);
75
76 PERF_BLOCK_END("DualVariableWarmStartSolving");
77 solver_flag = true;
78 } else {
79 AWARN << "dual warm up fail.";
80 ptop.get_optimization_results(l_warm_up, n_warm_up, s_warm_up);
81 solver_flag = false;
82 }
83 } else if (planner_open_space_config_.dual_variable_warm_start_config()
84 .qp_format() == IPOPTQP) {
85 DualVariableWarmStartIPOPTQPInterface* ptop =
86 new DualVariableWarmStartIPOPTQPInterface(
87 horizon, ts, ego, obstacles_edges_num, obstacles_num, obstacles_A,
88 obstacles_b, xWS, planner_open_space_config_);
89
90 Ipopt::SmartPtr<Ipopt::TNLP> problem = ptop;
91 // Create an instance of the IpoptApplication
92 Ipopt::SmartPtr<Ipopt::IpoptApplication> app = IpoptApplicationFactory();
93
94 app->Options()->SetIntegerValue(
95 "print_level",
96 planner_open_space_config_.dual_variable_warm_start_config()
97 .ipopt_config()
98 .ipopt_print_level());
99 app->Options()->SetIntegerValue(
100 "mumps_mem_percent",
101 planner_open_space_config_.dual_variable_warm_start_config()
102 .ipopt_config()
103 .mumps_mem_percent());
104 app->Options()->SetNumericValue(
105 "mumps_pivtol",
106 planner_open_space_config_.dual_variable_warm_start_config()
107 .ipopt_config()
108 .mumps_pivtol());
109 app->Options()->SetIntegerValue(
110 "max_iter", planner_open_space_config_.dual_variable_warm_start_config()
111 .ipopt_config()
112 .ipopt_max_iter());
113 app->Options()->SetNumericValue(
114 "tol", planner_open_space_config_.dual_variable_warm_start_config()
115 .ipopt_config()
116 .ipopt_tol());
117 app->Options()->SetNumericValue(
118 "acceptable_constr_viol_tol",
119 planner_open_space_config_.dual_variable_warm_start_config()
120 .ipopt_config()
121 .ipopt_acceptable_constr_viol_tol());
122 app->Options()->SetNumericValue(
123 "min_hessian_perturbation",
124 planner_open_space_config_.dual_variable_warm_start_config()
125 .ipopt_config()
126 .ipopt_min_hessian_perturbation());
127 app->Options()->SetNumericValue(
128 "jacobian_regularization_value",
129 planner_open_space_config_.dual_variable_warm_start_config()
130 .ipopt_config()
131 .ipopt_jacobian_regularization_value());
132 app->Options()->SetStringValue(
133 "print_timing_statistics",
134 planner_open_space_config_.dual_variable_warm_start_config()
135 .ipopt_config()
136 .ipopt_print_timing_statistics());
137 app->Options()->SetStringValue(
138 "alpha_for_y",
139 planner_open_space_config_.dual_variable_warm_start_config()
140 .ipopt_config()
141 .ipopt_alpha_for_y());
142 app->Options()->SetStringValue(
143 "recalc_y", planner_open_space_config_.dual_variable_warm_start_config()
144 .ipopt_config()
145 .ipopt_recalc_y());
146 // for qp problem speed up
147 app->Options()->SetStringValue("mehrotra_algorithm", "yes");
148
149 Ipopt::ApplicationReturnStatus status = app->Initialize();
150 if (status != Ipopt::Solve_Succeeded) {
151 AERROR << "*** Dual variable wart start problem error during "
152 "initialization!";
153 return false;
154 }
155
156 status = app->OptimizeTNLP(problem);
157
158 if (status == Ipopt::Solve_Succeeded ||
159 status == Ipopt::Solved_To_Acceptable_Level) {
160 // Retrieve some statistics about the solve
161 Ipopt::Index iter_count = app->Statistics()->IterationCount();
162 ADEBUG << "*** The problem solved in " << iter_count << " iterations!";
163
164 Ipopt::Number final_obj = app->Statistics()->FinalObjective();
165 ADEBUG << "*** The final value of the objective function is " << final_obj
166 << '.';
167 PERF_BLOCK_END("DualVariableWarmStartSolving");
168 } else {
169 ADEBUG << "Solve not succeeding, return status: " << int(status);
170 }
171
172 ptop->get_optimization_results(l_warm_up, n_warm_up);
173
174 solver_flag = (status == Ipopt::Solve_Succeeded ||
175 status == Ipopt::Solved_To_Acceptable_Level);
176 } else if (planner_open_space_config_.dual_variable_warm_start_config()
177 .qp_format() == IPOPT) {
178 DualVariableWarmStartIPOPTInterface* ptop =
179 new DualVariableWarmStartIPOPTInterface(
180 horizon, ts, ego, obstacles_edges_num, obstacles_num, obstacles_A,
181 obstacles_b, xWS, planner_open_space_config_);
182
183 Ipopt::SmartPtr<Ipopt::TNLP> problem = ptop;
184 // Create an instance of the IpoptApplication
185 Ipopt::SmartPtr<Ipopt::IpoptApplication> app = IpoptApplicationFactory();
186
187 app->Options()->SetIntegerValue(
188 "print_level",
189 planner_open_space_config_.dual_variable_warm_start_config()
190 .ipopt_config()
191 .ipopt_print_level());
192 app->Options()->SetIntegerValue(
193 "mumps_mem_percent",
194 planner_open_space_config_.dual_variable_warm_start_config()
195 .ipopt_config()
196 .mumps_mem_percent());
197 app->Options()->SetNumericValue(
198 "mumps_pivtol",
199 planner_open_space_config_.dual_variable_warm_start_config()
200 .ipopt_config()
201 .mumps_pivtol());
202 app->Options()->SetIntegerValue(
203 "max_iter", planner_open_space_config_.dual_variable_warm_start_config()
204 .ipopt_config()
205 .ipopt_max_iter());
206 app->Options()->SetNumericValue(
207 "tol", planner_open_space_config_.dual_variable_warm_start_config()
208 .ipopt_config()
209 .ipopt_tol());
210 app->Options()->SetNumericValue(
211 "acceptable_constr_viol_tol",
212 planner_open_space_config_.dual_variable_warm_start_config()
213 .ipopt_config()
214 .ipopt_acceptable_constr_viol_tol());
215 app->Options()->SetNumericValue(
216 "min_hessian_perturbation",
217 planner_open_space_config_.dual_variable_warm_start_config()
218 .ipopt_config()
219 .ipopt_min_hessian_perturbation());
220 app->Options()->SetNumericValue(
221 "jacobian_regularization_value",
222 planner_open_space_config_.dual_variable_warm_start_config()
223 .ipopt_config()
224 .ipopt_jacobian_regularization_value());
225 app->Options()->SetStringValue(
226 "print_timing_statistics",
227 planner_open_space_config_.dual_variable_warm_start_config()
228 .ipopt_config()
229 .ipopt_print_timing_statistics());
230 app->Options()->SetStringValue(
231 "alpha_for_y",
232 planner_open_space_config_.dual_variable_warm_start_config()
233 .ipopt_config()
234 .ipopt_alpha_for_y());
235 app->Options()->SetStringValue(
236 "recalc_y", planner_open_space_config_.dual_variable_warm_start_config()
237 .ipopt_config()
238 .ipopt_recalc_y());
239
240 Ipopt::ApplicationReturnStatus status = app->Initialize();
241 if (status != Ipopt::Solve_Succeeded) {
242 AERROR << "*** Dual variable wart start problem error during "
243 "initialization!";
244 return false;
245 }
246
247 status = app->OptimizeTNLP(problem);
248 if (status == Ipopt::Solve_Succeeded ||
249 status == Ipopt::Solved_To_Acceptable_Level) {
250 // Retrieve some statistics about the solve
251 Ipopt::Index iter_count = app->Statistics()->IterationCount();
252 ADEBUG << "*** The problem solved in " << iter_count << " iterations!";
253
254 Ipopt::Number final_obj = app->Statistics()->FinalObjective();
255 ADEBUG << "*** The final value of the objective function is " << final_obj
256 << '.';
257 PERF_BLOCK_END("DualVariableWarmStartSolving");
258 } else {
259 ADEBUG << "Solve not succeeding, return status: " << int(status);
260 }
261
262 ptop->get_optimization_results(l_warm_up, n_warm_up);
263
264 solver_flag = (status == Ipopt::Solve_Succeeded ||
265 status == Ipopt::Solved_To_Acceptable_Level);
266 } else { // debug mode
267 DualVariableWarmStartOSQPInterface* ptop_osqp =
268 new DualVariableWarmStartOSQPInterface(
269 horizon, ts, ego, obstacles_edges_num, obstacles_num, obstacles_A,
270 obstacles_b, xWS, planner_open_space_config_);
271 bool succ = ptop_osqp->optimize();
272
273 if (!succ) {
274 AERROR << "dual warm up fail.";
275 ptop_osqp->get_optimization_results(l_warm_up, n_warm_up);
276 }
277
278 ADEBUG << "dual warm up done.";
279 ptop_osqp->get_optimization_results(l_warm_up, n_warm_up);
280
281 PERF_BLOCK_END("DualVariableWarmStartSolving");
282
283 // ipoptqp result
284 Eigen::MatrixXd l_warm_up_ipoptqp(l_warm_up->rows(), l_warm_up->cols());
285 Eigen::MatrixXd n_warm_up_ipoptqp(n_warm_up->rows(), n_warm_up->cols());
286
287 DualVariableWarmStartIPOPTQPInterface* ptop_ipoptqp =
288 new DualVariableWarmStartIPOPTQPInterface(
289 horizon, ts, ego, obstacles_edges_num, obstacles_num, obstacles_A,
290 obstacles_b, xWS, planner_open_space_config_);
291
292 Ipopt::SmartPtr<Ipopt::TNLP> problem = ptop_ipoptqp;
293 // Create an instance of the IpoptApplication
294 Ipopt::SmartPtr<Ipopt::IpoptApplication> app = IpoptApplicationFactory();
295
296 app->Options()->SetIntegerValue(
297 "print_level",
298 planner_open_space_config_.dual_variable_warm_start_config()
299 .ipopt_config()
300 .ipopt_print_level());
301 app->Options()->SetIntegerValue(
302 "mumps_mem_percent",
303 planner_open_space_config_.dual_variable_warm_start_config()
304 .ipopt_config()
305 .mumps_mem_percent());
306 app->Options()->SetNumericValue(
307 "mumps_pivtol",
308 planner_open_space_config_.dual_variable_warm_start_config()
309 .ipopt_config()
310 .mumps_pivtol());
311 app->Options()->SetIntegerValue(
312 "max_iter", planner_open_space_config_.dual_variable_warm_start_config()
313 .ipopt_config()
314 .ipopt_max_iter());
315 app->Options()->SetNumericValue(
316 "tol", planner_open_space_config_.dual_variable_warm_start_config()
317 .ipopt_config()
318 .ipopt_tol());
319 app->Options()->SetNumericValue(
320 "acceptable_constr_viol_tol",
321 planner_open_space_config_.dual_variable_warm_start_config()
322 .ipopt_config()
323 .ipopt_acceptable_constr_viol_tol());
324 app->Options()->SetNumericValue(
325 "min_hessian_perturbation",
326 planner_open_space_config_.dual_variable_warm_start_config()
327 .ipopt_config()
328 .ipopt_min_hessian_perturbation());
329 app->Options()->SetNumericValue(
330 "jacobian_regularization_value",
331 planner_open_space_config_.dual_variable_warm_start_config()
332 .ipopt_config()
333 .ipopt_jacobian_regularization_value());
334 app->Options()->SetStringValue(
335 "print_timing_statistics",
336 planner_open_space_config_.dual_variable_warm_start_config()
337 .ipopt_config()
338 .ipopt_print_timing_statistics());
339 app->Options()->SetStringValue(
340 "alpha_for_y",
341 planner_open_space_config_.dual_variable_warm_start_config()
342 .ipopt_config()
343 .ipopt_alpha_for_y());
344 app->Options()->SetStringValue(
345 "recalc_y", planner_open_space_config_.dual_variable_warm_start_config()
346 .ipopt_config()
347 .ipopt_recalc_y());
348 // for qp problem speed up
349 app->Options()->SetStringValue("mehrotra_algorithm", "yes");
350
351 Ipopt::ApplicationReturnStatus status = app->Initialize();
352 if (status != Ipopt::Solve_Succeeded) {
353 AERROR << "*** Dual variable wart start problem error during "
354 "initialization!";
355 return false;
356 }
357
358 status = app->OptimizeTNLP(problem);
359
360 if (status == Ipopt::Solve_Succeeded ||
361 status == Ipopt::Solved_To_Acceptable_Level) {
362 // Retrieve some statistics about the solve
363 Ipopt::Index iter_count = app->Statistics()->IterationCount();
364 ADEBUG << "*** IPOPTQP: The problem solved in " << iter_count
365 << " iterations!";
366 Ipopt::Number final_obj = app->Statistics()->FinalObjective();
367 ADEBUG << "*** IPOPTQP: The final value of the objective function is "
368 << final_obj << '.';
369 } else {
370 ADEBUG << "Solve not succeeding, return status: " << int(status);
371 }
372
373 ptop_ipoptqp->get_optimization_results(&l_warm_up_ipoptqp,
374 &n_warm_up_ipoptqp);
375
376 // ipopt result
377 Eigen::MatrixXd l_warm_up_ipopt(l_warm_up->rows(), l_warm_up->cols());
378 Eigen::MatrixXd n_warm_up_ipopt(n_warm_up->rows(), n_warm_up->cols());
379
380 DualVariableWarmStartIPOPTInterface* ptop_ipopt =
381 new DualVariableWarmStartIPOPTInterface(
382 horizon, ts, ego, obstacles_edges_num, obstacles_num, obstacles_A,
383 obstacles_b, xWS, planner_open_space_config_);
384
385 problem = ptop_ipopt;
386 // Create an instance of the IpoptApplication
387
388 status = app->Initialize();
389 if (status != Ipopt::Solve_Succeeded) {
390 AERROR << "*** Dual variable wart start problem error during "
391 "initialization!";
392 return false;
393 }
394
395 status = app->OptimizeTNLP(problem);
396 if (status == Ipopt::Solve_Succeeded ||
397 status == Ipopt::Solved_To_Acceptable_Level) {
398 // Retrieve some statistics about the solve
399 Ipopt::Index iter_count = app->Statistics()->IterationCount();
400 ADEBUG << "*** IPOPT: The problem solved in " << iter_count
401 << " iterations!";
402 Ipopt::Number final_obj = app->Statistics()->FinalObjective();
403 ADEBUG << "*** IPOPT: The final value of the objective function is "
404 << final_obj << '.';
405 } else {
406 ADEBUG << "Solve not succeeding, return status: " << int(status);
407 }
408
409 ptop_ipopt->get_optimization_results(&l_warm_up_ipopt, &n_warm_up_ipopt);
410
411 // compare three results
412 double l_max_diff1 = 0.0;
413 double l_max_diff2 = 0.0;
414 double l_max_diff3 = 0.0;
415 for (int c = 0; c < l_warm_up->cols(); ++c) {
416 for (int r = 0; r < l_warm_up->rows(); ++r) {
417 l_max_diff1 = std::max(l_max_diff1, std::abs(l_warm_up->coeff(r, c) -
418 l_warm_up_ipopt(r, c)));
419 l_max_diff2 = std::max(l_max_diff2, std::abs(l_warm_up->coeff(r, c) -
420 l_warm_up_ipoptqp(r, c)));
421 l_max_diff3 = std::max(l_max_diff3, std::abs(l_warm_up_ipoptqp(r, c) -
422 l_warm_up_ipopt(r, c)));
423 }
424 }
425 ADEBUG << "max l warm up diff between osqp & ipopt: " << l_max_diff1;
426 ADEBUG << "max l warm up diff between osqp & ipoptqp: " << l_max_diff2;
427 ADEBUG << "max l warm up diff between ipopt & ipoptqp: " << l_max_diff3;
428
429 double n_max_diff1 = 0.0;
430 double n_max_diff2 = 0.0;
431 double n_max_diff3 = 0.0;
432 for (int c = 0; c < n_warm_up->cols(); ++c) {
433 for (int r = 0; r < n_warm_up->rows(); ++r) {
434 n_max_diff1 = std::max(n_max_diff1, std::abs(n_warm_up->coeff(r, c) -
435 n_warm_up_ipopt(r, c)));
436 n_max_diff2 = std::max(n_max_diff2, std::abs(n_warm_up->coeff(r, c) -
437 n_warm_up_ipoptqp(r, c)));
438 n_max_diff3 = std::max(n_max_diff3, std::abs(n_warm_up_ipoptqp(r, c) -
439 n_warm_up_ipopt(r, c)));
440 }
441 }
442 ADEBUG << "max n warm up diff between osqp & ipopt: " << n_max_diff1;
443 ADEBUG << "max n warm up diff between osqp & ipoptqp: " << n_max_diff2;
444 ADEBUG << "max n warm up diff between ipopt & ipoptqp: " << n_max_diff3;
445
446 return true;
447 }
448
449 if (solver_flag == false) {
450 // if solver fails during dual warm up, insert zeros instead
451 for (int r = 0; r < l_warm_up->rows(); ++r) {
452 for (int c = 0; c < l_warm_up->cols(); ++c) {
453 (*l_warm_up)(r, c) = 0.0;
454 }
455 }
456
457 for (int r = 0; r < n_warm_up->rows(); ++r) {
458 for (int c = 0; c < n_warm_up->cols(); ++c) {
459 (*n_warm_up)(r, c) = 0.0;
460 }
461 }
462 }
463
464 return true;
465}
ADEBUG
#define ADEBUG
Definition log.h:41
AERROR
#define AERROR
Definition log.h:44
AWARN
#define AWARN
Definition log.h:43
PERF_BLOCK_START
#define PERF_BLOCK_START()
Definition perf_util.h:126
PERF_BLOCK_END
#define PERF_BLOCK_END
Definition profiler.h:53
apollo::planning::PlannerOpenSpaceConfig::dual_variable_warm_start_config
optional DualVariableWarmStartConfig dual_variable_warm_start_config
Definition planner_open_space_config.proto:30
该类的文档由以下文件生成: