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

#include <dual_variable_warm_start_ipopt_interface.h>

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

Public 成员函数

 DualVariableWarmStartIPOPTInterface (size_t horizon, double ts, const Eigen::MatrixXd &ego, const Eigen::MatrixXi &obstacles_edges_num, const size_t obstacles_num, const Eigen::MatrixXd &obstacles_A, const Eigen::MatrixXd &obstacles_b, const Eigen::MatrixXd &xWS, const PlannerOpenSpaceConfig &planner_open_space_config)
 
virtual ~DualVariableWarmStartIPOPTInterface ()=default
 
void get_optimization_results (Eigen::MatrixXd *l_warm_up, Eigen::MatrixXd *n_warm_up) const
 
bool get_nlp_info (int &n, int &m, int &nnz_jac_g, int &nnz_h_lag, IndexStyleEnum &index_style) override
 Method to return some info about the nlp
 
bool get_bounds_info (int n, double *x_l, double *x_u, int m, double *g_l, double *g_u) override
 Method to return the bounds for my problem
 
bool get_starting_point (int n, bool init_x, double *x, bool init_z, double *z_L, double *z_U, int m, bool init_lambda, double *lambda) override
 Method to return the starting point for the algorithm
 
bool eval_f (int n, const double *x, bool new_x, double &obj_value) override
 Method to return the objective value
 
bool eval_grad_f (int n, const double *x, bool new_x, double *grad_f) override
 Method to return the gradient of the objective
 
bool eval_g (int n, const double *x, bool new_x, int m, double *g) override
 Method to return the constraint residuals
 
bool eval_jac_g (int n, const double *x, bool new_x, int m, int nele_jac, int *iRow, int *jCol, double *values) override
 Method to return: 1) The structure of the jacobian (if "values" is nullptr) 2) The values of the jacobian (if "values" is not nullptr)
 
bool eval_h (int n, const double *x, bool new_x, double obj_factor, int m, const double *lambda, bool new_lambda, int nele_hess, int *iRow, int *jCol, double *values) override
 Method to return: 1) The structure of the hessian of the lagrangian (if "values" is nullptr) 2) The values of the hessian of the lagrangian (if "values" is not nullptr)
 

Solution Methods

void finalize_solution (Ipopt::SolverReturn status, int n, const double *x, const double *z_L, const double *z_U, int m, const double *g, const double *lambda, double obj_value, const Ipopt::IpoptData *ip_data, Ipopt::IpoptCalculatedQuantities *ip_cq) override
 This method is called when the algorithm is complete so the TNLP can store/write the solution
 
template<class T >
bool eval_obj (int n, const T *x, T *obj_value)
 Template to return the objective value
 
template<class T >
bool eval_constraints (int n, const T *x, int m, T *g)
 Template to compute constraints
 
void generate_tapes (int n, int m, int *nnz_h_lag)
 Method to generate the required tapes
 
 DualVariableWarmStartIPOPTInterface (size_t horizon, double ts, const Eigen::MatrixXd &ego, const Eigen::MatrixXi &obstacles_edges_num, const size_t obstacles_num, const Eigen::MatrixXd &obstacles_A, const Eigen::MatrixXd &obstacles_b, const Eigen::MatrixXd &xWS, const DualVariableWarmStartConfig &dual_variable_warm_start_config)
 

详细描述

在文件 dual_variable_warm_start_ipopt_interface.h 第 46 行定义.

构造及析构函数说明

◆ DualVariableWarmStartIPOPTInterface() [1/2]

apollo::planning::DualVariableWarmStartIPOPTInterface::DualVariableWarmStartIPOPTInterface ( size_t  horizon,
double  ts,
const Eigen::MatrixXd &  ego,
const Eigen::MatrixXi &  obstacles_edges_num,
const size_t  obstacles_num,
const Eigen::MatrixXd &  obstacles_A,
const Eigen::MatrixXd &  obstacles_b,
const Eigen::MatrixXd &  xWS,
const PlannerOpenSpaceConfig &  planner_open_space_config 
)

在文件 dual_variable_warm_start_ipopt_interface.cc 第 32 行定义.

41 :
42 ts_(ts),
43 ego_(ego),
44 obstacles_edges_num_(obstacles_edges_num),
45 obstacles_A_(obstacles_A),
46 obstacles_b_(obstacles_b),
47 xWS_(xWS) {
48 ACHECK(horizon < std::numeric_limits<int>::max()) << "Invalid cast on horizon in open space planner";
49 horizon_ = static_cast<int>(horizon);
50 ACHECK(obstacles_num < std::numeric_limits<int>::max()) << "Invalid cast on obstacles_num in open space planner";
51 obstacles_num_ = static_cast<int>(obstacles_num);
52 w_ev_ = ego_(1, 0) + ego_(3, 0);
53 l_ev_ = ego_(0, 0) + ego_(2, 0);
54 g_ = {l_ev_ / 2, w_ev_ / 2, l_ev_ / 2, w_ev_ / 2};
55 offset_ = (ego_(0, 0) + ego_(2, 0)) / 2 - ego_(2, 0);
56 obstacles_edges_sum_ = obstacles_edges_num_.sum();
57 l_start_index_ = 0;
58 n_start_index_ = l_start_index_ + obstacles_edges_sum_ * (horizon_ + 1);
59 d_start_index_ = n_start_index_ + 4 * obstacles_num_ * (horizon_ + 1);
60 l_warm_up_ = Eigen::MatrixXd::Zero(obstacles_edges_sum_, horizon_ + 1);
61 n_warm_up_ = Eigen::MatrixXd::Zero(4 * obstacles_num_, horizon_ + 1);
62 weight_d_ = planner_open_space_config.dual_variable_warm_start_config().weight_d();
63}
ACHECK
#define ACHECK(cond)
Definition log.h:80

◆ ~DualVariableWarmStartIPOPTInterface()

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

◆ DualVariableWarmStartIPOPTInterface() [2/2]

apollo::planning::DualVariableWarmStartIPOPTInterface::DualVariableWarmStartIPOPTInterface ( size_t  horizon,
double  ts,
const Eigen::MatrixXd &  ego,
const Eigen::MatrixXi &  obstacles_edges_num,
const size_t  obstacles_num,
const Eigen::MatrixXd &  obstacles_A,
const Eigen::MatrixXd &  obstacles_b,
const Eigen::MatrixXd &  xWS,
const DualVariableWarmStartConfig &  dual_variable_warm_start_config 
)

在文件 dual_variable_warm_start_ipopt_interface.cc 第 65 行定义.

74 :
75 ts_(ts),
76 ego_(ego),
77 obstacles_edges_num_(obstacles_edges_num),
78 obstacles_A_(obstacles_A),
79 obstacles_b_(obstacles_b),
80 xWS_(xWS) {
81 ACHECK(horizon < std::numeric_limits<int>::max()) << "Invalid cast on horizon in open space planner";
82 horizon_ = static_cast<int>(horizon);
83 ACHECK(obstacles_num < std::numeric_limits<int>::max()) << "Invalid cast on obstacles_num in open space planner";
84 obstacles_num_ = static_cast<int>(obstacles_num);
85 w_ev_ = ego_(1, 0) + ego_(3, 0);
86 l_ev_ = ego_(0, 0) + ego_(2, 0);
87 g_ = {l_ev_ / 2, w_ev_ / 2, l_ev_ / 2, w_ev_ / 2};
88 offset_ = (ego_(0, 0) + ego_(2, 0)) / 2 - ego_(2, 0);
89 obstacles_edges_sum_ = obstacles_edges_num_.sum();
90 l_start_index_ = 0;
91 n_start_index_ = l_start_index_ + obstacles_edges_sum_ * (horizon_ + 1);
92 d_start_index_ = n_start_index_ + 4 * obstacles_num_ * (horizon_ + 1);
93 l_warm_up_ = Eigen::MatrixXd::Zero(obstacles_edges_sum_, horizon_ + 1);
94 n_warm_up_ = Eigen::MatrixXd::Zero(4 * obstacles_num_, horizon_ + 1);
95 weight_d_ = dual_variable_warm_start_config.weight_d();
96}

成员函数说明

◆ eval_constraints()

template<class T >
bool apollo::planning::DualVariableWarmStartIPOPTInterface::eval_constraints ( int  n,
const T *  x,
int  m,
T *  g 
)

Template to compute constraints

在文件 dual_variable_warm_start_ipopt_interface.cc 第 659 行定义.

659 {
660 ADEBUG << "eval_constraints";
661 // state start index
662
663 // 1. Three obstacles related equal constraints, one equality constraints,
664 // [0, horizon_] * [0, obstacles_num_-1] * 4
665
666 int l_index = l_start_index_;
667 int n_index = n_start_index_;
668 int d_index = d_start_index_;
669 int constraint_index = 0;
670
671 for (int i = 0; i < horizon_ + 1; ++i) {
672 int edges_counter = 0;
673 // assume: stationary obstacles
674 for (int j = 0; j < obstacles_num_; ++j) {
675 int current_edges_num = obstacles_edges_num_(j, 0);
676 Eigen::MatrixXd Aj = obstacles_A_.block(edges_counter, 0, current_edges_num, 2);
677 Eigen::MatrixXd bj = obstacles_b_.block(edges_counter, 0, current_edges_num, 1);
678
679 // norm(A* lambda) <= 1
680 T tmp1 = 0.0;
681 T tmp2 = 0.0;
682 for (int k = 0; k < current_edges_num; ++k) {
683 tmp1 += Aj(k, 0) * x[l_index + k];
684 tmp2 += Aj(k, 1) * x[l_index + k];
685 }
686 g[constraint_index] = tmp1 * tmp1 + tmp2 * tmp2;
687
688 // G' * mu + R' * A' * lambda == 0
689 g[constraint_index + 1] = x[n_index] - x[n_index + 2] + cos(xWS_(2, i)) * tmp1 + sin(xWS_(2, i)) * tmp2;
690
691 g[constraint_index + 2] = x[n_index + 1] - x[n_index + 3] - sin(xWS_(2, i)) * tmp1 + cos(xWS_(2, i)) * tmp2;
692
693 // d - (-g'*mu + (A*t - b)*lambda) = 0
694 // TODO(QiL): Need to revise according to dual modeling
695 T tmp3 = 0.0;
696 for (int k = 0; k < 4; ++k) {
697 tmp3 += g_[k] * x[n_index + k];
698 }
699
700 T tmp4 = 0.0;
701 for (int k = 0; k < current_edges_num; ++k) {
702 tmp4 += bj(k, 0) * x[l_index + k];
703 }
704
705 g[constraint_index + 3] = x[d_index] + tmp3 - (xWS_(0, i) + cos(xWS_(2, i)) * offset_) * tmp1
706 - (xWS_(1, i) + sin(xWS_(2, i)) * offset_) * tmp2 + tmp4;
707
708 // Update index
709 edges_counter += current_edges_num;
710 l_index += current_edges_num;
711 n_index += 4;
712 d_index += 1;
713 constraint_index += 4;
714 }
715 }
716 l_index = l_start_index_;
717 n_index = n_start_index_;
718 d_index = d_start_index_;
719 for (int i = 0; i < lambda_horizon_; ++i) {
720 g[constraint_index] = x[l_index];
721 constraint_index++;
722 l_index++;
723 }
724 for (int i = 0; i < miu_horizon_; ++i) {
725 g[constraint_index] = x[n_index];
726 constraint_index++;
727 n_index++;
728 }
729 for (int i = 0; i < dual_formulation_horizon_; ++i) {
730 g[constraint_index] = x[d_index];
731 constraint_index++;
732 d_index++;
733 }
734
735 CHECK_EQ(constraint_index, m) << "No. of constraints wrong in eval_g. n : " << n;
736 return true;
737}
ADEBUG
#define ADEBUG
Definition log.h:41
apollo::common::math::cos
float cos(Angle16 a)
Definition angle.cc:42
apollo::common::math::sin
float sin(Angle16 a)
Definition angle.cc:25

◆ eval_f()

bool apollo::planning::DualVariableWarmStartIPOPTInterface::eval_f ( int  n,
const double *  x,
bool  new_x,
double &  obj_value 
)
override

Method to return the objective value

在文件 dual_variable_warm_start_ipopt_interface.cc 第 272 行定义.

272 {
273 eval_obj(n, x, &obj_value);
274 return true;
275}
apollo::planning::DualVariableWarmStartIPOPTInterface::eval_obj
bool eval_obj(int n, const T *x, T *obj_value)
Template to return the objective value
Definition dual_variable_warm_start_ipopt_interface.cc:644

◆ eval_g()

bool apollo::planning::DualVariableWarmStartIPOPTInterface::eval_g ( int  n,
const double *  x,
bool  new_x,
int  m,
double *  g 
)
override

Method to return the constraint residuals

在文件 dual_variable_warm_start_ipopt_interface.cc 第 291 行定义.

291 {
292 eval_constraints(n, x, m, g);
293 return true;
294}
apollo::planning::DualVariableWarmStartIPOPTInterface::eval_constraints
bool eval_constraints(int n, const T *x, int m, T *g)
Template to compute constraints
Definition dual_variable_warm_start_ipopt_interface.cc:659

◆ eval_grad_f()

bool apollo::planning::DualVariableWarmStartIPOPTInterface::eval_grad_f ( int  n,
const double *  x,
bool  new_x,
double *  grad_f 
)
override

Method to return the gradient of the objective

在文件 dual_variable_warm_start_ipopt_interface.cc 第 277 行定义.

277 {
278 // gradient(tag_f, n, x, grad_f);
279 // return true;
280 std::fill(grad_f, grad_f + n, 0.0);
281 int d_index = d_start_index_;
282 for (int i = 0; i < horizon_ + 1; ++i) {
283 for (int j = 0; j < obstacles_num_; ++j) {
284 grad_f[d_index] = weight_d_;
285 ++d_index;
286 }
287 }
288 return true;
289}

◆ eval_h()

bool apollo::planning::DualVariableWarmStartIPOPTInterface::eval_h ( int  n,
const double *  x,
bool  new_x,
double  obj_factor,
int  m,
const double *  lambda,
bool  new_lambda,
int  nele_hess,
int *  iRow,
int *  jCol,
double *  values 
)
override

Method to return: 1) The structure of the hessian of the lagrangian (if "values" is nullptr) 2) The values of the hessian of the lagrangian (if "values" is not nullptr)

在文件 dual_variable_warm_start_ipopt_interface.cc 第 558 行定义.

569 {
570 if (values == nullptr) {
571 // return the structure. This is a symmetric matrix, fill the lower left
572 // triangle only.
573 for (int idx = 0; idx < nnz_L; idx++) {
574 iRow[idx] = rind_L[idx];
575 jCol[idx] = cind_L[idx];
576 }
577 } else {
578 // return the values. This is a symmetric matrix, fill the lower left
579 // triangle only
580 obj_lam[0] = obj_factor;
581 for (int idx = 0; idx < m; idx++) {
582 obj_lam[1 + idx] = lambda[idx];
583 }
584 set_param_vec(tag_L, m + 1, obj_lam);
585 sparse_hess(tag_L, n, 1, const_cast<double*>(x), &nnz_L, &rind_L, &cind_L, &hessval, options_L);
586
587 for (int idx = 0; idx < nnz_L; idx++) {
588 values[idx] = hessval[idx];
589 }
590 }
591
592 return true;
593}
tag_L
#define tag_L
Definition cos_theta_ipopt_interface.h:38

◆ eval_jac_g()

bool apollo::planning::DualVariableWarmStartIPOPTInterface::eval_jac_g ( int  n,
const double *  x,
bool  new_x,
int  m,
int  nele_jac,
int *  iRow,
int *  jCol,
double *  values 
)
override

Method to return: 1) The structure of the jacobian (if "values" is nullptr) 2) The values of the jacobian (if "values" is not nullptr)

在文件 dual_variable_warm_start_ipopt_interface.cc 第 296 行定义.

304 {
305 // if (values == nullptr) {
306 // // return the structure of the jacobian
307
308 // for (int idx = 0; idx < nnz_jac; idx++) {
309 // iRow[idx] = rind_g[idx];
310 // jCol[idx] = cind_g[idx];
311 // }
312 // } else {
313 // // return the values of the jacobian of the constraints
314
315 // sparse_jac(tag_g, m, n, 1, x, &nnz_jac, &rind_g, &cind_g, &jacval,
316 // options_g);
317
318 // for (int idx = 0; idx < nnz_jac; idx++) {
319 // values[idx] = jacval[idx];
320 // }
321 // }
322 // return true;
323 ADEBUG << "eval_jac_g";
324
325 if (values == nullptr) {
326 int nz_index = 0;
327 int constraint_index = 0;
328
329 // 1. Three obstacles related equal constraints, one equality
330 // constraints,
331 // [0, horizon_] * [0, obstacles_num_-1] * 4
332 int l_index = l_start_index_;
333 int n_index = n_start_index_;
334 int d_index = d_start_index_;
335 for (int i = 0; i < horizon_ + 1; ++i) {
336 for (int j = 0; j < obstacles_num_; ++j) {
337 int current_edges_num = obstacles_edges_num_(j, 0);
338
339 // 1. norm(A* lambda <= 1)
340 for (int k = 0; k < current_edges_num; ++k) {
341 // with respect to l
342 iRow[nz_index] = constraint_index;
343 jCol[nz_index] = l_index + k;
344 ++nz_index;
345 }
346
347 // 2. G' * mu + R' * lambda == 0, part 1
348 // with respect to l
349 for (int k = 0; k < current_edges_num; ++k) {
350 iRow[nz_index] = constraint_index + 1;
351 jCol[nz_index] = l_index + k;
352 ++nz_index;
353 }
354
355 // With respect to n
356 iRow[nz_index] = constraint_index + 1;
357 jCol[nz_index] = n_index;
358 ++nz_index;
359
360 iRow[nz_index] = constraint_index + 1;
361 jCol[nz_index] = n_index + 2;
362 ++nz_index;
363
364 // 2. G' * mu + R' * lambda == 0, part 2
365 // with respect to l
366 for (int k = 0; k < current_edges_num; ++k) {
367 iRow[nz_index] = constraint_index + 2;
368 jCol[nz_index] = l_index + k;
369 ++nz_index;
370 }
371
372 // With respect to n
373 iRow[nz_index] = constraint_index + 2;
374 jCol[nz_index] = n_index + 1;
375 ++nz_index;
376
377 iRow[nz_index] = constraint_index + 2;
378 jCol[nz_index] = n_index + 3;
379 ++nz_index;
380
381 // -g'*mu + (A*t - b)*lambda > 0
382 // with respect to l
383 for (int k = 0; k < current_edges_num; ++k) {
384 iRow[nz_index] = constraint_index + 3;
385 jCol[nz_index] = l_index + k;
386 ++nz_index;
387 }
388
389 // with respect to n
390 for (int k = 0; k < 4; ++k) {
391 iRow[nz_index] = constraint_index + 3;
392 jCol[nz_index] = n_index + k;
393 ++nz_index;
394 }
395
396 // with resepct to d
397 iRow[nz_index] = constraint_index + 3;
398 jCol[nz_index] = d_index;
399 ++nz_index;
400
401 // Update index
402 l_index += current_edges_num;
403 n_index += 4;
404 d_index += 1;
405 constraint_index += 4;
406 }
407 }
408
409 l_index = l_start_index_;
410 n_index = n_start_index_;
411 d_index = d_start_index_;
412 for (int i = 0; i < lambda_horizon_; ++i) {
413 iRow[nz_index] = constraint_index;
414 jCol[nz_index] = l_index;
415 ++nz_index;
416 ++constraint_index;
417 ++l_index;
418 }
419 for (int i = 0; i < miu_horizon_; ++i) {
420 iRow[nz_index] = constraint_index;
421 jCol[nz_index] = n_index;
422 ++nz_index;
423 ++constraint_index;
424 ++n_index;
425 }
426 for (int i = 0; i < dual_formulation_horizon_; ++i) {
427 iRow[nz_index] = constraint_index;
428 jCol[nz_index] = d_index;
429 ++nz_index;
430 ++constraint_index;
431 ++d_index;
432 }
433
434 CHECK_EQ(constraint_index, m) << "No. of constraints wrong in eval_jac_g.";
435
436 ADEBUG << "nz_index here : " << nz_index << " nele_jac is : " << nele_jac;
437 } else {
438 std::fill(values, values + nele_jac, 0.0);
439 int nz_index = 0;
440
441 // 1. Three obstacles related equal constraints, one equality
442 // constraints,
443 // [0, horizon_] * [0, obstacles_num_-1] * 4
444 int l_index = l_start_index_;
445 int n_index = n_start_index_;
446
447 for (int i = 0; i < horizon_ + 1; ++i) {
448 int edges_counter = 0;
449 for (int j = 0; j < obstacles_num_; ++j) {
450 int current_edges_num = obstacles_edges_num_(j, 0);
451 Eigen::MatrixXd Aj = obstacles_A_.block(edges_counter, 0, current_edges_num, 2);
452 Eigen::MatrixXd bj = obstacles_b_.block(edges_counter, 0, current_edges_num, 1);
453
454 // TODO(QiL) : Remove redundant calculation
455 double tmp1 = 0;
456 double tmp2 = 0;
457 for (int k = 0; k < current_edges_num; ++k) {
458 // TODO(QiL) : replace this one directly with x
459 tmp1 += Aj(k, 0) * x[l_index + k];
460 tmp2 += Aj(k, 1) * x[l_index + k];
461 }
462
463 // 1. norm(A* lambda == 1)
464 for (int k = 0; k < current_edges_num; ++k) {
465 // with respect to l
466 values[nz_index] = 2 * tmp1 * Aj(k, 0) + 2 * tmp2 * Aj(k, 1); // t0~tk
467 ++nz_index;
468 }
469
470 // 2. G' * mu + R' * lambda == 0, part 1
471
472 // with respect to l
473 for (int k = 0; k < current_edges_num; ++k) {
474 values[nz_index] = std::cos(xWS_(2, i)) * Aj(k, 0) + std::sin(xWS_(2, i)) * Aj(k, 1); // v0~vn
475 ++nz_index;
476 }
477
478 // With respect to n
479 values[nz_index] = 1.0; // w0
480 ++nz_index;
481
482 values[nz_index] = -1.0; // w2
483 ++nz_index;
484
485 ADEBUG << "eval_jac_g, after adding part 2";
486 // 3. G' * mu + R' * lambda == 0, part 2
487
488 // with respect to l
489 for (int k = 0; k < current_edges_num; ++k) {
490 values[nz_index] = -std::sin(xWS_(2, i)) * Aj(k, 0) + std::cos(xWS_(2, i)) * Aj(k, 1); // y0~yn
491 ++nz_index;
492 }
493
494 // With respect to n
495 values[nz_index] = 1.0; // z1
496 ++nz_index;
497
498 values[nz_index] = -1.0; // z3
499 ++nz_index;
500
501 // 3. -g'*mu + (A*t - b)*lambda > 0
502 // TODO(QiL): Revise dual variables modeling here.
503 double tmp3 = 0.0;
504 double tmp4 = 0.0;
505 for (int k = 0; k < 4; ++k) {
506 tmp3 += -g_[k] * x[n_index + k];
507 }
508
509 for (int k = 0; k < current_edges_num; ++k) {
510 tmp4 += bj(k, 0) * x[l_index + k];
511 }
512
513 // with respect to l
514 for (int k = 0; k < current_edges_num; ++k) {
515 values[nz_index] = -(xWS_(0, i) + std::cos(xWS_(2, i)) * offset_) * Aj(k, 0)
516 - (xWS_(1, i) + std::sin(xWS_(2, i)) * offset_) * Aj(k, 1) + bj(k, 0); // ddk
517 ++nz_index;
518 }
519
520 // with respect to n
521 for (int k = 0; k < 4; ++k) {
522 values[nz_index] = g_[k]; // eek
523 ++nz_index;
524 }
525
526 // with respect to d
527 values[nz_index] = 1.0; // ffk
528 ++nz_index;
529
530 // Update index
531 edges_counter += current_edges_num;
532 l_index += current_edges_num;
533 n_index += 4;
534 }
535 }
536
537 for (int i = 0; i < lambda_horizon_; ++i) {
538 values[nz_index] = 1.0;
539 ++nz_index;
540 }
541 for (int i = 0; i < miu_horizon_; ++i) {
542 values[nz_index] = 1.0;
543 ++nz_index;
544 }
545 for (int i = 0; i < dual_formulation_horizon_; ++i) {
546 values[nz_index] = 1.0;
547 ++nz_index;
548 }
549
550 ADEBUG << "eval_jac_g, fulfilled obstacle constraint values";
551 CHECK_EQ(nz_index, nele_jac);
552 }
553
554 ADEBUG << "eval_jac_g done";
555 return true;
556}

◆ eval_obj()

template<class T >
bool apollo::planning::DualVariableWarmStartIPOPTInterface::eval_obj ( int  n,
const T *  x,
T *  obj_value 
)

Template to return the objective value

在文件 dual_variable_warm_start_ipopt_interface.cc 第 644 行定义.

644 {
645 ADEBUG << "eval_obj";
646 *obj_value = 0.0;
647 int d_index = d_start_index_;
648 for (int i = 0; i < horizon_ + 1; ++i) {
649 for (int j = 0; j < obstacles_num_; ++j) {
650 *obj_value += weight_d_ * x[d_index];
651 ++d_index;
652 }
653 }
654 return true;
655}

◆ finalize_solution()

void apollo::planning::DualVariableWarmStartIPOPTInterface::finalize_solution ( Ipopt::SolverReturn  status,
int  n,
const double *  x,
const double *  z_L,
const double *  z_U,
int  m,
const double *  g,
const double *  lambda,
double  obj_value,
const Ipopt::IpoptData *  ip_data,
Ipopt::IpoptCalculatedQuantities *  ip_cq 
)
override

This method is called when the algorithm is complete so the TNLP can store/write the solution

在文件 dual_variable_warm_start_ipopt_interface.cc 第 595 行定义.

606 {
607 int variable_index = 0;
608 // 1. lagrange constraint l, [0, obstacles_edges_sum_ - 1] * [0,
609 // horizon_]
610 for (int i = 0; i < horizon_ + 1; ++i) {
611 for (int j = 0; j < obstacles_edges_sum_; ++j) {
612 l_warm_up_(j, i) = x[variable_index];
613 ++variable_index;
614 }
615 }
616 ADEBUG << "variable_index after adding lagrange l : " << variable_index;
617
618 // 2. lagrange constraint n, [0, 4*obstacles_num-1] * [0, horizon_]
619 for (int i = 0; i < horizon_ + 1; ++i) {
620 for (int j = 0; j < 4 * obstacles_num_; ++j) {
621 n_warm_up_(j, i) = x[variable_index];
622 ++variable_index;
623 }
624 }
625 ADEBUG << "variable_index after adding lagrange n : " << variable_index;
626
627 // memory deallocation of ADOL-C variables
628 delete[] obj_lam;
629 free(rind_L);
630 free(cind_L);
631 free(hessval);
632}

◆ generate_tapes()

void apollo::planning::DualVariableWarmStartIPOPTInterface::generate_tapes ( int  n,
int  m,
int *  nnz_h_lag 
)

Method to generate the required tapes

在文件 dual_variable_warm_start_ipopt_interface.cc 第 740 行定义.

740 {
741 std::vector<double> xp(n);
742 std::vector<double> lamp(m);
743 std::vector<double> zl(m);
744 std::vector<double> zu(m);
745
746 std::vector<adouble> xa(n);
747 std::vector<adouble> g(m);
748 std::vector<double> lam(m);
749 double sig = 0.0;
750 adouble obj_value;
751
752 double dummy = 0.0;
753
754 obj_lam = new double[m + 1];
755
756 get_starting_point(n, 1, &xp[0], 0, &zl[0], &zu[0], m, 0, &lamp[0]);
757
758 // trace_on(tag_f);
759
760 // for (int idx = 0; idx < n; idx++) xa[idx] <<= xp[idx];
761
762 // eval_obj(n, xa, &obj_value);
763
764 // obj_value >>= dummy;
765
766 // trace_off();
767
768 // trace_on(tag_g);
769
770 // for (int idx = 0; idx < n; idx++) xa[idx] <<= xp[idx];
771
772 // eval_constraints(n, xa, m, g);
773
774 // for (int idx = 0; idx < m; idx++) g[idx] >>= dummy;
775
776 // trace_off();
777
778 trace_on(tag_L);
779
780 for (int idx = 0; idx < n; idx++) {
781 xa[idx] <<= xp[idx];
782 }
783 for (int idx = 0; idx < m; idx++) {
784 lam[idx] = 1.0;
785 }
786 sig = 1.0;
787
788 eval_obj(n, &xa[0], &obj_value);
789
790 obj_value *= mkparam(sig);
791 eval_constraints(n, &xa[0], m, &g[0]);
792
793 for (int idx = 0; idx < m; idx++) {
794 obj_value += g[idx] * mkparam(lam[idx]);
795 }
796
797 obj_value >>= dummy;
798
799 trace_off();
800
801 rind_L = nullptr;
802 cind_L = nullptr;
803
804 hessval = nullptr;
805
806 options_L[0] = 0;
807 options_L[1] = 1;
808
809 sparse_hess(tag_L, n, 0, &xp[0], &nnz_L, &rind_L, &cind_L, &hessval, options_L);
810 *nnz_h_lag = nnz_L;
811}
apollo::planning::DualVariableWarmStartIPOPTInterface::get_starting_point
bool get_starting_point(int n, bool init_x, double *x, bool init_z, double *z_L, double *z_U, int m, bool init_lambda, double *lambda) override
Method to return the starting point for the algorithm
Definition dual_variable_warm_start_ipopt_interface.cc:137

◆ get_bounds_info()

bool apollo::planning::DualVariableWarmStartIPOPTInterface::get_bounds_info ( int  n,
double *  x_l,
double *  x_u,
int  m,
double *  g_l,
double *  g_u 
)
override

Method to return the bounds for my problem

在文件 dual_variable_warm_start_ipopt_interface.cc 第 184 行定义.

190 {
191 int variable_index = 0;
192 // 1. lagrange constraint l, [0, obstacles_edges_sum_ - 1] * [0,
193 // horizon_]
194 for (int i = 0; i < horizon_ + 1; ++i) {
195 for (int j = 0; j < obstacles_edges_sum_; ++j) {
196 x_l[variable_index] = -2e19;
197 x_u[variable_index] = 2e19;
198 ++variable_index;
199 }
200 }
201 ADEBUG << "variable_index after adding lagrange l : " << variable_index;
202
203 // 2. lagrange constraint n, [0, 4*obstacles_num-1] * [0, horizon_]
204 for (int i = 0; i < horizon_ + 1; ++i) {
205 for (int j = 0; j < 4 * obstacles_num_; ++j) {
206 x_l[variable_index] = -2e19;
207 x_u[variable_index] = 2e19; // nlp_upper_bound_limit
208 ++variable_index;
209 }
210 }
211 ADEBUG << "variable_index after adding lagrange n : " << variable_index;
212
213 // 3. d, [0, obstacles_num-1] * [0, horizon_]
214 for (int i = 0; i < horizon_ + 1; ++i) {
215 for (int j = 0; j < obstacles_num_; ++j) {
216 // TODO(QiL): Load this from configuration
217 x_l[variable_index] = -2e19;
218 x_u[variable_index] = 2e19; // nlp_upper_bound_limit
219 ++variable_index;
220 }
221 }
222 ADEBUG << "variable_index after adding d : " << variable_index;
223
224 int constraint_index = 0;
225 for (int i = 0; i < horizon_ + 1; ++i) {
226 for (int j = 0; j < obstacles_num_; ++j) {
227 // a. norm(A'*lambda) <= 1
228 g_l[constraint_index] = -2e19;
229 g_u[constraint_index] = 1.0;
230
231 // b. G'*mu + R'*A*lambda = 0
232 g_l[constraint_index + 1] = 0.0;
233 g_u[constraint_index + 1] = 0.0;
234 g_l[constraint_index + 2] = 0.0;
235 g_u[constraint_index + 2] = 0.0;
236
237 // c. d - (-g'*mu + (A*t - b)*lambda) = 0
238 g_l[constraint_index + 3] = 0.0;
239 g_u[constraint_index + 3] = 0.0;
240 constraint_index += 4;
241 }
242 }
243
244 int l_index = l_start_index_;
245 int n_index = n_start_index_;
246 int d_index = d_start_index_;
247
248 for (int i = 0; i < lambda_horizon_; ++i) {
249 g_l[constraint_index] = 0.0;
250 g_u[constraint_index] = 2e19;
251 constraint_index++;
252 l_index++;
253 }
254 for (int i = 0; i < miu_horizon_; ++i) {
255 g_l[constraint_index] = 0.0;
256 g_u[constraint_index] = 2e19;
257 constraint_index++;
258 n_index++;
259 }
260 for (int i = 0; i < dual_formulation_horizon_; ++i) {
261 g_l[constraint_index] = 0.0;
262 g_u[constraint_index] = 2e19;
263 constraint_index++;
264 d_index++;
265 }
266
267 ADEBUG << "constraint_index after adding obstacles constraints: " << constraint_index;
268
269 return true;
270}

◆ get_nlp_info()

bool apollo::planning::DualVariableWarmStartIPOPTInterface::get_nlp_info ( int &  n,
int &  m,
int &  nnz_jac_g,
int &  nnz_h_lag,
IndexStyleEnum &  index_style 
)
override

Method to return some info about the nlp

在文件 dual_variable_warm_start_ipopt_interface.cc 第 98 行定义.

103 {
104 lambda_horizon_ = obstacles_edges_sum_ * (horizon_ + 1);
105
106 miu_horizon_ = obstacles_num_ * 4 * (horizon_ + 1);
107
108 dual_formulation_horizon_ = obstacles_num_ * (horizon_ + 1);
109
110 num_of_variables_ = lambda_horizon_ + miu_horizon_ + dual_formulation_horizon_;
111
112 num_of_constraints_ = 4 * obstacles_num_ * (horizon_ + 1) + num_of_variables_;
113
114 // number of variables
115 n = num_of_variables_;
116
117 // number of constraints
118 m = num_of_constraints_;
119
120 // number of nonzero Jacobian and Lagrangian.
121 generate_tapes(n, m, &nnz_h_lag);
122
123 int tmp = 0;
124 for (int i = 0; i < horizon_ + 1; ++i) {
125 for (int j = 0; j < obstacles_num_; ++j) {
126 int current_edges_num = obstacles_edges_num_(j, 0);
127 tmp += current_edges_num * 4 + 2 + 2 + 4 + 1;
128 }
129 }
130 nnz_jac_g = tmp + num_of_variables_;
131
132 ADEBUG << "nnz_jac_g : " << nnz_jac_g;
133 index_style = IndexStyleEnum::C_STYLE;
134 return true;
135}
apollo::planning::DualVariableWarmStartIPOPTInterface::generate_tapes
void generate_tapes(int n, int m, int *nnz_h_lag)
Method to generate the required tapes
Definition dual_variable_warm_start_ipopt_interface.cc:740

◆ get_optimization_results()

void apollo::planning::DualVariableWarmStartIPOPTInterface::get_optimization_results ( Eigen::MatrixXd *  l_warm_up,
Eigen::MatrixXd *  n_warm_up 
) const

在文件 dual_variable_warm_start_ipopt_interface.cc 第 634 行定义.

636 {
637 *l_warm_up = l_warm_up_;
638 *n_warm_up = n_warm_up_;
639}

◆ get_starting_point()

bool apollo::planning::DualVariableWarmStartIPOPTInterface::get_starting_point ( int  n,
bool  init_x,
double *  x,
bool  init_z,
double *  z_L,
double *  z_U,
int  m,
bool  init_lambda,
double *  lambda 
)
override

Method to return the starting point for the algorithm

在文件 dual_variable_warm_start_ipopt_interface.cc 第 137 行定义.

146 {
147 ADEBUG << "get_starting_point";
148 ACHECK(init_x) << "Warm start init_x setting failed";
149 ACHECK(!init_z) << "Warm start init_z setting failed";
150 ACHECK(!init_lambda) << "Warm start init_lambda setting failed";
151
152 int l_index = l_start_index_;
153 int n_index = n_start_index_;
154 int d_index = d_start_index_;
155 ADEBUG << "l_start_index_ : " << l_start_index_;
156 ADEBUG << "n_start_index_ : " << n_start_index_;
157 ADEBUG << "d_start_index_ : " << d_start_index_;
158 // 1. lagrange constraint l, obstacles_edges_sum_ * (horizon_+1)
159 for (int i = 0; i < horizon_ + 1; ++i) {
160 for (int j = 0; j < obstacles_edges_sum_; ++j) {
161 x[l_index] = 0.0;
162 ++l_index;
163 }
164 }
165
166 // 2. lagrange constraint n, 4*obstacles_num * (horizon_+1)
167 for (int i = 0; i < horizon_ + 1; ++i) {
168 for (int j = 0; j < 4 * obstacles_num_; ++j) {
169 x[n_index] = 0.0;
170 ++n_index;
171 }
172 }
173 // 3. d, [0, obstacles_num] * [0, horizon_]
174 for (int i = 0; i < horizon_ + 1; ++i) {
175 for (int j = 0; j < obstacles_num_; ++j) {
176 x[d_index] = 0.0;
177 ++d_index;
178 }
179 }
180 ADEBUG << "get_starting_point out";
181 return true;
182}
该类的文档由以下文件生成: