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

#include <dual_variable_warm_start_slack_osqp_interface.h>

apollo::planning::DualVariableWarmStartSlackOSQPInterface 的协作图:

Public 成员函数

 DualVariableWarmStartSlackOSQPInterface (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 ~DualVariableWarmStartSlackOSQPInterface ()=default
 
void get_optimization_results (Eigen::MatrixXd *l_warm_up, Eigen::MatrixXd *n_warm_up, Eigen::MatrixXd *s_warm_up) const
 
bool optimize ()
 
void assembleP (std::vector< c_float > *P_data, std::vector< c_int > *P_indices, std::vector< c_int > *P_indptr)
 
void assembleConstraint (std::vector< c_float > *A_data, std::vector< c_int > *A_indices, std::vector< c_int > *A_indptr)
 
void assembleA (const int r, const int c, const std::vector< c_float > &P_data, const std::vector< c_int > &P_indices, const std::vector< c_int > &P_indptr)
 
void checkSolution (const Eigen::MatrixXd &l_warm_up, const Eigen::MatrixXd &n_warm_up)
 
void printMatrix (const int r, const int c, const std::vector< c_float > &P_data, const std::vector< c_int > &P_indices, const std::vector< c_int > &P_indptr)
 
 DualVariableWarmStartSlackOSQPInterface (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_slack_osqp_interface.h 第 37 行定义.

构造及析构函数说明

◆ DualVariableWarmStartSlackOSQPInterface() [1/2]

apollo::planning::DualVariableWarmStartSlackOSQPInterface::DualVariableWarmStartSlackOSQPInterface ( 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_slack_osqp_interface.cc 第 33 行定义.

42 :
43 ts_(ts),
44 ego_(ego),
45 obstacles_edges_num_(obstacles_edges_num),
46 obstacles_A_(obstacles_A),
47 obstacles_b_(obstacles_b),
48 xWS_(xWS) {
49 ACHECK(horizon < std::numeric_limits<int>::max()) << "Invalid cast on horizon in open space planner";
50 horizon_ = static_cast<int>(horizon);
51 ACHECK(obstacles_num < std::numeric_limits<int>::max()) << "Invalid cast on obstacles_num in open space planner";
52 obstacles_num_ = static_cast<int>(obstacles_num);
53 w_ev_ = ego_(1, 0) + ego_(3, 0);
54 l_ev_ = ego_(0, 0) + ego_(2, 0);
55 g_ = {l_ev_ / 2, w_ev_ / 2, l_ev_ / 2, w_ev_ / 2};
56 offset_ = (ego_(0, 0) + ego_(2, 0)) / 2 - ego_(2, 0);
57 obstacles_edges_sum_ = obstacles_edges_num_.sum();
58
59 l_start_index_ = 0;
60 n_start_index_ = l_start_index_ + obstacles_edges_sum_ * (horizon_ + 1);
61 s_start_index_ = n_start_index_ + 4 * obstacles_num_ * (horizon_ + 1);
62
63 l_warm_up_ = Eigen::MatrixXd::Zero(obstacles_edges_sum_, horizon_ + 1);
64 n_warm_up_ = Eigen::MatrixXd::Zero(4 * obstacles_num_, horizon_ + 1);
65 slacks_ = Eigen::MatrixXd::Zero(obstacles_num_, horizon_ + 1);
66
67 // get_nlp_info
68 lambda_horizon_ = obstacles_edges_sum_ * (horizon_ + 1);
69 miu_horizon_ = obstacles_num_ * 4 * (horizon_ + 1);
70 slack_horizon_ = obstacles_num_ * (horizon_ + 1);
71
72 // number of variables
73 num_of_variables_ = lambda_horizon_ + miu_horizon_ + slack_horizon_;
74 // number of constraints
75 num_of_constraints_ = 3 * obstacles_num_ * (horizon_ + 1) + num_of_variables_;
76
77 min_safety_distance_ = planner_open_space_config.dual_variable_warm_start_config().min_safety_distance();
78 check_mode_ = planner_open_space_config.dual_variable_warm_start_config().debug_osqp();
79 beta_ = planner_open_space_config.dual_variable_warm_start_config().beta();
80 osqp_config_ = planner_open_space_config.dual_variable_warm_start_config().osqp_config();
81}
ACHECK
#define ACHECK(cond)
Definition log.h:80

◆ ~DualVariableWarmStartSlackOSQPInterface()

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

◆ DualVariableWarmStartSlackOSQPInterface() [2/2]

apollo::planning::DualVariableWarmStartSlackOSQPInterface::DualVariableWarmStartSlackOSQPInterface ( 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_slack_osqp_interface.cc 第 83 行定义.

92 :
93 ts_(ts),
94 ego_(ego),
95 obstacles_edges_num_(obstacles_edges_num),
96 obstacles_A_(obstacles_A),
97 obstacles_b_(obstacles_b),
98 xWS_(xWS) {
99 ACHECK(horizon < std::numeric_limits<int>::max()) << "Invalid cast on horizon in open space planner";
100 horizon_ = static_cast<int>(horizon);
101 ACHECK(obstacles_num < std::numeric_limits<int>::max()) << "Invalid cast on obstacles_num in open space planner";
102 obstacles_num_ = static_cast<int>(obstacles_num);
103 w_ev_ = ego_(1, 0) + ego_(3, 0);
104 l_ev_ = ego_(0, 0) + ego_(2, 0);
105 g_ = {l_ev_ / 2, w_ev_ / 2, l_ev_ / 2, w_ev_ / 2};
106 offset_ = (ego_(0, 0) + ego_(2, 0)) / 2 - ego_(2, 0);
107 obstacles_edges_sum_ = obstacles_edges_num_.sum();
108
109 l_start_index_ = 0;
110 n_start_index_ = l_start_index_ + obstacles_edges_sum_ * (horizon_ + 1);
111 s_start_index_ = n_start_index_ + 4 * obstacles_num_ * (horizon_ + 1);
112
113 l_warm_up_ = Eigen::MatrixXd::Zero(obstacles_edges_sum_, horizon_ + 1);
114 n_warm_up_ = Eigen::MatrixXd::Zero(4 * obstacles_num_, horizon_ + 1);
115 slacks_ = Eigen::MatrixXd::Zero(obstacles_num_, horizon_ + 1);
116
117 // get_nlp_info
118 lambda_horizon_ = obstacles_edges_sum_ * (horizon_ + 1);
119 miu_horizon_ = obstacles_num_ * 4 * (horizon_ + 1);
120 slack_horizon_ = obstacles_num_ * (horizon_ + 1);
121
122 // number of variables
123 num_of_variables_ = lambda_horizon_ + miu_horizon_ + slack_horizon_;
124 // number of constraints
125 num_of_constraints_ = 3 * obstacles_num_ * (horizon_ + 1) + num_of_variables_;
126
127 min_safety_distance_ = dual_variable_warm_start_config.min_safety_distance();
128 check_mode_ = dual_variable_warm_start_config.debug_osqp();
129 beta_ = dual_variable_warm_start_config.beta();
130 osqp_config_ = dual_variable_warm_start_config.osqp_config();
131}

成员函数说明

◆ assembleA()

void apollo::planning::DualVariableWarmStartSlackOSQPInterface::assembleA ( const int  r,
const int  c,
const std::vector< c_float > &  P_data,
const std::vector< c_int > &  P_indices,
const std::vector< c_int > &  P_indptr 
)

在文件 dual_variable_warm_start_slack_osqp_interface.cc 第 161 行定义.

166 {
167 constraint_A_ = Eigen::MatrixXf::Zero(r, c);
168
169 for (size_t i = 0; i < P_indptr.size() - 1; ++i) {
170 if (P_indptr[i] < 0 || P_indptr[i] >= static_cast<int>(P_indices.size())) {
171 continue;
172 }
173
174 for (auto idx = P_indptr[i]; idx < P_indptr[i + 1]; ++idx) {
175 int tmp_c = static_cast<int>(i);
176 int tmp_r = static_cast<int>(P_indices[idx]);
177 constraint_A_(tmp_r, tmp_c) = static_cast<float>(P_data[idx]);
178 }
179 }
180}

◆ assembleConstraint()

void apollo::planning::DualVariableWarmStartSlackOSQPInterface::assembleConstraint ( std::vector< c_float > *  A_data,
std::vector< c_int > *  A_indices,
std::vector< c_int > *  A_indptr 
)

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

421 {
422 /*
423 * The constraint matrix is as the form,
424 * |R' * A', G', 0|, #: 2 * obstacles_num_ * (horizon_ + 1)
425 * |A * t - b, -g, I|, #: obstacles_num_ * (horizon_ + 1)
426 * |I, 0, 0|, #: num_of_lambda
427 * |0, I, 0|, #: num_of_miu
428 * |0, 0, I|, #: num_of_slack
429 */
430 int r1_index = 0;
431 int r2_index = 2 * obstacles_num_ * (horizon_ + 1);
432 int r3_index = 3 * obstacles_num_ * (horizon_ + 1);
433 int r4_index = 3 * obstacles_num_ * (horizon_ + 1) + lambda_horizon_;
434 int r5_index = r4_index + miu_horizon_;
435 int first_row_location = 0;
436
437 // lambda variables
438 // lambda_horizon_ = obstacles_edges_sum_ * (horizon_ + 1);
439 for (int i = 0; i < horizon_ + 1; ++i) {
440 int edges_counter = 0;
441
442 Eigen::MatrixXd R(2, 2);
443 R << cos(xWS_(2, i)), sin(xWS_(2, i)), sin(xWS_(2, i)), cos(xWS_(2, i));
444
445 Eigen::MatrixXd t_trans(1, 2);
446 t_trans << (xWS_(0, i) + cos(xWS_(2, i)) * offset_), (xWS_(1, i) + sin(xWS_(2, i)) * offset_);
447
448 // assume: stationary obstacles
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 Eigen::MatrixXd r1_block(2, current_edges_num);
455 r1_block = R * Aj.transpose();
456
457 Eigen::MatrixXd r2_block(1, current_edges_num);
458 r2_block = t_trans * Aj.transpose() - bj.transpose();
459
460 // insert into A matrix, col by col
461 for (int k = 0; k < current_edges_num; ++k) {
462 A_data->emplace_back(r1_block(0, k));
463 A_indices->emplace_back(r1_index);
464
465 A_data->emplace_back(r1_block(1, k));
466 A_indices->emplace_back(r1_index + 1);
467
468 A_data->emplace_back(r2_block(0, k));
469 A_indices->emplace_back(r2_index);
470
471 A_data->emplace_back(1.0);
472 A_indices->emplace_back(r3_index);
473 r3_index++;
474
475 A_indptr->emplace_back(first_row_location);
476 first_row_location += 4;
477 }
478
479 // Update index
480 edges_counter += current_edges_num;
481 r1_index += 2;
482 r2_index += 1;
483 }
484 }
485
486 // miu variables, miu_horizon_ = obstacles_num_ * 4 * (horizon_ + 1);
487 // G: ((1, 0, -1, 0), (0, 1, 0, -1))
488 // g: g_
489 r1_index = 0;
490 r2_index = 2 * obstacles_num_ * (horizon_ + 1);
491 for (int i = 0; i < horizon_ + 1; ++i) {
492 for (int j = 0; j < obstacles_num_; ++j) {
493 for (int k = 0; k < 4; ++k) {
494 // update G
495 if (k < 2) {
496 A_data->emplace_back(1.0);
497 } else {
498 A_data->emplace_back(-1.0);
499 }
500 A_indices->emplace_back(r1_index + k % 2);
501
502 // update g'
503 A_data->emplace_back(-g_[k]);
504 A_indices->emplace_back(r2_index);
505
506 // update I
507 A_data->emplace_back(1.0);
508 A_indices->emplace_back(r4_index);
509 r4_index++;
510
511 // update col index
512 A_indptr->emplace_back(first_row_location);
513 first_row_location += 3;
514 }
515
516 // update index
517 r1_index += 2;
518 r2_index += 1;
519 }
520 }
521
522 // slack variables
523 // slack_horizon_ = obstacles_edges_sum_ * (horizon_ + 1);
524 r2_index = 2 * obstacles_num_ * (horizon_ + 1);
525 for (int i = 0; i < horizon_ + 1; ++i) {
526 for (int j = 0; j < obstacles_num_; ++j) {
527 A_data->emplace_back(1.0);
528 A_indices->emplace_back(r2_index);
529 ++r2_index;
530
531 A_data->emplace_back(1.0);
532 A_indices->emplace_back(r5_index);
533 ++r5_index;
534
535 A_indptr->emplace_back(first_row_location);
536 first_row_location += 2;
537 }
538 }
539
540 A_indptr->emplace_back(first_row_location);
541
542 CHECK_EQ(A_data->size(), A_indices->size());
543 CHECK_EQ(A_indptr->size(), static_cast<size_t>(num_of_variables_) + 1);
544}
apollo::common::math::cos
float cos(Angle16 a)
Definition angle.cc:42
apollo::common::math::sin
float sin(Angle16 a)
Definition angle.cc:25

◆ assembleP()

void apollo::planning::DualVariableWarmStartSlackOSQPInterface::assembleP ( std::vector< c_float > *  P_data,
std::vector< c_int > *  P_indices,
std::vector< c_int > *  P_indptr 
)

在文件 dual_variable_warm_start_slack_osqp_interface.cc 第 354 行定义.

357 {
358 // the objective function is norm(A' * lambda)
359 std::vector<c_float> P_tmp;
360 int edges_counter = 0;
361
362 for (int j = 0; j < obstacles_num_; ++j) {
363 int current_edges_num = obstacles_edges_num_(j, 0);
364 Eigen::MatrixXd Aj;
365 Aj = obstacles_A_.block(edges_counter, 0, current_edges_num, 2);
366 // Eigen::MatrixXd AAj(current_edges_num, current_edges_num);
367 Aj = Aj * Aj.transpose();
368
369 CHECK_EQ(current_edges_num, Aj.cols());
370 CHECK_EQ(current_edges_num, Aj.rows());
371
372 for (int c = 0; c < current_edges_num; ++c) {
373 for (int r = 0; r < current_edges_num; ++r) {
374 P_tmp.emplace_back(Aj(r, c));
375 }
376 }
377
378 // Update index
379 edges_counter += current_edges_num;
380 }
381
382 int l_index = l_start_index_;
383 int first_row_location = 0;
384 // the objective function is norm(A' * lambda)
385 for (int i = 0; i < horizon_ + 1; ++i) {
386 edges_counter = 0;
387
388 for (auto item : P_tmp) {
389 P_data->emplace_back(item);
390 }
391 // current assumption: stationary obstacles
392 for (int j = 0; j < obstacles_num_; ++j) {
393 int current_edges_num = obstacles_edges_num_(j, 0);
394
395 for (int c = 0; c < current_edges_num; ++c) {
396 P_indptr->emplace_back(first_row_location);
397 for (int r = 0; r < current_edges_num; ++r) {
398 P_indices->emplace_back(r + l_index);
399 }
400 first_row_location += current_edges_num;
401 }
402
403 // Update index
404 edges_counter += current_edges_num;
405 l_index += current_edges_num;
406 }
407 }
408
409 CHECK_EQ(P_indptr->size(), static_cast<size_t>(lambda_horizon_));
410 for (int i = lambda_horizon_; i < num_of_variables_ + 1; ++i) {
411 P_indptr->emplace_back(first_row_location);
412 }
413
414 CHECK_EQ(P_data->size(), P_indices->size());
415 CHECK_EQ(P_indptr->size(), static_cast<size_t>(num_of_variables_) + 1);
416}

◆ checkSolution()

void apollo::planning::DualVariableWarmStartSlackOSQPInterface::checkSolution ( const Eigen::MatrixXd &  l_warm_up,
const Eigen::MatrixXd &  n_warm_up 
)

在文件 dual_variable_warm_start_slack_osqp_interface.cc 第 348 行定义.

350 {
351 // TODO(Runxin): extend
352}

◆ get_optimization_results()

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

在文件 dual_variable_warm_start_slack_osqp_interface.cc 第 546 行定义.

549 {
550 *l_warm_up = l_warm_up_;
551 *n_warm_up = n_warm_up_;
552 *s_warm_up = slacks_;
553 // debug mode check slack values
554 double max_s = slacks_(0, 0);
555 double min_s = slacks_(0, 0);
556
557 for (int i = 0; i < horizon_ + 1; ++i) {
558 for (int j = 0; j < obstacles_num_; ++j) {
559 max_s = std::max(max_s, slacks_(j, i));
560 min_s = std::min(min_s, slacks_(j, i));
561 }
562 }
563
564 ADEBUG << "max_s: " << max_s;
565 ADEBUG << "min_s: " << min_s;
566}
ADEBUG
#define ADEBUG
Definition log.h:41

◆ optimize()

bool apollo::planning::DualVariableWarmStartSlackOSQPInterface::optimize ( )

在文件 dual_variable_warm_start_slack_osqp_interface.cc 第 182 行定义.

182 {
183 // int kNumParam = num_of_variables_;
184 // int kNumConst = num_of_constraints_;
185
186 bool succ = true;
187 // assemble P, quadratic term in objective
188 std::vector<c_float> P_data;
189 std::vector<c_int> P_indices;
190 std::vector<c_int> P_indptr;
191 assembleP(&P_data, &P_indices, &P_indptr);
192 if (check_mode_) {
193 AINFO << "print P_data in whole: ";
194 printMatrix(num_of_variables_, num_of_variables_, P_data, P_indices, P_indptr);
195 }
196 // assemble q, linear term in objective, \sum{beta * slacks}
197 c_float q[num_of_variables_]; // NOLINT
198 for (int i = 0; i < num_of_variables_; ++i) {
199 q[i] = 0.0;
200 if (i >= s_start_index_) {
201 q[i] = beta_;
202 }
203 }
204
205 // assemble A, linear term in constraints
206 std::vector<c_float> A_data;
207 std::vector<c_int> A_indices;
208 std::vector<c_int> A_indptr;
209 assembleConstraint(&A_data, &A_indices, &A_indptr);
210 if (check_mode_) {
211 AINFO << "print A_data in whole: ";
212 printMatrix(num_of_constraints_, num_of_variables_, A_data, A_indices, A_indptr);
213 assembleA(num_of_constraints_, num_of_variables_, A_data, A_indices, A_indptr);
214 }
215
216 // assemble lb & ub, slack_variable <= 0
217 c_float lb[num_of_constraints_]; // NOLINT
218 c_float ub[num_of_constraints_]; // NOLINT
219 int slack_indx = num_of_constraints_ - slack_horizon_;
220 for (int i = 0; i < num_of_constraints_; ++i) {
221 lb[i] = 0.0;
222 if (i >= slack_indx) {
223 lb[i] = -2e19;
224 }
225
226 if (i < 2 * obstacles_num_ * (horizon_ + 1)) {
227 ub[i] = 0.0;
228 } else if (i < slack_indx) {
229 ub[i] = 2e19;
230 } else {
231 ub[i] = 0.0;
232 }
233 }
234
235 // Problem settings
236 OSQPSettings* settings = reinterpret_cast<OSQPSettings*>(c_malloc(sizeof(OSQPSettings)));
237
238 // Define Solver settings as default
239 osqp_set_default_settings(settings);
240 settings->alpha = osqp_config_.alpha(); // Change alpha parameter
241 settings->eps_abs = osqp_config_.eps_abs();
242 settings->eps_rel = osqp_config_.eps_rel();
243 settings->max_iter = osqp_config_.max_iter();
244 settings->polish = osqp_config_.polish();
245 settings->verbose = osqp_config_.osqp_debug_log();
246
247 // Populate data
248 OSQPData* data = reinterpret_cast<OSQPData*>(c_malloc(sizeof(OSQPData)));
249 data->n = num_of_variables_;
250 data->m = num_of_constraints_;
251 data->P = csc_matrix(data->n, data->n, P_data.size(), P_data.data(), P_indices.data(), P_indptr.data());
252 data->q = q;
253 data->A = csc_matrix(data->m, data->n, A_data.size(), A_data.data(), A_indices.data(), A_indptr.data());
254 data->l = lb;
255 data->u = ub;
256
257 // Workspace
258 OSQPWorkspace* work = nullptr;
259 // osqp_setup(&work, data, settings);
260 work = osqp_setup(data, settings);
261
262 // Solve Problem
263 osqp_solve(work);
264
265 // check state
266 if (work->info->status_val != 1 && work->info->status_val != 2) {
267 AWARN << "OSQP dual warm up unsuccess, "
268 << "return status: " << work->info->status;
269 succ = false;
270 }
271
272 // transfer to make lambda's norm under 1
273 std::vector<double> lambda_norm;
274 int l_index = l_start_index_;
275 for (int i = 0; i < horizon_ + 1; ++i) {
276 int edges_counter = 0;
277 for (int j = 0; j < obstacles_num_; ++j) {
278 int current_edges_num = obstacles_edges_num_(j, 0);
279 Eigen::MatrixXd Aj = obstacles_A_.block(edges_counter, 0, current_edges_num, 2);
280
281 double tmp1 = 0;
282 double tmp2 = 0;
283 for (int k = 0; k < current_edges_num; ++k) {
284 tmp1 += Aj(k, 0) * work->solution->x[l_index + k];
285 tmp2 += Aj(k, 1) * work->solution->x[l_index + k];
286 }
287
288 // norm(A * lambda)
289 double tmp_norm = tmp1 * tmp1 + tmp2 * tmp2;
290 if (tmp_norm >= 1e-5) {
291 lambda_norm.push_back(0.75 / std::sqrt(tmp_norm));
292 } else {
293 lambda_norm.push_back(0.0);
294 }
295
296 edges_counter += current_edges_num;
297 l_index += current_edges_num;
298 }
299 }
300
301 // extract primal results
302 int variable_index = 0;
303 // 1. lagrange constraint l, [0, obstacles_edges_sum_ - 1] * [0,
304 // horizon_l
305 for (int i = 0; i < horizon_ + 1; ++i) {
306 int r_index = 0;
307 for (int j = 0; j < obstacles_num_; ++j) {
308 for (int k = 0; k < obstacles_edges_num_(j, 0); ++k) {
309 l_warm_up_(r_index, i) = lambda_norm[i * obstacles_num_ + j] * work->solution->x[variable_index];
310 ++variable_index;
311 ++r_index;
312 }
313 }
314 }
315
316 // 2. lagrange constraint n, [0, 4*obstacles_num-1] * [0, horizon_]
317 for (int i = 0; i < horizon_ + 1; ++i) {
318 int r_index = 0;
319 for (int j = 0; j < obstacles_num_; ++j) {
320 for (int k = 0; k < 4; ++k) {
321 n_warm_up_(r_index, i) = lambda_norm[i * obstacles_num_ + j] * work->solution->x[variable_index];
322 ++r_index;
323 ++variable_index;
324 }
325 }
326 }
327
328 // 3. slack variables
329 for (int i = 0; i < horizon_ + 1; ++i) {
330 for (int j = 0; j < obstacles_num_; ++j) {
331 slacks_(j, i) = lambda_norm[i * obstacles_num_ + j] * work->solution->x[variable_index];
332 ++variable_index;
333 }
334 }
335
336 succ = succ & (work->info->obj_val <= 1.0);
337
338 // Cleanup
339 osqp_cleanup(work);
340 c_free(data->A);
341 c_free(data->P);
342 c_free(data);
343 c_free(settings);
344
345 return succ;
346}
apollo::planning::DualVariableWarmStartSlackOSQPInterface::assembleP
void assembleP(std::vector< c_float > *P_data, std::vector< c_int > *P_indices, std::vector< c_int > *P_indptr)
Definition dual_variable_warm_start_slack_osqp_interface.cc:354
apollo::planning::DualVariableWarmStartSlackOSQPInterface::assembleA
void assembleA(const int r, const int c, const std::vector< c_float > &P_data, const std::vector< c_int > &P_indices, const std::vector< c_int > &P_indptr)
Definition dual_variable_warm_start_slack_osqp_interface.cc:161
apollo::planning::DualVariableWarmStartSlackOSQPInterface::printMatrix
void printMatrix(const int r, const int c, const std::vector< c_float > &P_data, const std::vector< c_int > &P_indices, const std::vector< c_int > &P_indptr)
Definition dual_variable_warm_start_slack_osqp_interface.cc:133
apollo::planning::DualVariableWarmStartSlackOSQPInterface::assembleConstraint
void assembleConstraint(std::vector< c_float > *A_data, std::vector< c_int > *A_indices, std::vector< c_int > *A_indptr)
Definition dual_variable_warm_start_slack_osqp_interface.cc:418
AINFO
#define AINFO
Definition log.h:42
AWARN
#define AWARN
Definition log.h:43

◆ printMatrix()

void apollo::planning::DualVariableWarmStartSlackOSQPInterface::printMatrix ( const int  r,
const int  c,
const std::vector< c_float > &  P_data,
const std::vector< c_int > &  P_indices,
const std::vector< c_int > &  P_indptr 
)

在文件 dual_variable_warm_start_slack_osqp_interface.cc 第 133 行定义.

138 {
139 Eigen::MatrixXf tmp = Eigen::MatrixXf::Zero(r, c);
140
141 for (size_t i = 0; i < P_indptr.size() - 1; ++i) {
142 if (P_indptr[i] < 0 || P_indptr[i] >= static_cast<int>(P_indices.size())) {
143 continue;
144 }
145
146 for (auto idx = P_indptr[i]; idx < P_indptr[i + 1]; ++idx) {
147 int tmp_c = static_cast<int>(i);
148 int tmp_r = static_cast<int>(P_indices[idx]);
149 tmp(tmp_r, tmp_c) = static_cast<float>(P_data[idx]);
150 }
151 }
152
153 AINFO << "row number: " << r;
154 AINFO << "col number: " << c;
155 for (int i = 0; i < r; ++i) {
156 AINFO << "row number: " << i;
157 AINFO << tmp.row(i);
158 }
159}
该类的文档由以下文件生成: