Apollo 10.0
自动驾驶开放平台
distance_approach_ipopt_cuda_interface.cc
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2019 The Apollo Authors. All Rights Reserved.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *****************************************************************************/
16
17/*
18 * @file
19 */
20#include "modules/planning/planning_open_space/trajectory_smoother/distance_approach_ipopt_cuda_interface.h"
21
22#include <algorithm>
23#include <limits>
24
25#if USE_GPU == 1
26#include "modules/planning/planning_open_space/trajectory_smoother/planning_block.h"
27#endif
28
29namespace apollo {
30namespace planning {
31
32DistanceApproachIPOPTCUDAInterface::DistanceApproachIPOPTCUDAInterface(
33 const size_t horizon, const double ts, const Eigen::MatrixXd& ego,
34 const Eigen::MatrixXd& xWS, const Eigen::MatrixXd& uWS,
35 const Eigen::MatrixXd& l_warm_up, const Eigen::MatrixXd& n_warm_up,
36 const Eigen::MatrixXd& x0, const Eigen::MatrixXd& xf,
37 const Eigen::MatrixXd& last_time_u, const std::vector<double>& XYbounds,
38 const Eigen::MatrixXi& obstacles_edges_num, const size_t obstacles_num,
39 const Eigen::MatrixXd& obstacles_A, const Eigen::MatrixXd& obstacles_b,
40 const PlannerOpenSpaceConfig& planner_open_space_config)
41 : ts_(ts),
42 ego_(ego),
43 xWS_(xWS),
44 uWS_(uWS),
45 l_warm_up_(l_warm_up),
46 n_warm_up_(n_warm_up),
47 x0_(x0),
48 xf_(xf),
49 last_time_u_(last_time_u),
50 XYbounds_(XYbounds),
51 obstacles_edges_num_(obstacles_edges_num),
52 obstacles_A_(obstacles_A),
53 obstacles_b_(obstacles_b) {
54 ACHECK(horizon < std::numeric_limits<int>::max())
55 << "Invalid cast on horizon in open space planner";
56 horizon_ = static_cast<int>(horizon);
57 ACHECK(obstacles_num < std::numeric_limits<int>::max())
58 << "Invalid cast on obstacles_num in open space planner";
59
60 obstacles_num_ = static_cast<int>(obstacles_num);
61 w_ev_ = ego_(1, 0) + ego_(3, 0);
62 l_ev_ = ego_(0, 0) + ego_(2, 0);
63 g_ = {l_ev_ / 2, w_ev_ / 2, l_ev_ / 2, w_ev_ / 2};
64 offset_ = (ego_(0, 0) + ego_(2, 0)) / 2 - ego_(2, 0);
65 obstacles_edges_sum_ = obstacles_edges_num_.sum();
66 state_result_ = Eigen::MatrixXd::Zero(4, horizon_ + 1);
67 dual_l_result_ = Eigen::MatrixXd::Zero(obstacles_edges_sum_, horizon_ + 1);
68 dual_n_result_ = Eigen::MatrixXd::Zero(4 * obstacles_num_, horizon_ + 1);
69 control_result_ = Eigen::MatrixXd::Zero(2, horizon_ + 1);
70 time_result_ = Eigen::MatrixXd::Zero(1, horizon_ + 1);
71 state_start_index_ = 0;
72 control_start_index_ = 4 * (horizon_ + 1);
73 time_start_index_ = control_start_index_ + 2 * horizon_;
74 l_start_index_ = time_start_index_ + (horizon_ + 1);
75 n_start_index_ = l_start_index_ + obstacles_edges_sum_ * (horizon_ + 1);
76
77 planner_open_space_config_ = planner_open_space_config;
78 distance_approach_config_ =
79 planner_open_space_config_.distance_approach_config();
80 weight_state_x_ = distance_approach_config_.weight_x();
81 weight_state_y_ = distance_approach_config_.weight_y();
82 weight_state_phi_ = distance_approach_config_.weight_phi();
83 weight_state_v_ = distance_approach_config_.weight_v();
84 weight_input_steer_ = distance_approach_config_.weight_steer();
85 weight_input_a_ = distance_approach_config_.weight_a();
86 weight_rate_steer_ = distance_approach_config_.weight_steer_rate();
87 weight_rate_a_ = distance_approach_config_.weight_a_rate();
88 weight_stitching_steer_ = distance_approach_config_.weight_steer_stitching();
89 weight_stitching_a_ = distance_approach_config_.weight_a_stitching();
90 weight_first_order_time_ =
91 distance_approach_config_.weight_first_order_time();
92 weight_second_order_time_ =
93 distance_approach_config_.weight_second_order_time();
94 min_safety_distance_ = distance_approach_config_.min_safety_distance();
95 max_steer_angle_ =
96 vehicle_param_.max_steer_angle() / vehicle_param_.steer_ratio();
97 max_speed_forward_ = distance_approach_config_.max_speed_forward();
98 max_speed_reverse_ = distance_approach_config_.max_speed_reverse();
99 max_acceleration_forward_ =
100 distance_approach_config_.max_acceleration_forward();
101 max_acceleration_reverse_ =
102 distance_approach_config_.max_acceleration_reverse();
103 min_time_sample_scaling_ =
104 distance_approach_config_.min_time_sample_scaling();
105 max_time_sample_scaling_ =
106 distance_approach_config_.max_time_sample_scaling();
107 max_steer_rate_ =
108 vehicle_param_.max_steer_angle_rate() / vehicle_param_.steer_ratio();
109 use_fix_time_ = distance_approach_config_.use_fix_time();
110 wheelbase_ = vehicle_param_.wheel_base();
111 enable_constraint_check_ =
112 distance_approach_config_.enable_constraint_check();
113}
114
115bool DistanceApproachIPOPTCUDAInterface::get_nlp_info(
116 int& n, int& m, int& nnz_jac_g, int& nnz_h_lag,
117 IndexStyleEnum& index_style) {
118 ADEBUG << "get_nlp_info";
119 // n1 : states variables, 4 * (N+1)
120 int n1 = 4 * (horizon_ + 1);
121 ADEBUG << "n1: " << n1;
122 // n2 : control inputs variables
123 int n2 = 2 * horizon_;
124 ADEBUG << "n2: " << n2;
125 // n3 : sampling time variables
126 int n3 = horizon_ + 1;
127 ADEBUG << "n3: " << n3;
128 // n4 : dual multiplier associated with obstacle shape
129 lambda_horizon_ = obstacles_edges_num_.sum() * (horizon_ + 1);
130 ADEBUG << "lambda_horizon_: " << lambda_horizon_;
131 // n5 : dual multipier associated with car shape, obstacles_num*4 * (N+1)
132 miu_horizon_ = obstacles_num_ * 4 * (horizon_ + 1);
133 ADEBUG << "miu_horizon_: " << miu_horizon_;
134 // m1 : dynamics constatins
135 int m1 = 4 * horizon_;
136
137 // m2 : control rate constraints (only steering)
138 int m2 = horizon_;
139
140 // m3 : sampling time equality constraints
141 int m3 = horizon_;
142
143 // m4 : obstacle constraints
144 int m4 = 4 * obstacles_num_ * (horizon_ + 1);
145
146 num_of_variables_ = n1 + n2 + n3 + lambda_horizon_ + miu_horizon_;
147 num_of_constraints_ =
148 m1 + m2 + m3 + m4 + (num_of_variables_ - (horizon_ + 1) + 2);
149
150 // number of variables
151 n = num_of_variables_;
152 ADEBUG << "num_of_variables_ " << num_of_variables_;
153 // number of constraints
154 m = num_of_constraints_;
155 ADEBUG << "num_of_constraints_ " << num_of_constraints_;
156
157 generate_tapes(n, m, &nnz_h_lag);
158 // // number of nonzero in Jacobian.
159 int tmp = 0;
160 for (int i = 0; i < horizon_ + 1; ++i) {
161 for (int j = 0; j < obstacles_num_; ++j) {
162 int current_edges_num = obstacles_edges_num_(j, 0);
163 tmp += current_edges_num * 4 + 9 + 4;
164 }
165 }
166 nnz_jac_g = 24 * horizon_ + 3 * horizon_ + 2 * horizon_ + tmp - 1 +
167 (num_of_variables_ - (horizon_ + 1) + 2);
168
169 index_style = IndexStyleEnum::C_STYLE;
170 ADEBUG << "get_nlp_info out";
171 return true;
172}
173
174bool DistanceApproachIPOPTCUDAInterface::get_bounds_info(int n, double* x_l,
175 double* x_u, int m,
176 double* g_l,
177 double* g_u) {
178 ADEBUG << "get_bounds_info";
179 ACHECK(XYbounds_.size() == 4)
180 << "XYbounds_ size is not 4, but" << XYbounds_.size();
181
182 // Variables: includes state, u, sample time and lagrange multipliers
183 // 1. state variables, 4 * [0, horizon]
184 // start point pose
185 int variable_index = 0;
186 for (int i = 0; i < 4; ++i) {
187 x_l[i] = -2e19;
188 x_u[i] = 2e19;
189 }
190 variable_index += 4;
191
192 // During horizons, 2 ~ N-1
193 for (int i = 1; i < horizon_; ++i) {
194 // x
195 x_l[variable_index] = -2e19;
196 x_u[variable_index] = 2e19;
197
198 // y
199 x_l[variable_index + 1] = -2e19;
200 x_u[variable_index + 1] = 2e19;
201
202 // phi
203 x_l[variable_index + 2] = -2e19;
204 x_u[variable_index + 2] = 2e19;
205
206 // v
207 x_l[variable_index + 3] = -2e19;
208 x_u[variable_index + 3] = 2e19;
209
210 variable_index += 4;
211 }
212
213 // end point pose
214 for (int i = 0; i < 4; ++i) {
215 x_l[variable_index + i] = -2e19;
216 x_u[variable_index + i] = 2e19;
217 }
218 variable_index += 4;
219 ADEBUG << "variable_index after adding state variables : " << variable_index;
220
221 // 2. control variables, 2 * [0, horizon_-1]
222 for (int i = 0; i < horizon_; ++i) {
223 // u1
224 x_l[variable_index] = -2e19;
225 x_u[variable_index] = 2e19;
226
227 // u2
228 x_l[variable_index + 1] = -2e19;
229 x_u[variable_index + 1] = 2e19;
230
231 variable_index += 2;
232 }
233 ADEBUG << "variable_index after adding control variables : "
234 << variable_index;
235
236 // 3. sampling time variables, 1 * [0, horizon_]
237 for (int i = 0; i < horizon_ + 1; ++i) {
238 x_l[variable_index] = -2e19;
239 x_u[variable_index] = 2e19;
240 ++variable_index;
241 }
242 ADEBUG << "variable_index after adding sample time : " << variable_index;
243 ADEBUG << "sample time fix time status is : " << use_fix_time_;
244
245 // 4. lagrange constraint l, [0, obstacles_edges_sum_ - 1] * [0,
246 // horizon_]
247 for (int i = 0; i < horizon_ + 1; ++i) {
248 for (int j = 0; j < obstacles_edges_sum_; ++j) {
249 x_l[variable_index] = 0.0;
250 x_u[variable_index] = 2e19; // nlp_upper_bound_limit
251 ++variable_index;
252 }
253 }
254 ADEBUG << "variable_index after adding lagrange l : " << variable_index;
255
256 // 5. lagrange constraint n, [0, 4*obstacles_num-1] * [0, horizon_]
257 for (int i = 0; i < horizon_ + 1; ++i) {
258 for (int j = 0; j < 4 * obstacles_num_; ++j) {
259 x_l[variable_index] = 0.0;
260 x_u[variable_index] = 2e19; // nlp_upper_bound_limit
261
262 ++variable_index;
263 }
264 }
265 ADEBUG << "variable_index after adding lagrange n : " << variable_index;
266
267 // Constraints: includes four state Euler forward constraints, three
268 // Obstacle related constraints
269
270 // 1. dynamics constraints 4 * [0, horizons-1]
271 int constraint_index = 0;
272 for (int i = 0; i < 4 * horizon_; ++i) {
273 g_l[i] = 0.0;
274 g_u[i] = 0.0;
275 }
276 constraint_index += 4 * horizon_;
277
278 ADEBUG << "constraint_index after adding Euler forward dynamics constraints: "
279 << constraint_index;
280
281 // 2. Control rate limit constraints, 1 * [0, horizons-1], only apply
282 // steering rate as of now
283 for (int i = 0; i < horizon_; ++i) {
284 g_l[constraint_index] = -max_steer_rate_;
285 g_u[constraint_index] = max_steer_rate_;
286 ++constraint_index;
287 }
288
289 ADEBUG << "constraint_index after adding steering rate constraints: "
290 << constraint_index;
291
292 // 3. Time constraints 1 * [0, horizons-1]
293 for (int i = 0; i < horizon_; ++i) {
294 g_l[constraint_index] = 0.0;
295 g_u[constraint_index] = 0.0;
296 ++constraint_index;
297 }
298
299 ADEBUG << "constraint_index after adding time constraints: "
300 << constraint_index;
301
302 // 4. Three obstacles related equal constraints, one equality constraints,
303 // [0, horizon_] * [0, obstacles_num_-1] * 4
304 for (int i = 0; i < horizon_ + 1; ++i) {
305 for (int j = 0; j < obstacles_num_; ++j) {
306 // a. norm(A'*lambda) <= 1
307 g_l[constraint_index] = -2e19;
308 g_u[constraint_index] = 1.0;
309
310 // b. G'*mu + R'*A*lambda = 0
311 g_l[constraint_index + 1] = 0.0;
312 g_u[constraint_index + 1] = 0.0;
313 g_l[constraint_index + 2] = 0.0;
314 g_u[constraint_index + 2] = 0.0;
315
316 // c. -g'*mu + (A*t - b)*lambda > min_safety_distance_
317 g_l[constraint_index + 3] = min_safety_distance_;
318 g_u[constraint_index + 3] = 2e19; // nlp_upper_bound_limit
319 constraint_index += 4;
320 }
321 }
322
323 // 5. load variable bounds as constraints
324 // start configuration
325 g_l[constraint_index] = x0_(0, 0);
326 g_u[constraint_index] = x0_(0, 0);
327 g_l[constraint_index + 1] = x0_(1, 0);
328 g_u[constraint_index + 1] = x0_(1, 0);
329 g_l[constraint_index + 2] = x0_(2, 0);
330 g_u[constraint_index + 2] = x0_(2, 0);
331 g_l[constraint_index + 3] = x0_(3, 0);
332 g_u[constraint_index + 3] = x0_(3, 0);
333 constraint_index += 4;
334
335 for (int i = 1; i < horizon_; ++i) {
336 g_l[constraint_index] = XYbounds_[0];
337 g_u[constraint_index] = XYbounds_[1];
338 g_l[constraint_index + 1] = XYbounds_[2];
339 g_u[constraint_index + 1] = XYbounds_[3];
340 g_l[constraint_index + 2] = -max_speed_reverse_;
341 g_u[constraint_index + 2] = max_speed_forward_;
342 constraint_index += 3;
343 }
344
345 // end configuration
346 g_l[constraint_index] = xf_(0, 0);
347 g_u[constraint_index] = xf_(0, 0);
348 g_l[constraint_index + 1] = xf_(1, 0);
349 g_u[constraint_index + 1] = xf_(1, 0);
350 g_l[constraint_index + 2] = xf_(2, 0);
351 g_u[constraint_index + 2] = xf_(2, 0);
352 g_l[constraint_index + 3] = xf_(3, 0);
353 g_u[constraint_index + 3] = xf_(3, 0);
354 constraint_index += 4;
355
356 for (int i = 0; i < horizon_; ++i) {
357 g_l[constraint_index] = -max_steer_angle_;
358 g_u[constraint_index] = max_steer_angle_;
359 g_l[constraint_index + 1] = -max_acceleration_reverse_;
360 g_u[constraint_index + 1] = max_acceleration_forward_;
361 constraint_index += 2;
362 }
363
364 for (int i = 0; i < horizon_ + 1; ++i) {
365 if (!use_fix_time_) {
366 g_l[constraint_index] = min_time_sample_scaling_;
367 g_u[constraint_index] = max_time_sample_scaling_;
368 } else {
369 g_l[constraint_index] = 1.0;
370 g_u[constraint_index] = 1.0;
371 }
372 constraint_index++;
373 }
374
375 for (int i = 0; i < lambda_horizon_; ++i) {
376 g_l[constraint_index] = 0.0;
377 g_u[constraint_index] = 2e19;
378 constraint_index++;
379 }
380
381 for (int i = 0; i < miu_horizon_; ++i) {
382 g_l[constraint_index] = 0.0;
383 g_u[constraint_index] = 2e19;
384 constraint_index++;
385 }
386
387 ADEBUG << "constraint_index after adding obstacles constraints: "
388 << constraint_index;
389 ADEBUG << "get_bounds_info_ out";
390 return true;
391}
392
393bool DistanceApproachIPOPTCUDAInterface::get_starting_point(
394 int n, bool init_x, double* x, bool init_z, double* z_L, double* z_U, int m,
395 bool init_lambda, double* lambda) {
396 ADEBUG << "get_starting_point";
397 ACHECK(init_x) << "Warm start init_x setting failed";
398
399 CHECK_EQ(horizon_, uWS_.cols());
400 CHECK_EQ(horizon_ + 1, xWS_.cols());
401
402 // 1. state variables 4 * (horizon_ + 1)
403 for (int i = 0; i < horizon_ + 1; ++i) {
404 int index = i * 4;
405 for (int j = 0; j < 4; ++j) {
406 x[index + j] = xWS_(j, i);
407 }
408 }
409
410 // 2. control variable initialization, 2 * horizon_
411 for (int i = 0; i < horizon_; ++i) {
412 int index = i * 2;
413 x[control_start_index_ + index] = uWS_(0, i);
414 x[control_start_index_ + index + 1] = uWS_(1, i);
415 }
416
417 // 2. time scale variable initialization, horizon_ + 1
418 for (int i = 0; i < horizon_ + 1; ++i) {
419 x[time_start_index_ + i] =
420 0.5 * (min_time_sample_scaling_ + max_time_sample_scaling_);
421 }
422
423 // 3. lagrange constraint l, obstacles_edges_sum_ * (horizon_+1)
424 for (int i = 0; i < horizon_ + 1; ++i) {
425 int index = i * obstacles_edges_sum_;
426 for (int j = 0; j < obstacles_edges_sum_; ++j) {
427 x[l_start_index_ + index + j] = l_warm_up_(j, i);
428 }
429 }
430
431 // 4. lagrange constraint m, 4*obstacles_num * (horizon_+1)
432 for (int i = 0; i < horizon_ + 1; ++i) {
433 int index = i * 4 * obstacles_num_;
434 for (int j = 0; j < 4 * obstacles_num_; ++j) {
435 x[n_start_index_ + index + j] = n_warm_up_(j, i);
436 }
437 }
438 ADEBUG << "get_starting_point out";
439 return true;
440}
441
442bool DistanceApproachIPOPTCUDAInterface::eval_f(int n, const double* x,
443 bool new_x, double& obj_value) {
444 eval_obj(n, x, &obj_value);
445 return true;
446}
447
448bool DistanceApproachIPOPTCUDAInterface::eval_grad_f(int n, const double* x,
449 bool new_x,
450 double* grad_f) {
451 gradient(tag_f, n, x, grad_f);
452 return true;
453}
454
455bool DistanceApproachIPOPTCUDAInterface::eval_grad_f_hand(int n,
456 const double* x,
457 bool new_x,
458 double* grad_f) {
459 ADEBUG << "eval_grad_f by hand";
460 // Objective is from eval_f:
461 // min control inputs
462 // min input rate
463 // min time (if the time step is not fixed)
464 // regularization wrt warm start trajectory
465 DCHECK(ts_ != 0) << "ts in distance_approach_ is 0";
466 int control_index = control_start_index_;
467 int time_index = time_start_index_;
468 int state_index = state_start_index_;
469
470 if (grad_f == nullptr) {
471 AERROR << "grad_f pt is nullptr";
472 return false;
473 } else {
474 std::fill(grad_f, grad_f + n, 0.0);
475 // 1. objective to minimize state diff to warm up
476 for (int i = 0; i < horizon_ + 1; ++i) {
477 grad_f[state_index] +=
478 2 * weight_state_x_ * (x[state_index] - xWS_(0, i));
479 grad_f[state_index + 1] +=
480 2 * weight_state_y_ * (x[state_index + 1] - xWS_(1, i));
481 grad_f[state_index + 2] +=
482 2 * weight_state_phi_ * (x[state_index + 2] - xWS_(2, i));
483 grad_f[state_index + 3] = 2 * weight_state_v_ * x[state_index + 3];
484 state_index += 4;
485 }
486
487 // 2. objective to minimize u square
488 for (int i = 0; i < horizon_; ++i) {
489 grad_f[control_index] += 2 * weight_input_steer_ * x[control_index];
490 grad_f[control_index + 1] += 2 * weight_input_a_ * x[control_index + 1];
491 control_index += 2;
492 }
493
494 // 3. objective to minimize input change rate for first horizon
495 // assume: x[time_index] > 0
496 control_index = control_start_index_;
497 double last_time_steer_rate =
498 (x[control_index] - last_time_u_(0, 0)) / x[time_index] / ts_;
499 double last_time_a_rate =
500 (x[control_index + 1] - last_time_u_(1, 0)) / x[time_index] / ts_;
501
502 grad_f[control_index] += 2.0 * last_time_steer_rate *
503 (weight_stitching_steer_ / x[time_index] / ts_);
504 grad_f[control_index + 1] +=
505 2.0 * last_time_a_rate * (weight_stitching_a_ / x[time_index] / ts_);
506 grad_f[time_index] +=
507 -2.0 *
508 (weight_stitching_steer_ * last_time_steer_rate * last_time_steer_rate +
509 weight_stitching_a_ * last_time_a_rate * last_time_a_rate) /
510 x[time_index];
511
512 // 4. objective to minimize input change rates, [0- horizon_ -2]
513 // assume: x[time_index] > 0
514 time_index++;
515 for (int i = 0; i < horizon_ - 1; ++i) {
516 double steering_rate =
517 (x[control_index + 2] - x[control_index]) / x[time_index] / ts_;
518 grad_f[control_index + 2] +=
519 2.0 * steering_rate * (weight_rate_steer_ / x[time_index] / ts_);
520 grad_f[control_index] +=
521 -2.0 * steering_rate * (weight_rate_steer_ / x[time_index] / ts_);
522 grad_f[time_index] += -2.0 * weight_rate_steer_ * steering_rate *
523 steering_rate / x[time_index];
524
525 double a_rate =
526 (x[control_index + 3] - x[control_index + 1]) / x[time_index] / ts_;
527 grad_f[control_index + 3] +=
528 2.0 * a_rate * (weight_rate_a_ / x[time_index] / ts_);
529 grad_f[control_index + 1] +=
530 -2.0 * a_rate * (weight_rate_a_ / x[time_index] / ts_);
531 grad_f[time_index] +=
532 -2.0 * weight_rate_a_ * a_rate * a_rate / x[time_index];
533
534 control_index += 2;
535 time_index++;
536 }
537
538 // 5. objective to minimize total time [0, horizon_]
539 time_index = time_start_index_;
540 for (int i = 0; i < horizon_ + 1; ++i) {
541 grad_f[time_index] += weight_first_order_time_ +
542 2.0 * weight_second_order_time_ * x[time_index];
543 time_index++;
544 }
545 }
546
547 return true;
548}
549
550bool DistanceApproachIPOPTCUDAInterface::eval_g(int n, const double* x,
551 bool new_x, int m, double* g) {
552 eval_constraints(n, x, m, g);
553 if (enable_constraint_check_) {
554 check_g(n, x, m, g);
555 }
556 return true;
557}
558
559bool DistanceApproachIPOPTCUDAInterface::eval_jac_g(int n, const double* x,
560 bool new_x, int m,
561 int nele_jac, int* iRow,
562 int* jCol, double* values) {
563 return eval_jac_g_ser(n, x, new_x, m, nele_jac, iRow, jCol, values);
564}
565
566bool DistanceApproachIPOPTCUDAInterface::eval_jac_g_par(int n, const double* x,
567 bool new_x, int m,
568 int nele_jac, int* iRow,
569 int* jCol,
570 double* values) {
571 ADEBUG << "eval_jac_g";
572 CHECK_EQ(n, num_of_variables_)
573 << "No. of variables wrong in eval_jac_g. n : " << n;
574 CHECK_EQ(m, num_of_constraints_)
575 << "No. of constraints wrong in eval_jac_g. n : " << m;
576
577 if (values == nullptr) {
578 int nz_index = 0;
579 int constraint_index = 0;
580 int state_index = state_start_index_;
581 int control_index = control_start_index_;
582 int time_index = time_start_index_;
583
584 // 1. State Constraint with respect to variables
585 for (int i = 0; i < horizon_; ++i) {
586 // g(0)' with respect to x0 ~ x7
587 iRow[nz_index] = state_index;
588 jCol[nz_index] = state_index;
589 ++nz_index;
590
591 iRow[nz_index] = state_index;
592 jCol[nz_index] = state_index + 2;
593 ++nz_index;
594
595 iRow[nz_index] = state_index;
596 jCol[nz_index] = state_index + 3;
597 ++nz_index;
598
599 iRow[nz_index] = state_index;
600 jCol[nz_index] = state_index + 4;
601 ++nz_index;
602
603 // g(0)' with respect to u0 ~ u1'
604 iRow[nz_index] = state_index;
605 jCol[nz_index] = control_index;
606 ++nz_index;
607
608 iRow[nz_index] = state_index;
609 jCol[nz_index] = control_index + 1;
610 ++nz_index;
611
612 // g(0)' with respect to time
613 iRow[nz_index] = state_index;
614 jCol[nz_index] = time_index;
615 ++nz_index;
616
617 // g(1)' with respect to x0 ~ x7
618 iRow[nz_index] = state_index + 1;
619 jCol[nz_index] = state_index + 1;
620 ++nz_index;
621
622 iRow[nz_index] = state_index + 1;
623 jCol[nz_index] = state_index + 2;
624 ++nz_index;
625
626 iRow[nz_index] = state_index + 1;
627 jCol[nz_index] = state_index + 3;
628 ++nz_index;
629
630 iRow[nz_index] = state_index + 1;
631 jCol[nz_index] = state_index + 5;
632 ++nz_index;
633
634 // g(1)' with respect to u0 ~ u1'
635 iRow[nz_index] = state_index + 1;
636 jCol[nz_index] = control_index;
637 ++nz_index;
638
639 iRow[nz_index] = state_index + 1;
640 jCol[nz_index] = control_index + 1;
641 ++nz_index;
642
643 // g(1)' with respect to time
644 iRow[nz_index] = state_index + 1;
645 jCol[nz_index] = time_index;
646 ++nz_index;
647
648 // g(2)' with respect to x0 ~ x7
649 iRow[nz_index] = state_index + 2;
650 jCol[nz_index] = state_index + 2;
651 ++nz_index;
652
653 iRow[nz_index] = state_index + 2;
654 jCol[nz_index] = state_index + 3;
655 ++nz_index;
656
657 iRow[nz_index] = state_index + 2;
658 jCol[nz_index] = state_index + 6;
659 ++nz_index;
660
661 // g(2)' with respect to u0 ~ u1'
662 iRow[nz_index] = state_index + 2;
663 jCol[nz_index] = control_index;
664 ++nz_index;
665
666 iRow[nz_index] = state_index + 2;
667 jCol[nz_index] = control_index + 1;
668 ++nz_index;
669
670 // g(2)' with respect to time
671 iRow[nz_index] = state_index + 2;
672 jCol[nz_index] = time_index;
673 ++nz_index;
674
675 // g(3)' with x0 ~ x7
676 iRow[nz_index] = state_index + 3;
677 jCol[nz_index] = state_index + 3;
678 ++nz_index;
679
680 iRow[nz_index] = state_index + 3;
681 jCol[nz_index] = state_index + 7;
682 ++nz_index;
683
684 // g(3)' with respect to u0 ~ u1'
685 iRow[nz_index] = state_index + 3;
686 jCol[nz_index] = control_index + 1;
687 ++nz_index;
688
689 // g(3)' with respect to time
690 iRow[nz_index] = state_index + 3;
691 jCol[nz_index] = time_index;
692 ++nz_index;
693
694 state_index += 4;
695 control_index += 2;
696 time_index++;
697 constraint_index += 4;
698 }
699
700 // 2. only have control rate constraints on u0 , range [0, horizon_-1]
701 control_index = control_start_index_;
702 state_index = state_start_index_;
703 time_index = time_start_index_;
704
705 // First one, with respect to u(0, 0)
706 iRow[nz_index] = constraint_index;
707 jCol[nz_index] = control_index;
708 ++nz_index;
709
710 // First element, with respect to time
711 iRow[nz_index] = constraint_index;
712 jCol[nz_index] = time_index;
713 ++nz_index;
714
715 control_index += 2;
716 time_index++;
717 constraint_index++;
718
719 for (int i = 1; i < horizon_; ++i) {
720 // with respect to u(0, i-1)
721 iRow[nz_index] = constraint_index;
722 jCol[nz_index] = control_index - 2;
723 ++nz_index;
724
725 // with respect to u(0, i)
726 iRow[nz_index] = constraint_index;
727 jCol[nz_index] = control_index;
728 ++nz_index;
729
730 // with respect to time
731 iRow[nz_index] = constraint_index;
732 jCol[nz_index] = time_index;
733 ++nz_index;
734
735 // only consider rate limits on u0
736 control_index += 2;
737 constraint_index++;
738 time_index++;
739 }
740
741 // 3. Time constraints [0, horizon_ -1]
742 time_index = time_start_index_;
743
744 for (int i = 0; i < horizon_; ++i) {
745 // with respect to timescale(0, i-1)
746 iRow[nz_index] = constraint_index;
747 jCol[nz_index] = time_index;
748 ++nz_index;
749
750 // with respect to timescale(0, i)
751 iRow[nz_index] = constraint_index;
752 jCol[nz_index] = time_index + 1;
753 ++nz_index;
754
755 time_index++;
756 constraint_index++;
757 }
758
759 // 4. Three obstacles related equal constraints, one equality constraints,
760 // [0, horizon_] * [0, obstacles_num_-1] * 4
761 state_index = state_start_index_;
762 int l_index = l_start_index_;
763 int n_index = n_start_index_;
764
765#pragma omp parallel for schedule(dynamic, 1) num_threads(4)
766 for (int iter = 0; iter < (horizon_ + 1) * obstacles_num_; iter++) {
767 int i = iter / obstacles_num_;
768 int j = iter % obstacles_num_;
769 int current_edges_num = obstacles_edges_num_(j, 0);
770 int nz_index_tmp = nz_index;
771 int l_index_tmp = l_index;
772 // count nz_len
773 for (int jj = 0; jj < obstacles_num_; ++jj) {
774 if (jj < j) {
775 nz_index_tmp += 4 * (i + 1) * obstacles_edges_num_(jj, 0);
776 nz_index_tmp += 13 * (i + 1);
777 l_index_tmp += (i + 1) * obstacles_edges_num_(jj, 0);
778 } else {
779 nz_index_tmp += 4 * i * obstacles_edges_num_(jj, 0);
780 nz_index_tmp += 13 * i;
781 l_index_tmp += i * obstacles_edges_num_(jj, 0);
782 }
783 }
784 int n_index_tmp = n_index + (i * obstacles_num_ + j) * 4;
785 int constraint_index_tmp =
786 constraint_index + (i * obstacles_num_ + j) * 4;
787 int state_index_tmp = state_index + i * 4;
788
789 // 1. norm(A* lambda == 1)
790 for (int k = 0; k < current_edges_num; ++k) {
791 // with respect to l
792 iRow[nz_index_tmp] = constraint_index_tmp;
793 jCol[nz_index_tmp] = l_index_tmp + k;
794 ++nz_index_tmp; // current_edges_num
795 }
796
797 // 2. G' * mu + R' * lambda == 0, part 1
798 // With respect to x
799 iRow[nz_index_tmp] = constraint_index_tmp + 1;
800 jCol[nz_index_tmp] = state_index_tmp + 2;
801 ++nz_index_tmp; // 1
802
803 // with respect to l
804 for (int k = 0; k < current_edges_num; ++k) {
805 iRow[nz_index_tmp] = constraint_index_tmp + 1;
806 jCol[nz_index_tmp] = l_index_tmp + k;
807 ++nz_index_tmp; // current_edges_num
808 }
809
810 // With respect to n
811 iRow[nz_index_tmp] = constraint_index_tmp + 1;
812 jCol[nz_index_tmp] = n_index_tmp;
813 ++nz_index_tmp; // 1
814
815 iRow[nz_index_tmp] = constraint_index_tmp + 1;
816 jCol[nz_index_tmp] = n_index_tmp + 2;
817 ++nz_index_tmp; // 1
818
819 // 2. G' * mu + R' * lambda == 0, part 2
820 // With respect to x
821 iRow[nz_index_tmp] = constraint_index_tmp + 2;
822 jCol[nz_index_tmp] = state_index_tmp + 2;
823 ++nz_index_tmp; // 1
824
825 // with respect to l
826 for (int k = 0; k < current_edges_num; ++k) {
827 iRow[nz_index_tmp] = constraint_index_tmp + 2;
828 jCol[nz_index_tmp] = l_index_tmp + k;
829 ++nz_index_tmp; // current_edges_num
830 }
831
832 // With respect to n
833 iRow[nz_index_tmp] = constraint_index_tmp + 2;
834 jCol[nz_index_tmp] = n_index_tmp + 1;
835 ++nz_index_tmp; // 1
836
837 iRow[nz_index_tmp] = constraint_index_tmp + 2;
838 jCol[nz_index_tmp] = n_index_tmp + 3;
839 ++nz_index_tmp; // 1
840
841 // 3. -g'*mu + (A*t - b)*lambda > 0
842 // With respect to x
843 iRow[nz_index_tmp] = constraint_index_tmp + 3;
844 jCol[nz_index_tmp] = state_index_tmp;
845 ++nz_index_tmp; // 1
846
847 iRow[nz_index_tmp] = constraint_index_tmp + 3;
848 jCol[nz_index_tmp] = state_index_tmp + 1;
849 ++nz_index_tmp; // 1
850
851 iRow[nz_index_tmp] = constraint_index_tmp + 3;
852 jCol[nz_index_tmp] = state_index_tmp + 2;
853 ++nz_index_tmp; // 1
854
855 // with respect to l
856 for (int k = 0; k < current_edges_num; ++k) {
857 iRow[nz_index_tmp] = constraint_index_tmp + 3;
858 jCol[nz_index_tmp] = l_index_tmp + k;
859 ++nz_index_tmp; // current_edges_num
860 }
861
862 // with respect to n
863 for (int k = 0; k < 4; ++k) {
864 iRow[nz_index_tmp] = constraint_index_tmp + 3;
865 jCol[nz_index_tmp] = n_index_tmp + k;
866 ++nz_index_tmp; // 4
867 }
868 }
869 // update index
870 for (int jj = 0; jj < obstacles_num_; ++jj) {
871 nz_index += 4 * (horizon_ + 1) * obstacles_edges_num_(jj, 0);
872 nz_index += 13 * (horizon_ + 1);
873 }
874 constraint_index += 4 * (horizon_ + 1) * obstacles_num_;
875 state_index += 4 * (horizon_ + 1);
876
877 // 5. load variable bounds as constraints
878 state_index = state_start_index_;
879 control_index = control_start_index_;
880 time_index = time_start_index_;
881 l_index = l_start_index_;
882 n_index = n_start_index_;
883
884 // start configuration
885 iRow[nz_index] = constraint_index;
886 jCol[nz_index] = state_index;
887 nz_index++;
888 iRow[nz_index] = constraint_index + 1;
889 jCol[nz_index] = state_index + 1;
890 nz_index++;
891 iRow[nz_index] = constraint_index + 2;
892 jCol[nz_index] = state_index + 2;
893 nz_index++;
894 iRow[nz_index] = constraint_index + 3;
895 jCol[nz_index] = state_index + 3;
896 nz_index++;
897 constraint_index += 4;
898 state_index += 4;
899
900 for (int i = 1; i < horizon_; ++i) {
901 iRow[nz_index] = constraint_index;
902 jCol[nz_index] = state_index;
903 nz_index++;
904 iRow[nz_index] = constraint_index + 1;
905 jCol[nz_index] = state_index + 1;
906 nz_index++;
907 iRow[nz_index] = constraint_index + 2;
908 jCol[nz_index] = state_index + 3;
909 nz_index++;
910 constraint_index += 3;
911 state_index += 4;
912 }
913
914 // end configuration
915 iRow[nz_index] = constraint_index;
916 jCol[nz_index] = state_index;
917 nz_index++;
918 iRow[nz_index] = constraint_index + 1;
919 jCol[nz_index] = state_index + 1;
920 nz_index++;
921 iRow[nz_index] = constraint_index + 2;
922 jCol[nz_index] = state_index + 2;
923 nz_index++;
924 iRow[nz_index] = constraint_index + 3;
925 jCol[nz_index] = state_index + 3;
926 nz_index++;
927 constraint_index += 4;
928 state_index += 4;
929
930 for (int i = 0; i < horizon_; ++i) {
931 iRow[nz_index] = constraint_index;
932 jCol[nz_index] = control_index;
933 nz_index++;
934 iRow[nz_index] = constraint_index + 1;
935 jCol[nz_index] = control_index + 1;
936 nz_index++;
937 constraint_index += 2;
938 control_index += 2;
939 }
940
941 for (int i = 0; i < horizon_ + 1; ++i) {
942 iRow[nz_index] = constraint_index;
943 jCol[nz_index] = time_index;
944 nz_index++;
945 constraint_index++;
946 time_index++;
947 }
948
949 for (int i = 0; i < lambda_horizon_; ++i) {
950 iRow[nz_index] = constraint_index;
951 jCol[nz_index] = l_index;
952 nz_index++;
953 constraint_index++;
954 l_index++;
955 }
956
957 for (int i = 0; i < miu_horizon_; ++i) {
958 iRow[nz_index] = constraint_index;
959 jCol[nz_index] = n_index;
960 nz_index++;
961 constraint_index++;
962 n_index++;
963 }
964
965 CHECK_EQ(nz_index, static_cast<int>(nele_jac));
966 CHECK_EQ(constraint_index, static_cast<int>(m));
967 } else {
968 std::fill(values, values + nele_jac, 0.0);
969 int nz_index = 0;
970
971 int time_index = time_start_index_;
972 int state_index = state_start_index_;
973 int control_index = control_start_index_;
974 // TODO(QiL) : initially implemented to be debug friendly, later iterate
975 // towards better efficiency
976 // 1. state constraints 4 * [0, horizons-1]
977 for (int i = 0; i < horizon_; ++i) {
978 values[nz_index] = -1.0;
979 ++nz_index;
980
981 values[nz_index] =
982 x[time_index] * ts_ *
983 (x[state_index + 3] +
984 x[time_index] * ts_ * 0.5 * x[control_index + 1]) *
985 std::sin(x[state_index + 2] +
986 x[time_index] * ts_ * 0.5 * x[state_index + 3] *
987 std::tan(x[control_index]) / wheelbase_); // a.
988 ++nz_index;
989
990 values[nz_index] =
991 -1.0 *
992 (x[time_index] * ts_ *
993 std::cos(x[state_index + 2] +
994 x[time_index] * ts_ * 0.5 * x[state_index + 3] *
995 std::tan(x[control_index]) / wheelbase_) +
996 x[time_index] * ts_ *
997 (x[state_index + 3] +
998 x[time_index] * ts_ * 0.5 * x[control_index + 1]) *
999 (-1) * x[time_index] * ts_ * 0.5 * std::tan(x[control_index]) /
1000 wheelbase_ *
1001 std::sin(x[state_index + 2] +
1002 x[time_index] * ts_ * 0.5 * x[state_index + 3] *
1003 std::tan(x[control_index]) / wheelbase_)); // b
1004 ++nz_index;
1005
1006 values[nz_index] = 1.0;
1007 ++nz_index;
1008
1009 values[nz_index] =
1010 x[time_index] * ts_ *
1011 (x[state_index + 3] +
1012 x[time_index] * ts_ * 0.5 * x[control_index + 1]) *
1013 std::sin(x[state_index + 2] +
1014 x[time_index] * ts_ * 0.5 * x[state_index + 3] *
1015 std::tan(x[control_index]) / wheelbase_) *
1016 x[time_index] * ts_ * 0.5 * x[state_index + 3] /
1017 (std::cos(x[control_index]) * std::cos(x[control_index])) /
1018 wheelbase_; // c
1019 ++nz_index;
1020
1021 values[nz_index] =
1022 -1.0 * (x[time_index] * ts_ * x[time_index] * ts_ * 0.5 *
1023 std::cos(x[state_index + 2] +
1024 x[time_index] * ts_ * 0.5 * x[state_index + 3] *
1025 std::tan(x[control_index]) / wheelbase_)); // d
1026 ++nz_index;
1027
1028 values[nz_index] =
1029 -1.0 *
1030 (ts_ *
1031 (x[state_index + 3] +
1032 x[time_index] * ts_ * 0.5 * x[control_index + 1]) *
1033 std::cos(x[state_index + 2] +
1034 x[time_index] * ts_ * 0.5 * x[state_index + 3] *
1035 std::tan(x[control_index]) / wheelbase_) +
1036 x[time_index] * ts_ * ts_ * 0.5 * x[control_index + 1] *
1037 std::cos(x[state_index + 2] +
1038 x[time_index] * ts_ * 0.5 * x[state_index + 3] *
1039 std::tan(x[control_index]) / wheelbase_) -
1040 x[time_index] * ts_ *
1041 (x[state_index + 3] +
1042 x[time_index] * ts_ * 0.5 * x[control_index + 1]) *
1043 x[state_index + 3] * ts_ * 0.5 * std::tan(x[control_index]) /
1044 wheelbase_ *
1045 std::sin(x[state_index + 2] +
1046 x[time_index] * ts_ * 0.5 * x[state_index + 3] *
1047 std::tan(x[control_index]) / wheelbase_)); // e
1048 ++nz_index;
1049
1050 values[nz_index] = -1.0;
1051 ++nz_index;
1052
1053 values[nz_index] =
1054 -1.0 * (x[time_index] * ts_ *
1055 (x[state_index + 3] +
1056 x[time_index] * ts_ * 0.5 * x[control_index + 1]) *
1057 std::cos(x[state_index + 2] +
1058 x[time_index] * ts_ * 0.5 * x[state_index + 3] *
1059 std::tan(x[control_index]) / wheelbase_)); // f.
1060 ++nz_index;
1061
1062 values[nz_index] =
1063 -1.0 *
1064 (x[time_index] * ts_ *
1065 std::sin(x[state_index + 2] +
1066 x[time_index] * ts_ * 0.5 * x[state_index + 3] *
1067 std::tan(x[control_index]) / wheelbase_) +
1068 x[time_index] * ts_ *
1069 (x[state_index + 3] +
1070 x[time_index] * ts_ * 0.5 * x[control_index + 1]) *
1071 x[time_index] * ts_ * 0.5 * std::tan(x[control_index]) /
1072 wheelbase_ *
1073 std::cos(x[state_index + 2] +
1074 x[time_index] * ts_ * 0.5 * x[state_index + 3] *
1075 std::tan(x[control_index]) / wheelbase_)); // g
1076 ++nz_index;
1077
1078 values[nz_index] = 1.0;
1079 ++nz_index;
1080
1081 values[nz_index] =
1082 -1.0 * (x[time_index] * ts_ *
1083 (x[state_index + 3] +
1084 x[time_index] * ts_ * 0.5 * x[control_index + 1]) *
1085 std::cos(x[state_index + 2] +
1086 x[time_index] * ts_ * 0.5 * x[state_index + 3] *
1087 std::tan(x[control_index]) / wheelbase_) *
1088 x[time_index] * ts_ * 0.5 * x[state_index + 3] /
1089 (std::cos(x[control_index]) * std::cos(x[control_index])) /
1090 wheelbase_); // h
1091 ++nz_index;
1092
1093 values[nz_index] =
1094 -1.0 * (x[time_index] * ts_ * x[time_index] * ts_ * 0.5 *
1095 std::sin(x[state_index + 2] +
1096 x[time_index] * ts_ * 0.5 * x[state_index + 3] *
1097 std::tan(x[control_index]) / wheelbase_)); // i
1098 ++nz_index;
1099
1100 values[nz_index] =
1101 -1.0 *
1102 (ts_ *
1103 (x[state_index + 3] +
1104 x[time_index] * ts_ * 0.5 * x[control_index + 1]) *
1105 std::sin(x[state_index + 2] +
1106 x[time_index] * ts_ * 0.5 * x[state_index + 3] *
1107 std::tan(x[control_index]) / wheelbase_) +
1108 x[time_index] * ts_ * ts_ * 0.5 * x[control_index + 1] *
1109 std::sin(x[state_index + 2] +
1110 x[time_index] * ts_ * 0.5 * x[state_index + 3] *
1111 std::tan(x[control_index]) / wheelbase_) +
1112 x[time_index] * ts_ *
1113 (x[state_index + 3] +
1114 x[time_index] * ts_ * 0.5 * x[control_index + 1]) *
1115 x[state_index + 3] * ts_ * 0.5 * std::tan(x[control_index]) /
1116 wheelbase_ *
1117 std::cos(x[state_index + 2] +
1118 x[time_index] * ts_ * 0.5 * x[state_index + 3] *
1119 std::tan(x[control_index]) / wheelbase_)); // j
1120 ++nz_index;
1121
1122 values[nz_index] = -1.0;
1123 ++nz_index;
1124
1125 values[nz_index] = -1.0 * x[time_index] * ts_ *
1126 std::tan(x[control_index]) / wheelbase_; // k.
1127 ++nz_index;
1128
1129 values[nz_index] = 1.0;
1130 ++nz_index;
1131
1132 values[nz_index] =
1133 -1.0 * (x[time_index] * ts_ *
1134 (x[state_index + 3] +
1135 x[time_index] * ts_ * 0.5 * x[control_index + 1]) /
1136 (std::cos(x[control_index]) * std::cos(x[control_index])) /
1137 wheelbase_); // l.
1138 ++nz_index;
1139
1140 values[nz_index] =
1141 -1.0 * (x[time_index] * ts_ * x[time_index] * ts_ * 0.5 *
1142 std::tan(x[control_index]) / wheelbase_); // m.
1143 ++nz_index;
1144
1145 values[nz_index] =
1146 -1.0 * (ts_ *
1147 (x[state_index + 3] +
1148 x[time_index] * ts_ * 0.5 * x[control_index + 1]) *
1149 std::tan(x[control_index]) / wheelbase_ +
1150 x[time_index] * ts_ * ts_ * 0.5 * x[control_index + 1] *
1151 std::tan(x[control_index]) / wheelbase_); // n.
1152 ++nz_index;
1153
1154 values[nz_index] = -1.0;
1155 ++nz_index;
1156
1157 values[nz_index] = 1.0;
1158 ++nz_index;
1159
1160 values[nz_index] = -1.0 * ts_ * x[time_index]; // o.
1161 ++nz_index;
1162
1163 values[nz_index] = -1.0 * ts_ * x[control_index + 1]; // p.
1164 ++nz_index;
1165
1166 state_index += 4;
1167 control_index += 2;
1168 time_index++;
1169 }
1170
1171 // 2. control rate constraints 1 * [0, horizons-1]
1172 control_index = control_start_index_;
1173 state_index = state_start_index_;
1174 time_index = time_start_index_;
1175
1176 // First horizon
1177
1178 // with respect to u(0, 0)
1179 values[nz_index] = 1.0 / x[time_index] / ts_; // q
1180 ++nz_index;
1181
1182 // with respect to time
1183 values[nz_index] = -1.0 * (x[control_index] - last_time_u_(0, 0)) /
1184 x[time_index] / x[time_index] / ts_;
1185 ++nz_index;
1186 time_index++;
1187 control_index += 2;
1188
1189 for (int i = 1; i < horizon_; ++i) {
1190 // with respect to u(0, i-1)
1191
1192 values[nz_index] = -1.0 / x[time_index] / ts_;
1193 ++nz_index;
1194
1195 // with respect to u(0, i)
1196 values[nz_index] = 1.0 / x[time_index] / ts_;
1197 ++nz_index;
1198
1199 // with respect to time
1200 values[nz_index] = -1.0 * (x[control_index] - x[control_index - 2]) /
1201 x[time_index] / x[time_index] / ts_;
1202 ++nz_index;
1203
1204 control_index += 2;
1205 time_index++;
1206 }
1207
1208 ADEBUG << "After fulfilled control rate constraints derivative, nz_index : "
1209 << nz_index << " nele_jac : " << nele_jac;
1210
1211 // 3. Time constraints [0, horizon_ -1]
1212 time_index = time_start_index_;
1213 for (int i = 0; i < horizon_; ++i) {
1214 // with respect to timescale(0, i-1)
1215 values[nz_index] = -1.0;
1216 ++nz_index;
1217
1218 // with respect to timescale(0, i)
1219 values[nz_index] = 1.0;
1220 ++nz_index;
1221
1222 time_index++;
1223 }
1224
1225 ADEBUG << "After fulfilled time constraints derivative, nz_index : "
1226 << nz_index << " nele_jac : " << nele_jac;
1227
1228 // 4. Three obstacles related equal constraints, one equality constraints,
1229 // [0, horizon_] * [0, obstacles_num_-1] * 4
1230
1231 state_index = state_start_index_;
1232 int l_index = l_start_index_;
1233 int n_index = n_start_index_;
1234
1235#pragma omp parallel for schedule(dynamic, 1) num_threads(4)
1236 for (int iter = 0; iter < (horizon_ + 1) * obstacles_num_; iter++) {
1237 int i = iter / obstacles_num_;
1238 int j = iter % obstacles_num_;
1239
1240 int current_edges_num = obstacles_edges_num_(j, 0);
1241 int edges_counter = 0;
1242 int nz_index_tmp = nz_index;
1243 int l_index_tmp = l_index;
1244
1245 // count nz_len
1246 for (int jj = 0; jj < obstacles_num_; ++jj) {
1247 if (jj < j) {
1248 nz_index_tmp += 4 * (i + 1) * obstacles_edges_num_(jj, 0);
1249 nz_index_tmp += 13 * (i + 1);
1250 l_index_tmp += (i + 1) * obstacles_edges_num_(jj, 0);
1251 edges_counter += obstacles_edges_num_(jj, 0);
1252 } else {
1253 nz_index_tmp += 4 * i * obstacles_edges_num_(jj, 0);
1254 nz_index_tmp += 13 * i;
1255 l_index_tmp += i * obstacles_edges_num_(jj, 0);
1256 }
1257 }
1258 int n_index_tmp = n_index + (i * obstacles_num_ + j) * 4;
1259 int state_index_tmp = state_index + i * 4;
1260
1261 Eigen::MatrixXd Aj =
1262 obstacles_A_.block(edges_counter, 0, current_edges_num, 2);
1263 Eigen::MatrixXd bj =
1264 obstacles_b_.block(edges_counter, 0, current_edges_num, 1);
1265
1266 // TODO(QiL) : Remove redundant calculation
1267 double tmp1 = 0;
1268 double tmp2 = 0;
1269 for (int k = 0; k < current_edges_num; ++k) {
1270 // TODO(QiL) : replace this one directly with x
1271 tmp1 += Aj(k, 0) * x[l_index_tmp + k];
1272 tmp2 += Aj(k, 1) * x[l_index_tmp + k];
1273 }
1274
1275 // 1. norm(A* lambda == 1)
1276 for (int k = 0; k < current_edges_num; ++k) {
1277 // with respect to l
1278 values[nz_index_tmp] =
1279 2 * tmp1 * Aj(k, 0) + 2 * tmp2 * Aj(k, 1); // t0~tk
1280 ++nz_index_tmp; // current_edges_num
1281 }
1282
1283 // 2. G' * mu + R' * lambda == 0, part 1
1284 // With respect to x
1285 values[nz_index_tmp] = -std::sin(x[state_index_tmp + 2]) * tmp1 +
1286 std::cos(x[state_index_tmp + 2]) * tmp2; // u
1287 ++nz_index_tmp; // 1
1288
1289 // with respect to l
1290 for (int k = 0; k < current_edges_num; ++k) {
1291 values[nz_index_tmp] =
1292 std::cos(x[state_index_tmp + 2]) * Aj(k, 0) +
1293 std::sin(x[state_index_tmp + 2]) * Aj(k, 1); // v0~vn
1294 ++nz_index_tmp; // current_edges_num
1295 }
1296
1297 // With respect to n
1298 values[nz_index_tmp] = 1.0; // w0
1299 ++nz_index_tmp; // 1
1300
1301 values[nz_index_tmp] = -1.0; // w2
1302 ++nz_index_tmp; // 1
1303
1304 // 3. G' * mu + R' * lambda == 0, part 2
1305 // With respect to x
1306 values[nz_index_tmp] = -std::cos(x[state_index_tmp + 2]) * tmp1 -
1307 std::sin(x[state_index_tmp + 2]) * tmp2; // x
1308 ++nz_index_tmp; // 1
1309
1310 // with respect to l
1311 for (int k = 0; k < current_edges_num; ++k) {
1312 values[nz_index_tmp] =
1313 -std::sin(x[state_index_tmp + 2]) * Aj(k, 0) +
1314 std::cos(x[state_index_tmp + 2]) * Aj(k, 1); // y0~yn
1315 ++nz_index_tmp; // current_edges_num
1316 }
1317
1318 // With respect to n
1319 values[nz_index_tmp] = 1.0; // z1
1320 ++nz_index_tmp; // 1
1321
1322 values[nz_index_tmp] = -1.0; // z3
1323 ++nz_index_tmp; // 1
1324
1325 // 3. -g'*mu + (A*t - b)*lambda > 0
1326 double tmp3 = 0.0;
1327 double tmp4 = 0.0;
1328 for (int k = 0; k < 4; ++k) {
1329 tmp3 += -g_[k] * x[n_index_tmp + k];
1330 }
1331
1332 for (int k = 0; k < current_edges_num; ++k) {
1333 tmp4 += bj(k, 0) * x[l_index_tmp + k];
1334 }
1335
1336 // With respect to x
1337 values[nz_index_tmp] = tmp1; // aa1
1338 ++nz_index_tmp; // 1
1339
1340 values[nz_index_tmp] = tmp2; // bb1
1341 ++nz_index_tmp; // 1
1342
1343 values[nz_index_tmp] =
1344 -std::sin(x[state_index_tmp + 2]) * offset_ * tmp1 +
1345 std::cos(x[state_index_tmp + 2]) * offset_ * tmp2; // cc1
1346 ++nz_index_tmp; // 1
1347
1348 // with respect to l
1349 for (int k = 0; k < current_edges_num; ++k) {
1350 values[nz_index_tmp] =
1351 (x[state_index_tmp] + std::cos(x[state_index_tmp + 2]) * offset_) *
1352 Aj(k, 0) +
1353 (x[state_index_tmp + 1] +
1354 std::sin(x[state_index_tmp + 2]) * offset_) *
1355 Aj(k, 1) -
1356 bj(k, 0); // ddk
1357 ++nz_index_tmp; // current_edges_num
1358 }
1359
1360 // with respect to n
1361 for (int k = 0; k < 4; ++k) {
1362 values[nz_index_tmp] = -g_[k]; // eek
1363 ++nz_index_tmp; // 4
1364 }
1365 }
1366 // update index
1367 for (int jj = 0; jj < obstacles_num_; ++jj) {
1368 nz_index += 4 * (horizon_ + 1) * obstacles_edges_num_(jj, 0);
1369 nz_index += 13 * (horizon_ + 1);
1370 }
1371
1372 // 5. load variable bounds as constraints
1373 state_index = state_start_index_;
1374 control_index = control_start_index_;
1375 time_index = time_start_index_;
1376 l_index = l_start_index_;
1377 n_index = n_start_index_;
1378
1379 // start configuration
1380 values[nz_index] = 1.0;
1381 nz_index++;
1382 values[nz_index] = 1.0;
1383 nz_index++;
1384 values[nz_index] = 1.0;
1385 nz_index++;
1386 values[nz_index] = 1.0;
1387 nz_index++;
1388
1389 for (int i = 1; i < horizon_; ++i) {
1390 values[nz_index] = 1.0;
1391 nz_index++;
1392 values[nz_index] = 1.0;
1393 nz_index++;
1394 values[nz_index] = 1.0;
1395 nz_index++;
1396 }
1397
1398 // end configuration
1399 values[nz_index] = 1.0;
1400 nz_index++;
1401 values[nz_index] = 1.0;
1402 nz_index++;
1403 values[nz_index] = 1.0;
1404 nz_index++;
1405 values[nz_index] = 1.0;
1406 nz_index++;
1407
1408 for (int i = 0; i < horizon_; ++i) {
1409 values[nz_index] = 1.0;
1410 nz_index++;
1411 values[nz_index] = 1.0;
1412 nz_index++;
1413 }
1414
1415 for (int i = 0; i < horizon_ + 1; ++i) {
1416 values[nz_index] = 1.0;
1417 nz_index++;
1418 }
1419
1420 for (int i = 0; i < lambda_horizon_; ++i) {
1421 values[nz_index] = 1.0;
1422 nz_index++;
1423 }
1424
1425 for (int i = 0; i < miu_horizon_; ++i) {
1426 values[nz_index] = 1.0;
1427 nz_index++;
1428 }
1429
1430 ADEBUG << "eval_jac_g, fulfilled obstacle constraint values";
1431 CHECK_EQ(nz_index, static_cast<int>(nele_jac));
1432 }
1433
1434 ADEBUG << "eval_jac_g done";
1435 return true;
1436} // NOLINT
1437
1438bool DistanceApproachIPOPTCUDAInterface::eval_jac_g_ser(int n, const double* x,
1439 bool new_x, int m,
1440 int nele_jac, int* iRow,
1441 int* jCol,
1442 double* values) {
1443 ADEBUG << "eval_jac_g";
1444 CHECK_EQ(n, num_of_variables_)
1445 << "No. of variables wrong in eval_jac_g. n : " << n;
1446 CHECK_EQ(m, num_of_constraints_)
1447 << "No. of constraints wrong in eval_jac_g. n : " << m;
1448
1449 if (values == nullptr) {
1450 int nz_index = 0;
1451 int constraint_index = 0;
1452 int state_index = state_start_index_;
1453 int control_index = control_start_index_;
1454 int time_index = time_start_index_;
1455
1456 // 1. State Constraint with respect to variables
1457 for (int i = 0; i < horizon_; ++i) {
1458 // g(0)' with respect to x0 ~ x7
1459 iRow[nz_index] = state_index;
1460 jCol[nz_index] = state_index;
1461 ++nz_index;
1462
1463 iRow[nz_index] = state_index;
1464 jCol[nz_index] = state_index + 2;
1465 ++nz_index;
1466
1467 iRow[nz_index] = state_index;
1468 jCol[nz_index] = state_index + 3;
1469 ++nz_index;
1470
1471 iRow[nz_index] = state_index;
1472 jCol[nz_index] = state_index + 4;
1473 ++nz_index;
1474
1475 // g(0)' with respect to u0 ~ u1'
1476 iRow[nz_index] = state_index;
1477 jCol[nz_index] = control_index;
1478 ++nz_index;
1479
1480 iRow[nz_index] = state_index;
1481 jCol[nz_index] = control_index + 1;
1482 ++nz_index;
1483
1484 // g(0)' with respect to time
1485 iRow[nz_index] = state_index;
1486 jCol[nz_index] = time_index;
1487 ++nz_index;
1488
1489 // g(1)' with respect to x0 ~ x7
1490 iRow[nz_index] = state_index + 1;
1491 jCol[nz_index] = state_index + 1;
1492 ++nz_index;
1493
1494 iRow[nz_index] = state_index + 1;
1495 jCol[nz_index] = state_index + 2;
1496 ++nz_index;
1497
1498 iRow[nz_index] = state_index + 1;
1499 jCol[nz_index] = state_index + 3;
1500 ++nz_index;
1501
1502 iRow[nz_index] = state_index + 1;
1503 jCol[nz_index] = state_index + 5;
1504 ++nz_index;
1505
1506 // g(1)' with respect to u0 ~ u1'
1507 iRow[nz_index] = state_index + 1;
1508 jCol[nz_index] = control_index;
1509 ++nz_index;
1510
1511 iRow[nz_index] = state_index + 1;
1512 jCol[nz_index] = control_index + 1;
1513 ++nz_index;
1514
1515 // g(1)' with respect to time
1516 iRow[nz_index] = state_index + 1;
1517 jCol[nz_index] = time_index;
1518 ++nz_index;
1519
1520 // g(2)' with respect to x0 ~ x7
1521 iRow[nz_index] = state_index + 2;
1522 jCol[nz_index] = state_index + 2;
1523 ++nz_index;
1524
1525 iRow[nz_index] = state_index + 2;
1526 jCol[nz_index] = state_index + 3;
1527 ++nz_index;
1528
1529 iRow[nz_index] = state_index + 2;
1530 jCol[nz_index] = state_index + 6;
1531 ++nz_index;
1532
1533 // g(2)' with respect to u0 ~ u1'
1534 iRow[nz_index] = state_index + 2;
1535 jCol[nz_index] = control_index;
1536 ++nz_index;
1537
1538 iRow[nz_index] = state_index + 2;
1539 jCol[nz_index] = control_index + 1;
1540 ++nz_index;
1541
1542 // g(2)' with respect to time
1543 iRow[nz_index] = state_index + 2;
1544 jCol[nz_index] = time_index;
1545 ++nz_index;
1546
1547 // g(3)' with x0 ~ x7
1548 iRow[nz_index] = state_index + 3;
1549 jCol[nz_index] = state_index + 3;
1550 ++nz_index;
1551
1552 iRow[nz_index] = state_index + 3;
1553 jCol[nz_index] = state_index + 7;
1554 ++nz_index;
1555
1556 // g(3)' with respect to u0 ~ u1'
1557 iRow[nz_index] = state_index + 3;
1558 jCol[nz_index] = control_index + 1;
1559 ++nz_index;
1560
1561 // g(3)' with respect to time
1562 iRow[nz_index] = state_index + 3;
1563 jCol[nz_index] = time_index;
1564 ++nz_index;
1565
1566 state_index += 4;
1567 control_index += 2;
1568 time_index++;
1569 constraint_index += 4;
1570 }
1571
1572 // 2. only have control rate constraints on u0 , range [0, horizon_-1]
1573 control_index = control_start_index_;
1574 state_index = state_start_index_;
1575 time_index = time_start_index_;
1576
1577 // First one, with respect to u(0, 0)
1578 iRow[nz_index] = constraint_index;
1579 jCol[nz_index] = control_index;
1580 ++nz_index;
1581
1582 // First element, with respect to time
1583 iRow[nz_index] = constraint_index;
1584 jCol[nz_index] = time_index;
1585 ++nz_index;
1586
1587 control_index += 2;
1588 time_index++;
1589 constraint_index++;
1590
1591 for (int i = 1; i < horizon_; ++i) {
1592 // with respect to u(0, i-1)
1593 iRow[nz_index] = constraint_index;
1594 jCol[nz_index] = control_index - 2;
1595 ++nz_index;
1596
1597 // with respect to u(0, i)
1598 iRow[nz_index] = constraint_index;
1599 jCol[nz_index] = control_index;
1600 ++nz_index;
1601
1602 // with respect to time
1603 iRow[nz_index] = constraint_index;
1604 jCol[nz_index] = time_index;
1605 ++nz_index;
1606
1607 // only consider rate limits on u0
1608 control_index += 2;
1609 constraint_index++;
1610 time_index++;
1611 }
1612
1613 // 3. Time constraints [0, horizon_ -1]
1614 time_index = time_start_index_;
1615
1616 for (int i = 0; i < horizon_; ++i) {
1617 // with respect to timescale(0, i-1)
1618 iRow[nz_index] = constraint_index;
1619 jCol[nz_index] = time_index;
1620 ++nz_index;
1621
1622 // with respect to timescale(0, i)
1623 iRow[nz_index] = constraint_index;
1624 jCol[nz_index] = time_index + 1;
1625 ++nz_index;
1626
1627 time_index++;
1628 constraint_index++;
1629 }
1630
1631 // 4. Three obstacles related equal constraints, one equality constraints,
1632 // [0, horizon_] * [0, obstacles_num_-1] * 4
1633 state_index = state_start_index_;
1634 int l_index = l_start_index_;
1635 int n_index = n_start_index_;
1636 for (int i = 0; i < horizon_ + 1; ++i) {
1637 for (int j = 0; j < obstacles_num_; ++j) {
1638 int current_edges_num = obstacles_edges_num_(j, 0);
1639
1640 // 1. norm(A* lambda == 1)
1641 for (int k = 0; k < current_edges_num; ++k) {
1642 // with respect to l
1643 iRow[nz_index] = constraint_index;
1644 jCol[nz_index] = l_index + k;
1645 ++nz_index;
1646 }
1647
1648 // 2. G' * mu + R' * lambda == 0, part 1
1649 // With respect to x
1650 iRow[nz_index] = constraint_index + 1;
1651 jCol[nz_index] = state_index + 2;
1652 ++nz_index;
1653
1654 // with respect to l
1655 for (int k = 0; k < current_edges_num; ++k) {
1656 iRow[nz_index] = constraint_index + 1;
1657 jCol[nz_index] = l_index + k;
1658 ++nz_index;
1659 }
1660
1661 // With respect to n
1662 iRow[nz_index] = constraint_index + 1;
1663 jCol[nz_index] = n_index;
1664 ++nz_index;
1665
1666 iRow[nz_index] = constraint_index + 1;
1667 jCol[nz_index] = n_index + 2;
1668 ++nz_index;
1669
1670 // 2. G' * mu + R' * lambda == 0, part 2
1671 // With respect to x
1672 iRow[nz_index] = constraint_index + 2;
1673 jCol[nz_index] = state_index + 2;
1674 ++nz_index;
1675
1676 // with respect to l
1677 for (int k = 0; k < current_edges_num; ++k) {
1678 iRow[nz_index] = constraint_index + 2;
1679 jCol[nz_index] = l_index + k;
1680 ++nz_index;
1681 }
1682
1683 // With respect to n
1684 iRow[nz_index] = constraint_index + 2;
1685 jCol[nz_index] = n_index + 1;
1686 ++nz_index;
1687
1688 iRow[nz_index] = constraint_index + 2;
1689 jCol[nz_index] = n_index + 3;
1690 ++nz_index;
1691
1692 // -g'*mu + (A*t - b)*lambda > 0
1693 // With respect to x
1694 iRow[nz_index] = constraint_index + 3;
1695 jCol[nz_index] = state_index;
1696 ++nz_index;
1697
1698 iRow[nz_index] = constraint_index + 3;
1699 jCol[nz_index] = state_index + 1;
1700 ++nz_index;
1701
1702 iRow[nz_index] = constraint_index + 3;
1703 jCol[nz_index] = state_index + 2;
1704 ++nz_index;
1705
1706 // with respect to l
1707 for (int k = 0; k < current_edges_num; ++k) {
1708 iRow[nz_index] = constraint_index + 3;
1709 jCol[nz_index] = l_index + k;
1710 ++nz_index;
1711 }
1712
1713 // with respect to n
1714 for (int k = 0; k < 4; ++k) {
1715 iRow[nz_index] = constraint_index + 3;
1716 jCol[nz_index] = n_index + k;
1717 ++nz_index;
1718 }
1719
1720 // Update index
1721 l_index += current_edges_num;
1722 n_index += 4;
1723 constraint_index += 4;
1724 }
1725 state_index += 4;
1726 }
1727
1728 // 5. load variable bounds as constraints
1729 state_index = state_start_index_;
1730 control_index = control_start_index_;
1731 time_index = time_start_index_;
1732 l_index = l_start_index_;
1733 n_index = n_start_index_;
1734
1735 // start configuration
1736 iRow[nz_index] = constraint_index;
1737 jCol[nz_index] = state_index;
1738 nz_index++;
1739 iRow[nz_index] = constraint_index + 1;
1740 jCol[nz_index] = state_index + 1;
1741 nz_index++;
1742 iRow[nz_index] = constraint_index + 2;
1743 jCol[nz_index] = state_index + 2;
1744 nz_index++;
1745 iRow[nz_index] = constraint_index + 3;
1746 jCol[nz_index] = state_index + 3;
1747 nz_index++;
1748 constraint_index += 4;
1749 state_index += 4;
1750
1751 for (int i = 1; i < horizon_; ++i) {
1752 iRow[nz_index] = constraint_index;
1753 jCol[nz_index] = state_index;
1754 nz_index++;
1755 iRow[nz_index] = constraint_index + 1;
1756 jCol[nz_index] = state_index + 1;
1757 nz_index++;
1758 iRow[nz_index] = constraint_index + 2;
1759 jCol[nz_index] = state_index + 3;
1760 nz_index++;
1761 constraint_index += 3;
1762 state_index += 4;
1763 }
1764
1765 // end configuration
1766 iRow[nz_index] = constraint_index;
1767 jCol[nz_index] = state_index;
1768 nz_index++;
1769 iRow[nz_index] = constraint_index + 1;
1770 jCol[nz_index] = state_index + 1;
1771 nz_index++;
1772 iRow[nz_index] = constraint_index + 2;
1773 jCol[nz_index] = state_index + 2;
1774 nz_index++;
1775 iRow[nz_index] = constraint_index + 3;
1776 jCol[nz_index] = state_index + 3;
1777 nz_index++;
1778 constraint_index += 4;
1779 state_index += 4;
1780
1781 for (int i = 0; i < horizon_; ++i) {
1782 iRow[nz_index] = constraint_index;
1783 jCol[nz_index] = control_index;
1784 nz_index++;
1785 iRow[nz_index] = constraint_index + 1;
1786 jCol[nz_index] = control_index + 1;
1787 nz_index++;
1788 constraint_index += 2;
1789 control_index += 2;
1790 }
1791
1792 for (int i = 0; i < horizon_ + 1; ++i) {
1793 iRow[nz_index] = constraint_index;
1794 jCol[nz_index] = time_index;
1795 nz_index++;
1796 constraint_index++;
1797 time_index++;
1798 }
1799
1800 for (int i = 0; i < lambda_horizon_; ++i) {
1801 iRow[nz_index] = constraint_index;
1802 jCol[nz_index] = l_index;
1803 nz_index++;
1804 constraint_index++;
1805 l_index++;
1806 }
1807
1808 for (int i = 0; i < miu_horizon_; ++i) {
1809 iRow[nz_index] = constraint_index;
1810 jCol[nz_index] = n_index;
1811 nz_index++;
1812 constraint_index++;
1813 n_index++;
1814 }
1815
1816 CHECK_EQ(nz_index, static_cast<int>(nele_jac));
1817 CHECK_EQ(constraint_index, static_cast<int>(m));
1818 } else {
1819 std::fill(values, values + nele_jac, 0.0);
1820 int nz_index = 0;
1821
1822 int time_index = time_start_index_;
1823 int state_index = state_start_index_;
1824 int control_index = control_start_index_;
1825
1826 // TODO(QiL) : initially implemented to be debug friendly, later iterate
1827 // towards better efficiency
1828 // 1. state constraints 4 * [0, horizons-1]
1829 for (int i = 0; i < horizon_; ++i) {
1830 values[nz_index] = -1.0;
1831 ++nz_index;
1832
1833 values[nz_index] =
1834 x[time_index] * ts_ *
1835 (x[state_index + 3] +
1836 x[time_index] * ts_ * 0.5 * x[control_index + 1]) *
1837 std::sin(x[state_index + 2] +
1838 x[time_index] * ts_ * 0.5 * x[state_index + 3] *
1839 std::tan(x[control_index]) / wheelbase_); // a.
1840 ++nz_index;
1841
1842 values[nz_index] =
1843 -1.0 *
1844 (x[time_index] * ts_ *
1845 std::cos(x[state_index + 2] +
1846 x[time_index] * ts_ * 0.5 * x[state_index + 3] *
1847 std::tan(x[control_index]) / wheelbase_) +
1848 x[time_index] * ts_ *
1849 (x[state_index + 3] +
1850 x[time_index] * ts_ * 0.5 * x[control_index + 1]) *
1851 (-1) * x[time_index] * ts_ * 0.5 * std::tan(x[control_index]) /
1852 wheelbase_ *
1853 std::sin(x[state_index + 2] +
1854 x[time_index] * ts_ * 0.5 * x[state_index + 3] *
1855 std::tan(x[control_index]) / wheelbase_)); // b
1856 ++nz_index;
1857
1858 values[nz_index] = 1.0;
1859 ++nz_index;
1860
1861 values[nz_index] =
1862 x[time_index] * ts_ *
1863 (x[state_index + 3] +
1864 x[time_index] * ts_ * 0.5 * x[control_index + 1]) *
1865 std::sin(x[state_index + 2] +
1866 x[time_index] * ts_ * 0.5 * x[state_index + 3] *
1867 std::tan(x[control_index]) / wheelbase_) *
1868 x[time_index] * ts_ * 0.5 * x[state_index + 3] /
1869 (std::cos(x[control_index]) * std::cos(x[control_index])) /
1870 wheelbase_; // c
1871 ++nz_index;
1872
1873 values[nz_index] =
1874 -1.0 * (x[time_index] * ts_ * x[time_index] * ts_ * 0.5 *
1875 std::cos(x[state_index + 2] +
1876 x[time_index] * ts_ * 0.5 * x[state_index + 3] *
1877 std::tan(x[control_index]) / wheelbase_)); // d
1878 ++nz_index;
1879
1880 values[nz_index] =
1881 -1.0 *
1882 (ts_ *
1883 (x[state_index + 3] +
1884 x[time_index] * ts_ * 0.5 * x[control_index + 1]) *
1885 std::cos(x[state_index + 2] +
1886 x[time_index] * ts_ * 0.5 * x[state_index + 3] *
1887 std::tan(x[control_index]) / wheelbase_) +
1888 x[time_index] * ts_ * ts_ * 0.5 * x[control_index + 1] *
1889 std::cos(x[state_index + 2] +
1890 x[time_index] * ts_ * 0.5 * x[state_index + 3] *
1891 std::tan(x[control_index]) / wheelbase_) -
1892 x[time_index] * ts_ *
1893 (x[state_index + 3] +
1894 x[time_index] * ts_ * 0.5 * x[control_index + 1]) *
1895 x[state_index + 3] * ts_ * 0.5 * std::tan(x[control_index]) /
1896 wheelbase_ *
1897 std::sin(x[state_index + 2] +
1898 x[time_index] * ts_ * 0.5 * x[state_index + 3] *
1899 std::tan(x[control_index]) / wheelbase_)); // e
1900 ++nz_index;
1901
1902 values[nz_index] = -1.0;
1903 ++nz_index;
1904
1905 values[nz_index] =
1906 -1.0 * (x[time_index] * ts_ *
1907 (x[state_index + 3] +
1908 x[time_index] * ts_ * 0.5 * x[control_index + 1]) *
1909 std::cos(x[state_index + 2] +
1910 x[time_index] * ts_ * 0.5 * x[state_index + 3] *
1911 std::tan(x[control_index]) / wheelbase_)); // f.
1912 ++nz_index;
1913
1914 values[nz_index] =
1915 -1.0 *
1916 (x[time_index] * ts_ *
1917 std::sin(x[state_index + 2] +
1918 x[time_index] * ts_ * 0.5 * x[state_index + 3] *
1919 std::tan(x[control_index]) / wheelbase_) +
1920 x[time_index] * ts_ *
1921 (x[state_index + 3] +
1922 x[time_index] * ts_ * 0.5 * x[control_index + 1]) *
1923 x[time_index] * ts_ * 0.5 * std::tan(x[control_index]) /
1924 wheelbase_ *
1925 std::cos(x[state_index + 2] +
1926 x[time_index] * ts_ * 0.5 * x[state_index + 3] *
1927 std::tan(x[control_index]) / wheelbase_)); // g
1928 ++nz_index;
1929
1930 values[nz_index] = 1.0;
1931 ++nz_index;
1932
1933 values[nz_index] =
1934 -1.0 * (x[time_index] * ts_ *
1935 (x[state_index + 3] +
1936 x[time_index] * ts_ * 0.5 * x[control_index + 1]) *
1937 std::cos(x[state_index + 2] +
1938 x[time_index] * ts_ * 0.5 * x[state_index + 3] *
1939 std::tan(x[control_index]) / wheelbase_) *
1940 x[time_index] * ts_ * 0.5 * x[state_index + 3] /
1941 (std::cos(x[control_index]) * std::cos(x[control_index])) /
1942 wheelbase_); // h
1943 ++nz_index;
1944
1945 values[nz_index] =
1946 -1.0 * (x[time_index] * ts_ * x[time_index] * ts_ * 0.5 *
1947 std::sin(x[state_index + 2] +
1948 x[time_index] * ts_ * 0.5 * x[state_index + 3] *
1949 std::tan(x[control_index]) / wheelbase_)); // i
1950 ++nz_index;
1951
1952 values[nz_index] =
1953 -1.0 *
1954 (ts_ *
1955 (x[state_index + 3] +
1956 x[time_index] * ts_ * 0.5 * x[control_index + 1]) *
1957 std::sin(x[state_index + 2] +
1958 x[time_index] * ts_ * 0.5 * x[state_index + 3] *
1959 std::tan(x[control_index]) / wheelbase_) +
1960 x[time_index] * ts_ * ts_ * 0.5 * x[control_index + 1] *
1961 std::sin(x[state_index + 2] +
1962 x[time_index] * ts_ * 0.5 * x[state_index + 3] *
1963 std::tan(x[control_index]) / wheelbase_) +
1964 x[time_index] * ts_ *
1965 (x[state_index + 3] +
1966 x[time_index] * ts_ * 0.5 * x[control_index + 1]) *
1967 x[state_index + 3] * ts_ * 0.5 * std::tan(x[control_index]) /
1968 wheelbase_ *
1969 std::cos(x[state_index + 2] +
1970 x[time_index] * ts_ * 0.5 * x[state_index + 3] *
1971 std::tan(x[control_index]) / wheelbase_)); // j
1972 ++nz_index;
1973
1974 values[nz_index] = -1.0;
1975 ++nz_index;
1976
1977 values[nz_index] = -1.0 * x[time_index] * ts_ *
1978 std::tan(x[control_index]) / wheelbase_; // k.
1979 ++nz_index;
1980
1981 values[nz_index] = 1.0;
1982 ++nz_index;
1983
1984 values[nz_index] =
1985 -1.0 * (x[time_index] * ts_ *
1986 (x[state_index + 3] +
1987 x[time_index] * ts_ * 0.5 * x[control_index + 1]) /
1988 (std::cos(x[control_index]) * std::cos(x[control_index])) /
1989 wheelbase_); // l.
1990 ++nz_index;
1991
1992 values[nz_index] =
1993 -1.0 * (x[time_index] * ts_ * x[time_index] * ts_ * 0.5 *
1994 std::tan(x[control_index]) / wheelbase_); // m.
1995 ++nz_index;
1996
1997 values[nz_index] =
1998 -1.0 * (ts_ *
1999 (x[state_index + 3] +
2000 x[time_index] * ts_ * 0.5 * x[control_index + 1]) *
2001 std::tan(x[control_index]) / wheelbase_ +
2002 x[time_index] * ts_ * ts_ * 0.5 * x[control_index + 1] *
2003 std::tan(x[control_index]) / wheelbase_); // n.
2004 ++nz_index;
2005
2006 values[nz_index] = -1.0;
2007 ++nz_index;
2008
2009 values[nz_index] = 1.0;
2010 ++nz_index;
2011
2012 values[nz_index] = -1.0 * ts_ * x[time_index]; // o.
2013 ++nz_index;
2014
2015 values[nz_index] = -1.0 * ts_ * x[control_index + 1]; // p.
2016 ++nz_index;
2017
2018 state_index += 4;
2019 control_index += 2;
2020 time_index++;
2021 }
2022
2023 // 2. control rate constraints 1 * [0, horizons-1]
2024 control_index = control_start_index_;
2025 state_index = state_start_index_;
2026 time_index = time_start_index_;
2027
2028 // First horizon
2029
2030 // with respect to u(0, 0)
2031 values[nz_index] = 1.0 / x[time_index] / ts_; // q
2032 ++nz_index;
2033
2034 // with respect to time
2035 values[nz_index] = -1.0 * (x[control_index] - last_time_u_(0, 0)) /
2036 x[time_index] / x[time_index] / ts_;
2037 ++nz_index;
2038 time_index++;
2039 control_index += 2;
2040
2041 for (int i = 1; i < horizon_; ++i) {
2042 // with respect to u(0, i-1)
2043
2044 values[nz_index] = -1.0 / x[time_index] / ts_;
2045 ++nz_index;
2046
2047 // with respect to u(0, i)
2048 values[nz_index] = 1.0 / x[time_index] / ts_;
2049 ++nz_index;
2050
2051 // with respect to time
2052 values[nz_index] = -1.0 * (x[control_index] - x[control_index - 2]) /
2053 x[time_index] / x[time_index] / ts_;
2054 ++nz_index;
2055
2056 control_index += 2;
2057 time_index++;
2058 }
2059
2060 ADEBUG << "After fulfilled control rate constraints derivative, nz_index : "
2061 << nz_index << " nele_jac : " << nele_jac;
2062
2063 // 3. Time constraints [0, horizon_ -1]
2064 time_index = time_start_index_;
2065 for (int i = 0; i < horizon_; ++i) {
2066 // with respect to timescale(0, i-1)
2067 values[nz_index] = -1.0;
2068 ++nz_index;
2069
2070 // with respect to timescale(0, i)
2071 values[nz_index] = 1.0;
2072 ++nz_index;
2073
2074 time_index++;
2075 }
2076
2077 ADEBUG << "After fulfilled time constraints derivative, nz_index : "
2078 << nz_index << " nele_jac : " << nele_jac;
2079
2080 // 4. Three obstacles related equal constraints, one equality constraints,
2081 // [0, horizon_] * [0, obstacles_num_-1] * 4
2082
2083 state_index = state_start_index_;
2084 int l_index = l_start_index_;
2085 int n_index = n_start_index_;
2086
2087 for (int i = 0; i < horizon_ + 1; ++i) {
2088 int edges_counter = 0;
2089 for (int j = 0; j < obstacles_num_; ++j) {
2090 int current_edges_num = obstacles_edges_num_(j, 0);
2091 Eigen::MatrixXd Aj =
2092 obstacles_A_.block(edges_counter, 0, current_edges_num, 2);
2093 Eigen::MatrixXd bj =
2094 obstacles_b_.block(edges_counter, 0, current_edges_num, 1);
2095
2096 // TODO(QiL) : Remove redundant calculation
2097 double tmp1 = 0;
2098 double tmp2 = 0;
2099 for (int k = 0; k < current_edges_num; ++k) {
2100 // TODO(QiL) : replace this one directly with x
2101 tmp1 += Aj(k, 0) * x[l_index + k];
2102 tmp2 += Aj(k, 1) * x[l_index + k];
2103 }
2104
2105 // 1. norm(A* lambda == 1)
2106 for (int k = 0; k < current_edges_num; ++k) {
2107 // with respect to l
2108 values[nz_index] =
2109 2 * tmp1 * Aj(k, 0) + 2 * tmp2 * Aj(k, 1); // t0~tk
2110 ++nz_index;
2111 }
2112
2113 // 2. G' * mu + R' * lambda == 0, part 1
2114 // With respect to x
2115 values[nz_index] = -std::sin(x[state_index + 2]) * tmp1 +
2116 std::cos(x[state_index + 2]) * tmp2; // u
2117 ++nz_index;
2118
2119 // with respect to l
2120 for (int k = 0; k < current_edges_num; ++k) {
2121 values[nz_index] = std::cos(x[state_index + 2]) * Aj(k, 0) +
2122 std::sin(x[state_index + 2]) * Aj(k, 1); // v0~vn
2123 ++nz_index;
2124 }
2125
2126 // With respect to n
2127 values[nz_index] = 1.0; // w0
2128 ++nz_index;
2129
2130 values[nz_index] = -1.0; // w2
2131 ++nz_index;
2132
2133 // 3. G' * mu + R' * lambda == 0, part 2
2134 // With respect to x
2135 values[nz_index] = -std::cos(x[state_index + 2]) * tmp1 -
2136 std::sin(x[state_index + 2]) * tmp2; // x
2137 ++nz_index;
2138
2139 // with respect to l
2140 for (int k = 0; k < current_edges_num; ++k) {
2141 values[nz_index] = -std::sin(x[state_index + 2]) * Aj(k, 0) +
2142 std::cos(x[state_index + 2]) * Aj(k, 1); // y0~yn
2143 ++nz_index;
2144 }
2145
2146 // With respect to n
2147 values[nz_index] = 1.0; // z1
2148 ++nz_index;
2149
2150 values[nz_index] = -1.0; // z3
2151 ++nz_index;
2152
2153 // 3. -g'*mu + (A*t - b)*lambda > 0
2154 double tmp3 = 0.0;
2155 double tmp4 = 0.0;
2156 for (int k = 0; k < 4; ++k) {
2157 tmp3 += -g_[k] * x[n_index + k];
2158 }
2159
2160 for (int k = 0; k < current_edges_num; ++k) {
2161 tmp4 += bj(k, 0) * x[l_index + k];
2162 }
2163
2164 // With respect to x
2165 values[nz_index] = tmp1; // aa1
2166 ++nz_index;
2167
2168 values[nz_index] = tmp2; // bb1
2169 ++nz_index;
2170
2171 values[nz_index] =
2172 -std::sin(x[state_index + 2]) * offset_ * tmp1 +
2173 std::cos(x[state_index + 2]) * offset_ * tmp2; // cc1
2174 ++nz_index;
2175
2176 // with respect to l
2177 for (int k = 0; k < current_edges_num; ++k) {
2178 values[nz_index] =
2179 (x[state_index] + std::cos(x[state_index + 2]) * offset_) *
2180 Aj(k, 0) +
2181 (x[state_index + 1] + std::sin(x[state_index + 2]) * offset_) *
2182 Aj(k, 1) -
2183 bj(k, 0); // ddk
2184 ++nz_index;
2185 }
2186
2187 // with respect to n
2188 for (int k = 0; k < 4; ++k) {
2189 values[nz_index] = -g_[k]; // eek
2190 ++nz_index;
2191 }
2192
2193 // Update index
2194 edges_counter += current_edges_num;
2195 l_index += current_edges_num;
2196 n_index += 4;
2197 }
2198 state_index += 4;
2199 }
2200
2201 // 5. load variable bounds as constraints
2202 state_index = state_start_index_;
2203 control_index = control_start_index_;
2204 time_index = time_start_index_;
2205 l_index = l_start_index_;
2206 n_index = n_start_index_;
2207
2208 // start configuration
2209 values[nz_index] = 1.0;
2210 nz_index++;
2211 values[nz_index] = 1.0;
2212 nz_index++;
2213 values[nz_index] = 1.0;
2214 nz_index++;
2215 values[nz_index] = 1.0;
2216 nz_index++;
2217
2218 for (int i = 1; i < horizon_; ++i) {
2219 values[nz_index] = 1.0;
2220 nz_index++;
2221 values[nz_index] = 1.0;
2222 nz_index++;
2223 values[nz_index] = 1.0;
2224 nz_index++;
2225 }
2226
2227 // end configuration
2228 values[nz_index] = 1.0;
2229 nz_index++;
2230 values[nz_index] = 1.0;
2231 nz_index++;
2232 values[nz_index] = 1.0;
2233 nz_index++;
2234 values[nz_index] = 1.0;
2235 nz_index++;
2236
2237 for (int i = 0; i < horizon_; ++i) {
2238 values[nz_index] = 1.0;
2239 nz_index++;
2240 values[nz_index] = 1.0;
2241 nz_index++;
2242 }
2243
2244 for (int i = 0; i < horizon_ + 1; ++i) {
2245 values[nz_index] = 1.0;
2246 nz_index++;
2247 }
2248
2249 for (int i = 0; i < lambda_horizon_; ++i) {
2250 values[nz_index] = 1.0;
2251 nz_index++;
2252 }
2253
2254 for (int i = 0; i < miu_horizon_; ++i) {
2255 values[nz_index] = 1.0;
2256 nz_index++;
2257 }
2258
2259 ADEBUG << "eval_jac_g, fulfilled obstacle constraint values";
2260 CHECK_EQ(nz_index, static_cast<int>(nele_jac));
2261 }
2262
2263 ADEBUG << "eval_jac_g done";
2264 return true;
2265} // NOLINT
2266
2267bool DistanceApproachIPOPTCUDAInterface::eval_h(int n, const double* x,
2268 bool new_x, double obj_factor,
2269 int m, const double* lambda,
2270 bool new_lambda, int nele_hess,
2271 int* iRow, int* jCol,
2272 double* values) {
2273 if (values == nullptr) {
2274 // return the structure. This is a symmetric matrix, fill the lower left
2275 // triangle only.
2276#if USE_GPU == 1
2277 fill_lower_left(iRow, jCol, rind_L, cind_L, nnz_L);
2278#else
2279 AFATAL << "CUDA enabled without GPU!";
2280#endif
2281 } else {
2282 // return the values. This is a symmetric matrix, fill the lower left
2283 // triangle only
2284
2285 obj_lam[0] = obj_factor;
2286#if USE_GPU == 1
2287 data_transfer(&obj_lam[1], lambda, m);
2288#else
2289 AFATAL << "CUDA enabled without GPU!";
2290#endif
2291
2292 set_param_vec(tag_L, m + 1, obj_lam);
2293 sparse_hess(tag_L, n, 1, const_cast<double*>(x), &nnz_L, &rind_L, &cind_L,
2294 &hessval, options_L);
2295
2296#if USE_GPU == 1
2297 if (!data_transfer(values, hessval, nnz_L)) {
2298 for (int idx = 0; idx < nnz_L; idx++) {
2299 values[idx] = hessval[idx];
2300 }
2301 }
2302#endif
2303 }
2304
2305 return true;
2306}
2307
2308void DistanceApproachIPOPTCUDAInterface::finalize_solution(
2309 Ipopt::SolverReturn status, int n, const double* x, const double* z_L,
2310 const double* z_U, int m, const double* g, const double* lambda,
2311 double obj_value, const Ipopt::IpoptData* ip_data,
2312 Ipopt::IpoptCalculatedQuantities* ip_cq) {
2313 ADEBUG << "finalize_solution";
2314 int state_index = state_start_index_;
2315 int control_index = control_start_index_;
2316 int time_index = time_start_index_;
2317 int dual_l_index = l_start_index_;
2318 int dual_n_index = n_start_index_;
2319 // 1. state variables, 4 * [0, horizon]
2320 // 2. control variables, 2 * [0, horizon_-1]
2321 // 3. sampling time variables, 1 * [0, horizon_]
2322 // 4. dual_l obstacles_edges_sum_ * [0, horizon]
2323 // 5. dual_n obstacles_num * [0, horizon]
2324 for (int i = 0; i < horizon_; ++i) {
2325 state_result_(0, i) = x[state_index];
2326 state_result_(1, i) = x[state_index + 1];
2327 state_result_(2, i) = x[state_index + 2];
2328 state_result_(3, i) = x[state_index + 3];
2329 control_result_(0, i) = x[control_index];
2330 control_result_(1, i) = x[control_index + 1];
2331 time_result_(0, i) = x[time_index];
2332 for (int j = 0; j < obstacles_edges_sum_; ++j) {
2333 dual_l_result_(j, i) = x[dual_l_index + j];
2334 }
2335 for (int k = 0; k < 4 * obstacles_num_; k++) {
2336 dual_n_result_(k, i) = x[dual_n_index + k];
2337 }
2338 state_index += 4;
2339 control_index += 2;
2340 time_index++;
2341 dual_l_index += obstacles_edges_sum_;
2342 dual_n_index += 4 * obstacles_num_;
2343 }
2344 state_result_(0, 0) = x0_(0, 0);
2345 state_result_(1, 0) = x0_(1, 0);
2346 state_result_(2, 0) = x0_(2, 0);
2347 state_result_(3, 0) = x0_(3, 0);
2348 // push back last horizon for state and time variables
2349 state_result_(0, horizon_) = xf_(0, 0);
2350 state_result_(1, horizon_) = xf_(1, 0);
2351 state_result_(2, horizon_) = xf_(2, 0);
2352 state_result_(3, horizon_) = xf_(3, 0);
2353 time_result_(0, horizon_) = x[time_index];
2354 time_result_ = ts_ * time_result_;
2355 for (int j = 0; j < obstacles_edges_sum_; ++j) {
2356 dual_l_result_(j, horizon_) = x[dual_l_index + j];
2357 }
2358 for (int k = 0; k < 4 * obstacles_num_; k++) {
2359 dual_n_result_(k, horizon_) = x[dual_n_index + k];
2360 }
2361 // memory deallocation of ADOL-C variables
2362 delete[] obj_lam;
2363 free(rind_L);
2364 free(cind_L);
2365 free(hessval);
2366
2367 ADEBUG << "finalize_solution done!";
2368}
2369
2370void DistanceApproachIPOPTCUDAInterface::get_optimization_results(
2371 Eigen::MatrixXd* state_result, Eigen::MatrixXd* control_result,
2372 Eigen::MatrixXd* time_result, Eigen::MatrixXd* dual_l_result,
2373 Eigen::MatrixXd* dual_n_result) const {
2374 ADEBUG << "get_optimization_results";
2375 *state_result = state_result_;
2376 *control_result = control_result_;
2377 *time_result = time_result_;
2378 *dual_l_result = dual_l_result_;
2379 *dual_n_result = dual_n_result_;
2380
2381 if (!distance_approach_config_.enable_initial_final_check()) {
2382 return;
2383 }
2384 CHECK_EQ(state_result_.cols(), xWS_.cols());
2385 CHECK_EQ(state_result_.rows(), xWS_.rows());
2386 double state_diff_max = 0.0;
2387 for (int i = 0; i < horizon_ + 1; ++i) {
2388 for (int j = 0; j < 4; ++j) {
2389 state_diff_max =
2390 std::max(std::abs(xWS_(j, i) - state_result_(j, i)), state_diff_max);
2391 }
2392 }
2393
2394 // 2. control variable initialization, 2 * horizon_
2395 // CHECK_EQ(control_result_.cols(), uWS_.cols());
2396 CHECK_EQ(control_result_.rows(), uWS_.rows());
2397 double control_diff_max = 0.0;
2398 for (int i = 0; i < horizon_; ++i) {
2399 control_diff_max = std::max(std::abs(uWS_(0, i) - control_result_(0, i)),
2400 control_diff_max);
2401 control_diff_max = std::max(std::abs(uWS_(1, i) - control_result_(1, i)),
2402 control_diff_max);
2403 }
2404
2405 // 2. time scale variable initialization, horizon_ + 1, no
2406
2407 // 3. lagrange constraint l, obstacles_edges_sum_ * (horizon_+1)
2408 CHECK_EQ(dual_l_result_.cols(), l_warm_up_.cols());
2409 CHECK_EQ(dual_l_result_.rows(), l_warm_up_.rows());
2410 double l_diff_max = 0.0;
2411 for (int i = 0; i < horizon_ + 1; ++i) {
2412 for (int j = 0; j < obstacles_edges_sum_; ++j) {
2413 l_diff_max = std::max(std::abs(l_warm_up_(j, i) - dual_l_result_(j, i)),
2414 l_diff_max);
2415 }
2416 }
2417
2418 // 4. lagrange constraint m, 4*obstacles_num * (horizon_+1)
2419 CHECK_EQ(n_warm_up_.cols(), dual_n_result_.cols());
2420 CHECK_EQ(n_warm_up_.rows(), dual_n_result_.rows());
2421 double n_diff_max = 0.0;
2422 for (int i = 0; i < horizon_ + 1; ++i) {
2423 for (int j = 0; j < 4 * obstacles_num_; ++j) {
2424 n_diff_max = std::max(std::abs(n_warm_up_(j, i) - dual_n_result_(j, i)),
2425 n_diff_max);
2426 }
2427 }
2428
2429 ADEBUG << "state_diff_max: " << state_diff_max;
2430 ADEBUG << "control_diff_max: " << control_diff_max;
2431 ADEBUG << "dual_l_diff_max: " << l_diff_max;
2432 ADEBUG << "dual_n_diff_max: " << n_diff_max;
2433}
2434
2435//*************** start ADOL-C part ***********************************
2436template <class T>
2437bool DistanceApproachIPOPTCUDAInterface::eval_obj(int n, const T* x,
2438 T* obj_value) {
2439 ADEBUG << "eval_obj";
2440 // Objective is :
2441 // min control inputs
2442 // min input rate
2443 // min time (if the time step is not fixed)
2444 // regularization wrt warm start trajectory
2445 DCHECK(ts_ != 0) << "ts in distance_approach_ is 0";
2446 int control_index = control_start_index_;
2447 int time_index = time_start_index_;
2448 int state_index = state_start_index_;
2449
2450 // TODO(QiL): Initial implementation towards earlier understanding and debug
2451 // purpose, later code refine towards improving efficiency
2452
2453 *obj_value = 0.0;
2454 // 1. objective to minimize state diff to warm up
2455 for (int i = 0; i < horizon_ + 1; ++i) {
2456 T x1_diff = x[state_index] - xWS_(0, i);
2457 T x2_diff = x[state_index + 1] - xWS_(1, i);
2458 T x3_diff = x[state_index + 2] - xWS_(2, i);
2459 T x4_abs = x[state_index + 3];
2460 *obj_value += weight_state_x_ * x1_diff * x1_diff +
2461 weight_state_y_ * x2_diff * x2_diff +
2462 weight_state_phi_ * x3_diff * x3_diff +
2463 weight_state_v_ * x4_abs * x4_abs;
2464 state_index += 4;
2465 }
2466
2467 // 2. objective to minimize u square
2468 for (int i = 0; i < horizon_; ++i) {
2469 *obj_value += weight_input_steer_ * x[control_index] * x[control_index] +
2470 weight_input_a_ * x[control_index + 1] * x[control_index + 1];
2471 control_index += 2;
2472 }
2473
2474 // 3. objective to minimize input change rate for first horizon
2475 control_index = control_start_index_;
2476 T last_time_steer_rate =
2477 (x[control_index] - last_time_u_(0, 0)) / x[time_index] / ts_;
2478 T last_time_a_rate =
2479 (x[control_index + 1] - last_time_u_(1, 0)) / x[time_index] / ts_;
2480 *obj_value +=
2481 weight_stitching_steer_ * last_time_steer_rate * last_time_steer_rate +
2482 weight_stitching_a_ * last_time_a_rate * last_time_a_rate;
2483
2484 // 4. objective to minimize input change rates, [0- horizon_ -2]
2485 time_index++;
2486 for (int i = 0; i < horizon_ - 1; ++i) {
2487 T steering_rate =
2488 (x[control_index + 2] - x[control_index]) / x[time_index] / ts_;
2489 T a_rate =
2490 (x[control_index + 3] - x[control_index + 1]) / x[time_index] / ts_;
2491 *obj_value += weight_rate_steer_ * steering_rate * steering_rate +
2492 weight_rate_a_ * a_rate * a_rate;
2493 control_index += 2;
2494 time_index++;
2495 }
2496
2497 // 5. objective to minimize total time [0, horizon_]
2498 time_index = time_start_index_;
2499 for (int i = 0; i < horizon_ + 1; ++i) {
2500 T first_order_penalty = weight_first_order_time_ * x[time_index];
2501 T second_order_penalty =
2502 weight_second_order_time_ * x[time_index] * x[time_index];
2503 *obj_value += first_order_penalty + second_order_penalty;
2504 time_index++;
2505 }
2506
2507 ADEBUG << "objective value after this iteration : " << *obj_value;
2508 return true;
2509}
2510
2511template <class T>
2512bool DistanceApproachIPOPTCUDAInterface::eval_constraints(int n, const T* x,
2513 int m, T* g) {
2514 ADEBUG << "eval_constraints";
2515 // state start index
2516 int state_index = state_start_index_;
2517
2518 // control start index.
2519 int control_index = control_start_index_;
2520
2521 // time start index
2522 int time_index = time_start_index_;
2523
2524 int constraint_index = 0;
2525
2526 // // 1. state constraints 4 * [0, horizons-1]
2527 for (int i = 0; i < horizon_; ++i) {
2528 // x1
2529 // TODO(QiL) : optimize and remove redundant calculation in next
2530 // iteration.
2531 g[constraint_index] =
2532 x[state_index + 4] -
2533 (x[state_index] +
2534 ts_ * x[time_index] *
2535 (x[state_index + 3] +
2536 ts_ * x[time_index] * 0.5 * x[control_index + 1]) *
2537 cos(x[state_index + 2] + ts_ * x[time_index] * 0.5 *
2538 x[state_index + 3] *
2539 tan(x[control_index]) / wheelbase_));
2540 // x2
2541 g[constraint_index + 1] =
2542 x[state_index + 5] -
2543 (x[state_index + 1] +
2544 ts_ * x[time_index] *
2545 (x[state_index + 3] +
2546 ts_ * x[time_index] * 0.5 * x[control_index + 1]) *
2547 sin(x[state_index + 2] + ts_ * x[time_index] * 0.5 *
2548 x[state_index + 3] *
2549 tan(x[control_index]) / wheelbase_));
2550
2551 // x3
2552 g[constraint_index + 2] =
2553 x[state_index + 6] -
2554 (x[state_index + 2] +
2555 ts_ * x[time_index] *
2556 (x[state_index + 3] +
2557 ts_ * x[time_index] * 0.5 * x[control_index + 1]) *
2558 tan(x[control_index]) / wheelbase_);
2559
2560 // x4
2561 g[constraint_index + 3] =
2562 x[state_index + 7] -
2563 (x[state_index + 3] + ts_ * x[time_index] * x[control_index + 1]);
2564
2565 control_index += 2;
2566 constraint_index += 4;
2567 time_index++;
2568 state_index += 4;
2569 }
2570
2571 ADEBUG << "constraint_index after adding Euler forward dynamics constraints "
2572 "updated: "
2573 << constraint_index;
2574
2575 // 2. Control rate limit constraints, 1 * [0, horizons-1], only apply
2576 // steering rate as of now
2577 control_index = control_start_index_;
2578 time_index = time_start_index_;
2579
2580 // First rate is compare first with stitch point
2581 g[constraint_index] =
2582 (x[control_index] - last_time_u_(0, 0)) / x[time_index] / ts_;
2583 control_index += 2;
2584 constraint_index++;
2585 time_index++;
2586
2587 for (int i = 1; i < horizon_; ++i) {
2588 g[constraint_index] =
2589 (x[control_index] - x[control_index - 2]) / x[time_index] / ts_;
2590 constraint_index++;
2591 control_index += 2;
2592 time_index++;
2593 }
2594
2595 // 3. Time constraints 1 * [0, horizons-1]
2596 time_index = time_start_index_;
2597 for (int i = 0; i < horizon_; ++i) {
2598 g[constraint_index] = x[time_index + 1] - x[time_index];
2599 constraint_index++;
2600 time_index++;
2601 }
2602
2603 ADEBUG << "constraint_index after adding time constraints "
2604 "updated: "
2605 << constraint_index;
2606
2607 // 4. Three obstacles related equal constraints, one equality constraints,
2608 // [0, horizon_] * [0, obstacles_num_-1] * 4
2609 state_index = state_start_index_;
2610 int l_index = l_start_index_;
2611 int n_index = n_start_index_;
2612
2613 for (int i = 0; i < horizon_ + 1; ++i) {
2614 int edges_counter = 0;
2615 for (int j = 0; j < obstacles_num_; ++j) {
2616 int current_edges_num = obstacles_edges_num_(j, 0);
2617 Eigen::MatrixXd Aj =
2618 obstacles_A_.block(edges_counter, 0, current_edges_num, 2);
2619 Eigen::MatrixXd bj =
2620 obstacles_b_.block(edges_counter, 0, current_edges_num, 1);
2621
2622 // norm(A* lambda) <= 1
2623 T tmp1 = 0.0;
2624 T tmp2 = 0.0;
2625 for (int k = 0; k < current_edges_num; ++k) {
2626 // TODO(QiL) : replace this one directly with x
2627 tmp1 += Aj(k, 0) * x[l_index + k];
2628 tmp2 += Aj(k, 1) * x[l_index + k];
2629 }
2630 g[constraint_index] = tmp1 * tmp1 + tmp2 * tmp2;
2631
2632 // G' * mu + R' * lambda == 0
2633 g[constraint_index + 1] = x[n_index] - x[n_index + 2] +
2634 cos(x[state_index + 2]) * tmp1 +
2635 sin(x[state_index + 2]) * tmp2;
2636
2637 g[constraint_index + 2] = x[n_index + 1] - x[n_index + 3] -
2638 sin(x[state_index + 2]) * tmp1 +
2639 cos(x[state_index + 2]) * tmp2;
2640
2641 // -g'*mu + (A*t - b)*lambda > 0
2642 T tmp3 = 0.0;
2643 for (int k = 0; k < 4; ++k) {
2644 tmp3 += -g_[k] * x[n_index + k];
2645 }
2646
2647 T tmp4 = 0.0;
2648 for (int k = 0; k < current_edges_num; ++k) {
2649 tmp4 += bj(k, 0) * x[l_index + k];
2650 }
2651
2652 g[constraint_index + 3] =
2653 tmp3 + (x[state_index] + cos(x[state_index + 2]) * offset_) * tmp1 +
2654 (x[state_index + 1] + sin(x[state_index + 2]) * offset_) * tmp2 -
2655 tmp4;
2656
2657 // Update index
2658 edges_counter += current_edges_num;
2659 l_index += current_edges_num;
2660 n_index += 4;
2661 constraint_index += 4;
2662 }
2663 state_index += 4;
2664 }
2665 ADEBUG << "constraint_index after obstacles avoidance constraints "
2666 "updated: "
2667 << constraint_index;
2668
2669 // 5. load variable bounds as constraints
2670 state_index = state_start_index_;
2671 control_index = control_start_index_;
2672 time_index = time_start_index_;
2673 l_index = l_start_index_;
2674 n_index = n_start_index_;
2675
2676 // start configuration
2677 g[constraint_index] = x[state_index];
2678 g[constraint_index + 1] = x[state_index + 1];
2679 g[constraint_index + 2] = x[state_index + 2];
2680 g[constraint_index + 3] = x[state_index + 3];
2681 constraint_index += 4;
2682 state_index += 4;
2683
2684 // constraints on x,y,v
2685 for (int i = 1; i < horizon_; ++i) {
2686 g[constraint_index] = x[state_index];
2687 g[constraint_index + 1] = x[state_index + 1];
2688 g[constraint_index + 2] = x[state_index + 3];
2689 constraint_index += 3;
2690 state_index += 4;
2691 }
2692
2693 // end configuration
2694 g[constraint_index] = x[state_index];
2695 g[constraint_index + 1] = x[state_index + 1];
2696 g[constraint_index + 2] = x[state_index + 2];
2697 g[constraint_index + 3] = x[state_index + 3];
2698 constraint_index += 4;
2699 state_index += 4;
2700
2701 for (int i = 0; i < horizon_; ++i) {
2702 g[constraint_index] = x[control_index];
2703 g[constraint_index + 1] = x[control_index + 1];
2704 constraint_index += 2;
2705 control_index += 2;
2706 }
2707
2708 for (int i = 0; i < horizon_ + 1; ++i) {
2709 g[constraint_index] = x[time_index];
2710 constraint_index++;
2711 time_index++;
2712 }
2713
2714 for (int i = 0; i < lambda_horizon_; ++i) {
2715 g[constraint_index] = x[l_index];
2716 constraint_index++;
2717 l_index++;
2718 }
2719
2720 for (int i = 0; i < miu_horizon_; ++i) {
2721 g[constraint_index] = x[n_index];
2722 constraint_index++;
2723 n_index++;
2724 }
2725
2726 return true;
2727}
2728
2729bool DistanceApproachIPOPTCUDAInterface::check_g(int n, const double* x, int m,
2730 const double* g) {
2731 int kN = n;
2732 int kM = m;
2733 double x_u_tmp[kN];
2734 double x_l_tmp[kN];
2735 double g_u_tmp[kM];
2736 double g_l_tmp[kM];
2737
2738 get_bounds_info(n, x_l_tmp, x_u_tmp, m, g_l_tmp, g_u_tmp);
2739
2740 double delta_v = 1e-4;
2741 for (int idx = 0; idx < n; ++idx) {
2742 x_u_tmp[idx] = x_u_tmp[idx] + delta_v;
2743 x_l_tmp[idx] = x_l_tmp[idx] - delta_v;
2744 if (x[idx] > x_u_tmp[idx] || x[idx] < x_l_tmp[idx]) {
2745 ADEBUG << "x idx unfeasible: " << idx << ", x: " << x[idx]
2746 << ", lower: " << x_l_tmp[idx] << ", upper: " << x_u_tmp[idx];
2747 }
2748 }
2749
2750 // m1 : dynamics constatins
2751 int m1 = 4 * horizon_;
2752
2753 // m2 : control rate constraints (only steering)
2754 int m2 = m1 + horizon_;
2755
2756 // m3 : sampling time equality constraints
2757 int m3 = m2 + horizon_;
2758
2759 // m4 : obstacle constraints
2760 int m4 = m3 + 4 * obstacles_num_ * (horizon_ + 1);
2761
2762 // 5. load variable bounds as constraints
2763 // start configuration
2764 int m5 = m4 + 3 + 1;
2765
2766 // constraints on x,y,v
2767 int m6 = m5 + 3 * (horizon_ - 1);
2768
2769 // end configuration
2770 int m7 = m6 + 3 + 1;
2771
2772 // control variable bnd
2773 int m8 = m7 + 2 * horizon_;
2774
2775 // time interval variable bnd
2776 int m9 = m8 + (horizon_ + 1);
2777
2778 // lambda_horizon_
2779 int m10 = m9 + lambda_horizon_;
2780
2781 // miu_horizon_
2782 int m11 = m10 + miu_horizon_;
2783
2784 ADEBUG << "dynamics constatins to: " << m1;
2785 ADEBUG << "control rate constraints (only steering) to: " << m2;
2786 ADEBUG << "sampling time equality constraints to: " << m3;
2787 ADEBUG << "obstacle constraints to: " << m4;
2788 ADEBUG << "start conf constraints to: " << m5;
2789 ADEBUG << "constraints on x,y,v to: " << m6;
2790 ADEBUG << "end constraints to: " << m7;
2791 ADEBUG << "control bnd to: " << m8;
2792 ADEBUG << "time interval constraints to: " << m9;
2793 ADEBUG << "lambda constraints to: " << m10;
2794 ADEBUG << "miu constraints to: " << m11;
2795 ADEBUG << "total variables: " << num_of_variables_;
2796
2797 for (int idx = 0; idx < m; ++idx) {
2798 if (g[idx] > g_u_tmp[idx] + delta_v || g[idx] < g_l_tmp[idx] - delta_v) {
2799 ADEBUG << "constratins idx unfeasible: " << idx << ", g: " << g[idx]
2800 << ", lower: " << g_l_tmp[idx] << ", upper: " << g_u_tmp[idx];
2801 }
2802 }
2803 return true;
2804}
2805
2806void DistanceApproachIPOPTCUDAInterface::generate_tapes(int n, int m,
2807 int* nnz_h_lag) {
2808 std::vector<double> xp(n);
2809 std::vector<double> lamp(m);
2810 std::vector<double> zl(m);
2811 std::vector<double> zu(m);
2812
2813 std::vector<adouble> xa(n);
2814 std::vector<adouble> g(m);
2815 std::vector<double> lam(m);
2816 double sig = 0.0;
2817 adouble obj_value;
2818
2819 double dummy = 0.0;
2820
2821 obj_lam = new double[m + 1];
2822
2823 get_starting_point(n, 1, &xp[0], 0, &zl[0], &zu[0], m, 0, &lamp[0]);
2824
2825 trace_on(tag_f);
2826
2827 for (int idx = 0; idx < n; idx++) {
2828 xa[idx] <<= xp[idx];
2829 }
2830
2831 eval_obj(n, &xa[0], &obj_value);
2832
2833 obj_value >>= dummy;
2834
2835 trace_off();
2836
2837 trace_on(tag_g);
2838
2839 for (int idx = 0; idx < n; idx++) {
2840 xa[idx] <<= xp[idx];
2841 }
2842
2843 eval_constraints(n, &xa[0], m, &g[0]);
2844
2845 for (int idx = 0; idx < m; idx++) {
2846 g[idx] >>= dummy;
2847 }
2848
2849 trace_off();
2850
2851 trace_on(tag_L);
2852
2853 for (int idx = 0; idx < n; idx++) {
2854 xa[idx] <<= xp[idx];
2855 }
2856 for (int idx = 0; idx < m; idx++) {
2857 lam[idx] = 1.0;
2858 }
2859 sig = 1.0;
2860
2861 eval_obj(n, &xa[0], &obj_value);
2862
2863 obj_value *= mkparam(sig);
2864 eval_constraints(n, &xa[0], m, &g[0]);
2865
2866 for (int idx = 0; idx < m; idx++) {
2867 obj_value += g[idx] * mkparam(lam[idx]);
2868 }
2869
2870 obj_value >>= dummy;
2871
2872 trace_off();
2873
2874 rind_L = nullptr;
2875 cind_L = nullptr;
2876
2877 hessval = nullptr;
2878
2879 options_L[0] = 0;
2880 options_L[1] = 1;
2881
2882 sparse_hess(tag_L, n, 0, &xp[0], &nnz_L, &rind_L, &cind_L, &hessval,
2883 options_L);
2884 *nnz_h_lag = nnz_L;
2885}
2886//*************** end ADOL-C part ***********************************
2887
2888} // namespace planning
2889} // namespace apollo
apollo::planning::DistanceApproachIPOPTCUDAInterface::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 distance_approach_ipopt_cuda_interface.cc:393
apollo::planning::DistanceApproachIPOPTCUDAInterface::eval_jac_g
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 jaco...
Definition distance_approach_ipopt_cuda_interface.cc:559
apollo::planning::DistanceApproachIPOPTCUDAInterface::DistanceApproachIPOPTCUDAInterface
DistanceApproachIPOPTCUDAInterface(const size_t horizon, const double ts, const Eigen::MatrixXd &ego, const Eigen::MatrixXd &xWS, const Eigen::MatrixXd &uWS, const Eigen::MatrixXd &l_warm_up, const Eigen::MatrixXd &n_warm_up, const Eigen::MatrixXd &x0, const Eigen::MatrixXd &xf, const Eigen::MatrixXd &last_time_u, const std::vector< double > &XYbounds, const Eigen::MatrixXi &obstacles_edges_num, const size_t obstacles_num, const Eigen::MatrixXd &obstacles_A, const Eigen::MatrixXd &obstacles_b, const PlannerOpenSpaceConfig &planner_open_space_config)
Definition distance_approach_ipopt_cuda_interface.cc:32
apollo::planning::DistanceApproachIPOPTCUDAInterface::check_g
bool check_g(int n, const double *x, int m, const double *g)
Check unfeasible constraints for further study
Definition distance_approach_ipopt_cuda_interface.cc:2729
apollo::planning::DistanceApproachIPOPTCUDAInterface::get_optimization_results
void get_optimization_results(Eigen::MatrixXd *state_result, Eigen::MatrixXd *control_result, Eigen::MatrixXd *time_result, Eigen::MatrixXd *dual_l_result, Eigen::MatrixXd *dual_n_result) const
Definition distance_approach_ipopt_cuda_interface.cc:2370
apollo::planning::DistanceApproachIPOPTCUDAInterface::eval_jac_g_par
bool eval_jac_g_par(int n, const double *x, bool new_x, int m, int nele_jac, int *iRow, int *jCol, double *values)
Definition distance_approach_ipopt_cuda_interface.cc:566
apollo::planning::DistanceApproachIPOPTCUDAInterface::eval_jac_g_ser
bool eval_jac_g_ser(int n, const double *x, bool new_x, int m, int nele_jac, int *iRow, int *jCol, double *values)
Definition distance_approach_ipopt_cuda_interface.cc:1438
apollo::planning::DistanceApproachIPOPTCUDAInterface::get_nlp_info
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
Definition distance_approach_ipopt_cuda_interface.cc:115
apollo::planning::DistanceApproachIPOPTCUDAInterface::eval_grad_f_hand
bool eval_grad_f_hand(int n, const double *x, bool new_x, double *grad_f)
Definition distance_approach_ipopt_cuda_interface.cc:455
apollo::planning::DistanceApproachIPOPTCUDAInterface::eval_g
bool eval_g(int n, const double *x, bool new_x, int m, double *g) override
Method to return the constraint residuals
Definition distance_approach_ipopt_cuda_interface.cc:550
apollo::planning::DistanceApproachIPOPTCUDAInterface::finalize_solution
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
Definition distance_approach_ipopt_cuda_interface.cc:2308
apollo::planning::DistanceApproachIPOPTCUDAInterface::eval_grad_f
bool eval_grad_f(int n, const double *x, bool new_x, double *grad_f) override
Method to return the gradient of the objective
Definition distance_approach_ipopt_cuda_interface.cc:448
apollo::planning::DistanceApproachIPOPTCUDAInterface::eval_h
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 v...
Definition distance_approach_ipopt_cuda_interface.cc:2267
apollo::planning::DistanceApproachIPOPTCUDAInterface::eval_obj
bool eval_obj(int n, const T *x, T *obj_value)
Template to return the objective value
Definition distance_approach_ipopt_cuda_interface.cc:2437
apollo::planning::DistanceApproachIPOPTCUDAInterface::get_bounds_info
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
Definition distance_approach_ipopt_cuda_interface.cc:174
apollo::planning::DistanceApproachIPOPTCUDAInterface::eval_f
bool eval_f(int n, const double *x, bool new_x, double &obj_value) override
Method to return the objective value
Definition distance_approach_ipopt_cuda_interface.cc:442
apollo::planning::DistanceApproachIPOPTCUDAInterface::generate_tapes
void generate_tapes(int n, int m, int *nnz_h_lag)
Method to generate the required tapes by ADOL-C
Definition distance_approach_ipopt_cuda_interface.cc:2806
apollo::planning::DistanceApproachIPOPTCUDAInterface::eval_constraints
bool eval_constraints(int n, const T *x, int m, T *g)
Template to compute constraints
Definition distance_approach_ipopt_cuda_interface.cc:2512
planning
Planning module main class.
tag_f
#define tag_f
Definition cos_theta_ipopt_interface.h:36
tag_g
#define tag_g
Definition cos_theta_ipopt_interface.h:37
tag_L
#define tag_L
Definition cos_theta_ipopt_interface.h:38
distance_approach_ipopt_cuda_interface.h
ACHECK
#define ACHECK(cond)
Definition log.h:80
ADEBUG
#define ADEBUG
Definition log.h:41
AERROR
#define AERROR
Definition log.h:44
AFATAL
#define AFATAL
Definition log.h:45
apollo::planning::fill_lower_left
bool fill_lower_left(int *iRow, int *jCol, unsigned int *rind_L, unsigned int *cind_L, const int nnz_L)
apollo::planning::data_transfer
bool data_transfer(T *dst, const T *src, const int size)
apollo
class register implement
Definition arena_queue.h:37
planning_block.h