Apollo 11.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 行定义.

33 {
34 planner_open_space_config_ = planner_open_space_config;
35 dual_variable_warm_start_config_ = planner_open_space_config_.dual_variable_warm_start_config();
36}
apollo::planning::PlannerOpenSpaceConfig::dual_variable_warm_start_config
optional DualVariableWarmStartConfig dual_variable_warm_start_config
Definition planner_open_space_config.proto:30

◆ ~DualVariableWarmStartProblem()

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

◆ DualVariableWarmStartProblem() [2/2]

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

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

39 {
40 dual_variable_warm_start_config_ = dual_variable_warm_start_config;
41}

成员函数说明

◆ 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 第 43 行定义.

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