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

40 : ts_(ts),
41 ego_(ego),
42 obstacles_edges_num_(obstacles_edges_num),
43 obstacles_A_(obstacles_A),
44 obstacles_b_(obstacles_b),
45 xWS_(xWS) {
46 ACHECK(horizon < std::numeric_limits<int>::max())
47 << "Invalid cast on horizon in open space planner";
48 horizon_ = static_cast<int>(horizon);
49 ACHECK(obstacles_num < std::numeric_limits<int>::max())
50 << "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
58 l_start_index_ = 0;
59 n_start_index_ = l_start_index_ + obstacles_edges_sum_ * (horizon_ + 1);
60 s_start_index_ = n_start_index_ + 4 * obstacles_num_ * (horizon_ + 1);
61
62 l_warm_up_ = Eigen::MatrixXd::Zero(obstacles_edges_sum_, horizon_ + 1);
63 n_warm_up_ = Eigen::MatrixXd::Zero(4 * obstacles_num_, horizon_ + 1);
64 slacks_ = Eigen::MatrixXd::Zero(obstacles_num_, horizon_ + 1);
65
66 // get_nlp_info
67 lambda_horizon_ = obstacles_edges_sum_ * (horizon_ + 1);
68 miu_horizon_ = obstacles_num_ * 4 * (horizon_ + 1);
69 slack_horizon_ = obstacles_num_ * (horizon_ + 1);
70
71 // number of variables
72 num_of_variables_ = lambda_horizon_ + miu_horizon_ + slack_horizon_;
73 // number of constraints
74 num_of_constraints_ = 3 * obstacles_num_ * (horizon_ + 1) + num_of_variables_;
75
76 min_safety_distance_ =
77 planner_open_space_config.dual_variable_warm_start_config()
78 .min_safety_distance();
79 check_mode_ =
80 planner_open_space_config.dual_variable_warm_start_config().debug_osqp();
81 beta_ = planner_open_space_config.dual_variable_warm_start_config().beta();
82 osqp_config_ =
83 planner_open_space_config.dual_variable_warm_start_config().osqp_config();
84}
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 
)

成员函数说明

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

113 {
114 constraint_A_ = Eigen::MatrixXf::Zero(r, c);
115
116 for (size_t i = 0; i < P_indptr.size() - 1; ++i) {
117 if (P_indptr[i] < 0 || P_indptr[i] >= static_cast<int>(P_indices.size())) {
118 continue;
119 }
120
121 for (auto idx = P_indptr[i]; idx < P_indptr[i + 1]; ++idx) {
122 int tmp_c = static_cast<int>(i);
123 int tmp_r = static_cast<int>(P_indices[idx]);
124 constraint_A_(tmp_r, tmp_c) = static_cast<float>(P_data[idx]);
125 }
126 }
127}

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

375 {
376 /*
377 * The constraint matrix is as the form,
378 * |R' * A', G', 0|, #: 2 * obstacles_num_ * (horizon_ + 1)
379 * |A * t - b, -g, I|, #: obstacles_num_ * (horizon_ + 1)
380 * |I, 0, 0|, #: num_of_lambda
381 * |0, I, 0|, #: num_of_miu
382 * |0, 0, I|, #: num_of_slack
383 */
384 int r1_index = 0;
385 int r2_index = 2 * obstacles_num_ * (horizon_ + 1);
386 int r3_index = 3 * obstacles_num_ * (horizon_ + 1);
387 int r4_index = 3 * obstacles_num_ * (horizon_ + 1) + lambda_horizon_;
388 int r5_index = r4_index + miu_horizon_;
389 int first_row_location = 0;
390
391 // lambda variables
392 // lambda_horizon_ = obstacles_edges_sum_ * (horizon_ + 1);
393 for (int i = 0; i < horizon_ + 1; ++i) {
394 int edges_counter = 0;
395
396 Eigen::MatrixXd R(2, 2);
397 R << cos(xWS_(2, i)), sin(xWS_(2, i)), sin(xWS_(2, i)), cos(xWS_(2, i));
398
399 Eigen::MatrixXd t_trans(1, 2);
400 t_trans << (xWS_(0, i) + cos(xWS_(2, i)) * offset_),
401 (xWS_(1, i) + sin(xWS_(2, i)) * offset_);
402
403 // assume: stationary obstacles
404 for (int j = 0; j < obstacles_num_; ++j) {
405 int current_edges_num = obstacles_edges_num_(j, 0);
406 Eigen::MatrixXd Aj =
407 obstacles_A_.block(edges_counter, 0, current_edges_num, 2);
408 Eigen::MatrixXd bj =
409 obstacles_b_.block(edges_counter, 0, current_edges_num, 1);
410
411 Eigen::MatrixXd r1_block(2, current_edges_num);
412 r1_block = R * Aj.transpose();
413
414 Eigen::MatrixXd r2_block(1, current_edges_num);
415 r2_block = t_trans * Aj.transpose() - bj.transpose();
416
417 // insert into A matrix, col by col
418 for (int k = 0; k < current_edges_num; ++k) {
419 A_data->emplace_back(r1_block(0, k));
420 A_indices->emplace_back(r1_index);
421
422 A_data->emplace_back(r1_block(1, k));
423 A_indices->emplace_back(r1_index + 1);
424
425 A_data->emplace_back(r2_block(0, k));
426 A_indices->emplace_back(r2_index);
427
428 A_data->emplace_back(1.0);
429 A_indices->emplace_back(r3_index);
430 r3_index++;
431
432 A_indptr->emplace_back(first_row_location);
433 first_row_location += 4;
434 }
435
436 // Update index
437 edges_counter += current_edges_num;
438 r1_index += 2;
439 r2_index += 1;
440 }
441 }
442
443 // miu variables, miu_horizon_ = obstacles_num_ * 4 * (horizon_ + 1);
444 // G: ((1, 0, -1, 0), (0, 1, 0, -1))
445 // g: g_
446 r1_index = 0;
447 r2_index = 2 * obstacles_num_ * (horizon_ + 1);
448 for (int i = 0; i < horizon_ + 1; ++i) {
449 for (int j = 0; j < obstacles_num_; ++j) {
450 for (int k = 0; k < 4; ++k) {
451 // update G
452 if (k < 2) {
453 A_data->emplace_back(1.0);
454 } else {
455 A_data->emplace_back(-1.0);
456 }
457 A_indices->emplace_back(r1_index + k % 2);
458
459 // update g'
460 A_data->emplace_back(-g_[k]);
461 A_indices->emplace_back(r2_index);
462
463 // update I
464 A_data->emplace_back(1.0);
465 A_indices->emplace_back(r4_index);
466 r4_index++;
467
468 // update col index
469 A_indptr->emplace_back(first_row_location);
470 first_row_location += 3;
471 }
472
473 // update index
474 r1_index += 2;
475 r2_index += 1;
476 }
477 }
478
479 // slack variables
480 // slack_horizon_ = obstacles_edges_sum_ * (horizon_ + 1);
481 r2_index = 2 * obstacles_num_ * (horizon_ + 1);
482 for (int i = 0; i < horizon_ + 1; ++i) {
483 for (int j = 0; j < obstacles_num_; ++j) {
484 A_data->emplace_back(1.0);
485 A_indices->emplace_back(r2_index);
486 ++r2_index;
487
488 A_data->emplace_back(1.0);
489 A_indices->emplace_back(r5_index);
490 ++r5_index;
491
492 A_indptr->emplace_back(first_row_location);
493 first_row_location += 2;
494 }
495 }
496
497 A_indptr->emplace_back(first_row_location);
498
499 CHECK_EQ(A_data->size(), A_indices->size());
500 CHECK_EQ(A_indptr->size(), static_cast<size_t>(num_of_variables_) + 1);
501}
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 第 310 行定义.

312 {
313 // the objective function is norm(A' * lambda)
314 std::vector<c_float> P_tmp;
315 int edges_counter = 0;
316
317 for (int j = 0; j < obstacles_num_; ++j) {
318 int current_edges_num = obstacles_edges_num_(j, 0);
319 Eigen::MatrixXd Aj;
320 Aj = obstacles_A_.block(edges_counter, 0, current_edges_num, 2);
321 // Eigen::MatrixXd AAj(current_edges_num, current_edges_num);
322 Aj = Aj * Aj.transpose();
323
324 CHECK_EQ(current_edges_num, Aj.cols());
325 CHECK_EQ(current_edges_num, Aj.rows());
326
327 for (int c = 0; c < current_edges_num; ++c) {
328 for (int r = 0; r < current_edges_num; ++r) {
329 P_tmp.emplace_back(Aj(r, c));
330 }
331 }
332
333 // Update index
334 edges_counter += current_edges_num;
335 }
336
337 int l_index = l_start_index_;
338 int first_row_location = 0;
339 // the objective function is norm(A' * lambda)
340 for (int i = 0; i < horizon_ + 1; ++i) {
341 edges_counter = 0;
342
343 for (auto item : P_tmp) {
344 P_data->emplace_back(item);
345 }
346 // current assumption: stationary obstacles
347 for (int j = 0; j < obstacles_num_; ++j) {
348 int current_edges_num = obstacles_edges_num_(j, 0);
349
350 for (int c = 0; c < current_edges_num; ++c) {
351 P_indptr->emplace_back(first_row_location);
352 for (int r = 0; r < current_edges_num; ++r) {
353 P_indices->emplace_back(r + l_index);
354 }
355 first_row_location += current_edges_num;
356 }
357
358 // Update index
359 edges_counter += current_edges_num;
360 l_index += current_edges_num;
361 }
362 }
363
364 CHECK_EQ(P_indptr->size(), static_cast<size_t>(lambda_horizon_));
365 for (int i = lambda_horizon_; i < num_of_variables_ + 1; ++i) {
366 P_indptr->emplace_back(first_row_location);
367 }
368
369 CHECK_EQ(P_data->size(), P_indices->size());
370 CHECK_EQ(P_indptr->size(), static_cast<size_t>(num_of_variables_) + 1);
371}

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

306 {
307 // TODO(Runxin): extend
308}

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

505 {
506 *l_warm_up = l_warm_up_;
507 *n_warm_up = n_warm_up_;
508 *s_warm_up = slacks_;
509 // debug mode check slack values
510 double max_s = slacks_(0, 0);
511 double min_s = slacks_(0, 0);
512
513 for (int i = 0; i < horizon_ + 1; ++i) {
514 for (int j = 0; j < obstacles_num_; ++j) {
515 max_s = std::max(max_s, slacks_(j, i));
516 min_s = std::min(min_s, slacks_(j, i));
517 }
518 }
519
520 ADEBUG << "max_s: " << max_s;
521 ADEBUG << "min_s: " << min_s;
522}
ADEBUG
#define ADEBUG
Definition log.h:41

◆ optimize()

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

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

129 {
130 // int kNumParam = num_of_variables_;
131 // int kNumConst = num_of_constraints_;
132
133 bool succ = true;
134 // assemble P, quadratic term in objective
135 std::vector<c_float> P_data;
136 std::vector<c_int> P_indices;
137 std::vector<c_int> P_indptr;
138 assembleP(&P_data, &P_indices, &P_indptr);
139 if (check_mode_) {
140 AINFO << "print P_data in whole: ";
141 printMatrix(num_of_variables_, num_of_variables_, P_data, P_indices,
142 P_indptr);
143 }
144 // assemble q, linear term in objective, \sum{beta * slacks}
145 c_float q[num_of_variables_]; // NOLINT
146 for (int i = 0; i < num_of_variables_; ++i) {
147 q[i] = 0.0;
148 if (i >= s_start_index_) {
149 q[i] = beta_;
150 }
151 }
152
153 // assemble A, linear term in constraints
154 std::vector<c_float> A_data;
155 std::vector<c_int> A_indices;
156 std::vector<c_int> A_indptr;
157 assembleConstraint(&A_data, &A_indices, &A_indptr);
158 if (check_mode_) {
159 AINFO << "print A_data in whole: ";
160 printMatrix(num_of_constraints_, num_of_variables_, A_data, A_indices,
161 A_indptr);
162 assembleA(num_of_constraints_, num_of_variables_, A_data, A_indices,
163 A_indptr);
164 }
165
166 // assemble lb & ub, slack_variable <= 0
167 c_float lb[num_of_constraints_]; // NOLINT
168 c_float ub[num_of_constraints_]; // NOLINT
169 int slack_indx = num_of_constraints_ - slack_horizon_;
170 for (int i = 0; i < num_of_constraints_; ++i) {
171 lb[i] = 0.0;
172 if (i >= slack_indx) {
173 lb[i] = -2e19;
174 }
175
176 if (i < 2 * obstacles_num_ * (horizon_ + 1)) {
177 ub[i] = 0.0;
178 } else if (i < slack_indx) {
179 ub[i] = 2e19;
180 } else {
181 ub[i] = 0.0;
182 }
183 }
184
185 // Problem settings
186 OSQPSettings* settings =
187 reinterpret_cast<OSQPSettings*>(c_malloc(sizeof(OSQPSettings)));
188
189 // Define Solver settings as default
190 osqp_set_default_settings(settings);
191 settings->alpha = osqp_config_.alpha(); // Change alpha parameter
192 settings->eps_abs = osqp_config_.eps_abs();
193 settings->eps_rel = osqp_config_.eps_rel();
194 settings->max_iter = osqp_config_.max_iter();
195 settings->polish = osqp_config_.polish();
196 settings->verbose = osqp_config_.osqp_debug_log();
197
198 // Populate data
199 OSQPData* data = reinterpret_cast<OSQPData*>(c_malloc(sizeof(OSQPData)));
200 data->n = num_of_variables_;
201 data->m = num_of_constraints_;
202 data->P = csc_matrix(data->n, data->n, P_data.size(), P_data.data(),
203 P_indices.data(), P_indptr.data());
204 data->q = q;
205 data->A = csc_matrix(data->m, data->n, A_data.size(), A_data.data(),
206 A_indices.data(), A_indptr.data());
207 data->l = lb;
208 data->u = ub;
209
210 // Workspace
211 OSQPWorkspace* work = nullptr;
212 // osqp_setup(&work, data, settings);
213 work = osqp_setup(data, settings);
214
215 // Solve Problem
216 osqp_solve(work);
217
218 // check state
219 if (work->info->status_val != 1 && work->info->status_val != 2) {
220 AWARN << "OSQP dual warm up unsuccess, "
221 << "return status: " << work->info->status;
222 succ = false;
223 }
224
225 // transfer to make lambda's norm under 1
226 std::vector<double> lambda_norm;
227 int l_index = l_start_index_;
228 for (int i = 0; i < horizon_ + 1; ++i) {
229 int edges_counter = 0;
230 for (int j = 0; j < obstacles_num_; ++j) {
231 int current_edges_num = obstacles_edges_num_(j, 0);
232 Eigen::MatrixXd Aj =
233 obstacles_A_.block(edges_counter, 0, current_edges_num, 2);
234
235 double tmp1 = 0;
236 double tmp2 = 0;
237 for (int k = 0; k < current_edges_num; ++k) {
238 tmp1 += Aj(k, 0) * work->solution->x[l_index + k];
239 tmp2 += Aj(k, 1) * work->solution->x[l_index + k];
240 }
241
242 // norm(A * lambda)
243 double tmp_norm = tmp1 * tmp1 + tmp2 * tmp2;
244 if (tmp_norm >= 1e-5) {
245 lambda_norm.push_back(0.75 / std::sqrt(tmp_norm));
246 } else {
247 lambda_norm.push_back(0.0);
248 }
249
250 edges_counter += current_edges_num;
251 l_index += current_edges_num;
252 }
253 }
254
255 // extract primal results
256 int variable_index = 0;
257 // 1. lagrange constraint l, [0, obstacles_edges_sum_ - 1] * [0,
258 // horizon_l
259 for (int i = 0; i < horizon_ + 1; ++i) {
260 int r_index = 0;
261 for (int j = 0; j < obstacles_num_; ++j) {
262 for (int k = 0; k < obstacles_edges_num_(j, 0); ++k) {
263 l_warm_up_(r_index, i) = lambda_norm[i * obstacles_num_ + j] *
264 work->solution->x[variable_index];
265 ++variable_index;
266 ++r_index;
267 }
268 }
269 }
270
271 // 2. lagrange constraint n, [0, 4*obstacles_num-1] * [0, horizon_]
272 for (int i = 0; i < horizon_ + 1; ++i) {
273 int r_index = 0;
274 for (int j = 0; j < obstacles_num_; ++j) {
275 for (int k = 0; k < 4; ++k) {
276 n_warm_up_(r_index, i) = lambda_norm[i * obstacles_num_ + j] *
277 work->solution->x[variable_index];
278 ++r_index;
279 ++variable_index;
280 }
281 }
282 }
283
284 // 3. slack variables
285 for (int i = 0; i < horizon_ + 1; ++i) {
286 for (int j = 0; j < obstacles_num_; ++j) {
287 slacks_(j, i) = lambda_norm[i * obstacles_num_ + j] *
288 work->solution->x[variable_index];
289 ++variable_index;
290 }
291 }
292
293 succ = succ & (work->info->obj_val <= 1.0);
294
295 // Cleanup
296 osqp_cleanup(work);
297 c_free(data->A);
298 c_free(data->P);
299 c_free(data);
300 c_free(settings);
301
302 return succ;
303}
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:310
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:111
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:86
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:373
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 第 86 行定义.

88 {
89 Eigen::MatrixXf tmp = Eigen::MatrixXf::Zero(r, c);
90
91 for (size_t i = 0; i < P_indptr.size() - 1; ++i) {
92 if (P_indptr[i] < 0 || P_indptr[i] >= static_cast<int>(P_indices.size())) {
93 continue;
94 }
95
96 for (auto idx = P_indptr[i]; idx < P_indptr[i + 1]; ++idx) {
97 int tmp_c = static_cast<int>(i);
98 int tmp_r = static_cast<int>(P_indices[idx]);
99 tmp(tmp_r, tmp_c) = static_cast<float>(P_data[idx]);
100 }
101 }
102
103 AINFO << "row number: " << r;
104 AINFO << "col number: " << c;
105 for (int i = 0; i < r; ++i) {
106 AINFO << "row number: " << i;
107 AINFO << tmp.row(i);
108 }
109}
该类的文档由以下文件生成: