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

#include <spiral_problem_interface.h>

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

Public 成员函数

 SpiralProblemInterface (std::vector< Eigen::Vector2d > points)
 
virtual ~SpiralProblemInterface ()=default
 
void set_default_max_point_deviation (const double point_max_deviation)
 
void set_start_point (const double x, const double y, const double theta, const double kappa, const double dkappa)
 
void set_end_point (const double x, const double y, const double theta, const double kappa, const double dkappa)
 
void set_end_point_position (const double x, const double y)
 
void set_element_weight_curve_length (const double weight_curve_length)
 
void set_element_weight_kappa (const double weight_kappa)
 
void set_element_weight_dkappa (const double weight_dkappa)
 
void get_optimization_results (std::vector< double > *ptr_theta, std::vector< double > *ptr_kappa, std::vector< double > *ptr_dkappa, std::vector< double > *ptr_s, std::vector< double > *ptr_x, std::vector< double > *ptr_y) 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

static constexpr size_t N = 10
 
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
 

详细描述

在文件 spiral_problem_interface.h 第 38 行定义.

构造及析构函数说明

◆ SpiralProblemInterface()

apollo::planning::SpiralProblemInterface::SpiralProblemInterface ( std::vector< Eigen::Vector2d >  points)
explicit

在文件 spiral_problem_interface.cc 第 48 行定义.

50 : init_points_(std::move(points)) {
51 num_of_points_ = static_cast<int>(init_points_.size());
52 CHECK_GT(num_of_points_, 1);
53
54 point_distances_.reserve(num_of_points_ - 1);
55 for (int i = 0; i + 1 < num_of_points_; ++i) {
56 point_distances_.push_back((init_points_[i + 1] - init_points_[i]).norm());
57 }
58
59 std::vector<double> normalized_theta;
60 for (int i = 0; i + 1 < num_of_points_; ++i) {
61 Eigen::Vector2d v = init_points_[i + 1] - init_points_[i];
62 double theta = std::atan2(v.y(), v.x());
63 normalized_theta.push_back(theta);
64 }
65 normalized_theta.push_back(normalized_theta.back());
66
67 relative_theta_.push_back(normalized_theta.front());
68 for (int i = 1; i < num_of_points_; ++i) {
69 double theta_diff =
70 common::math::AngleDiff(relative_theta_.back(), normalized_theta[i]);
71 relative_theta_.push_back(relative_theta_.back() + theta_diff);
72 }
73
74 piecewise_paths_.resize(num_of_points_ - 1);
75}
apollo::common::math::AngleDiff
double AngleDiff(const double from, const double to)
Calculate the difference between angle from and to
Definition math_utils.cc:61

◆ ~SpiralProblemInterface()

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

成员函数说明

◆ eval_f()

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

Method to return the objective value

在文件 spiral_problem_interface.cc 第 270 行定义.

271 {
272 CHECK_EQ(n, num_of_variables_);
273 if (new_x) {
274 update_piecewise_spiral_paths(x, n);
275 }
276
277 obj_value = 0.0;
278 for (int i = 0; i + 1 < num_of_points_; ++i) {
279 const auto& spiral_curve = piecewise_paths_[i];
280 double delta_s = spiral_curve.ParamLength();
281
282 obj_value += delta_s * weight_curve_length_;
283
284 for (int j = 0; j < num_of_internal_points_; ++j) {
285 double ratio =
286 static_cast<double>(j) / static_cast<double>(num_of_internal_points_);
287 double s = ratio * delta_s;
288
289 double kappa = spiral_curve.Evaluate(1, s);
290 obj_value += kappa * kappa * weight_kappa_;
291
292 double dkappa = spiral_curve.Evaluate(2, s);
293 obj_value += dkappa * dkappa * weight_dkappa_;
294 }
295 }
296 return true;
297}

◆ eval_g()

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

Method to return the constraint residuals

在文件 spiral_problem_interface.cc 第 377 行定义.

378 {
379 CHECK_EQ(n, num_of_variables_);
380 CHECK_EQ(m, num_of_constraints_);
381
382 if (new_x) {
383 update_piecewise_spiral_paths(x, n);
384 }
385
386 // first, fill in the positional equality constraints
387 for (int i = 0; i + 1 < num_of_points_; ++i) {
388 int index0 = i * 5;
389 int index1 = (i + 1) * 5;
390
391 const auto& spiral_curve = piecewise_paths_[i];
392 double delta_s = spiral_curve.ParamLength();
393
394 double x_diff = x[index1 + 3] - x[index0 + 3] -
395 spiral_curve.ComputeCartesianDeviationX(delta_s);
396 g[i * 2] = x_diff * x_diff;
397
398 double y_diff = x[index1 + 4] - x[index0 + 4] -
399 spiral_curve.ComputeCartesianDeviationY(delta_s);
400 g[i * 2 + 1] = y_diff * y_diff;
401 }
402
403 // second, fill in the positional deviation constraints
404 int constraint_offset = 2 * (num_of_points_ - 1);
405 for (int i = 0; i < num_of_points_; ++i) {
406 int variable_index = i * 5;
407 double x_cor = x[variable_index + 3];
408 double y_cor = x[variable_index + 4];
409
410 double x_diff = x_cor - init_points_[i].x();
411 double y_diff = y_cor - init_points_[i].y();
412
413 g[constraint_offset + i] = x_diff * x_diff + y_diff * y_diff;
414 }
415 return true;
416}

◆ eval_grad_f()

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

Method to return the gradient of the objective

在文件 spiral_problem_interface.cc 第 299 行定义.

300 {
301 CHECK_EQ(n, num_of_variables_);
302 std::fill(grad_f, grad_f + n, 0.0);
303
304 if (new_x) {
305 update_piecewise_spiral_paths(x, n);
306 }
307
308 int variable_offset = num_of_points_ * 5;
309 for (int i = 0; i + 1 < num_of_points_; ++i) {
310 int index0 = i * 5;
311 int index1 = (i + 1) * 5;
312
313 auto& spiral_curve = piecewise_paths_[i];
314 double delta_s = spiral_curve.ParamLength();
315
316 grad_f[variable_offset + i] += weight_curve_length_ * 1.0;
317
318 for (int j = 0; j < num_of_internal_points_; ++j) {
319 double ratio =
320 static_cast<double>(j) / static_cast<double>(num_of_internal_points_);
321 double s = ratio * delta_s;
322
323 double kappa = spiral_curve.Evaluate(1, s);
324 grad_f[index0] += weight_kappa_ * 2.0 * kappa *
325 spiral_curve.DeriveKappaDerivative(
326 THETA0, j, num_of_internal_points_);
327 grad_f[index0 + 1] += weight_kappa_ * 2.0 * kappa *
328 spiral_curve.DeriveKappaDerivative(
329 KAPPA0, j, num_of_internal_points_);
330 grad_f[index0 + 2] += weight_kappa_ * 2.0 * kappa *
331 spiral_curve.DeriveKappaDerivative(
332 DKAPPA0, j, num_of_internal_points_);
333
334 grad_f[index1] += weight_kappa_ * 2.0 * kappa *
335 spiral_curve.DeriveKappaDerivative(
336 THETA1, j, num_of_internal_points_);
337 grad_f[index1 + 1] += weight_kappa_ * 2.0 * kappa *
338 spiral_curve.DeriveKappaDerivative(
339 KAPPA1, j, num_of_internal_points_);
340 grad_f[index1 + 2] += weight_kappa_ * 2.0 * kappa *
341 spiral_curve.DeriveKappaDerivative(
342 DKAPPA1, j, num_of_internal_points_);
343
344 grad_f[variable_offset + i] += weight_kappa_ * 2.0 * kappa *
345 spiral_curve.DeriveKappaDerivative(
346 DELTA_S, j, num_of_internal_points_);
347
348 double dkappa = spiral_curve.Evaluate(2, s);
349 grad_f[index0] += weight_dkappa_ * 2.0 * dkappa *
350 spiral_curve.DeriveDKappaDerivative(
351 THETA0, j, num_of_internal_points_);
352 grad_f[index0 + 1] += weight_dkappa_ * 2.0 * dkappa *
353 spiral_curve.DeriveDKappaDerivative(
354 KAPPA0, j, num_of_internal_points_);
355 grad_f[index0 + 2] += weight_dkappa_ * 2.0 * dkappa *
356 spiral_curve.DeriveDKappaDerivative(
357 DKAPPA0, j, num_of_internal_points_);
358
359 grad_f[index1] += weight_dkappa_ * 2.0 * dkappa *
360 spiral_curve.DeriveDKappaDerivative(
361 THETA1, j, num_of_internal_points_);
362 grad_f[index1 + 1] += weight_dkappa_ * 2.0 * dkappa *
363 spiral_curve.DeriveDKappaDerivative(
364 KAPPA1, j, num_of_internal_points_);
365 grad_f[index1 + 2] += weight_dkappa_ * 2.0 * dkappa *
366 spiral_curve.DeriveDKappaDerivative(
367 DKAPPA1, j, num_of_internal_points_);
368
369 grad_f[variable_offset + i] += weight_dkappa_ * 2.0 * dkappa *
370 spiral_curve.DeriveDKappaDerivative(
371 DELTA_S, j, num_of_internal_points_);
372 }
373 }
374 return true;
375}

◆ eval_h()

bool apollo::planning::SpiralProblemInterface::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)

在文件 spiral_problem_interface.cc 第 653 行定义.

657 {
658 ACHECK(false);
659 return true;
660}
ACHECK
#define ACHECK(cond)
Definition log.h:80

◆ eval_jac_g()

bool apollo::planning::SpiralProblemInterface::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)

在文件 spiral_problem_interface.cc 第 418 行定义.

420 {
421 CHECK_EQ(n, num_of_variables_);
422 CHECK_EQ(m, num_of_constraints_);
423
424 if (values == nullptr) {
425 int nz_index = 0;
426
427 int variable_offset = num_of_points_ * 5;
428 for (int i = 0; i + 1 < num_of_points_; ++i) {
429 int variable_index = i * 5;
430
431 // theta0
432 iRow[nz_index] = i * 2;
433 jCol[nz_index] = variable_index + 0;
434 ++nz_index;
435
436 // kappa0
437 iRow[nz_index] = i * 2;
438 jCol[nz_index] = variable_index + 1;
439 ++nz_index;
440
441 // dkappa0
442 iRow[nz_index] = i * 2;
443 jCol[nz_index] = variable_index + 2;
444 ++nz_index;
445
446 // x0
447 iRow[nz_index] = i * 2;
448 jCol[nz_index] = variable_index + 3;
449 ++nz_index;
450
451 // theta1
452 iRow[nz_index] = i * 2;
453 jCol[nz_index] = variable_index + 5;
454 ++nz_index;
455
456 // kappa1
457 iRow[nz_index] = i * 2;
458 jCol[nz_index] = variable_index + 6;
459 ++nz_index;
460
461 // dkappa1
462 iRow[nz_index] = i * 2;
463 jCol[nz_index] = variable_index + 7;
464 ++nz_index;
465
466 // x1
467 iRow[nz_index] = i * 2;
468 jCol[nz_index] = variable_index + 8;
469 ++nz_index;
470
471 // s
472 iRow[nz_index] = i * 2;
473 jCol[nz_index] = variable_offset + i;
474 ++nz_index;
475
476 // theta0
477 iRow[nz_index] = i * 2 + 1;
478 jCol[nz_index] = variable_index + 0;
479 ++nz_index;
480
481 // kappa0
482 iRow[nz_index] = i * 2 + 1;
483 jCol[nz_index] = variable_index + 1;
484 ++nz_index;
485
486 // dkappa0
487 iRow[nz_index] = i * 2 + 1;
488 jCol[nz_index] = variable_index + 2;
489 ++nz_index;
490
491 // y0
492 iRow[nz_index] = i * 2 + 1;
493 jCol[nz_index] = variable_index + 4;
494 ++nz_index;
495
496 // theta1
497 iRow[nz_index] = i * 2 + 1;
498 jCol[nz_index] = variable_index + 5;
499 ++nz_index;
500
501 // kappa1
502 iRow[nz_index] = i * 2 + 1;
503 jCol[nz_index] = variable_index + 6;
504 ++nz_index;
505
506 // dkappa1
507 iRow[nz_index] = i * 2 + 1;
508 jCol[nz_index] = variable_index + 7;
509 ++nz_index;
510
511 // y1
512 iRow[nz_index] = i * 2 + 1;
513 jCol[nz_index] = variable_index + 9;
514 ++nz_index;
515
516 // s
517 iRow[nz_index] = i * 2 + 1;
518 jCol[nz_index] = variable_offset + i;
519 ++nz_index;
520 }
521
522 int constraint_offset = 2 * (num_of_points_ - 1);
523 for (int i = 0; i < num_of_points_; ++i) {
524 iRow[nz_index] = constraint_offset + i;
525 jCol[nz_index] = i * 5 + 3;
526 ++nz_index;
527
528 iRow[nz_index] = constraint_offset + i;
529 jCol[nz_index] = i * 5 + 4;
530 ++nz_index;
531 }
532
533 CHECK_EQ(nz_index, nele_jac);
534 } else {
535 if (new_x) {
536 update_piecewise_spiral_paths(x, n);
537 }
538
539 std::fill(values, values + nele_jac, 0.0);
540 // first, positional equality constraints
541 int nz_index = 0;
542
543 for (int i = 0; i + 1 < num_of_points_; ++i) {
544 int index0 = i * 5;
545 int index1 = (i + 1) * 5;
546
547 auto& spiral_curve = piecewise_paths_[i];
548 double delta_s = spiral_curve.ParamLength();
549
550 double x_diff = x[index1 + 3] - x[index0 + 3] -
551 spiral_curve.ComputeCartesianDeviationX(delta_s);
552 double y_diff = x[index1 + 4] - x[index0 + 4] -
553 spiral_curve.ComputeCartesianDeviationY(delta_s);
554
555 auto pos_theta0 = spiral_curve.DeriveCartesianDeviation(THETA0);
556 auto pos_kappa0 = spiral_curve.DeriveCartesianDeviation(KAPPA0);
557 auto pos_dkappa0 = spiral_curve.DeriveCartesianDeviation(DKAPPA0);
558
559 auto pos_theta1 = spiral_curve.DeriveCartesianDeviation(THETA1);
560 auto pos_kappa1 = spiral_curve.DeriveCartesianDeviation(KAPPA1);
561 auto pos_dkappa1 = spiral_curve.DeriveCartesianDeviation(DKAPPA1);
562
563 auto pos_delta_s = spiral_curve.DeriveCartesianDeviation(DELTA_S);
564
565 // for x coordinate
566 // theta0
567 values[nz_index] += 2.0 * x_diff * (-pos_theta0.first);
568 ++nz_index;
569
570 // kappa0
571 values[nz_index] += 2.0 * x_diff * (-pos_kappa0.first);
572 ++nz_index;
573
574 // dkappa0
575 values[nz_index] += 2.0 * x_diff * (-pos_dkappa0.first);
576 ++nz_index;
577
578 // x0
579 values[nz_index] += 2.0 * x_diff * (-1.0);
580 ++nz_index;
581
582 // theta1
583 values[nz_index] += 2.0 * x_diff * (-pos_theta1.first);
584 ++nz_index;
585
586 // kappa1
587 values[nz_index] += 2.0 * x_diff * (-pos_kappa1.first);
588 ++nz_index;
589
590 // dkappa1
591 values[nz_index] += 2.0 * x_diff * (-pos_dkappa1.first);
592 ++nz_index;
593
594 // x1
595 values[nz_index] += 2.0 * x_diff;
596 ++nz_index;
597
598 // delta_s
599 values[nz_index] += 2.0 * x_diff * (-pos_delta_s.first);
600 ++nz_index;
601
602 // for y coordinate
603 // theta0
604 values[nz_index] += 2.0 * y_diff * (-pos_theta0.second);
605 ++nz_index;
606
607 // kappa0
608 values[nz_index] += 2.0 * y_diff * (-pos_kappa0.second);
609 ++nz_index;
610
611 // dkappa0
612 values[nz_index] += 2.0 * y_diff * (-pos_dkappa0.second);
613 ++nz_index;
614
615 // y0
616 values[nz_index] += 2.0 * y_diff * (-1.0);
617 ++nz_index;
618
619 // theta1
620 values[nz_index] += 2.0 * y_diff * (-pos_theta1.second);
621 ++nz_index;
622
623 // kappa1
624 values[nz_index] += 2.0 * y_diff * (-pos_kappa1.second);
625 ++nz_index;
626
627 // dkappa1
628 values[nz_index] += 2.0 * y_diff * (-pos_dkappa1.second);
629 ++nz_index;
630
631 // y1
632 values[nz_index] += 2.0 * y_diff;
633 ++nz_index;
634
635 // delta_s
636 values[nz_index] += 2.0 * y_diff * (-pos_delta_s.second);
637 ++nz_index;
638 }
639
640 for (int i = 0; i < num_of_points_; ++i) {
641 values[nz_index] = 2.0 * (x[i * 5 + 3] - init_points_[i].x());
642 ++nz_index;
643
644 values[nz_index] = 2.0 * (x[i * 5 + 4] - init_points_[i].y());
645 ++nz_index;
646 }
647
648 CHECK_EQ(nz_index, nele_jac);
649 }
650 return true;
651}

◆ finalize_solution()

void apollo::planning::SpiralProblemInterface::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

在文件 spiral_problem_interface.cc 第 662 行定义.

666 {
667 opt_theta_.reserve(num_of_points_);
668 opt_kappa_.reserve(num_of_points_);
669 opt_dkappa_.reserve(num_of_points_);
670 opt_x_.reserve(num_of_points_);
671 opt_y_.reserve(num_of_points_);
672 for (int i = 0; i < num_of_points_; ++i) {
673 int index = i * 5;
674 opt_theta_.push_back(x[index]);
675 opt_kappa_.push_back(x[index + 1]);
676 opt_dkappa_.push_back(x[index + 2]);
677 opt_x_.push_back(x[index + 3]);
678 opt_y_.push_back(x[index + 4]);
679 }
680
681 opt_s_.reserve(num_of_points_ - 1);
682 int variable_offset = num_of_points_ * 5;
683 for (int i = 0; i + 1 < num_of_points_; ++i) {
684 opt_s_.push_back(x[variable_offset + i]);
685 }
686}

◆ get_bounds_info()

bool apollo::planning::SpiralProblemInterface::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

在文件 spiral_problem_interface.cc 第 114 行定义.

115 {
116 CHECK_EQ(n, num_of_variables_);
117 CHECK_EQ(m, num_of_constraints_);
118
119 // variables
120 // a. for theta, kappa, dkappa, x, y
121 for (int i = 0; i < num_of_points_; ++i) {
122 int index = i * 5;
123
124 double theta_lower = 0.0;
125 double theta_upper = 0.0;
126 double kappa_lower = 0.0;
127 double kappa_upper = 0.0;
128 double dkappa_lower = 0.0;
129 double dkappa_upper = 0.0;
130 double x_lower = 0.0;
131 double x_upper = 0.0;
132 double y_lower = 0.0;
133 double y_upper = 0.0;
134 if (i == 0 && has_fixed_start_point_) {
135 theta_lower = start_theta_;
136 theta_upper = start_theta_;
137 kappa_lower = start_kappa_;
138 kappa_upper = start_kappa_;
139 dkappa_lower = start_dkappa_;
140 dkappa_upper = start_dkappa_;
141 x_lower = start_x_;
142 x_upper = start_x_;
143 y_lower = start_y_;
144 y_upper = start_y_;
145
146 } else if (i + 1 == num_of_points_ && has_fixed_end_point_) {
147 theta_lower = end_theta_;
148 theta_upper = end_theta_;
149 kappa_lower = end_kappa_;
150 kappa_upper = end_kappa_;
151 dkappa_lower = end_dkappa_;
152 dkappa_upper = end_dkappa_;
153 x_lower = end_x_;
154 x_upper = end_x_;
155 y_lower = end_y_;
156 y_upper = end_y_;
157 } else if (i + 1 == num_of_points_ && has_fixed_end_point_position_) {
158 theta_lower = relative_theta_[i] - M_PI * 0.2;
159 theta_upper = relative_theta_[i] + M_PI * 0.2;
160 kappa_lower = -0.25;
161 kappa_upper = 0.25;
162 dkappa_lower = -0.02;
163 dkappa_upper = 0.02;
164 x_lower = end_x_;
165 x_upper = end_x_;
166 y_lower = end_y_;
167 y_upper = end_y_;
168 } else {
169 theta_lower = relative_theta_[i] - M_PI * 0.2;
170 theta_upper = relative_theta_[i] + M_PI * 0.2;
171 kappa_lower = -0.25;
172 kappa_upper = 0.25;
173 dkappa_lower = -0.02;
174 dkappa_upper = 0.02;
175 x_lower = init_points_[i].x() - default_max_point_deviation_;
176 x_upper = init_points_[i].x() + default_max_point_deviation_;
177 y_lower = init_points_[i].y() - default_max_point_deviation_;
178 y_upper = init_points_[i].y() + default_max_point_deviation_;
179 }
180
181 // theta
182 x_l[index] = theta_lower;
183 x_u[index] = theta_upper;
184
185 // kappa
186 x_l[index + 1] = kappa_lower;
187 x_u[index + 1] = kappa_upper;
188
189 // dkappa
190 x_l[index + 2] = dkappa_lower;
191 x_u[index + 2] = dkappa_upper;
192
193 // x
194 x_l[index + 3] = x_lower;
195 x_u[index + 3] = x_upper;
196
197 // y
198 x_l[index + 4] = y_lower;
199 x_u[index + 4] = y_upper;
200 }
201
202 // b. for delta_s
203 int variable_offset = num_of_points_ * 5;
204 for (int i = 0; i + 1 < num_of_points_; ++i) {
205 x_l[variable_offset + i] =
206 point_distances_[i] - 2.0 * default_max_point_deviation_;
207 x_u[variable_offset + i] = point_distances_[i] * M_PI * 0.5;
208 }
209
210 // constraints
211 // a. positional equality constraints
212 for (int i = 0; i + 1 < num_of_points_; ++i) {
213 // for x
214 g_l[i * 2] = 0.0;
215 g_u[i * 2] = 0.0;
216
217 // for y
218 g_l[i * 2 + 1] = 0.0;
219 g_u[i * 2 + 1] = 0.0;
220 }
221 // b. positional deviation constraints
222 int constraint_offset = 2 * (num_of_points_ - 1);
223 for (int i = 0; i < num_of_points_; ++i) {
224 g_l[constraint_offset + i] = 0.0;
225 g_u[constraint_offset + i] =
226 default_max_point_deviation_ * default_max_point_deviation_;
227 }
228 return true;
229}

◆ get_nlp_info()

bool apollo::planning::SpiralProblemInterface::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

在文件 spiral_problem_interface.cc 第 89 行定义.

91 {
92 // number of variables
93 n = num_of_points_ * 5 + num_of_points_ - 1;
94 num_of_variables_ = n;
95
96 // number of constraints
97 // b. positional equality constraints;
98 // totally 2 * (num_of_points - 1) considering x and y separately
99 m = (num_of_points_ - 1) * 2;
100 // a. positional movements; totally num_of_points
101 m += num_of_points_;
102 num_of_constraints_ = m;
103
104 // number of nonzero constraint jacobian.
105 nnz_jac_g = (num_of_points_ - 1) * 2 * 9 + num_of_points_ * 2;
106
107 // number of nonzero hessian and lagrangian.
108 nnz_h_lag = 0;
109
110 index_style = IndexStyleEnum::C_STYLE;
111 return true;
112}

◆ get_optimization_results()

void apollo::planning::SpiralProblemInterface::get_optimization_results ( std::vector< double > *  ptr_theta,
std::vector< double > *  ptr_kappa,
std::vector< double > *  ptr_dkappa,
std::vector< double > *  ptr_s,
std::vector< double > *  ptr_x,
std::vector< double > *  ptr_y 
) const

在文件 spiral_problem_interface.cc 第 77 行定义.

80 {
81 *ptr_theta = opt_theta_;
82 *ptr_kappa = opt_kappa_;
83 *ptr_dkappa = opt_dkappa_;
84 *ptr_s = opt_s_;
85 *ptr_x = opt_x_;
86 *ptr_y = opt_y_;
87}

◆ get_starting_point()

bool apollo::planning::SpiralProblemInterface::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

在文件 spiral_problem_interface.cc 第 231 行定义.

235 {
236 CHECK_EQ(n, num_of_variables_);
237 ACHECK(init_x);
238 ACHECK(!init_z);
239 ACHECK(!init_lambda);
240
241 for (int i = 0; i < num_of_points_; ++i) {
242 int index = i * 5;
243 x[index] = relative_theta_[i];
244 x[index + 1] = 0.0;
245 x[index + 2] = 0.0;
246 x[index + 3] = init_points_[i].x();
247 x[index + 4] = init_points_[i].y();
248 }
249
250 int variable_offset = num_of_points_ * 5;
251 for (int i = 0; i + 1 < num_of_points_; ++i) {
252 double delta_theta = relative_theta_[i + 1] - relative_theta_[i];
253 x[variable_offset + i] = point_distances_[i] / std::cos(0.5 * delta_theta);
254 }
255
256 for (int i = 0; i + 1 < num_of_points_; ++i) {
257 double delta_theta = relative_theta_[i + 1] - relative_theta_[i];
258 x[(i + 1) * 5 + 1] = delta_theta / x[variable_offset + i];
259 }
260 x[1] = x[6];
261
262 if (has_fixed_start_point_) {
263 x[0] = start_theta_;
264 x[1] = start_kappa_;
265 x[2] = start_dkappa_;
266 }
267 return true;
268}

◆ set_default_max_point_deviation()

void apollo::planning::SpiralProblemInterface::set_default_max_point_deviation ( const double  point_max_deviation)

在文件 spiral_problem_interface.cc 第 703 行定义.

704 {
705 default_max_point_deviation_ = max_point_deviation;
706}

◆ set_element_weight_curve_length()

void apollo::planning::SpiralProblemInterface::set_element_weight_curve_length ( const double  weight_curve_length)

在文件 spiral_problem_interface.cc 第 739 行定义.

740 {
741 weight_curve_length_ = weight_curve_length;
742}

◆ set_element_weight_dkappa()

void apollo::planning::SpiralProblemInterface::set_element_weight_dkappa ( const double  weight_dkappa)

在文件 spiral_problem_interface.cc 第 749 行定义.

750 {
751 weight_dkappa_ = weight_dkappa;
752}

◆ set_element_weight_kappa()

void apollo::planning::SpiralProblemInterface::set_element_weight_kappa ( const double  weight_kappa)

在文件 spiral_problem_interface.cc 第 744 行定义.

745 {
746 weight_kappa_ = weight_kappa;
747}

◆ set_end_point()

void apollo::planning::SpiralProblemInterface::set_end_point ( const double  x,
const double  y,
const double  theta,
const double  kappa,
const double  dkappa 
)

在文件 spiral_problem_interface.cc 第 720 行定义.

723 {
724 has_fixed_end_point_ = true;
725 end_x_ = x;
726 end_y_ = y;
727 end_theta_ = theta;
728 end_kappa_ = kappa;
729 end_dkappa_ = dkappa;
730}

◆ set_end_point_position()

void apollo::planning::SpiralProblemInterface::set_end_point_position ( const double  x,
const double  y 
)

在文件 spiral_problem_interface.cc 第 732 行定义.

733 {
734 has_fixed_end_point_position_ = true;
735 end_x_ = x;
736 end_y_ = y;
737}

◆ set_start_point()

void apollo::planning::SpiralProblemInterface::set_start_point ( const double  x,
const double  y,
const double  theta,
const double  kappa,
const double  dkappa 
)

在文件 spiral_problem_interface.cc 第 708 行定义.

711 {
712 has_fixed_start_point_ = true;
713 start_x_ = x;
714 start_y_ = y;
715 start_theta_ = theta;
716 start_kappa_ = kappa;
717 start_dkappa_ = dkappa;
718}

类成员变量说明

◆ N

constexpr size_t apollo::planning::SpiralProblemInterface::N = 10
staticconstexpr

在文件 spiral_problem_interface.h 第 115 行定义.

该类的文档由以下文件生成: