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

#include <distance_approach_ipopt_cuda_interface.h>

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

Public 成员函数

 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)
 
virtual ~DistanceApproachIPOPTCUDAInterface ()=default
 
bool get_nlp_info (int &n, int &m, int &nnz_jac_g, int &nnz_h_lag, IndexStyleEnum &index_style) override
 Method to return some info about the nlp
 
bool get_bounds_info (int n, double *x_l, double *x_u, int m, double *g_l, double *g_u) override
 Method to return the bounds for my problem
 
bool get_starting_point (int n, bool init_x, double *x, bool init_z, double *z_L, double *z_U, int m, bool init_lambda, double *lambda) override
 Method to return the starting point for the algorithm
 
bool eval_f (int n, const double *x, bool new_x, double &obj_value) override
 Method to return the objective value
 
bool eval_grad_f (int n, const double *x, bool new_x, double *grad_f) override
 Method to return the gradient of the objective
 
bool eval_grad_f_hand (int n, const double *x, bool new_x, double *grad_f)
 
bool eval_g (int n, const double *x, bool new_x, int m, double *g) override
 Method to return the constraint residuals
 
bool check_g (int n, const double *x, int m, const double *g)
 Check unfeasible constraints for further study
 
bool eval_jac_g (int n, const double *x, bool new_x, int m, int nele_jac, int *iRow, int *jCol, double *values) override
 Method to return: 1) The structure of the jacobian (if "values" is nullptr) 2) The values of the jacobian (if "values" is not nullptr)
 
bool eval_jac_g_ser (int n, const double *x, bool new_x, int m, int nele_jac, int *iRow, int *jCol, double *values)
 
bool eval_jac_g_par (int n, const double *x, bool new_x, int m, int nele_jac, int *iRow, int *jCol, double *values)
 
bool eval_h (int n, const double *x, bool new_x, double obj_factor, int m, const double *lambda, bool new_lambda, int nele_hess, int *iRow, int *jCol, double *values) override
 Method to return: 1) The structure of the hessian of the lagrangian (if "values" is nullptr) 2) The values of the hessian of the lagrangian (if "values" is not nullptr)
 
- Public 成员函数 继承自 apollo::planning::DistanceApproachInterface
virtual ~DistanceApproachInterface ()=default
 

Solution Methods

void finalize_solution (Ipopt::SolverReturn status, int n, const double *x, const double *z_L, const double *z_U, int m, const double *g, const double *lambda, double obj_value, const Ipopt::IpoptData *ip_data, Ipopt::IpoptCalculatedQuantities *ip_cq) override
 This method is called when the algorithm is complete so the TNLP can store/write the solution
 
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
 
template<class T >
bool eval_obj (int n, const T *x, T *obj_value)
 Template to return the objective value
 
template<class T >
bool eval_constraints (int n, const T *x, int m, T *g)
 Template to compute constraints
 
void generate_tapes (int n, int m, int *nnz_h_lag)
 Method to generate the required tapes by ADOL-C
 
 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 DistanceApproachConfig &distance_approach_config)
 

详细描述

在文件 distance_approach_ipopt_cuda_interface.h 第 56 行定义.

构造及析构函数说明

◆ DistanceApproachIPOPTCUDAInterface() [1/2]

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

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

48 :
49 ts_(ts),
50 ego_(ego),
51 xWS_(xWS),
52 uWS_(uWS),
53 l_warm_up_(l_warm_up),
54 n_warm_up_(n_warm_up),
55 x0_(x0),
56 xf_(xf),
57 last_time_u_(last_time_u),
58 XYbounds_(XYbounds),
59 obstacles_edges_num_(obstacles_edges_num),
60 obstacles_A_(obstacles_A),
61 obstacles_b_(obstacles_b) {
62 ACHECK(horizon < std::numeric_limits<int>::max()) << "Invalid cast on horizon in open space planner";
63 horizon_ = static_cast<int>(horizon);
64 ACHECK(obstacles_num < std::numeric_limits<int>::max()) << "Invalid cast on obstacles_num in open space planner";
65
66 obstacles_num_ = static_cast<int>(obstacles_num);
67 w_ev_ = ego_(1, 0) + ego_(3, 0);
68 l_ev_ = ego_(0, 0) + ego_(2, 0);
69 g_ = {l_ev_ / 2, w_ev_ / 2, l_ev_ / 2, w_ev_ / 2};
70 offset_ = (ego_(0, 0) + ego_(2, 0)) / 2 - ego_(2, 0);
71 obstacles_edges_sum_ = obstacles_edges_num_.sum();
72 state_result_ = Eigen::MatrixXd::Zero(4, horizon_ + 1);
73 dual_l_result_ = Eigen::MatrixXd::Zero(obstacles_edges_sum_, horizon_ + 1);
74 dual_n_result_ = Eigen::MatrixXd::Zero(4 * obstacles_num_, horizon_ + 1);
75 control_result_ = Eigen::MatrixXd::Zero(2, horizon_ + 1);
76 time_result_ = Eigen::MatrixXd::Zero(1, horizon_ + 1);
77 state_start_index_ = 0;
78 control_start_index_ = 4 * (horizon_ + 1);
79 time_start_index_ = control_start_index_ + 2 * horizon_;
80 l_start_index_ = time_start_index_ + (horizon_ + 1);
81 n_start_index_ = l_start_index_ + obstacles_edges_sum_ * (horizon_ + 1);
82
83 planner_open_space_config_ = planner_open_space_config;
84 distance_approach_config_ = planner_open_space_config_.distance_approach_config();
85 weight_state_x_ = distance_approach_config_.weight_x();
86 weight_state_y_ = distance_approach_config_.weight_y();
87 weight_state_phi_ = distance_approach_config_.weight_phi();
88 weight_state_v_ = distance_approach_config_.weight_v();
89 weight_input_steer_ = distance_approach_config_.weight_steer();
90 weight_input_a_ = distance_approach_config_.weight_a();
91 weight_rate_steer_ = distance_approach_config_.weight_steer_rate();
92 weight_rate_a_ = distance_approach_config_.weight_a_rate();
93 weight_stitching_steer_ = distance_approach_config_.weight_steer_stitching();
94 weight_stitching_a_ = distance_approach_config_.weight_a_stitching();
95 weight_first_order_time_ = distance_approach_config_.weight_first_order_time();
96 weight_second_order_time_ = distance_approach_config_.weight_second_order_time();
97 min_safety_distance_ = distance_approach_config_.min_safety_distance();
98 max_steer_angle_ = vehicle_param_.max_steer_angle() / vehicle_param_.steer_ratio();
99 max_speed_forward_ = distance_approach_config_.max_speed_forward();
100 max_speed_reverse_ = distance_approach_config_.max_speed_reverse();
101 max_acceleration_forward_ = distance_approach_config_.max_acceleration_forward();
102 max_acceleration_reverse_ = distance_approach_config_.max_acceleration_reverse();
103 min_time_sample_scaling_ = distance_approach_config_.min_time_sample_scaling();
104 max_time_sample_scaling_ = distance_approach_config_.max_time_sample_scaling();
105 max_steer_rate_ = vehicle_param_.max_steer_angle_rate() / vehicle_param_.steer_ratio();
106 use_fix_time_ = distance_approach_config_.use_fix_time();
107 wheelbase_ = vehicle_param_.wheel_base();
108 enable_constraint_check_ = distance_approach_config_.enable_constraint_check();
109}
ACHECK
#define ACHECK(cond)
Definition log.h:80

◆ ~DistanceApproachIPOPTCUDAInterface()

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

◆ DistanceApproachIPOPTCUDAInterface() [2/2]

apollo::planning::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 DistanceApproachConfig &  distance_approach_config 
)

在文件 distance_approach_ipopt_cuda_interface.cc 第 111 行定义.

127 :
128 ts_(ts),
129 ego_(ego),
130 xWS_(xWS),
131 uWS_(uWS),
132 l_warm_up_(l_warm_up),
133 n_warm_up_(n_warm_up),
134 x0_(x0),
135 xf_(xf),
136 last_time_u_(last_time_u),
137 XYbounds_(XYbounds),
138 obstacles_edges_num_(obstacles_edges_num),
139 obstacles_A_(obstacles_A),
140 obstacles_b_(obstacles_b),
141 distance_approach_config_(distance_approach_config) {
142 ACHECK(horizon < std::numeric_limits<int>::max()) << "Invalid cast on horizon in open space planner";
143 horizon_ = static_cast<int>(horizon);
144 ACHECK(obstacles_num < std::numeric_limits<int>::max()) << "Invalid cast on obstacles_num in open space planner";
145
146 obstacles_num_ = static_cast<int>(obstacles_num);
147 w_ev_ = ego_(1, 0) + ego_(3, 0);
148 l_ev_ = ego_(0, 0) + ego_(2, 0);
149 g_ = {l_ev_ / 2, w_ev_ / 2, l_ev_ / 2, w_ev_ / 2};
150 offset_ = (ego_(0, 0) + ego_(2, 0)) / 2 - ego_(2, 0);
151 obstacles_edges_sum_ = obstacles_edges_num_.sum();
152 state_result_ = Eigen::MatrixXd::Zero(4, horizon_ + 1);
153 dual_l_result_ = Eigen::MatrixXd::Zero(obstacles_edges_sum_, horizon_ + 1);
154 dual_n_result_ = Eigen::MatrixXd::Zero(4 * obstacles_num_, horizon_ + 1);
155 control_result_ = Eigen::MatrixXd::Zero(2, horizon_ + 1);
156 time_result_ = Eigen::MatrixXd::Zero(1, horizon_ + 1);
157 state_start_index_ = 0;
158 control_start_index_ = 4 * (horizon_ + 1);
159 time_start_index_ = control_start_index_ + 2 * horizon_;
160 l_start_index_ = time_start_index_ + (horizon_ + 1);
161 n_start_index_ = l_start_index_ + obstacles_edges_sum_ * (horizon_ + 1);
162
163 weight_state_x_ = distance_approach_config_.weight_x();
164 weight_state_y_ = distance_approach_config_.weight_y();
165 weight_state_phi_ = distance_approach_config_.weight_phi();
166 weight_state_v_ = distance_approach_config_.weight_v();
167 weight_input_steer_ = distance_approach_config_.weight_steer();
168 weight_input_a_ = distance_approach_config_.weight_a();
169 weight_rate_steer_ = distance_approach_config_.weight_steer_rate();
170 weight_rate_a_ = distance_approach_config_.weight_a_rate();
171 weight_stitching_steer_ = distance_approach_config_.weight_steer_stitching();
172 weight_stitching_a_ = distance_approach_config_.weight_a_stitching();
173 weight_first_order_time_ = distance_approach_config_.weight_first_order_time();
174 weight_second_order_time_ = distance_approach_config_.weight_second_order_time();
175 min_safety_distance_ = distance_approach_config_.min_safety_distance();
176 max_steer_angle_ = vehicle_param_.max_steer_angle() / vehicle_param_.steer_ratio();
177 max_speed_forward_ = distance_approach_config_.max_speed_forward();
178 max_speed_reverse_ = distance_approach_config_.max_speed_reverse();
179 max_acceleration_forward_ = distance_approach_config_.max_acceleration_forward();
180 max_acceleration_reverse_ = distance_approach_config_.max_acceleration_reverse();
181 min_time_sample_scaling_ = distance_approach_config_.min_time_sample_scaling();
182 max_time_sample_scaling_ = distance_approach_config_.max_time_sample_scaling();
183 max_steer_rate_ = vehicle_param_.max_steer_angle_rate() / vehicle_param_.steer_ratio();
184 use_fix_time_ = distance_approach_config_.use_fix_time();
185 wheelbase_ = vehicle_param_.wheel_base();
186 enable_constraint_check_ = distance_approach_config_.enable_constraint_check();
187}

成员函数说明

◆ check_g()

bool apollo::planning::DistanceApproachIPOPTCUDAInterface::check_g ( int  n,
const double *  x,
int  m,
const double *  g 
)
virtual

Check unfeasible constraints for further study

实现了 apollo::planning::DistanceApproachInterface.

在文件 distance_approach_ipopt_cuda_interface.cc 第 2702 行定义.

2702 {
2703 int kN = n;
2704 int kM = m;
2705 double x_u_tmp[kN];
2706 double x_l_tmp[kN];
2707 double g_u_tmp[kM];
2708 double g_l_tmp[kM];
2709
2710 get_bounds_info(n, x_l_tmp, x_u_tmp, m, g_l_tmp, g_u_tmp);
2711
2712 double delta_v = 1e-4;
2713 for (int idx = 0; idx < n; ++idx) {
2714 x_u_tmp[idx] = x_u_tmp[idx] + delta_v;
2715 x_l_tmp[idx] = x_l_tmp[idx] - delta_v;
2716 if (x[idx] > x_u_tmp[idx] || x[idx] < x_l_tmp[idx]) {
2717 ADEBUG << "x idx unfeasible: " << idx << ", x: " << x[idx] << ", lower: " << x_l_tmp[idx]
2718 << ", upper: " << x_u_tmp[idx];
2719 }
2720 }
2721
2722 // m1 : dynamics constatins
2723 int m1 = 4 * horizon_;
2724
2725 // m2 : control rate constraints (only steering)
2726 int m2 = m1 + horizon_;
2727
2728 // m3 : sampling time equality constraints
2729 int m3 = m2 + horizon_;
2730
2731 // m4 : obstacle constraints
2732 int m4 = m3 + 4 * obstacles_num_ * (horizon_ + 1);
2733
2734 // 5. load variable bounds as constraints
2735 // start configuration
2736 int m5 = m4 + 3 + 1;
2737
2738 // constraints on x,y,v
2739 int m6 = m5 + 3 * (horizon_ - 1);
2740
2741 // end configuration
2742 int m7 = m6 + 3 + 1;
2743
2744 // control variable bnd
2745 int m8 = m7 + 2 * horizon_;
2746
2747 // time interval variable bnd
2748 int m9 = m8 + (horizon_ + 1);
2749
2750 // lambda_horizon_
2751 int m10 = m9 + lambda_horizon_;
2752
2753 // miu_horizon_
2754 int m11 = m10 + miu_horizon_;
2755
2756 ADEBUG << "dynamics constatins to: " << m1;
2757 ADEBUG << "control rate constraints (only steering) to: " << m2;
2758 ADEBUG << "sampling time equality constraints to: " << m3;
2759 ADEBUG << "obstacle constraints to: " << m4;
2760 ADEBUG << "start conf constraints to: " << m5;
2761 ADEBUG << "constraints on x,y,v to: " << m6;
2762 ADEBUG << "end constraints to: " << m7;
2763 ADEBUG << "control bnd to: " << m8;
2764 ADEBUG << "time interval constraints to: " << m9;
2765 ADEBUG << "lambda constraints to: " << m10;
2766 ADEBUG << "miu constraints to: " << m11;
2767 ADEBUG << "total variables: " << num_of_variables_;
2768
2769 for (int idx = 0; idx < m; ++idx) {
2770 if (g[idx] > g_u_tmp[idx] + delta_v || g[idx] < g_l_tmp[idx] - delta_v) {
2771 ADEBUG << "constratins idx unfeasible: " << idx << ", g: " << g[idx] << ", lower: " << g_l_tmp[idx]
2772 << ", upper: " << g_u_tmp[idx];
2773 }
2774 }
2775 return true;
2776}
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:249
ADEBUG
#define ADEBUG
Definition log.h:41

◆ eval_constraints()

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

Template to compute constraints

在文件 distance_approach_ipopt_cuda_interface.cc 第 2504 行定义.

2504 {
2505 ADEBUG << "eval_constraints";
2506 // state start index
2507 int state_index = state_start_index_;
2508
2509 // control start index.
2510 int control_index = control_start_index_;
2511
2512 // time start index
2513 int time_index = time_start_index_;
2514
2515 int constraint_index = 0;
2516
2517 // // 1. state constraints 4 * [0, horizons-1]
2518 for (int i = 0; i < horizon_; ++i) {
2519 // x1
2520 // TODO(QiL) : optimize and remove redundant calculation in next
2521 // iteration.
2522 g[constraint_index] = x[state_index + 4]
2523 - (x[state_index]
2524 + ts_ * x[time_index] * (x[state_index + 3] + ts_ * x[time_index] * 0.5 * x[control_index + 1])
2525 * cos(x[state_index + 2]
2526 + ts_ * x[time_index] * 0.5 * x[state_index + 3] * tan(x[control_index])
2527 / wheelbase_));
2528 // x2
2529 g[constraint_index + 1] = x[state_index + 5]
2530 - (x[state_index + 1]
2531 + ts_ * x[time_index] * (x[state_index + 3] + ts_ * x[time_index] * 0.5 * x[control_index + 1])
2532 * sin(x[state_index + 2]
2533 + ts_ * x[time_index] * 0.5 * x[state_index + 3] * tan(x[control_index])
2534 / wheelbase_));
2535
2536 // x3
2537 g[constraint_index + 2] = x[state_index + 6]
2538 - (x[state_index + 2]
2539 + ts_ * x[time_index] * (x[state_index + 3] + ts_ * x[time_index] * 0.5 * x[control_index + 1])
2540 * tan(x[control_index]) / wheelbase_);
2541
2542 // x4
2543 g[constraint_index + 3]
2544 = x[state_index + 7] - (x[state_index + 3] + ts_ * x[time_index] * x[control_index + 1]);
2545
2546 control_index += 2;
2547 constraint_index += 4;
2548 time_index++;
2549 state_index += 4;
2550 }
2551
2552 ADEBUG << "constraint_index after adding Euler forward dynamics constraints "
2553 "updated: "
2554 << constraint_index;
2555
2556 // 2. Control rate limit constraints, 1 * [0, horizons-1], only apply
2557 // steering rate as of now
2558 control_index = control_start_index_;
2559 time_index = time_start_index_;
2560
2561 // First rate is compare first with stitch point
2562 g[constraint_index] = (x[control_index] - last_time_u_(0, 0)) / x[time_index] / ts_;
2563 control_index += 2;
2564 constraint_index++;
2565 time_index++;
2566
2567 for (int i = 1; i < horizon_; ++i) {
2568 g[constraint_index] = (x[control_index] - x[control_index - 2]) / x[time_index] / ts_;
2569 constraint_index++;
2570 control_index += 2;
2571 time_index++;
2572 }
2573
2574 // 3. Time constraints 1 * [0, horizons-1]
2575 time_index = time_start_index_;
2576 for (int i = 0; i < horizon_; ++i) {
2577 g[constraint_index] = x[time_index + 1] - x[time_index];
2578 constraint_index++;
2579 time_index++;
2580 }
2581
2582 ADEBUG << "constraint_index after adding time constraints "
2583 "updated: "
2584 << constraint_index;
2585
2586 // 4. Three obstacles related equal constraints, one equality constraints,
2587 // [0, horizon_] * [0, obstacles_num_-1] * 4
2588 state_index = state_start_index_;
2589 int l_index = l_start_index_;
2590 int n_index = n_start_index_;
2591
2592 for (int i = 0; i < horizon_ + 1; ++i) {
2593 int edges_counter = 0;
2594 for (int j = 0; j < obstacles_num_; ++j) {
2595 int current_edges_num = obstacles_edges_num_(j, 0);
2596 Eigen::MatrixXd Aj = obstacles_A_.block(edges_counter, 0, current_edges_num, 2);
2597 Eigen::MatrixXd bj = obstacles_b_.block(edges_counter, 0, current_edges_num, 1);
2598
2599 // norm(A* lambda) <= 1
2600 T tmp1 = 0.0;
2601 T tmp2 = 0.0;
2602 for (int k = 0; k < current_edges_num; ++k) {
2603 // TODO(QiL) : replace this one directly with x
2604 tmp1 += Aj(k, 0) * x[l_index + k];
2605 tmp2 += Aj(k, 1) * x[l_index + k];
2606 }
2607 g[constraint_index] = tmp1 * tmp1 + tmp2 * tmp2;
2608
2609 // G' * mu + R' * lambda == 0
2610 g[constraint_index + 1]
2611 = x[n_index] - x[n_index + 2] + cos(x[state_index + 2]) * tmp1 + sin(x[state_index + 2]) * tmp2;
2612
2613 g[constraint_index + 2]
2614 = x[n_index + 1] - x[n_index + 3] - sin(x[state_index + 2]) * tmp1 + cos(x[state_index + 2]) * tmp2;
2615
2616 // -g'*mu + (A*t - b)*lambda > 0
2617 T tmp3 = 0.0;
2618 for (int k = 0; k < 4; ++k) {
2619 tmp3 += -g_[k] * x[n_index + k];
2620 }
2621
2622 T tmp4 = 0.0;
2623 for (int k = 0; k < current_edges_num; ++k) {
2624 tmp4 += bj(k, 0) * x[l_index + k];
2625 }
2626
2627 g[constraint_index + 3] = tmp3 + (x[state_index] + cos(x[state_index + 2]) * offset_) * tmp1
2628 + (x[state_index + 1] + sin(x[state_index + 2]) * offset_) * tmp2 - tmp4;
2629
2630 // Update index
2631 edges_counter += current_edges_num;
2632 l_index += current_edges_num;
2633 n_index += 4;
2634 constraint_index += 4;
2635 }
2636 state_index += 4;
2637 }
2638 ADEBUG << "constraint_index after obstacles avoidance constraints "
2639 "updated: "
2640 << constraint_index;
2641
2642 // 5. load variable bounds as constraints
2643 state_index = state_start_index_;
2644 control_index = control_start_index_;
2645 time_index = time_start_index_;
2646 l_index = l_start_index_;
2647 n_index = n_start_index_;
2648
2649 // start configuration
2650 g[constraint_index] = x[state_index];
2651 g[constraint_index + 1] = x[state_index + 1];
2652 g[constraint_index + 2] = x[state_index + 2];
2653 g[constraint_index + 3] = x[state_index + 3];
2654 constraint_index += 4;
2655 state_index += 4;
2656
2657 // constraints on x,y,v
2658 for (int i = 1; i < horizon_; ++i) {
2659 g[constraint_index] = x[state_index];
2660 g[constraint_index + 1] = x[state_index + 1];
2661 g[constraint_index + 2] = x[state_index + 3];
2662 constraint_index += 3;
2663 state_index += 4;
2664 }
2665
2666 // end configuration
2667 g[constraint_index] = x[state_index];
2668 g[constraint_index + 1] = x[state_index + 1];
2669 g[constraint_index + 2] = x[state_index + 2];
2670 g[constraint_index + 3] = x[state_index + 3];
2671 constraint_index += 4;
2672 state_index += 4;
2673
2674 for (int i = 0; i < horizon_; ++i) {
2675 g[constraint_index] = x[control_index];
2676 g[constraint_index + 1] = x[control_index + 1];
2677 constraint_index += 2;
2678 control_index += 2;
2679 }
2680
2681 for (int i = 0; i < horizon_ + 1; ++i) {
2682 g[constraint_index] = x[time_index];
2683 constraint_index++;
2684 time_index++;
2685 }
2686
2687 for (int i = 0; i < lambda_horizon_; ++i) {
2688 g[constraint_index] = x[l_index];
2689 constraint_index++;
2690 l_index++;
2691 }
2692
2693 for (int i = 0; i < miu_horizon_; ++i) {
2694 g[constraint_index] = x[n_index];
2695 constraint_index++;
2696 n_index++;
2697 }
2698
2699 return true;
2700}
apollo::common::math::cos
float cos(Angle16 a)
Definition angle.cc:42
apollo::common::math::sin
float sin(Angle16 a)
Definition angle.cc:25
apollo::common::math::tan
float tan(Angle16 a)
Definition angle.cc:47

◆ eval_f()

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

Method to return the objective value

实现了 apollo::planning::DistanceApproachInterface.

在文件 distance_approach_ipopt_cuda_interface.cc 第 520 行定义.

520 {
521 eval_obj(n, x, &obj_value);
522 return true;
523}
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:2439

◆ eval_g()

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

Method to return the constraint residuals

实现了 apollo::planning::DistanceApproachInterface.

在文件 distance_approach_ipopt_cuda_interface.cc 第 605 行定义.

605 {
606 eval_constraints(n, x, m, g);
607 if (enable_constraint_check_) {
608 check_g(n, x, m, g);
609 }
610 return true;
611}
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:2702
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:2504

◆ eval_grad_f()

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

Method to return the gradient of the objective

实现了 apollo::planning::DistanceApproachInterface.

在文件 distance_approach_ipopt_cuda_interface.cc 第 525 行定义.

525 {
526 gradient(tag_f, n, x, grad_f);
527 return true;
528}
tag_f
#define tag_f
Definition cos_theta_ipopt_interface.h:36

◆ eval_grad_f_hand()

bool apollo::planning::DistanceApproachIPOPTCUDAInterface::eval_grad_f_hand ( int  n,
const double *  x,
bool  new_x,
double *  grad_f 
)

在文件 distance_approach_ipopt_cuda_interface.cc 第 530 行定义.

530 {
531 ADEBUG << "eval_grad_f by hand";
532 // Objective is from eval_f:
533 // min control inputs
534 // min input rate
535 // min time (if the time step is not fixed)
536 // regularization wrt warm start trajectory
537 DCHECK(ts_ != 0) << "ts in distance_approach_ is 0";
538 int control_index = control_start_index_;
539 int time_index = time_start_index_;
540 int state_index = state_start_index_;
541
542 if (grad_f == nullptr) {
543 AERROR << "grad_f pt is nullptr";
544 return false;
545 } else {
546 std::fill(grad_f, grad_f + n, 0.0);
547 // 1. objective to minimize state diff to warm up
548 for (int i = 0; i < horizon_ + 1; ++i) {
549 grad_f[state_index] += 2 * weight_state_x_ * (x[state_index] - xWS_(0, i));
550 grad_f[state_index + 1] += 2 * weight_state_y_ * (x[state_index + 1] - xWS_(1, i));
551 grad_f[state_index + 2] += 2 * weight_state_phi_ * (x[state_index + 2] - xWS_(2, i));
552 grad_f[state_index + 3] = 2 * weight_state_v_ * x[state_index + 3];
553 state_index += 4;
554 }
555
556 // 2. objective to minimize u square
557 for (int i = 0; i < horizon_; ++i) {
558 grad_f[control_index] += 2 * weight_input_steer_ * x[control_index];
559 grad_f[control_index + 1] += 2 * weight_input_a_ * x[control_index + 1];
560 control_index += 2;
561 }
562
563 // 3. objective to minimize input change rate for first horizon
564 // assume: x[time_index] > 0
565 control_index = control_start_index_;
566 double last_time_steer_rate = (x[control_index] - last_time_u_(0, 0)) / x[time_index] / ts_;
567 double last_time_a_rate = (x[control_index + 1] - last_time_u_(1, 0)) / x[time_index] / ts_;
568
569 grad_f[control_index] += 2.0 * last_time_steer_rate * (weight_stitching_steer_ / x[time_index] / ts_);
570 grad_f[control_index + 1] += 2.0 * last_time_a_rate * (weight_stitching_a_ / x[time_index] / ts_);
571 grad_f[time_index] += -2.0
572 * (weight_stitching_steer_ * last_time_steer_rate * last_time_steer_rate
573 + weight_stitching_a_ * last_time_a_rate * last_time_a_rate)
574 / x[time_index];
575
576 // 4. objective to minimize input change rates, [0- horizon_ -2]
577 // assume: x[time_index] > 0
578 time_index++;
579 for (int i = 0; i < horizon_ - 1; ++i) {
580 double steering_rate = (x[control_index + 2] - x[control_index]) / x[time_index] / ts_;
581 grad_f[control_index + 2] += 2.0 * steering_rate * (weight_rate_steer_ / x[time_index] / ts_);
582 grad_f[control_index] += -2.0 * steering_rate * (weight_rate_steer_ / x[time_index] / ts_);
583 grad_f[time_index] += -2.0 * weight_rate_steer_ * steering_rate * steering_rate / x[time_index];
584
585 double a_rate = (x[control_index + 3] - x[control_index + 1]) / x[time_index] / ts_;
586 grad_f[control_index + 3] += 2.0 * a_rate * (weight_rate_a_ / x[time_index] / ts_);
587 grad_f[control_index + 1] += -2.0 * a_rate * (weight_rate_a_ / x[time_index] / ts_);
588 grad_f[time_index] += -2.0 * weight_rate_a_ * a_rate * a_rate / x[time_index];
589
590 control_index += 2;
591 time_index++;
592 }
593
594 // 5. objective to minimize total time [0, horizon_]
595 time_index = time_start_index_;
596 for (int i = 0; i < horizon_ + 1; ++i) {
597 grad_f[time_index] += weight_first_order_time_ + 2.0 * weight_second_order_time_ * x[time_index];
598 time_index++;
599 }
600 }
601
602 return true;
603}
AERROR
#define AERROR
Definition log.h:44

◆ eval_h()

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

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

实现了 apollo::planning::DistanceApproachInterface.

在文件 distance_approach_ipopt_cuda_interface.cc 第 2260 行定义.

2271 {
2272 if (values == nullptr) {
2273 // return the structure. This is a symmetric matrix, fill the lower left
2274 // triangle only.
2275#if USE_GPU == 1
2276 fill_lower_left(iRow, jCol, rind_L, cind_L, nnz_L);
2277#else
2278 AFATAL << "CUDA enabled without GPU!";
2279#endif
2280 } else {
2281 // return the values. This is a symmetric matrix, fill the lower left
2282 // triangle only
2283
2284 obj_lam[0] = obj_factor;
2285#if USE_GPU == 1
2286 data_transfer(&obj_lam[1], lambda, m);
2287#else
2288 AFATAL << "CUDA enabled without GPU!";
2289#endif
2290
2291 set_param_vec(tag_L, m + 1, obj_lam);
2292 sparse_hess(tag_L, n, 1, const_cast<double*>(x), &nnz_L, &rind_L, &cind_L, &hessval, options_L);
2293
2294#if USE_GPU == 1
2295 if (!data_transfer(values, hessval, nnz_L)) {
2296 for (int idx = 0; idx < nnz_L; idx++) {
2297 values[idx] = hessval[idx];
2298 }
2299 }
2300#endif
2301 }
2302
2303 return true;
2304}
tag_L
#define tag_L
Definition cos_theta_ipopt_interface.h:38
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)

◆ eval_jac_g()

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

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

实现了 apollo::planning::DistanceApproachInterface.

在文件 distance_approach_ipopt_cuda_interface.cc 第 613 行定义.

621 {
622 return eval_jac_g_ser(n, x, new_x, m, nele_jac, iRow, jCol, values);
623}
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:1463

◆ eval_jac_g_par()

bool apollo::planning::DistanceApproachIPOPTCUDAInterface::eval_jac_g_par ( int  n,
const double *  x,
bool  new_x,
int  m,
int  nele_jac,
int *  iRow,
int *  jCol,
double *  values 
)

在文件 distance_approach_ipopt_cuda_interface.cc 第 625 行定义.

633 {
634 ADEBUG << "eval_jac_g";
635 CHECK_EQ(n, num_of_variables_) << "No. of variables wrong in eval_jac_g. n : " << n;
636 CHECK_EQ(m, num_of_constraints_) << "No. of constraints wrong in eval_jac_g. n : " << m;
637
638 if (values == nullptr) {
639 int nz_index = 0;
640 int constraint_index = 0;
641 int state_index = state_start_index_;
642 int control_index = control_start_index_;
643 int time_index = time_start_index_;
644
645 // 1. State Constraint with respect to variables
646 for (int i = 0; i < horizon_; ++i) {
647 // g(0)' with respect to x0 ~ x7
648 iRow[nz_index] = state_index;
649 jCol[nz_index] = state_index;
650 ++nz_index;
651
652 iRow[nz_index] = state_index;
653 jCol[nz_index] = state_index + 2;
654 ++nz_index;
655
656 iRow[nz_index] = state_index;
657 jCol[nz_index] = state_index + 3;
658 ++nz_index;
659
660 iRow[nz_index] = state_index;
661 jCol[nz_index] = state_index + 4;
662 ++nz_index;
663
664 // g(0)' with respect to u0 ~ u1'
665 iRow[nz_index] = state_index;
666 jCol[nz_index] = control_index;
667 ++nz_index;
668
669 iRow[nz_index] = state_index;
670 jCol[nz_index] = control_index + 1;
671 ++nz_index;
672
673 // g(0)' with respect to time
674 iRow[nz_index] = state_index;
675 jCol[nz_index] = time_index;
676 ++nz_index;
677
678 // g(1)' with respect to x0 ~ x7
679 iRow[nz_index] = state_index + 1;
680 jCol[nz_index] = state_index + 1;
681 ++nz_index;
682
683 iRow[nz_index] = state_index + 1;
684 jCol[nz_index] = state_index + 2;
685 ++nz_index;
686
687 iRow[nz_index] = state_index + 1;
688 jCol[nz_index] = state_index + 3;
689 ++nz_index;
690
691 iRow[nz_index] = state_index + 1;
692 jCol[nz_index] = state_index + 5;
693 ++nz_index;
694
695 // g(1)' with respect to u0 ~ u1'
696 iRow[nz_index] = state_index + 1;
697 jCol[nz_index] = control_index;
698 ++nz_index;
699
700 iRow[nz_index] = state_index + 1;
701 jCol[nz_index] = control_index + 1;
702 ++nz_index;
703
704 // g(1)' with respect to time
705 iRow[nz_index] = state_index + 1;
706 jCol[nz_index] = time_index;
707 ++nz_index;
708
709 // g(2)' with respect to x0 ~ x7
710 iRow[nz_index] = state_index + 2;
711 jCol[nz_index] = state_index + 2;
712 ++nz_index;
713
714 iRow[nz_index] = state_index + 2;
715 jCol[nz_index] = state_index + 3;
716 ++nz_index;
717
718 iRow[nz_index] = state_index + 2;
719 jCol[nz_index] = state_index + 6;
720 ++nz_index;
721
722 // g(2)' with respect to u0 ~ u1'
723 iRow[nz_index] = state_index + 2;
724 jCol[nz_index] = control_index;
725 ++nz_index;
726
727 iRow[nz_index] = state_index + 2;
728 jCol[nz_index] = control_index + 1;
729 ++nz_index;
730
731 // g(2)' with respect to time
732 iRow[nz_index] = state_index + 2;
733 jCol[nz_index] = time_index;
734 ++nz_index;
735
736 // g(3)' with x0 ~ x7
737 iRow[nz_index] = state_index + 3;
738 jCol[nz_index] = state_index + 3;
739 ++nz_index;
740
741 iRow[nz_index] = state_index + 3;
742 jCol[nz_index] = state_index + 7;
743 ++nz_index;
744
745 // g(3)' with respect to u0 ~ u1'
746 iRow[nz_index] = state_index + 3;
747 jCol[nz_index] = control_index + 1;
748 ++nz_index;
749
750 // g(3)' with respect to time
751 iRow[nz_index] = state_index + 3;
752 jCol[nz_index] = time_index;
753 ++nz_index;
754
755 state_index += 4;
756 control_index += 2;
757 time_index++;
758 constraint_index += 4;
759 }
760
761 // 2. only have control rate constraints on u0 , range [0, horizon_-1]
762 control_index = control_start_index_;
763 state_index = state_start_index_;
764 time_index = time_start_index_;
765
766 // First one, with respect to u(0, 0)
767 iRow[nz_index] = constraint_index;
768 jCol[nz_index] = control_index;
769 ++nz_index;
770
771 // First element, with respect to time
772 iRow[nz_index] = constraint_index;
773 jCol[nz_index] = time_index;
774 ++nz_index;
775
776 control_index += 2;
777 time_index++;
778 constraint_index++;
779
780 for (int i = 1; i < horizon_; ++i) {
781 // with respect to u(0, i-1)
782 iRow[nz_index] = constraint_index;
783 jCol[nz_index] = control_index - 2;
784 ++nz_index;
785
786 // with respect to u(0, i)
787 iRow[nz_index] = constraint_index;
788 jCol[nz_index] = control_index;
789 ++nz_index;
790
791 // with respect to time
792 iRow[nz_index] = constraint_index;
793 jCol[nz_index] = time_index;
794 ++nz_index;
795
796 // only consider rate limits on u0
797 control_index += 2;
798 constraint_index++;
799 time_index++;
800 }
801
802 // 3. Time constraints [0, horizon_ -1]
803 time_index = time_start_index_;
804
805 for (int i = 0; i < horizon_; ++i) {
806 // with respect to timescale(0, i-1)
807 iRow[nz_index] = constraint_index;
808 jCol[nz_index] = time_index;
809 ++nz_index;
810
811 // with respect to timescale(0, i)
812 iRow[nz_index] = constraint_index;
813 jCol[nz_index] = time_index + 1;
814 ++nz_index;
815
816 time_index++;
817 constraint_index++;
818 }
819
820 // 4. Three obstacles related equal constraints, one equality constraints,
821 // [0, horizon_] * [0, obstacles_num_-1] * 4
822 state_index = state_start_index_;
823 int l_index = l_start_index_;
824 int n_index = n_start_index_;
825
826#pragma omp parallel for schedule(dynamic, 1) num_threads(4)
827 for (int iter = 0; iter < (horizon_ + 1) * obstacles_num_; iter++) {
828 int i = iter / obstacles_num_;
829 int j = iter % obstacles_num_;
830 int current_edges_num = obstacles_edges_num_(j, 0);
831 int nz_index_tmp = nz_index;
832 int l_index_tmp = l_index;
833 // count nz_len
834 for (int jj = 0; jj < obstacles_num_; ++jj) {
835 if (jj < j) {
836 nz_index_tmp += 4 * (i + 1) * obstacles_edges_num_(jj, 0);
837 nz_index_tmp += 13 * (i + 1);
838 l_index_tmp += (i + 1) * obstacles_edges_num_(jj, 0);
839 } else {
840 nz_index_tmp += 4 * i * obstacles_edges_num_(jj, 0);
841 nz_index_tmp += 13 * i;
842 l_index_tmp += i * obstacles_edges_num_(jj, 0);
843 }
844 }
845 int n_index_tmp = n_index + (i * obstacles_num_ + j) * 4;
846 int constraint_index_tmp = constraint_index + (i * obstacles_num_ + j) * 4;
847 int state_index_tmp = state_index + i * 4;
848
849 // 1. norm(A* lambda == 1)
850 for (int k = 0; k < current_edges_num; ++k) {
851 // with respect to l
852 iRow[nz_index_tmp] = constraint_index_tmp;
853 jCol[nz_index_tmp] = l_index_tmp + k;
854 ++nz_index_tmp; // current_edges_num
855 }
856
857 // 2. G' * mu + R' * lambda == 0, part 1
858 // With respect to x
859 iRow[nz_index_tmp] = constraint_index_tmp + 1;
860 jCol[nz_index_tmp] = state_index_tmp + 2;
861 ++nz_index_tmp; // 1
862
863 // with respect to l
864 for (int k = 0; k < current_edges_num; ++k) {
865 iRow[nz_index_tmp] = constraint_index_tmp + 1;
866 jCol[nz_index_tmp] = l_index_tmp + k;
867 ++nz_index_tmp; // current_edges_num
868 }
869
870 // With respect to n
871 iRow[nz_index_tmp] = constraint_index_tmp + 1;
872 jCol[nz_index_tmp] = n_index_tmp;
873 ++nz_index_tmp; // 1
874
875 iRow[nz_index_tmp] = constraint_index_tmp + 1;
876 jCol[nz_index_tmp] = n_index_tmp + 2;
877 ++nz_index_tmp; // 1
878
879 // 2. G' * mu + R' * lambda == 0, part 2
880 // With respect to x
881 iRow[nz_index_tmp] = constraint_index_tmp + 2;
882 jCol[nz_index_tmp] = state_index_tmp + 2;
883 ++nz_index_tmp; // 1
884
885 // with respect to l
886 for (int k = 0; k < current_edges_num; ++k) {
887 iRow[nz_index_tmp] = constraint_index_tmp + 2;
888 jCol[nz_index_tmp] = l_index_tmp + k;
889 ++nz_index_tmp; // current_edges_num
890 }
891
892 // With respect to n
893 iRow[nz_index_tmp] = constraint_index_tmp + 2;
894 jCol[nz_index_tmp] = n_index_tmp + 1;
895 ++nz_index_tmp; // 1
896
897 iRow[nz_index_tmp] = constraint_index_tmp + 2;
898 jCol[nz_index_tmp] = n_index_tmp + 3;
899 ++nz_index_tmp; // 1
900
901 // 3. -g'*mu + (A*t - b)*lambda > 0
902 // With respect to x
903 iRow[nz_index_tmp] = constraint_index_tmp + 3;
904 jCol[nz_index_tmp] = state_index_tmp;
905 ++nz_index_tmp; // 1
906
907 iRow[nz_index_tmp] = constraint_index_tmp + 3;
908 jCol[nz_index_tmp] = state_index_tmp + 1;
909 ++nz_index_tmp; // 1
910
911 iRow[nz_index_tmp] = constraint_index_tmp + 3;
912 jCol[nz_index_tmp] = state_index_tmp + 2;
913 ++nz_index_tmp; // 1
914
915 // with respect to l
916 for (int k = 0; k < current_edges_num; ++k) {
917 iRow[nz_index_tmp] = constraint_index_tmp + 3;
918 jCol[nz_index_tmp] = l_index_tmp + k;
919 ++nz_index_tmp; // current_edges_num
920 }
921
922 // with respect to n
923 for (int k = 0; k < 4; ++k) {
924 iRow[nz_index_tmp] = constraint_index_tmp + 3;
925 jCol[nz_index_tmp] = n_index_tmp + k;
926 ++nz_index_tmp; // 4
927 }
928 }
929 // update index
930 for (int jj = 0; jj < obstacles_num_; ++jj) {
931 nz_index += 4 * (horizon_ + 1) * obstacles_edges_num_(jj, 0);
932 nz_index += 13 * (horizon_ + 1);
933 }
934 constraint_index += 4 * (horizon_ + 1) * obstacles_num_;
935 state_index += 4 * (horizon_ + 1);
936
937 // 5. load variable bounds as constraints
938 state_index = state_start_index_;
939 control_index = control_start_index_;
940 time_index = time_start_index_;
941 l_index = l_start_index_;
942 n_index = n_start_index_;
943
944 // start configuration
945 iRow[nz_index] = constraint_index;
946 jCol[nz_index] = state_index;
947 nz_index++;
948 iRow[nz_index] = constraint_index + 1;
949 jCol[nz_index] = state_index + 1;
950 nz_index++;
951 iRow[nz_index] = constraint_index + 2;
952 jCol[nz_index] = state_index + 2;
953 nz_index++;
954 iRow[nz_index] = constraint_index + 3;
955 jCol[nz_index] = state_index + 3;
956 nz_index++;
957 constraint_index += 4;
958 state_index += 4;
959
960 for (int i = 1; i < horizon_; ++i) {
961 iRow[nz_index] = constraint_index;
962 jCol[nz_index] = state_index;
963 nz_index++;
964 iRow[nz_index] = constraint_index + 1;
965 jCol[nz_index] = state_index + 1;
966 nz_index++;
967 iRow[nz_index] = constraint_index + 2;
968 jCol[nz_index] = state_index + 3;
969 nz_index++;
970 constraint_index += 3;
971 state_index += 4;
972 }
973
974 // end configuration
975 iRow[nz_index] = constraint_index;
976 jCol[nz_index] = state_index;
977 nz_index++;
978 iRow[nz_index] = constraint_index + 1;
979 jCol[nz_index] = state_index + 1;
980 nz_index++;
981 iRow[nz_index] = constraint_index + 2;
982 jCol[nz_index] = state_index + 2;
983 nz_index++;
984 iRow[nz_index] = constraint_index + 3;
985 jCol[nz_index] = state_index + 3;
986 nz_index++;
987 constraint_index += 4;
988 state_index += 4;
989
990 for (int i = 0; i < horizon_; ++i) {
991 iRow[nz_index] = constraint_index;
992 jCol[nz_index] = control_index;
993 nz_index++;
994 iRow[nz_index] = constraint_index + 1;
995 jCol[nz_index] = control_index + 1;
996 nz_index++;
997 constraint_index += 2;
998 control_index += 2;
999 }
1000
1001 for (int i = 0; i < horizon_ + 1; ++i) {
1002 iRow[nz_index] = constraint_index;
1003 jCol[nz_index] = time_index;
1004 nz_index++;
1005 constraint_index++;
1006 time_index++;
1007 }
1008
1009 for (int i = 0; i < lambda_horizon_; ++i) {
1010 iRow[nz_index] = constraint_index;
1011 jCol[nz_index] = l_index;
1012 nz_index++;
1013 constraint_index++;
1014 l_index++;
1015 }
1016
1017 for (int i = 0; i < miu_horizon_; ++i) {
1018 iRow[nz_index] = constraint_index;
1019 jCol[nz_index] = n_index;
1020 nz_index++;
1021 constraint_index++;
1022 n_index++;
1023 }
1024
1025 CHECK_EQ(nz_index, static_cast<int>(nele_jac));
1026 CHECK_EQ(constraint_index, static_cast<int>(m));
1027 } else {
1028 std::fill(values, values + nele_jac, 0.0);
1029 int nz_index = 0;
1030
1031 int time_index = time_start_index_;
1032 int state_index = state_start_index_;
1033 int control_index = control_start_index_;
1034 // TODO(QiL) : initially implemented to be debug friendly, later iterate
1035 // towards better efficiency
1036 // 1. state constraints 4 * [0, horizons-1]
1037 for (int i = 0; i < horizon_; ++i) {
1038 values[nz_index] = -1.0;
1039 ++nz_index;
1040
1041 values[nz_index] = x[time_index] * ts_
1042 * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
1043 * std::sin(x[state_index + 2]
1044 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1045 / wheelbase_); // a.
1046 ++nz_index;
1047
1048 values[nz_index] = -1.0
1049 * (x[time_index] * ts_
1050 * std::cos(
1051 x[state_index + 2]
1052 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1053 / wheelbase_)
1054 + x[time_index] * ts_ * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
1055 * (-1) * x[time_index] * ts_ * 0.5 * std::tan(x[control_index]) / wheelbase_
1056 * std::sin(
1057 x[state_index + 2]
1058 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1059 / wheelbase_)); // b
1060 ++nz_index;
1061
1062 values[nz_index] = 1.0;
1063 ++nz_index;
1064
1065 values[nz_index] = x[time_index] * ts_
1066 * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
1067 * std::sin(x[state_index + 2]
1068 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1069 / wheelbase_)
1070 * x[time_index] * ts_ * 0.5 * x[state_index + 3]
1071 / (std::cos(x[control_index]) * std::cos(x[control_index])) / wheelbase_; // c
1072 ++nz_index;
1073
1074 values[nz_index] = -1.0
1075 * (x[time_index] * ts_ * x[time_index] * ts_ * 0.5
1076 * std::cos(
1077 x[state_index + 2]
1078 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1079 / wheelbase_)); // d
1080 ++nz_index;
1081
1082 values[nz_index] = -1.0
1083 * (ts_ * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
1084 * std::cos(
1085 x[state_index + 2]
1086 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1087 / wheelbase_)
1088 + x[time_index] * ts_ * ts_ * 0.5 * x[control_index + 1]
1089 * std::cos(
1090 x[state_index + 2]
1091 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1092 / wheelbase_)
1093 - x[time_index] * ts_ * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
1094 * x[state_index + 3] * ts_ * 0.5 * std::tan(x[control_index]) / wheelbase_
1095 * std::sin(
1096 x[state_index + 2]
1097 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1098 / wheelbase_)); // e
1099 ++nz_index;
1100
1101 values[nz_index] = -1.0;
1102 ++nz_index;
1103
1104 values[nz_index] = -1.0
1105 * (x[time_index] * ts_ * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
1106 * std::cos(
1107 x[state_index + 2]
1108 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1109 / wheelbase_)); // f.
1110 ++nz_index;
1111
1112 values[nz_index] = -1.0
1113 * (x[time_index] * ts_
1114 * std::sin(
1115 x[state_index + 2]
1116 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1117 / wheelbase_)
1118 + x[time_index] * ts_ * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
1119 * x[time_index] * ts_ * 0.5 * std::tan(x[control_index]) / wheelbase_
1120 * std::cos(
1121 x[state_index + 2]
1122 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1123 / wheelbase_)); // g
1124 ++nz_index;
1125
1126 values[nz_index] = 1.0;
1127 ++nz_index;
1128
1129 values[nz_index] = -1.0
1130 * (x[time_index] * ts_ * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
1131 * std::cos(
1132 x[state_index + 2]
1133 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1134 / wheelbase_)
1135 * x[time_index] * ts_ * 0.5 * x[state_index + 3]
1136 / (std::cos(x[control_index]) * std::cos(x[control_index])) / wheelbase_); // h
1137 ++nz_index;
1138
1139 values[nz_index] = -1.0
1140 * (x[time_index] * ts_ * x[time_index] * ts_ * 0.5
1141 * std::sin(
1142 x[state_index + 2]
1143 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1144 / wheelbase_)); // i
1145 ++nz_index;
1146
1147 values[nz_index] = -1.0
1148 * (ts_ * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
1149 * std::sin(
1150 x[state_index + 2]
1151 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1152 / wheelbase_)
1153 + x[time_index] * ts_ * ts_ * 0.5 * x[control_index + 1]
1154 * std::sin(
1155 x[state_index + 2]
1156 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1157 / wheelbase_)
1158 + x[time_index] * ts_ * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
1159 * x[state_index + 3] * ts_ * 0.5 * std::tan(x[control_index]) / wheelbase_
1160 * std::cos(
1161 x[state_index + 2]
1162 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1163 / wheelbase_)); // j
1164 ++nz_index;
1165
1166 values[nz_index] = -1.0;
1167 ++nz_index;
1168
1169 values[nz_index] = -1.0 * x[time_index] * ts_ * std::tan(x[control_index]) / wheelbase_; // k.
1170 ++nz_index;
1171
1172 values[nz_index] = 1.0;
1173 ++nz_index;
1174
1175 values[nz_index] = -1.0
1176 * (x[time_index] * ts_ * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
1177 / (std::cos(x[control_index]) * std::cos(x[control_index])) / wheelbase_); // l.
1178 ++nz_index;
1179
1180 values[nz_index] = -1.0
1181 * (x[time_index] * ts_ * x[time_index] * ts_ * 0.5 * std::tan(x[control_index])
1182 / wheelbase_); // m.
1183 ++nz_index;
1184
1185 values[nz_index] = -1.0
1186 * (ts_ * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
1187 * std::tan(x[control_index]) / wheelbase_
1188 + x[time_index] * ts_ * ts_ * 0.5 * x[control_index + 1] * std::tan(x[control_index])
1189 / wheelbase_); // n.
1190 ++nz_index;
1191
1192 values[nz_index] = -1.0;
1193 ++nz_index;
1194
1195 values[nz_index] = 1.0;
1196 ++nz_index;
1197
1198 values[nz_index] = -1.0 * ts_ * x[time_index]; // o.
1199 ++nz_index;
1200
1201 values[nz_index] = -1.0 * ts_ * x[control_index + 1]; // p.
1202 ++nz_index;
1203
1204 state_index += 4;
1205 control_index += 2;
1206 time_index++;
1207 }
1208
1209 // 2. control rate constraints 1 * [0, horizons-1]
1210 control_index = control_start_index_;
1211 state_index = state_start_index_;
1212 time_index = time_start_index_;
1213
1214 // First horizon
1215
1216 // with respect to u(0, 0)
1217 values[nz_index] = 1.0 / x[time_index] / ts_; // q
1218 ++nz_index;
1219
1220 // with respect to time
1221 values[nz_index] = -1.0 * (x[control_index] - last_time_u_(0, 0)) / x[time_index] / x[time_index] / ts_;
1222 ++nz_index;
1223 time_index++;
1224 control_index += 2;
1225
1226 for (int i = 1; i < horizon_; ++i) {
1227 // with respect to u(0, i-1)
1228
1229 values[nz_index] = -1.0 / x[time_index] / ts_;
1230 ++nz_index;
1231
1232 // with respect to u(0, i)
1233 values[nz_index] = 1.0 / x[time_index] / ts_;
1234 ++nz_index;
1235
1236 // with respect to time
1237 values[nz_index] = -1.0 * (x[control_index] - x[control_index - 2]) / x[time_index] / x[time_index] / ts_;
1238 ++nz_index;
1239
1240 control_index += 2;
1241 time_index++;
1242 }
1243
1244 ADEBUG << "After fulfilled control rate constraints derivative, nz_index : " << nz_index
1245 << " nele_jac : " << nele_jac;
1246
1247 // 3. Time constraints [0, horizon_ -1]
1248 time_index = time_start_index_;
1249 for (int i = 0; i < horizon_; ++i) {
1250 // with respect to timescale(0, i-1)
1251 values[nz_index] = -1.0;
1252 ++nz_index;
1253
1254 // with respect to timescale(0, i)
1255 values[nz_index] = 1.0;
1256 ++nz_index;
1257
1258 time_index++;
1259 }
1260
1261 ADEBUG << "After fulfilled time constraints derivative, nz_index : " << nz_index << " nele_jac : " << nele_jac;
1262
1263 // 4. Three obstacles related equal constraints, one equality constraints,
1264 // [0, horizon_] * [0, obstacles_num_-1] * 4
1265
1266 state_index = state_start_index_;
1267 int l_index = l_start_index_;
1268 int n_index = n_start_index_;
1269
1270#pragma omp parallel for schedule(dynamic, 1) num_threads(4)
1271 for (int iter = 0; iter < (horizon_ + 1) * obstacles_num_; iter++) {
1272 int i = iter / obstacles_num_;
1273 int j = iter % obstacles_num_;
1274
1275 int current_edges_num = obstacles_edges_num_(j, 0);
1276 int edges_counter = 0;
1277 int nz_index_tmp = nz_index;
1278 int l_index_tmp = l_index;
1279
1280 // count nz_len
1281 for (int jj = 0; jj < obstacles_num_; ++jj) {
1282 if (jj < j) {
1283 nz_index_tmp += 4 * (i + 1) * obstacles_edges_num_(jj, 0);
1284 nz_index_tmp += 13 * (i + 1);
1285 l_index_tmp += (i + 1) * obstacles_edges_num_(jj, 0);
1286 edges_counter += obstacles_edges_num_(jj, 0);
1287 } else {
1288 nz_index_tmp += 4 * i * obstacles_edges_num_(jj, 0);
1289 nz_index_tmp += 13 * i;
1290 l_index_tmp += i * obstacles_edges_num_(jj, 0);
1291 }
1292 }
1293 int n_index_tmp = n_index + (i * obstacles_num_ + j) * 4;
1294 int state_index_tmp = state_index + i * 4;
1295
1296 Eigen::MatrixXd Aj = obstacles_A_.block(edges_counter, 0, current_edges_num, 2);
1297 Eigen::MatrixXd bj = obstacles_b_.block(edges_counter, 0, current_edges_num, 1);
1298
1299 // TODO(QiL) : Remove redundant calculation
1300 double tmp1 = 0;
1301 double tmp2 = 0;
1302 for (int k = 0; k < current_edges_num; ++k) {
1303 // TODO(QiL) : replace this one directly with x
1304 tmp1 += Aj(k, 0) * x[l_index_tmp + k];
1305 tmp2 += Aj(k, 1) * x[l_index_tmp + k];
1306 }
1307
1308 // 1. norm(A* lambda == 1)
1309 for (int k = 0; k < current_edges_num; ++k) {
1310 // with respect to l
1311 values[nz_index_tmp] = 2 * tmp1 * Aj(k, 0) + 2 * tmp2 * Aj(k, 1); // t0~tk
1312 ++nz_index_tmp; // current_edges_num
1313 }
1314
1315 // 2. G' * mu + R' * lambda == 0, part 1
1316 // With respect to x
1317 values[nz_index_tmp]
1318 = -std::sin(x[state_index_tmp + 2]) * tmp1 + std::cos(x[state_index_tmp + 2]) * tmp2; // u
1319 ++nz_index_tmp; // 1
1320
1321 // with respect to l
1322 for (int k = 0; k < current_edges_num; ++k) {
1323 values[nz_index_tmp] = std::cos(x[state_index_tmp + 2]) * Aj(k, 0)
1324 + std::sin(x[state_index_tmp + 2]) * Aj(k, 1); // v0~vn
1325 ++nz_index_tmp; // current_edges_num
1326 }
1327
1328 // With respect to n
1329 values[nz_index_tmp] = 1.0; // w0
1330 ++nz_index_tmp; // 1
1331
1332 values[nz_index_tmp] = -1.0; // w2
1333 ++nz_index_tmp; // 1
1334
1335 // 3. G' * mu + R' * lambda == 0, part 2
1336 // With respect to x
1337 values[nz_index_tmp]
1338 = -std::cos(x[state_index_tmp + 2]) * tmp1 - std::sin(x[state_index_tmp + 2]) * tmp2; // x
1339 ++nz_index_tmp; // 1
1340
1341 // with respect to l
1342 for (int k = 0; k < current_edges_num; ++k) {
1343 values[nz_index_tmp] = -std::sin(x[state_index_tmp + 2]) * Aj(k, 0)
1344 + std::cos(x[state_index_tmp + 2]) * Aj(k, 1); // y0~yn
1345 ++nz_index_tmp; // current_edges_num
1346 }
1347
1348 // With respect to n
1349 values[nz_index_tmp] = 1.0; // z1
1350 ++nz_index_tmp; // 1
1351
1352 values[nz_index_tmp] = -1.0; // z3
1353 ++nz_index_tmp; // 1
1354
1355 // 3. -g'*mu + (A*t - b)*lambda > 0
1356 double tmp3 = 0.0;
1357 double tmp4 = 0.0;
1358 for (int k = 0; k < 4; ++k) {
1359 tmp3 += -g_[k] * x[n_index_tmp + k];
1360 }
1361
1362 for (int k = 0; k < current_edges_num; ++k) {
1363 tmp4 += bj(k, 0) * x[l_index_tmp + k];
1364 }
1365
1366 // With respect to x
1367 values[nz_index_tmp] = tmp1; // aa1
1368 ++nz_index_tmp; // 1
1369
1370 values[nz_index_tmp] = tmp2; // bb1
1371 ++nz_index_tmp; // 1
1372
1373 values[nz_index_tmp] = -std::sin(x[state_index_tmp + 2]) * offset_ * tmp1
1374 + std::cos(x[state_index_tmp + 2]) * offset_ * tmp2; // cc1
1375 ++nz_index_tmp; // 1
1376
1377 // with respect to l
1378 for (int k = 0; k < current_edges_num; ++k) {
1379 values[nz_index_tmp] = (x[state_index_tmp] + std::cos(x[state_index_tmp + 2]) * offset_) * Aj(k, 0)
1380 + (x[state_index_tmp + 1] + std::sin(x[state_index_tmp + 2]) * offset_) * Aj(k, 1)
1381 - bj(k, 0); // ddk
1382 ++nz_index_tmp; // current_edges_num
1383 }
1384
1385 // with respect to n
1386 for (int k = 0; k < 4; ++k) {
1387 values[nz_index_tmp] = -g_[k]; // eek
1388 ++nz_index_tmp; // 4
1389 }
1390 }
1391 // update index
1392 for (int jj = 0; jj < obstacles_num_; ++jj) {
1393 nz_index += 4 * (horizon_ + 1) * obstacles_edges_num_(jj, 0);
1394 nz_index += 13 * (horizon_ + 1);
1395 }
1396
1397 // 5. load variable bounds as constraints
1398 state_index = state_start_index_;
1399 control_index = control_start_index_;
1400 time_index = time_start_index_;
1401 l_index = l_start_index_;
1402 n_index = n_start_index_;
1403
1404 // start configuration
1405 values[nz_index] = 1.0;
1406 nz_index++;
1407 values[nz_index] = 1.0;
1408 nz_index++;
1409 values[nz_index] = 1.0;
1410 nz_index++;
1411 values[nz_index] = 1.0;
1412 nz_index++;
1413
1414 for (int i = 1; i < horizon_; ++i) {
1415 values[nz_index] = 1.0;
1416 nz_index++;
1417 values[nz_index] = 1.0;
1418 nz_index++;
1419 values[nz_index] = 1.0;
1420 nz_index++;
1421 }
1422
1423 // end configuration
1424 values[nz_index] = 1.0;
1425 nz_index++;
1426 values[nz_index] = 1.0;
1427 nz_index++;
1428 values[nz_index] = 1.0;
1429 nz_index++;
1430 values[nz_index] = 1.0;
1431 nz_index++;
1432
1433 for (int i = 0; i < horizon_; ++i) {
1434 values[nz_index] = 1.0;
1435 nz_index++;
1436 values[nz_index] = 1.0;
1437 nz_index++;
1438 }
1439
1440 for (int i = 0; i < horizon_ + 1; ++i) {
1441 values[nz_index] = 1.0;
1442 nz_index++;
1443 }
1444
1445 for (int i = 0; i < lambda_horizon_; ++i) {
1446 values[nz_index] = 1.0;
1447 nz_index++;
1448 }
1449
1450 for (int i = 0; i < miu_horizon_; ++i) {
1451 values[nz_index] = 1.0;
1452 nz_index++;
1453 }
1454
1455 ADEBUG << "eval_jac_g, fulfilled obstacle constraint values";
1456 CHECK_EQ(nz_index, static_cast<int>(nele_jac));
1457 }
1458
1459 ADEBUG << "eval_jac_g done";
1460 return true;
1461} // NOLINT

◆ eval_jac_g_ser()

bool apollo::planning::DistanceApproachIPOPTCUDAInterface::eval_jac_g_ser ( int  n,
const double *  x,
bool  new_x,
int  m,
int  nele_jac,
int *  iRow,
int *  jCol,
double *  values 
)
virtual

实现了 apollo::planning::DistanceApproachInterface.

在文件 distance_approach_ipopt_cuda_interface.cc 第 1463 行定义.

1471 {
1472 ADEBUG << "eval_jac_g";
1473 CHECK_EQ(n, num_of_variables_) << "No. of variables wrong in eval_jac_g. n : " << n;
1474 CHECK_EQ(m, num_of_constraints_) << "No. of constraints wrong in eval_jac_g. n : " << m;
1475
1476 if (values == nullptr) {
1477 int nz_index = 0;
1478 int constraint_index = 0;
1479 int state_index = state_start_index_;
1480 int control_index = control_start_index_;
1481 int time_index = time_start_index_;
1482
1483 // 1. State Constraint with respect to variables
1484 for (int i = 0; i < horizon_; ++i) {
1485 // g(0)' with respect to x0 ~ x7
1486 iRow[nz_index] = state_index;
1487 jCol[nz_index] = state_index;
1488 ++nz_index;
1489
1490 iRow[nz_index] = state_index;
1491 jCol[nz_index] = state_index + 2;
1492 ++nz_index;
1493
1494 iRow[nz_index] = state_index;
1495 jCol[nz_index] = state_index + 3;
1496 ++nz_index;
1497
1498 iRow[nz_index] = state_index;
1499 jCol[nz_index] = state_index + 4;
1500 ++nz_index;
1501
1502 // g(0)' with respect to u0 ~ u1'
1503 iRow[nz_index] = state_index;
1504 jCol[nz_index] = control_index;
1505 ++nz_index;
1506
1507 iRow[nz_index] = state_index;
1508 jCol[nz_index] = control_index + 1;
1509 ++nz_index;
1510
1511 // g(0)' with respect to time
1512 iRow[nz_index] = state_index;
1513 jCol[nz_index] = time_index;
1514 ++nz_index;
1515
1516 // g(1)' with respect to x0 ~ x7
1517 iRow[nz_index] = state_index + 1;
1518 jCol[nz_index] = state_index + 1;
1519 ++nz_index;
1520
1521 iRow[nz_index] = state_index + 1;
1522 jCol[nz_index] = state_index + 2;
1523 ++nz_index;
1524
1525 iRow[nz_index] = state_index + 1;
1526 jCol[nz_index] = state_index + 3;
1527 ++nz_index;
1528
1529 iRow[nz_index] = state_index + 1;
1530 jCol[nz_index] = state_index + 5;
1531 ++nz_index;
1532
1533 // g(1)' with respect to u0 ~ u1'
1534 iRow[nz_index] = state_index + 1;
1535 jCol[nz_index] = control_index;
1536 ++nz_index;
1537
1538 iRow[nz_index] = state_index + 1;
1539 jCol[nz_index] = control_index + 1;
1540 ++nz_index;
1541
1542 // g(1)' with respect to time
1543 iRow[nz_index] = state_index + 1;
1544 jCol[nz_index] = time_index;
1545 ++nz_index;
1546
1547 // g(2)' with respect to x0 ~ x7
1548 iRow[nz_index] = state_index + 2;
1549 jCol[nz_index] = state_index + 2;
1550 ++nz_index;
1551
1552 iRow[nz_index] = state_index + 2;
1553 jCol[nz_index] = state_index + 3;
1554 ++nz_index;
1555
1556 iRow[nz_index] = state_index + 2;
1557 jCol[nz_index] = state_index + 6;
1558 ++nz_index;
1559
1560 // g(2)' with respect to u0 ~ u1'
1561 iRow[nz_index] = state_index + 2;
1562 jCol[nz_index] = control_index;
1563 ++nz_index;
1564
1565 iRow[nz_index] = state_index + 2;
1566 jCol[nz_index] = control_index + 1;
1567 ++nz_index;
1568
1569 // g(2)' with respect to time
1570 iRow[nz_index] = state_index + 2;
1571 jCol[nz_index] = time_index;
1572 ++nz_index;
1573
1574 // g(3)' with x0 ~ x7
1575 iRow[nz_index] = state_index + 3;
1576 jCol[nz_index] = state_index + 3;
1577 ++nz_index;
1578
1579 iRow[nz_index] = state_index + 3;
1580 jCol[nz_index] = state_index + 7;
1581 ++nz_index;
1582
1583 // g(3)' with respect to u0 ~ u1'
1584 iRow[nz_index] = state_index + 3;
1585 jCol[nz_index] = control_index + 1;
1586 ++nz_index;
1587
1588 // g(3)' with respect to time
1589 iRow[nz_index] = state_index + 3;
1590 jCol[nz_index] = time_index;
1591 ++nz_index;
1592
1593 state_index += 4;
1594 control_index += 2;
1595 time_index++;
1596 constraint_index += 4;
1597 }
1598
1599 // 2. only have control rate constraints on u0 , range [0, horizon_-1]
1600 control_index = control_start_index_;
1601 state_index = state_start_index_;
1602 time_index = time_start_index_;
1603
1604 // First one, with respect to u(0, 0)
1605 iRow[nz_index] = constraint_index;
1606 jCol[nz_index] = control_index;
1607 ++nz_index;
1608
1609 // First element, with respect to time
1610 iRow[nz_index] = constraint_index;
1611 jCol[nz_index] = time_index;
1612 ++nz_index;
1613
1614 control_index += 2;
1615 time_index++;
1616 constraint_index++;
1617
1618 for (int i = 1; i < horizon_; ++i) {
1619 // with respect to u(0, i-1)
1620 iRow[nz_index] = constraint_index;
1621 jCol[nz_index] = control_index - 2;
1622 ++nz_index;
1623
1624 // with respect to u(0, i)
1625 iRow[nz_index] = constraint_index;
1626 jCol[nz_index] = control_index;
1627 ++nz_index;
1628
1629 // with respect to time
1630 iRow[nz_index] = constraint_index;
1631 jCol[nz_index] = time_index;
1632 ++nz_index;
1633
1634 // only consider rate limits on u0
1635 control_index += 2;
1636 constraint_index++;
1637 time_index++;
1638 }
1639
1640 // 3. Time constraints [0, horizon_ -1]
1641 time_index = time_start_index_;
1642
1643 for (int i = 0; i < horizon_; ++i) {
1644 // with respect to timescale(0, i-1)
1645 iRow[nz_index] = constraint_index;
1646 jCol[nz_index] = time_index;
1647 ++nz_index;
1648
1649 // with respect to timescale(0, i)
1650 iRow[nz_index] = constraint_index;
1651 jCol[nz_index] = time_index + 1;
1652 ++nz_index;
1653
1654 time_index++;
1655 constraint_index++;
1656 }
1657
1658 // 4. Three obstacles related equal constraints, one equality constraints,
1659 // [0, horizon_] * [0, obstacles_num_-1] * 4
1660 state_index = state_start_index_;
1661 int l_index = l_start_index_;
1662 int n_index = n_start_index_;
1663 for (int i = 0; i < horizon_ + 1; ++i) {
1664 for (int j = 0; j < obstacles_num_; ++j) {
1665 int current_edges_num = obstacles_edges_num_(j, 0);
1666
1667 // 1. norm(A* lambda == 1)
1668 for (int k = 0; k < current_edges_num; ++k) {
1669 // with respect to l
1670 iRow[nz_index] = constraint_index;
1671 jCol[nz_index] = l_index + k;
1672 ++nz_index;
1673 }
1674
1675 // 2. G' * mu + R' * lambda == 0, part 1
1676 // With respect to x
1677 iRow[nz_index] = constraint_index + 1;
1678 jCol[nz_index] = state_index + 2;
1679 ++nz_index;
1680
1681 // with respect to l
1682 for (int k = 0; k < current_edges_num; ++k) {
1683 iRow[nz_index] = constraint_index + 1;
1684 jCol[nz_index] = l_index + k;
1685 ++nz_index;
1686 }
1687
1688 // With respect to n
1689 iRow[nz_index] = constraint_index + 1;
1690 jCol[nz_index] = n_index;
1691 ++nz_index;
1692
1693 iRow[nz_index] = constraint_index + 1;
1694 jCol[nz_index] = n_index + 2;
1695 ++nz_index;
1696
1697 // 2. G' * mu + R' * lambda == 0, part 2
1698 // With respect to x
1699 iRow[nz_index] = constraint_index + 2;
1700 jCol[nz_index] = state_index + 2;
1701 ++nz_index;
1702
1703 // with respect to l
1704 for (int k = 0; k < current_edges_num; ++k) {
1705 iRow[nz_index] = constraint_index + 2;
1706 jCol[nz_index] = l_index + k;
1707 ++nz_index;
1708 }
1709
1710 // With respect to n
1711 iRow[nz_index] = constraint_index + 2;
1712 jCol[nz_index] = n_index + 1;
1713 ++nz_index;
1714
1715 iRow[nz_index] = constraint_index + 2;
1716 jCol[nz_index] = n_index + 3;
1717 ++nz_index;
1718
1719 // -g'*mu + (A*t - b)*lambda > 0
1720 // With respect to x
1721 iRow[nz_index] = constraint_index + 3;
1722 jCol[nz_index] = state_index;
1723 ++nz_index;
1724
1725 iRow[nz_index] = constraint_index + 3;
1726 jCol[nz_index] = state_index + 1;
1727 ++nz_index;
1728
1729 iRow[nz_index] = constraint_index + 3;
1730 jCol[nz_index] = state_index + 2;
1731 ++nz_index;
1732
1733 // with respect to l
1734 for (int k = 0; k < current_edges_num; ++k) {
1735 iRow[nz_index] = constraint_index + 3;
1736 jCol[nz_index] = l_index + k;
1737 ++nz_index;
1738 }
1739
1740 // with respect to n
1741 for (int k = 0; k < 4; ++k) {
1742 iRow[nz_index] = constraint_index + 3;
1743 jCol[nz_index] = n_index + k;
1744 ++nz_index;
1745 }
1746
1747 // Update index
1748 l_index += current_edges_num;
1749 n_index += 4;
1750 constraint_index += 4;
1751 }
1752 state_index += 4;
1753 }
1754
1755 // 5. load variable bounds as constraints
1756 state_index = state_start_index_;
1757 control_index = control_start_index_;
1758 time_index = time_start_index_;
1759 l_index = l_start_index_;
1760 n_index = n_start_index_;
1761
1762 // start configuration
1763 iRow[nz_index] = constraint_index;
1764 jCol[nz_index] = state_index;
1765 nz_index++;
1766 iRow[nz_index] = constraint_index + 1;
1767 jCol[nz_index] = state_index + 1;
1768 nz_index++;
1769 iRow[nz_index] = constraint_index + 2;
1770 jCol[nz_index] = state_index + 2;
1771 nz_index++;
1772 iRow[nz_index] = constraint_index + 3;
1773 jCol[nz_index] = state_index + 3;
1774 nz_index++;
1775 constraint_index += 4;
1776 state_index += 4;
1777
1778 for (int i = 1; i < horizon_; ++i) {
1779 iRow[nz_index] = constraint_index;
1780 jCol[nz_index] = state_index;
1781 nz_index++;
1782 iRow[nz_index] = constraint_index + 1;
1783 jCol[nz_index] = state_index + 1;
1784 nz_index++;
1785 iRow[nz_index] = constraint_index + 2;
1786 jCol[nz_index] = state_index + 3;
1787 nz_index++;
1788 constraint_index += 3;
1789 state_index += 4;
1790 }
1791
1792 // end configuration
1793 iRow[nz_index] = constraint_index;
1794 jCol[nz_index] = state_index;
1795 nz_index++;
1796 iRow[nz_index] = constraint_index + 1;
1797 jCol[nz_index] = state_index + 1;
1798 nz_index++;
1799 iRow[nz_index] = constraint_index + 2;
1800 jCol[nz_index] = state_index + 2;
1801 nz_index++;
1802 iRow[nz_index] = constraint_index + 3;
1803 jCol[nz_index] = state_index + 3;
1804 nz_index++;
1805 constraint_index += 4;
1806 state_index += 4;
1807
1808 for (int i = 0; i < horizon_; ++i) {
1809 iRow[nz_index] = constraint_index;
1810 jCol[nz_index] = control_index;
1811 nz_index++;
1812 iRow[nz_index] = constraint_index + 1;
1813 jCol[nz_index] = control_index + 1;
1814 nz_index++;
1815 constraint_index += 2;
1816 control_index += 2;
1817 }
1818
1819 for (int i = 0; i < horizon_ + 1; ++i) {
1820 iRow[nz_index] = constraint_index;
1821 jCol[nz_index] = time_index;
1822 nz_index++;
1823 constraint_index++;
1824 time_index++;
1825 }
1826
1827 for (int i = 0; i < lambda_horizon_; ++i) {
1828 iRow[nz_index] = constraint_index;
1829 jCol[nz_index] = l_index;
1830 nz_index++;
1831 constraint_index++;
1832 l_index++;
1833 }
1834
1835 for (int i = 0; i < miu_horizon_; ++i) {
1836 iRow[nz_index] = constraint_index;
1837 jCol[nz_index] = n_index;
1838 nz_index++;
1839 constraint_index++;
1840 n_index++;
1841 }
1842
1843 CHECK_EQ(nz_index, static_cast<int>(nele_jac));
1844 CHECK_EQ(constraint_index, static_cast<int>(m));
1845 } else {
1846 std::fill(values, values + nele_jac, 0.0);
1847 int nz_index = 0;
1848
1849 int time_index = time_start_index_;
1850 int state_index = state_start_index_;
1851 int control_index = control_start_index_;
1852
1853 // TODO(QiL) : initially implemented to be debug friendly, later iterate
1854 // towards better efficiency
1855 // 1. state constraints 4 * [0, horizons-1]
1856 for (int i = 0; i < horizon_; ++i) {
1857 values[nz_index] = -1.0;
1858 ++nz_index;
1859
1860 values[nz_index] = x[time_index] * ts_
1861 * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
1862 * std::sin(x[state_index + 2]
1863 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1864 / wheelbase_); // a.
1865 ++nz_index;
1866
1867 values[nz_index] = -1.0
1868 * (x[time_index] * ts_
1869 * std::cos(
1870 x[state_index + 2]
1871 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1872 / wheelbase_)
1873 + x[time_index] * ts_ * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
1874 * (-1) * x[time_index] * ts_ * 0.5 * std::tan(x[control_index]) / wheelbase_
1875 * std::sin(
1876 x[state_index + 2]
1877 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1878 / wheelbase_)); // b
1879 ++nz_index;
1880
1881 values[nz_index] = 1.0;
1882 ++nz_index;
1883
1884 values[nz_index] = x[time_index] * ts_
1885 * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
1886 * std::sin(x[state_index + 2]
1887 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1888 / wheelbase_)
1889 * x[time_index] * ts_ * 0.5 * x[state_index + 3]
1890 / (std::cos(x[control_index]) * std::cos(x[control_index])) / wheelbase_; // c
1891 ++nz_index;
1892
1893 values[nz_index] = -1.0
1894 * (x[time_index] * ts_ * x[time_index] * ts_ * 0.5
1895 * std::cos(
1896 x[state_index + 2]
1897 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1898 / wheelbase_)); // d
1899 ++nz_index;
1900
1901 values[nz_index] = -1.0
1902 * (ts_ * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
1903 * std::cos(
1904 x[state_index + 2]
1905 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1906 / wheelbase_)
1907 + x[time_index] * ts_ * ts_ * 0.5 * x[control_index + 1]
1908 * std::cos(
1909 x[state_index + 2]
1910 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1911 / wheelbase_)
1912 - x[time_index] * ts_ * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
1913 * x[state_index + 3] * ts_ * 0.5 * std::tan(x[control_index]) / wheelbase_
1914 * std::sin(
1915 x[state_index + 2]
1916 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1917 / wheelbase_)); // e
1918 ++nz_index;
1919
1920 values[nz_index] = -1.0;
1921 ++nz_index;
1922
1923 values[nz_index] = -1.0
1924 * (x[time_index] * ts_ * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
1925 * std::cos(
1926 x[state_index + 2]
1927 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1928 / wheelbase_)); // f.
1929 ++nz_index;
1930
1931 values[nz_index] = -1.0
1932 * (x[time_index] * ts_
1933 * std::sin(
1934 x[state_index + 2]
1935 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1936 / wheelbase_)
1937 + x[time_index] * ts_ * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
1938 * x[time_index] * ts_ * 0.5 * std::tan(x[control_index]) / wheelbase_
1939 * std::cos(
1940 x[state_index + 2]
1941 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1942 / wheelbase_)); // g
1943 ++nz_index;
1944
1945 values[nz_index] = 1.0;
1946 ++nz_index;
1947
1948 values[nz_index] = -1.0
1949 * (x[time_index] * ts_ * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
1950 * std::cos(
1951 x[state_index + 2]
1952 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1953 / wheelbase_)
1954 * x[time_index] * ts_ * 0.5 * x[state_index + 3]
1955 / (std::cos(x[control_index]) * std::cos(x[control_index])) / wheelbase_); // h
1956 ++nz_index;
1957
1958 values[nz_index] = -1.0
1959 * (x[time_index] * ts_ * x[time_index] * ts_ * 0.5
1960 * std::sin(
1961 x[state_index + 2]
1962 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1963 / wheelbase_)); // i
1964 ++nz_index;
1965
1966 values[nz_index] = -1.0
1967 * (ts_ * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
1968 * std::sin(
1969 x[state_index + 2]
1970 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1971 / wheelbase_)
1972 + x[time_index] * ts_ * ts_ * 0.5 * x[control_index + 1]
1973 * std::sin(
1974 x[state_index + 2]
1975 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1976 / wheelbase_)
1977 + x[time_index] * ts_ * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
1978 * x[state_index + 3] * ts_ * 0.5 * std::tan(x[control_index]) / wheelbase_
1979 * std::cos(
1980 x[state_index + 2]
1981 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1982 / wheelbase_)); // j
1983 ++nz_index;
1984
1985 values[nz_index] = -1.0;
1986 ++nz_index;
1987
1988 values[nz_index] = -1.0 * x[time_index] * ts_ * std::tan(x[control_index]) / wheelbase_; // k.
1989 ++nz_index;
1990
1991 values[nz_index] = 1.0;
1992 ++nz_index;
1993
1994 values[nz_index] = -1.0
1995 * (x[time_index] * ts_ * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
1996 / (std::cos(x[control_index]) * std::cos(x[control_index])) / wheelbase_); // l.
1997 ++nz_index;
1998
1999 values[nz_index] = -1.0
2000 * (x[time_index] * ts_ * x[time_index] * ts_ * 0.5 * std::tan(x[control_index])
2001 / wheelbase_); // m.
2002 ++nz_index;
2003
2004 values[nz_index] = -1.0
2005 * (ts_ * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
2006 * std::tan(x[control_index]) / wheelbase_
2007 + x[time_index] * ts_ * ts_ * 0.5 * x[control_index + 1] * std::tan(x[control_index])
2008 / wheelbase_); // n.
2009 ++nz_index;
2010
2011 values[nz_index] = -1.0;
2012 ++nz_index;
2013
2014 values[nz_index] = 1.0;
2015 ++nz_index;
2016
2017 values[nz_index] = -1.0 * ts_ * x[time_index]; // o.
2018 ++nz_index;
2019
2020 values[nz_index] = -1.0 * ts_ * x[control_index + 1]; // p.
2021 ++nz_index;
2022
2023 state_index += 4;
2024 control_index += 2;
2025 time_index++;
2026 }
2027
2028 // 2. control rate constraints 1 * [0, horizons-1]
2029 control_index = control_start_index_;
2030 state_index = state_start_index_;
2031 time_index = time_start_index_;
2032
2033 // First horizon
2034
2035 // with respect to u(0, 0)
2036 values[nz_index] = 1.0 / x[time_index] / ts_; // q
2037 ++nz_index;
2038
2039 // with respect to time
2040 values[nz_index] = -1.0 * (x[control_index] - last_time_u_(0, 0)) / x[time_index] / x[time_index] / ts_;
2041 ++nz_index;
2042 time_index++;
2043 control_index += 2;
2044
2045 for (int i = 1; i < horizon_; ++i) {
2046 // with respect to u(0, i-1)
2047
2048 values[nz_index] = -1.0 / x[time_index] / ts_;
2049 ++nz_index;
2050
2051 // with respect to u(0, i)
2052 values[nz_index] = 1.0 / x[time_index] / ts_;
2053 ++nz_index;
2054
2055 // with respect to time
2056 values[nz_index] = -1.0 * (x[control_index] - x[control_index - 2]) / x[time_index] / x[time_index] / ts_;
2057 ++nz_index;
2058
2059 control_index += 2;
2060 time_index++;
2061 }
2062
2063 ADEBUG << "After fulfilled control rate constraints derivative, nz_index : " << nz_index
2064 << " nele_jac : " << nele_jac;
2065
2066 // 3. Time constraints [0, horizon_ -1]
2067 time_index = time_start_index_;
2068 for (int i = 0; i < horizon_; ++i) {
2069 // with respect to timescale(0, i-1)
2070 values[nz_index] = -1.0;
2071 ++nz_index;
2072
2073 // with respect to timescale(0, i)
2074 values[nz_index] = 1.0;
2075 ++nz_index;
2076
2077 time_index++;
2078 }
2079
2080 ADEBUG << "After fulfilled time constraints derivative, nz_index : " << nz_index << " nele_jac : " << nele_jac;
2081
2082 // 4. Three obstacles related equal constraints, one equality constraints,
2083 // [0, horizon_] * [0, obstacles_num_-1] * 4
2084
2085 state_index = state_start_index_;
2086 int l_index = l_start_index_;
2087 int n_index = n_start_index_;
2088
2089 for (int i = 0; i < horizon_ + 1; ++i) {
2090 int edges_counter = 0;
2091 for (int j = 0; j < obstacles_num_; ++j) {
2092 int current_edges_num = obstacles_edges_num_(j, 0);
2093 Eigen::MatrixXd Aj = obstacles_A_.block(edges_counter, 0, current_edges_num, 2);
2094 Eigen::MatrixXd bj = 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] = 2 * tmp1 * Aj(k, 0) + 2 * tmp2 * Aj(k, 1); // t0~tk
2109 ++nz_index;
2110 }
2111
2112 // 2. G' * mu + R' * lambda == 0, part 1
2113 // With respect to x
2114 values[nz_index] = -std::sin(x[state_index + 2]) * tmp1 + std::cos(x[state_index + 2]) * tmp2; // u
2115 ++nz_index;
2116
2117 // with respect to l
2118 for (int k = 0; k < current_edges_num; ++k) {
2119 values[nz_index] = std::cos(x[state_index + 2]) * Aj(k, 0)
2120 + std::sin(x[state_index + 2]) * Aj(k, 1); // v0~vn
2121 ++nz_index;
2122 }
2123
2124 // With respect to n
2125 values[nz_index] = 1.0; // w0
2126 ++nz_index;
2127
2128 values[nz_index] = -1.0; // w2
2129 ++nz_index;
2130
2131 // 3. G' * mu + R' * lambda == 0, part 2
2132 // With respect to x
2133 values[nz_index] = -std::cos(x[state_index + 2]) * tmp1 - std::sin(x[state_index + 2]) * tmp2; // x
2134 ++nz_index;
2135
2136 // with respect to l
2137 for (int k = 0; k < current_edges_num; ++k) {
2138 values[nz_index] = -std::sin(x[state_index + 2]) * Aj(k, 0)
2139 + std::cos(x[state_index + 2]) * Aj(k, 1); // y0~yn
2140 ++nz_index;
2141 }
2142
2143 // With respect to n
2144 values[nz_index] = 1.0; // z1
2145 ++nz_index;
2146
2147 values[nz_index] = -1.0; // z3
2148 ++nz_index;
2149
2150 // 3. -g'*mu + (A*t - b)*lambda > 0
2151 double tmp3 = 0.0;
2152 double tmp4 = 0.0;
2153 for (int k = 0; k < 4; ++k) {
2154 tmp3 += -g_[k] * x[n_index + k];
2155 }
2156
2157 for (int k = 0; k < current_edges_num; ++k) {
2158 tmp4 += bj(k, 0) * x[l_index + k];
2159 }
2160
2161 // With respect to x
2162 values[nz_index] = tmp1; // aa1
2163 ++nz_index;
2164
2165 values[nz_index] = tmp2; // bb1
2166 ++nz_index;
2167
2168 values[nz_index] = -std::sin(x[state_index + 2]) * offset_ * tmp1
2169 + std::cos(x[state_index + 2]) * offset_ * tmp2; // cc1
2170 ++nz_index;
2171
2172 // with respect to l
2173 for (int k = 0; k < current_edges_num; ++k) {
2174 values[nz_index] = (x[state_index] + std::cos(x[state_index + 2]) * offset_) * Aj(k, 0)
2175 + (x[state_index + 1] + std::sin(x[state_index + 2]) * offset_) * Aj(k, 1)
2176 - bj(k, 0); // ddk
2177 ++nz_index;
2178 }
2179
2180 // with respect to n
2181 for (int k = 0; k < 4; ++k) {
2182 values[nz_index] = -g_[k]; // eek
2183 ++nz_index;
2184 }
2185
2186 // Update index
2187 edges_counter += current_edges_num;
2188 l_index += current_edges_num;
2189 n_index += 4;
2190 }
2191 state_index += 4;
2192 }
2193
2194 // 5. load variable bounds as constraints
2195 state_index = state_start_index_;
2196 control_index = control_start_index_;
2197 time_index = time_start_index_;
2198 l_index = l_start_index_;
2199 n_index = n_start_index_;
2200
2201 // start configuration
2202 values[nz_index] = 1.0;
2203 nz_index++;
2204 values[nz_index] = 1.0;
2205 nz_index++;
2206 values[nz_index] = 1.0;
2207 nz_index++;
2208 values[nz_index] = 1.0;
2209 nz_index++;
2210
2211 for (int i = 1; i < horizon_; ++i) {
2212 values[nz_index] = 1.0;
2213 nz_index++;
2214 values[nz_index] = 1.0;
2215 nz_index++;
2216 values[nz_index] = 1.0;
2217 nz_index++;
2218 }
2219
2220 // end configuration
2221 values[nz_index] = 1.0;
2222 nz_index++;
2223 values[nz_index] = 1.0;
2224 nz_index++;
2225 values[nz_index] = 1.0;
2226 nz_index++;
2227 values[nz_index] = 1.0;
2228 nz_index++;
2229
2230 for (int i = 0; i < horizon_; ++i) {
2231 values[nz_index] = 1.0;
2232 nz_index++;
2233 values[nz_index] = 1.0;
2234 nz_index++;
2235 }
2236
2237 for (int i = 0; i < horizon_ + 1; ++i) {
2238 values[nz_index] = 1.0;
2239 nz_index++;
2240 }
2241
2242 for (int i = 0; i < lambda_horizon_; ++i) {
2243 values[nz_index] = 1.0;
2244 nz_index++;
2245 }
2246
2247 for (int i = 0; i < miu_horizon_; ++i) {
2248 values[nz_index] = 1.0;
2249 nz_index++;
2250 }
2251
2252 ADEBUG << "eval_jac_g, fulfilled obstacle constraint values";
2253 CHECK_EQ(nz_index, static_cast<int>(nele_jac));
2254 }
2255
2256 ADEBUG << "eval_jac_g done";
2257 return true;
2258} // NOLINT

◆ eval_obj()

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

Template to return the objective value

在文件 distance_approach_ipopt_cuda_interface.cc 第 2439 行定义.

2439 {
2440 ADEBUG << "eval_obj";
2441 // Objective is :
2442 // min control inputs
2443 // min input rate
2444 // min time (if the time step is not fixed)
2445 // regularization wrt warm start trajectory
2446 DCHECK(ts_ != 0) << "ts in distance_approach_ is 0";
2447 int control_index = control_start_index_;
2448 int time_index = time_start_index_;
2449 int state_index = state_start_index_;
2450
2451 // TODO(QiL): Initial implementation towards earlier understanding and debug
2452 // purpose, later code refine towards improving efficiency
2453
2454 *obj_value = 0.0;
2455 // 1. objective to minimize state diff to warm up
2456 for (int i = 0; i < horizon_ + 1; ++i) {
2457 T x1_diff = x[state_index] - xWS_(0, i);
2458 T x2_diff = x[state_index + 1] - xWS_(1, i);
2459 T x3_diff = x[state_index + 2] - xWS_(2, i);
2460 T x4_abs = x[state_index + 3];
2461 *obj_value += weight_state_x_ * x1_diff * x1_diff + weight_state_y_ * x2_diff * x2_diff
2462 + weight_state_phi_ * x3_diff * x3_diff + weight_state_v_ * x4_abs * x4_abs;
2463 state_index += 4;
2464 }
2465
2466 // 2. objective to minimize u square
2467 for (int i = 0; i < horizon_; ++i) {
2468 *obj_value += weight_input_steer_ * x[control_index] * x[control_index]
2469 + weight_input_a_ * x[control_index + 1] * x[control_index + 1];
2470 control_index += 2;
2471 }
2472
2473 // 3. objective to minimize input change rate for first horizon
2474 control_index = control_start_index_;
2475 T last_time_steer_rate = (x[control_index] - last_time_u_(0, 0)) / x[time_index] / ts_;
2476 T last_time_a_rate = (x[control_index + 1] - last_time_u_(1, 0)) / x[time_index] / ts_;
2477 *obj_value += weight_stitching_steer_ * last_time_steer_rate * last_time_steer_rate
2478 + weight_stitching_a_ * last_time_a_rate * last_time_a_rate;
2479
2480 // 4. objective to minimize input change rates, [0- horizon_ -2]
2481 time_index++;
2482 for (int i = 0; i < horizon_ - 1; ++i) {
2483 T steering_rate = (x[control_index + 2] - x[control_index]) / x[time_index] / ts_;
2484 T a_rate = (x[control_index + 3] - x[control_index + 1]) / x[time_index] / ts_;
2485 *obj_value += weight_rate_steer_ * steering_rate * steering_rate + weight_rate_a_ * a_rate * a_rate;
2486 control_index += 2;
2487 time_index++;
2488 }
2489
2490 // 5. objective to minimize total time [0, horizon_]
2491 time_index = time_start_index_;
2492 for (int i = 0; i < horizon_ + 1; ++i) {
2493 T first_order_penalty = weight_first_order_time_ * x[time_index];
2494 T second_order_penalty = weight_second_order_time_ * x[time_index] * x[time_index];
2495 *obj_value += first_order_penalty + second_order_penalty;
2496 time_index++;
2497 }
2498
2499 ADEBUG << "objective value after this iteration : " << *obj_value;
2500 return true;
2501}

◆ finalize_solution()

void apollo::planning::DistanceApproachIPOPTCUDAInterface::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 
)
overridevirtual

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

实现了 apollo::planning::DistanceApproachInterface.

在文件 distance_approach_ipopt_cuda_interface.cc 第 2306 行定义.

2317 {
2318 ADEBUG << "finalize_solution";
2319 int state_index = state_start_index_;
2320 int control_index = control_start_index_;
2321 int time_index = time_start_index_;
2322 int dual_l_index = l_start_index_;
2323 int dual_n_index = n_start_index_;
2324 // 1. state variables, 4 * [0, horizon]
2325 // 2. control variables, 2 * [0, horizon_-1]
2326 // 3. sampling time variables, 1 * [0, horizon_]
2327 // 4. dual_l obstacles_edges_sum_ * [0, horizon]
2328 // 5. dual_n obstacles_num * [0, horizon]
2329 for (int i = 0; i < horizon_; ++i) {
2330 state_result_(0, i) = x[state_index];
2331 state_result_(1, i) = x[state_index + 1];
2332 state_result_(2, i) = x[state_index + 2];
2333 state_result_(3, i) = x[state_index + 3];
2334 control_result_(0, i) = x[control_index];
2335 control_result_(1, i) = x[control_index + 1];
2336 time_result_(0, i) = x[time_index];
2337 for (int j = 0; j < obstacles_edges_sum_; ++j) {
2338 dual_l_result_(j, i) = x[dual_l_index + j];
2339 }
2340 for (int k = 0; k < 4 * obstacles_num_; k++) {
2341 dual_n_result_(k, i) = x[dual_n_index + k];
2342 }
2343 state_index += 4;
2344 control_index += 2;
2345 time_index++;
2346 dual_l_index += obstacles_edges_sum_;
2347 dual_n_index += 4 * obstacles_num_;
2348 }
2349 state_result_(0, 0) = x0_(0, 0);
2350 state_result_(1, 0) = x0_(1, 0);
2351 state_result_(2, 0) = x0_(2, 0);
2352 state_result_(3, 0) = x0_(3, 0);
2353 // push back last horizon for state and time variables
2354 state_result_(0, horizon_) = xf_(0, 0);
2355 state_result_(1, horizon_) = xf_(1, 0);
2356 state_result_(2, horizon_) = xf_(2, 0);
2357 state_result_(3, horizon_) = xf_(3, 0);
2358 time_result_(0, horizon_) = x[time_index];
2359 time_result_ = ts_ * time_result_;
2360 for (int j = 0; j < obstacles_edges_sum_; ++j) {
2361 dual_l_result_(j, horizon_) = x[dual_l_index + j];
2362 }
2363 for (int k = 0; k < 4 * obstacles_num_; k++) {
2364 dual_n_result_(k, horizon_) = x[dual_n_index + k];
2365 }
2366 // memory deallocation of ADOL-C variables
2367 delete[] obj_lam;
2368 free(rind_L);
2369 free(cind_L);
2370 free(hessval);
2371
2372 ADEBUG << "finalize_solution done!";
2373}

◆ generate_tapes()

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

Method to generate the required tapes by ADOL-C

在文件 distance_approach_ipopt_cuda_interface.cc 第 2778 行定义.

2778 {
2779 std::vector<double> xp(n);
2780 std::vector<double> lamp(m);
2781 std::vector<double> zl(m);
2782 std::vector<double> zu(m);
2783
2784 std::vector<adouble> xa(n);
2785 std::vector<adouble> g(m);
2786 std::vector<double> lam(m);
2787 double sig = 0.0;
2788 adouble obj_value;
2789
2790 double dummy = 0.0;
2791
2792 obj_lam = new double[m + 1];
2793
2794 get_starting_point(n, 1, &xp[0], 0, &zl[0], &zu[0], m, 0, &lamp[0]);
2795
2796 trace_on(tag_f);
2797
2798 for (int idx = 0; idx < n; idx++) {
2799 xa[idx] <<= xp[idx];
2800 }
2801
2802 eval_obj(n, &xa[0], &obj_value);
2803
2804 obj_value >>= dummy;
2805
2806 trace_off();
2807
2808 trace_on(tag_g);
2809
2810 for (int idx = 0; idx < n; idx++) {
2811 xa[idx] <<= xp[idx];
2812 }
2813
2814 eval_constraints(n, &xa[0], m, &g[0]);
2815
2816 for (int idx = 0; idx < m; idx++) {
2817 g[idx] >>= dummy;
2818 }
2819
2820 trace_off();
2821
2822 trace_on(tag_L);
2823
2824 for (int idx = 0; idx < n; idx++) {
2825 xa[idx] <<= xp[idx];
2826 }
2827 for (int idx = 0; idx < m; idx++) {
2828 lam[idx] = 1.0;
2829 }
2830 sig = 1.0;
2831
2832 eval_obj(n, &xa[0], &obj_value);
2833
2834 obj_value *= mkparam(sig);
2835 eval_constraints(n, &xa[0], m, &g[0]);
2836
2837 for (int idx = 0; idx < m; idx++) {
2838 obj_value += g[idx] * mkparam(lam[idx]);
2839 }
2840
2841 obj_value >>= dummy;
2842
2843 trace_off();
2844
2845 rind_L = nullptr;
2846 cind_L = nullptr;
2847
2848 hessval = nullptr;
2849
2850 options_L[0] = 0;
2851 options_L[1] = 1;
2852
2853 sparse_hess(tag_L, n, 0, &xp[0], &nnz_L, &rind_L, &cind_L, &hessval, options_L);
2854 *nnz_h_lag = nnz_L;
2855}
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:465
tag_g
#define tag_g
Definition cos_theta_ipopt_interface.h:37

◆ get_bounds_info()

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

Method to return the bounds for my problem

实现了 apollo::planning::DistanceApproachInterface.

在文件 distance_approach_ipopt_cuda_interface.cc 第 249 行定义.

255 {
256 ADEBUG << "get_bounds_info";
257 ACHECK(XYbounds_.size() == 4) << "XYbounds_ size is not 4, but" << XYbounds_.size();
258
259 // Variables: includes state, u, sample time and lagrange multipliers
260 // 1. state variables, 4 * [0, horizon]
261 // start point pose
262 int variable_index = 0;
263 for (int i = 0; i < 4; ++i) {
264 x_l[i] = -2e19;
265 x_u[i] = 2e19;
266 }
267 variable_index += 4;
268
269 // During horizons, 2 ~ N-1
270 for (int i = 1; i < horizon_; ++i) {
271 // x
272 x_l[variable_index] = -2e19;
273 x_u[variable_index] = 2e19;
274
275 // y
276 x_l[variable_index + 1] = -2e19;
277 x_u[variable_index + 1] = 2e19;
278
279 // phi
280 x_l[variable_index + 2] = -2e19;
281 x_u[variable_index + 2] = 2e19;
282
283 // v
284 x_l[variable_index + 3] = -2e19;
285 x_u[variable_index + 3] = 2e19;
286
287 variable_index += 4;
288 }
289
290 // end point pose
291 for (int i = 0; i < 4; ++i) {
292 x_l[variable_index + i] = -2e19;
293 x_u[variable_index + i] = 2e19;
294 }
295 variable_index += 4;
296 ADEBUG << "variable_index after adding state variables : " << variable_index;
297
298 // 2. control variables, 2 * [0, horizon_-1]
299 for (int i = 0; i < horizon_; ++i) {
300 // u1
301 x_l[variable_index] = -2e19;
302 x_u[variable_index] = 2e19;
303
304 // u2
305 x_l[variable_index + 1] = -2e19;
306 x_u[variable_index + 1] = 2e19;
307
308 variable_index += 2;
309 }
310 ADEBUG << "variable_index after adding control variables : " << variable_index;
311
312 // 3. sampling time variables, 1 * [0, horizon_]
313 for (int i = 0; i < horizon_ + 1; ++i) {
314 x_l[variable_index] = -2e19;
315 x_u[variable_index] = 2e19;
316 ++variable_index;
317 }
318 ADEBUG << "variable_index after adding sample time : " << variable_index;
319 ADEBUG << "sample time fix time status is : " << use_fix_time_;
320
321 // 4. lagrange constraint l, [0, obstacles_edges_sum_ - 1] * [0,
322 // horizon_]
323 for (int i = 0; i < horizon_ + 1; ++i) {
324 for (int j = 0; j < obstacles_edges_sum_; ++j) {
325 x_l[variable_index] = 0.0;
326 x_u[variable_index] = 2e19; // nlp_upper_bound_limit
327 ++variable_index;
328 }
329 }
330 ADEBUG << "variable_index after adding lagrange l : " << variable_index;
331
332 // 5. lagrange constraint n, [0, 4*obstacles_num-1] * [0, horizon_]
333 for (int i = 0; i < horizon_ + 1; ++i) {
334 for (int j = 0; j < 4 * obstacles_num_; ++j) {
335 x_l[variable_index] = 0.0;
336 x_u[variable_index] = 2e19; // nlp_upper_bound_limit
337
338 ++variable_index;
339 }
340 }
341 ADEBUG << "variable_index after adding lagrange n : " << variable_index;
342
343 // Constraints: includes four state Euler forward constraints, three
344 // Obstacle related constraints
345
346 // 1. dynamics constraints 4 * [0, horizons-1]
347 int constraint_index = 0;
348 for (int i = 0; i < 4 * horizon_; ++i) {
349 g_l[i] = 0.0;
350 g_u[i] = 0.0;
351 }
352 constraint_index += 4 * horizon_;
353
354 ADEBUG << "constraint_index after adding Euler forward dynamics constraints: " << constraint_index;
355
356 // 2. Control rate limit constraints, 1 * [0, horizons-1], only apply
357 // steering rate as of now
358 for (int i = 0; i < horizon_; ++i) {
359 g_l[constraint_index] = -max_steer_rate_;
360 g_u[constraint_index] = max_steer_rate_;
361 ++constraint_index;
362 }
363
364 ADEBUG << "constraint_index after adding steering rate constraints: " << constraint_index;
365
366 // 3. Time constraints 1 * [0, horizons-1]
367 for (int i = 0; i < horizon_; ++i) {
368 g_l[constraint_index] = 0.0;
369 g_u[constraint_index] = 0.0;
370 ++constraint_index;
371 }
372
373 ADEBUG << "constraint_index after adding time constraints: " << constraint_index;
374
375 // 4. Three obstacles related equal constraints, one equality constraints,
376 // [0, horizon_] * [0, obstacles_num_-1] * 4
377 for (int i = 0; i < horizon_ + 1; ++i) {
378 for (int j = 0; j < obstacles_num_; ++j) {
379 // a. norm(A'*lambda) <= 1
380 g_l[constraint_index] = -2e19;
381 g_u[constraint_index] = 1.0;
382
383 // b. G'*mu + R'*A*lambda = 0
384 g_l[constraint_index + 1] = 0.0;
385 g_u[constraint_index + 1] = 0.0;
386 g_l[constraint_index + 2] = 0.0;
387 g_u[constraint_index + 2] = 0.0;
388
389 // c. -g'*mu + (A*t - b)*lambda > min_safety_distance_
390 g_l[constraint_index + 3] = min_safety_distance_;
391 g_u[constraint_index + 3] = 2e19; // nlp_upper_bound_limit
392 constraint_index += 4;
393 }
394 }
395
396 // 5. load variable bounds as constraints
397 // start configuration
398 g_l[constraint_index] = x0_(0, 0);
399 g_u[constraint_index] = x0_(0, 0);
400 g_l[constraint_index + 1] = x0_(1, 0);
401 g_u[constraint_index + 1] = x0_(1, 0);
402 g_l[constraint_index + 2] = x0_(2, 0);
403 g_u[constraint_index + 2] = x0_(2, 0);
404 g_l[constraint_index + 3] = x0_(3, 0);
405 g_u[constraint_index + 3] = x0_(3, 0);
406 constraint_index += 4;
407
408 for (int i = 1; i < horizon_; ++i) {
409 g_l[constraint_index] = XYbounds_[0];
410 g_u[constraint_index] = XYbounds_[1];
411 g_l[constraint_index + 1] = XYbounds_[2];
412 g_u[constraint_index + 1] = XYbounds_[3];
413 g_l[constraint_index + 2] = -max_speed_reverse_;
414 g_u[constraint_index + 2] = max_speed_forward_;
415 constraint_index += 3;
416 }
417
418 // end configuration
419 g_l[constraint_index] = xf_(0, 0);
420 g_u[constraint_index] = xf_(0, 0);
421 g_l[constraint_index + 1] = xf_(1, 0);
422 g_u[constraint_index + 1] = xf_(1, 0);
423 g_l[constraint_index + 2] = xf_(2, 0);
424 g_u[constraint_index + 2] = xf_(2, 0);
425 g_l[constraint_index + 3] = xf_(3, 0);
426 g_u[constraint_index + 3] = xf_(3, 0);
427 constraint_index += 4;
428
429 for (int i = 0; i < horizon_; ++i) {
430 g_l[constraint_index] = -max_steer_angle_;
431 g_u[constraint_index] = max_steer_angle_;
432 g_l[constraint_index + 1] = -max_acceleration_reverse_;
433 g_u[constraint_index + 1] = max_acceleration_forward_;
434 constraint_index += 2;
435 }
436
437 for (int i = 0; i < horizon_ + 1; ++i) {
438 if (!use_fix_time_) {
439 g_l[constraint_index] = min_time_sample_scaling_;
440 g_u[constraint_index] = max_time_sample_scaling_;
441 } else {
442 g_l[constraint_index] = 1.0;
443 g_u[constraint_index] = 1.0;
444 }
445 constraint_index++;
446 }
447
448 for (int i = 0; i < lambda_horizon_; ++i) {
449 g_l[constraint_index] = 0.0;
450 g_u[constraint_index] = 2e19;
451 constraint_index++;
452 }
453
454 for (int i = 0; i < miu_horizon_; ++i) {
455 g_l[constraint_index] = 0.0;
456 g_u[constraint_index] = 2e19;
457 constraint_index++;
458 }
459
460 ADEBUG << "constraint_index after adding obstacles constraints: " << constraint_index;
461 ADEBUG << "get_bounds_info_ out";
462 return true;
463}

◆ get_nlp_info()

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

Method to return some info about the nlp

实现了 apollo::planning::DistanceApproachInterface.

在文件 distance_approach_ipopt_cuda_interface.cc 第 189 行定义.

194 {
195 ADEBUG << "get_nlp_info";
196 // n1 : states variables, 4 * (N+1)
197 int n1 = 4 * (horizon_ + 1);
198 ADEBUG << "n1: " << n1;
199 // n2 : control inputs variables
200 int n2 = 2 * horizon_;
201 ADEBUG << "n2: " << n2;
202 // n3 : sampling time variables
203 int n3 = horizon_ + 1;
204 ADEBUG << "n3: " << n3;
205 // n4 : dual multiplier associated with obstacle shape
206 lambda_horizon_ = obstacles_edges_num_.sum() * (horizon_ + 1);
207 ADEBUG << "lambda_horizon_: " << lambda_horizon_;
208 // n5 : dual multipier associated with car shape, obstacles_num*4 * (N+1)
209 miu_horizon_ = obstacles_num_ * 4 * (horizon_ + 1);
210 ADEBUG << "miu_horizon_: " << miu_horizon_;
211 // m1 : dynamics constatins
212 int m1 = 4 * horizon_;
213
214 // m2 : control rate constraints (only steering)
215 int m2 = horizon_;
216
217 // m3 : sampling time equality constraints
218 int m3 = horizon_;
219
220 // m4 : obstacle constraints
221 int m4 = 4 * obstacles_num_ * (horizon_ + 1);
222
223 num_of_variables_ = n1 + n2 + n3 + lambda_horizon_ + miu_horizon_;
224 num_of_constraints_ = m1 + m2 + m3 + m4 + (num_of_variables_ - (horizon_ + 1) + 2);
225
226 // number of variables
227 n = num_of_variables_;
228 ADEBUG << "num_of_variables_ " << num_of_variables_;
229 // number of constraints
230 m = num_of_constraints_;
231 ADEBUG << "num_of_constraints_ " << num_of_constraints_;
232
233 generate_tapes(n, m, &nnz_h_lag);
234 // // number of nonzero in Jacobian.
235 int tmp = 0;
236 for (int i = 0; i < horizon_ + 1; ++i) {
237 for (int j = 0; j < obstacles_num_; ++j) {
238 int current_edges_num = obstacles_edges_num_(j, 0);
239 tmp += current_edges_num * 4 + 9 + 4;
240 }
241 }
242 nnz_jac_g = 24 * horizon_ + 3 * horizon_ + 2 * horizon_ + tmp - 1 + (num_of_variables_ - (horizon_ + 1) + 2);
243
244 index_style = IndexStyleEnum::C_STYLE;
245 ADEBUG << "get_nlp_info out";
246 return true;
247}
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:2778

◆ get_optimization_results()

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

实现了 apollo::planning::DistanceApproachInterface.

在文件 distance_approach_ipopt_cuda_interface.cc 第 2375 行定义.

2380 {
2381 ADEBUG << "get_optimization_results";
2382 *state_result = state_result_;
2383 *control_result = control_result_;
2384 *time_result = time_result_;
2385 *dual_l_result = dual_l_result_;
2386 *dual_n_result = dual_n_result_;
2387
2388 if (!distance_approach_config_.enable_initial_final_check()) {
2389 return;
2390 }
2391 CHECK_EQ(state_result_.cols(), xWS_.cols());
2392 CHECK_EQ(state_result_.rows(), xWS_.rows());
2393 double state_diff_max = 0.0;
2394 for (int i = 0; i < horizon_ + 1; ++i) {
2395 for (int j = 0; j < 4; ++j) {
2396 state_diff_max = std::max(std::abs(xWS_(j, i) - state_result_(j, i)), state_diff_max);
2397 }
2398 }
2399
2400 // 2. control variable initialization, 2 * horizon_
2401 // CHECK_EQ(control_result_.cols(), uWS_.cols());
2402 CHECK_EQ(control_result_.rows(), uWS_.rows());
2403 double control_diff_max = 0.0;
2404 for (int i = 0; i < horizon_; ++i) {
2405 control_diff_max = std::max(std::abs(uWS_(0, i) - control_result_(0, i)), control_diff_max);
2406 control_diff_max = std::max(std::abs(uWS_(1, i) - control_result_(1, i)), control_diff_max);
2407 }
2408
2409 // 2. time scale variable initialization, horizon_ + 1, no
2410
2411 // 3. lagrange constraint l, obstacles_edges_sum_ * (horizon_+1)
2412 CHECK_EQ(dual_l_result_.cols(), l_warm_up_.cols());
2413 CHECK_EQ(dual_l_result_.rows(), l_warm_up_.rows());
2414 double l_diff_max = 0.0;
2415 for (int i = 0; i < horizon_ + 1; ++i) {
2416 for (int j = 0; j < obstacles_edges_sum_; ++j) {
2417 l_diff_max = std::max(std::abs(l_warm_up_(j, i) - dual_l_result_(j, i)), l_diff_max);
2418 }
2419 }
2420
2421 // 4. lagrange constraint m, 4*obstacles_num * (horizon_+1)
2422 CHECK_EQ(n_warm_up_.cols(), dual_n_result_.cols());
2423 CHECK_EQ(n_warm_up_.rows(), dual_n_result_.rows());
2424 double n_diff_max = 0.0;
2425 for (int i = 0; i < horizon_ + 1; ++i) {
2426 for (int j = 0; j < 4 * obstacles_num_; ++j) {
2427 n_diff_max = std::max(std::abs(n_warm_up_(j, i) - dual_n_result_(j, i)), n_diff_max);
2428 }
2429 }
2430
2431 ADEBUG << "state_diff_max: " << state_diff_max;
2432 ADEBUG << "control_diff_max: " << control_diff_max;
2433 ADEBUG << "dual_l_diff_max: " << l_diff_max;
2434 ADEBUG << "dual_n_diff_max: " << n_diff_max;
2435}

◆ get_starting_point()

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

Method to return the starting point for the algorithm

实现了 apollo::planning::DistanceApproachInterface.

在文件 distance_approach_ipopt_cuda_interface.cc 第 465 行定义.

474 {
475 ADEBUG << "get_starting_point";
476 ACHECK(init_x) << "Warm start init_x setting failed";
477
478 CHECK_EQ(horizon_, uWS_.cols());
479 CHECK_EQ(horizon_ + 1, xWS_.cols());
480
481 // 1. state variables 4 * (horizon_ + 1)
482 for (int i = 0; i < horizon_ + 1; ++i) {
483 int index = i * 4;
484 for (int j = 0; j < 4; ++j) {
485 x[index + j] = xWS_(j, i);
486 }
487 }
488
489 // 2. control variable initialization, 2 * horizon_
490 for (int i = 0; i < horizon_; ++i) {
491 int index = i * 2;
492 x[control_start_index_ + index] = uWS_(0, i);
493 x[control_start_index_ + index + 1] = uWS_(1, i);
494 }
495
496 // 2. time scale variable initialization, horizon_ + 1
497 for (int i = 0; i < horizon_ + 1; ++i) {
498 x[time_start_index_ + i] = 0.5 * (min_time_sample_scaling_ + max_time_sample_scaling_);
499 }
500
501 // 3. lagrange constraint l, obstacles_edges_sum_ * (horizon_+1)
502 for (int i = 0; i < horizon_ + 1; ++i) {
503 int index = i * obstacles_edges_sum_;
504 for (int j = 0; j < obstacles_edges_sum_; ++j) {
505 x[l_start_index_ + index + j] = l_warm_up_(j, i);
506 }
507 }
508
509 // 4. lagrange constraint m, 4*obstacles_num * (horizon_+1)
510 for (int i = 0; i < horizon_ + 1; ++i) {
511 int index = i * 4 * obstacles_num_;
512 for (int j = 0; j < 4 * obstacles_num_; ++j) {
513 x[n_start_index_ + index + j] = n_warm_up_(j, i);
514 }
515 }
516 ADEBUG << "get_starting_point out";
517 return true;
518}
该类的文档由以下文件生成: