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

#include <distance_approach_ipopt_relax_end_slack_interface.h>

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

Public 成员函数

 DistanceApproachIPOPTRelaxEndSlackInterface (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 &s_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 ~DistanceApproachIPOPTRelaxEndSlackInterface ()=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_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) override
 
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 override
 
template<class T >
void eval_obj (int n, const T *x, T *obj_value)
 Template to return the objective value
 
template<class T >
void eval_constraints (int n, const T *x, int m, T *g)
 Template to compute constraints
 
void generate_tapes (int n, int m, int *nnz_jac_g, int *nnz_h_lag)
 Method to generate the required tapes by ADOL-C
 
 DistanceApproachIPOPTRelaxEndSlackInterface (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 &s_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_relax_end_slack_interface.h 第 58 行定义.

构造及析构函数说明

◆ DistanceApproachIPOPTRelaxEndSlackInterface() [1/2]

apollo::planning::DistanceApproachIPOPTRelaxEndSlackInterface::DistanceApproachIPOPTRelaxEndSlackInterface ( 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 &  s_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_relax_end_slack_interface.cc 第 25 行定义.

42 :
43 ts_(ts),
44 ego_(ego),
45 xWS_(xWS),
46 uWS_(uWS),
47 l_warm_up_(l_warm_up),
48 n_warm_up_(n_warm_up),
49 slack_warm_up_(s_warm_up),
50 x0_(x0),
51 xf_(xf),
52 last_time_u_(last_time_u),
53 XYbounds_(XYbounds),
54 obstacles_edges_num_(obstacles_edges_num),
55 obstacles_A_(obstacles_A),
56 obstacles_b_(obstacles_b) {
57 ACHECK(horizon < std::numeric_limits<int>::max()) << "Invalid cast on horizon in open space planner";
58 horizon_ = static_cast<int>(horizon);
59 ACHECK(obstacles_num < std::numeric_limits<int>::max()) << "Invalid cast on obstacles_num in open space planner";
60
61 // TODO(RHe): use relax slacks from dual warm up
62 // slack_warm_up_ = Eigen::MatrixXd::Zero(obstacles_num, horizon_ + 1);
63
64 obstacles_num_ = static_cast<int>(obstacles_num);
65 w_ev_ = ego_(1, 0) + ego_(3, 0);
66 l_ev_ = ego_(0, 0) + ego_(2, 0);
67 g_ = {l_ev_ / 2, w_ev_ / 2, l_ev_ / 2, w_ev_ / 2};
68 offset_ = (ego_(0, 0) + ego_(2, 0)) / 2 - ego_(2, 0);
69 obstacles_edges_sum_ = obstacles_edges_num_.sum();
70 state_result_ = Eigen::MatrixXd::Zero(4, horizon_ + 1);
71 dual_l_result_ = Eigen::MatrixXd::Zero(obstacles_edges_sum_, horizon_ + 1);
72 dual_n_result_ = Eigen::MatrixXd::Zero(4 * obstacles_num_, horizon_ + 1);
73 slack_result_ = Eigen::MatrixXd::Zero(obstacles_num_, horizon_ + 1);
74 control_result_ = Eigen::MatrixXd::Zero(2, horizon_);
75 time_result_ = Eigen::MatrixXd::Zero(1, horizon_);
76
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 slack_index_ = n_start_index_ + 4 * obstacles_num_ * (horizon_ + 1);
83
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 weight_end_state_ = distance_approach_config_.weight_end_state();
98 weight_slack_ = distance_approach_config_.weight_slack();
99
100 min_safety_distance_ = distance_approach_config_.min_safety_distance();
101 max_steer_angle_ = vehicle_param_.max_steer_angle() / vehicle_param_.steer_ratio();
102 max_speed_forward_ = distance_approach_config_.max_speed_forward();
103 max_speed_reverse_ = distance_approach_config_.max_speed_reverse();
104 max_acceleration_forward_ = distance_approach_config_.max_acceleration_forward();
105 max_acceleration_reverse_ = distance_approach_config_.max_acceleration_reverse();
106 min_time_sample_scaling_ = distance_approach_config_.min_time_sample_scaling();
107 max_time_sample_scaling_ = distance_approach_config_.max_time_sample_scaling();
108 max_steer_rate_ = vehicle_param_.max_steer_angle_rate() / vehicle_param_.steer_ratio();
109 use_fix_time_ = distance_approach_config_.use_fix_time();
110 wheelbase_ = vehicle_param_.wheel_base();
111 enable_constraint_check_ = distance_approach_config_.enable_constraint_check();
112 enable_jacobian_ad_ = true;
113}
ACHECK
#define ACHECK(cond)
Definition log.h:80

◆ ~DistanceApproachIPOPTRelaxEndSlackInterface()

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

◆ DistanceApproachIPOPTRelaxEndSlackInterface() [2/2]

apollo::planning::DistanceApproachIPOPTRelaxEndSlackInterface::DistanceApproachIPOPTRelaxEndSlackInterface ( 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 &  s_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_relax_end_slack_interface.cc 第 115 行定义.

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

成员函数说明

◆ check_g()

bool apollo::planning::DistanceApproachIPOPTRelaxEndSlackInterface::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_relax_end_slack_interface.cc 第 1078 行定义.

1078 {
1079 int kN = n;
1080 int kM = m;
1081 double x_u_tmp[kN];
1082 double x_l_tmp[kN];
1083 double g_u_tmp[kM];
1084 double g_l_tmp[kM];
1085
1086 get_bounds_info(n, x_l_tmp, x_u_tmp, m, g_l_tmp, g_u_tmp);
1087
1088 const double delta_v = 1e-4;
1089 for (int idx = 0; idx < n; ++idx) {
1090 x_u_tmp[idx] = x_u_tmp[idx] + delta_v;
1091 x_l_tmp[idx] = x_l_tmp[idx] - delta_v;
1092 if (x[idx] > x_u_tmp[idx] || x[idx] < x_l_tmp[idx]) {
1093 AINFO << "x idx unfeasible: " << idx << ", x: " << x[idx] << ", lower: " << x_l_tmp[idx]
1094 << ", upper: " << x_u_tmp[idx];
1095 }
1096 }
1097
1098 // m1 : dynamics constatins
1099 int m1 = 4 * horizon_;
1100
1101 // m2 : control rate constraints (only steering)
1102 int m2 = m1 + horizon_;
1103
1104 // m3 : sampling time equality constraints
1105 int m3 = m2 + horizon_;
1106
1107 // m4 : obstacle constraints
1108 int m4 = m3 + 4 * obstacles_num_ * (horizon_ + 1);
1109
1110 // 5. load variable bounds as constraints
1111 // start configuration
1112 int m5 = m4 + 3 + 1;
1113
1114 // constraints on x,y,v
1115 int m6 = m5 + 3 * (horizon_ - 1);
1116
1117 // end configuration
1118 int m7 = m6 + 3 + 1;
1119
1120 // control variable bnd
1121 int m8 = m7 + 2 * horizon_;
1122
1123 // time interval variable bnd
1124 int m9 = m8 + (horizon_ + 1);
1125
1126 // lambda_horizon_
1127 int m10 = m9 + lambda_horizon_;
1128
1129 // miu_horizon_
1130 int m11 = m10 + miu_horizon_;
1131
1132 // slack_horizon_
1133 int m12 = m11 + slack_horizon_;
1134
1135 CHECK_EQ(m12, num_of_constraints_);
1136
1137 AINFO << "dynamics constatins to: " << m1;
1138 AINFO << "control rate constraints (only steering) to: " << m2;
1139 AINFO << "sampling time equality constraints to: " << m3;
1140 AINFO << "obstacle constraints to: " << m4;
1141 AINFO << "start conf constraints to: " << m5;
1142 AINFO << "constraints on x,y,v to: " << m6;
1143 AINFO << "end constraints to: " << m7;
1144 AINFO << "control bnd to: " << m8;
1145 AINFO << "time interval constraints to: " << m9;
1146 AINFO << "lambda constraints to: " << m10;
1147 AINFO << "miu constraints to: " << m11;
1148 AINFO << "slack constraint to: " << m12;
1149 AINFO << "total constraints: " << num_of_constraints_;
1150
1151 for (int idx = 0; idx < m; ++idx) {
1152 if (g[idx] > g_u_tmp[idx] + delta_v || g[idx] < g_l_tmp[idx] - delta_v) {
1153 AINFO << "constratins idx unfeasible: " << idx << ", g: " << g[idx] << ", lower: " << g_l_tmp[idx]
1154 << ", upper: " << g_u_tmp[idx];
1155 }
1156 }
1157 return true;
1158}
apollo::planning::DistanceApproachIPOPTRelaxEndSlackInterface::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_relax_end_slack_interface.cc:255
AINFO
#define AINFO
Definition log.h:42

◆ eval_constraints()

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

Template to compute constraints

在文件 distance_approach_ipopt_relax_end_slack_interface.cc 第 875 行定义.

875 {
876 // state start index
877 int state_index = state_start_index_;
878
879 // control start index.
880 int control_index = control_start_index_;
881
882 // time start index
883 int time_index = time_start_index_;
884
885 int constraint_index = 0;
886
887 // 1. state constraints 4 * [0, horizons-1]
888 for (int i = 0; i < horizon_; ++i) {
889 // x1
890 g[constraint_index] = x[state_index + 4]
891 - (x[state_index]
892 + ts_ * x[time_index] * (x[state_index + 3] + ts_ * x[time_index] * 0.5 * x[control_index + 1])
893 * cos(x[state_index + 2]
894 + ts_ * x[time_index] * 0.5 * x[state_index + 3] * tan(x[control_index])
895 / wheelbase_));
896
897 // x2
898 g[constraint_index + 1] = x[state_index + 5]
899 - (x[state_index + 1]
900 + ts_ * x[time_index] * (x[state_index + 3] + ts_ * x[time_index] * 0.5 * x[control_index + 1])
901 * sin(x[state_index + 2]
902 + ts_ * x[time_index] * 0.5 * x[state_index + 3] * tan(x[control_index])
903 / wheelbase_));
904
905 // x3
906 g[constraint_index + 2] = x[state_index + 6]
907 - (x[state_index + 2]
908 + ts_ * x[time_index] * (x[state_index + 3] + ts_ * x[time_index] * 0.5 * x[control_index + 1])
909 * tan(x[control_index]) / wheelbase_);
910
911 // x4
912 g[constraint_index + 3]
913 = x[state_index + 7] - (x[state_index + 3] + ts_ * x[time_index] * x[control_index + 1]);
914
915 control_index += 2;
916 constraint_index += 4;
917 time_index++;
918 state_index += 4;
919 }
920
921 ADEBUG << "constraint_index after adding Euler forward dynamics constraints "
922 "updated: "
923 << constraint_index;
924
925 // 2. Control rate limit constraints, 1 * [0, horizons-1], only apply
926 // steering rate as of now
927 control_index = control_start_index_;
928 time_index = time_start_index_;
929
930 // First rate is compare first with stitch point
931 g[constraint_index] = (x[control_index] - last_time_u_(0, 0)) / x[time_index] / ts_;
932 control_index += 2;
933 constraint_index++;
934 time_index++;
935
936 for (int i = 1; i < horizon_; ++i) {
937 g[constraint_index] = (x[control_index] - x[control_index - 2]) / x[time_index] / ts_;
938 constraint_index++;
939 control_index += 2;
940 time_index++;
941 }
942
943 // 3. Time constraints 1 * [0, horizons-1]
944 time_index = time_start_index_;
945 for (int i = 0; i < horizon_; ++i) {
946 g[constraint_index] = x[time_index + 1] - x[time_index];
947 constraint_index++;
948 time_index++;
949 }
950
951 ADEBUG << "constraint_index after adding time constraints "
952 "updated: "
953 << constraint_index;
954
955 // 4. Three obstacles related equal constraints, one equality constraints,
956 // [0, horizon_] * [0, obstacles_num_-1] * 4
957 state_index = state_start_index_;
958 int l_index = l_start_index_;
959 int n_index = n_start_index_;
960 int s_index = slack_index_;
961
962 for (int i = 0; i < horizon_ + 1; ++i) {
963 int edges_counter = 0;
964 for (int j = 0; j < obstacles_num_; ++j) {
965 int current_edges_num = obstacles_edges_num_(j, 0);
966 Eigen::MatrixXd Aj = obstacles_A_.block(edges_counter, 0, current_edges_num, 2);
967 Eigen::MatrixXd bj = obstacles_b_.block(edges_counter, 0, current_edges_num, 1);
968
969 // norm(A* lambda) <= 1
970 T tmp1 = 0.0;
971 T tmp2 = 0.0;
972 for (int k = 0; k < current_edges_num; ++k) {
973 // TODO(QiL) : replace this one directly with x
974 tmp1 += Aj(k, 0) * x[l_index + k];
975 tmp2 += Aj(k, 1) * x[l_index + k];
976 }
977 g[constraint_index] = tmp1 * tmp1 + tmp2 * tmp2;
978
979 // G' * mu + R' * lambda == 0
980 g[constraint_index + 1]
981 = x[n_index] - x[n_index + 2] + cos(x[state_index + 2]) * tmp1 + sin(x[state_index + 2]) * tmp2;
982
983 g[constraint_index + 2]
984 = x[n_index + 1] - x[n_index + 3] - sin(x[state_index + 2]) * tmp1 + cos(x[state_index + 2]) * tmp2;
985
986 // -g'*mu + (A*t - b)*lambda + d > 0
987 T tmp3 = 0.0;
988 for (int k = 0; k < 4; ++k) {
989 tmp3 += -g_[k] * x[n_index + k];
990 }
991
992 T tmp4 = 0.0;
993 for (int k = 0; k < current_edges_num; ++k) {
994 tmp4 += bj(k, 0) * x[l_index + k];
995 }
996
997 g[constraint_index + 3] = tmp3 + (x[state_index] + cos(x[state_index + 2]) * offset_) * tmp1
998 + (x[state_index + 1] + sin(x[state_index + 2]) * offset_) * tmp2 - tmp4 + x[s_index];
999
1000 // Update index
1001 edges_counter += current_edges_num;
1002 l_index += current_edges_num;
1003 n_index += 4;
1004 constraint_index += 4;
1005 ++s_index;
1006 }
1007 state_index += 4;
1008 }
1009 ADEBUG << "constraint_index after obstacles avoidance constraints "
1010 "updated: "
1011 << constraint_index;
1012
1013 // 5. load variable bounds as constraints
1014 state_index = state_start_index_;
1015 control_index = control_start_index_;
1016 time_index = time_start_index_;
1017 l_index = l_start_index_;
1018 n_index = n_start_index_;
1019 s_index = slack_index_;
1020
1021 // start configuration
1022 g[constraint_index] = x[state_index];
1023 g[constraint_index + 1] = x[state_index + 1];
1024 g[constraint_index + 2] = x[state_index + 2];
1025 g[constraint_index + 3] = x[state_index + 3];
1026 constraint_index += 4;
1027 state_index += 4;
1028
1029 // constraints on x,y,v
1030 for (int i = 1; i < horizon_; ++i) {
1031 g[constraint_index] = x[state_index];
1032 g[constraint_index + 1] = x[state_index + 1];
1033 g[constraint_index + 2] = x[state_index + 3];
1034 constraint_index += 3;
1035 state_index += 4;
1036 }
1037
1038 // end configuration
1039 g[constraint_index] = x[state_index];
1040 g[constraint_index + 1] = x[state_index + 1];
1041 g[constraint_index + 2] = x[state_index + 2];
1042 g[constraint_index + 3] = x[state_index + 3];
1043 constraint_index += 4;
1044 state_index += 4;
1045
1046 for (int i = 0; i < horizon_; ++i) {
1047 g[constraint_index] = x[control_index];
1048 g[constraint_index + 1] = x[control_index + 1];
1049 constraint_index += 2;
1050 control_index += 2;
1051 }
1052
1053 for (int i = 0; i < horizon_ + 1; ++i) {
1054 g[constraint_index] = x[time_index];
1055 ++constraint_index;
1056 ++time_index;
1057 }
1058
1059 for (int i = 0; i < lambda_horizon_; ++i) {
1060 g[constraint_index] = x[l_index];
1061 ++constraint_index;
1062 ++l_index;
1063 }
1064
1065 for (int i = 0; i < miu_horizon_; ++i) {
1066 g[constraint_index] = x[n_index];
1067 ++constraint_index;
1068 ++n_index;
1069 }
1070
1071 for (int i = 0; i < slack_horizon_; ++i) {
1072 g[constraint_index] = x[s_index];
1073 ++constraint_index;
1074 ++s_index;
1075 }
1076}
ADEBUG
#define ADEBUG
Definition log.h:41
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::DistanceApproachIPOPTRelaxEndSlackInterface::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_relax_end_slack_interface.cc 第 551 行定义.

551 {
552 eval_obj(n, x, &obj_value);
553 return true;
554}
apollo::planning::DistanceApproachIPOPTRelaxEndSlackInterface::eval_obj
void eval_obj(int n, const T *x, T *obj_value)
Template to return the objective value
Definition distance_approach_ipopt_relax_end_slack_interface.cc:803

◆ eval_g()

bool apollo::planning::DistanceApproachIPOPTRelaxEndSlackInterface::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_relax_end_slack_interface.cc 第 561 行定义.

561 {
562 eval_constraints(n, x, m, g);
563 // if (enable_constraint_check_) check_g(n, x, m, g);
564 return true;
565}
apollo::planning::DistanceApproachIPOPTRelaxEndSlackInterface::eval_constraints
void eval_constraints(int n, const T *x, int m, T *g)
Template to compute constraints
Definition distance_approach_ipopt_relax_end_slack_interface.cc:875

◆ eval_grad_f()

bool apollo::planning::DistanceApproachIPOPTRelaxEndSlackInterface::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_relax_end_slack_interface.cc 第 556 行定义.

556 {
557 gradient(tag_f, n, x, grad_f);
558 return true;
559}
tag_f
#define tag_f
Definition cos_theta_ipopt_interface.h:36

◆ eval_h()

bool apollo::planning::DistanceApproachIPOPTRelaxEndSlackInterface::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_relax_end_slack_interface.cc 第 609 行定义.

620 {
621 if (values == nullptr) {
622 // return the structure. This is a symmetric matrix, fill the lower left
623 // triangle only.
624
625 for (int idx = 0; idx < nnz_L; idx++) {
626 iRow[idx] = rind_L[idx];
627 jCol[idx] = cind_L[idx];
628 }
629 } else {
630 // return the values. This is a symmetric matrix, fill the lower left
631 // triangle only
632
633 obj_lam[0] = obj_factor;
634
635 for (int idx = 0; idx < m; idx++) {
636 obj_lam[1 + idx] = lambda[idx];
637 }
638
639 set_param_vec(tag_L, m + 1, obj_lam);
640 sparse_hess(tag_L, n, 1, const_cast<double*>(x), &nnz_L, &rind_L, &cind_L, &hessval, options_L);
641
642 for (int idx = 0; idx < nnz_L; idx++) {
643 values[idx] = hessval[idx];
644 }
645 }
646
647 return true;
648}
tag_L
#define tag_L
Definition cos_theta_ipopt_interface.h:38

◆ eval_jac_g()

bool apollo::planning::DistanceApproachIPOPTRelaxEndSlackInterface::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_relax_end_slack_interface.cc 第 567 行定义.

575 {
576 if (enable_jacobian_ad_) {
577 if (values == nullptr) {
578 // return the structure of the jacobian
579 for (int idx = 0; idx < nnz_jac; idx++) {
580 iRow[idx] = rind_g[idx];
581 jCol[idx] = cind_g[idx];
582 }
583 } else {
584 // return the values of the jacobian of the constraints
585 sparse_jac(tag_g, m, n, 1, x, &nnz_jac, &rind_g, &cind_g, &jacval, options_g);
586 for (int idx = 0; idx < nnz_jac; idx++) {
587 values[idx] = jacval[idx];
588 }
589 }
590 return true;
591 } else {
592 return eval_jac_g_ser(n, x, new_x, m, nele_jac, iRow, jCol, values);
593 }
594}
apollo::planning::DistanceApproachIPOPTRelaxEndSlackInterface::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) override
Definition distance_approach_ipopt_relax_end_slack_interface.cc:596
tag_g
#define tag_g
Definition cos_theta_ipopt_interface.h:37

◆ eval_jac_g_ser()

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

实现了 apollo::planning::DistanceApproachInterface.

在文件 distance_approach_ipopt_relax_end_slack_interface.cc 第 596 行定义.

604 {
605 AERROR << "NOT READY";
606 return false;
607}
AERROR
#define AERROR
Definition log.h:44

◆ eval_obj()

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

Template to return the objective value

在文件 distance_approach_ipopt_relax_end_slack_interface.cc 第 803 行定义.

803 {
804 // Objective is :
805 // min control inputs
806 // min input rate
807 // min time (if the time step is not fixed)
808 // regularization wrt warm start trajectory
809 DCHECK(ts_ != 0) << "ts in distance_approach_ is 0";
810 int control_index = control_start_index_;
811 int time_index = time_start_index_;
812 int state_index = state_start_index_;
813
814 // TODO(QiL): Initial implementation towards earlier understanding and debug
815 // purpose, later code refine towards improving efficiency
816
817 *obj_value = 0.0;
818 // 1. objective to minimize state diff to warm up
819 for (int i = 0; i < horizon_ + 1; ++i) {
820 T x1_diff = x[state_index] - xWS_(0, i);
821 T x2_diff = x[state_index + 1] - xWS_(1, i);
822 T x3_diff = x[state_index + 2] - xWS_(2, i);
823 T x4_abs = x[state_index + 3];
824 *obj_value += weight_state_x_ * x1_diff * x1_diff + weight_state_y_ * x2_diff * x2_diff
825 + weight_state_phi_ * x3_diff * x3_diff + weight_state_v_ * x4_abs * x4_abs;
826 state_index += 4;
827 }
828
829 // 2. objective to minimize u square
830 for (int i = 0; i < horizon_; ++i) {
831 *obj_value += weight_input_steer_ * x[control_index] * x[control_index]
832 + weight_input_a_ * x[control_index + 1] * x[control_index + 1];
833 control_index += 2;
834 }
835
836 // 3. objective to minimize input change rate for first horizon
837 control_index = control_start_index_;
838 T last_time_steer_rate = (x[control_index] - last_time_u_(0, 0)) / x[time_index] / ts_;
839 T last_time_a_rate = (x[control_index + 1] - last_time_u_(1, 0)) / x[time_index] / ts_;
840 *obj_value += weight_stitching_steer_ * last_time_steer_rate * last_time_steer_rate
841 + weight_stitching_a_ * last_time_a_rate * last_time_a_rate;
842
843 // 4. objective to minimize input change rates, [0- horizon_ -2]
844 time_index++;
845 for (int i = 0; i < horizon_ - 1; ++i) {
846 T steering_rate = (x[control_index + 2] - x[control_index]) / x[time_index] / ts_;
847 T a_rate = (x[control_index + 3] - x[control_index + 1]) / x[time_index] / ts_;
848 *obj_value += weight_rate_steer_ * steering_rate * steering_rate + weight_rate_a_ * a_rate * a_rate;
849 control_index += 2;
850 time_index++;
851 }
852
853 // 5. objective to minimize total time [0, horizon_]
854 time_index = time_start_index_;
855 for (int i = 0; i < horizon_ + 1; ++i) {
856 T first_order_penalty = weight_first_order_time_ * x[time_index];
857 T second_order_penalty = weight_second_order_time_ * x[time_index] * x[time_index];
858 *obj_value += first_order_penalty + second_order_penalty;
859 time_index++;
860 }
861
862 // 6. end state constraints
863 for (int i = 0; i < 4; ++i) {
864 *obj_value += weight_end_state_ * (x[state_start_index_ + 4 * horizon_ + i] - xf_(i, 0))
865 * (x[state_start_index_ + 4 * horizon_ + i] - xf_(i, 0));
866 }
867
868 // 7. slack variables
869 for (int i = 0; i < slack_horizon_; ++i) {
870 *obj_value += weight_slack_ * x[slack_index_ + i];
871 }
872}

◆ finalize_solution()

void apollo::planning::DistanceApproachIPOPTRelaxEndSlackInterface::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_relax_end_slack_interface.cc 第 650 行定义.

661 {
662 int state_index = state_start_index_;
663 int control_index = control_start_index_;
664 int time_index = time_start_index_;
665 int dual_l_index = l_start_index_;
666 int dual_n_index = n_start_index_;
667 int slack_index = slack_index_;
668
669 // enable_constraint_check_: for debug only
670 if (enable_constraint_check_) {
671 ADEBUG << "final resolution constraint checking";
672 check_g(n, x, m, g);
673 }
674 // 1. state variables, 4 * [0, horizon]
675 // 2. control variables, 2 * [0, horizon_-1]
676 // 3. sampling time variables, 1 * [0, horizon_]
677 // 4. dual_l, obstacles_edges_sum_ * [0, horizon]
678 // 5. dual_n, obstacles_num * [0, horizon]
679 // 6. slack_, obstacles_num * [0, horizon]
680 for (int i = 0; i < horizon_; ++i) {
681 state_result_(0, i) = x[state_index];
682 state_result_(1, i) = x[state_index + 1];
683 state_result_(2, i) = x[state_index + 2];
684 state_result_(3, i) = x[state_index + 3];
685 control_result_(0, i) = x[control_index];
686 control_result_(1, i) = x[control_index + 1];
687 time_result_(0, i) = ts_ * x[time_index];
688
689 for (int j = 0; j < obstacles_edges_sum_; ++j) {
690 dual_l_result_(j, i) = x[dual_l_index + j];
691 }
692 for (int k = 0; k < 4 * obstacles_num_; ++k) {
693 dual_n_result_(k, i) = x[dual_n_index + k];
694 }
695 for (int l = 0; l < obstacles_num_; ++l) {
696 slack_result_(l, i) = x[slack_index + l];
697 }
698
699 state_index += 4;
700 control_index += 2;
701 time_index++;
702 dual_l_index += obstacles_edges_sum_;
703 dual_n_index += 4 * obstacles_num_;
704 slack_index += obstacles_num_;
705 }
706 state_result_(0, 0) = x0_(0, 0);
707 state_result_(1, 0) = x0_(1, 0);
708 state_result_(2, 0) = x0_(2, 0);
709 state_result_(3, 0) = x0_(3, 0);
710 // push back last horizon for states
711 state_result_(0, horizon_) = xf_(0, 0);
712 state_result_(1, horizon_) = xf_(1, 0);
713 state_result_(2, horizon_) = xf_(2, 0);
714 state_result_(3, horizon_) = xf_(3, 0);
715
716 for (int j = 0; j < obstacles_edges_sum_; ++j) {
717 dual_l_result_(j, horizon_) = x[dual_l_index + j];
718 }
719 for (int k = 0; k < 4 * obstacles_num_; ++k) {
720 dual_n_result_(k, horizon_) = x[dual_n_index + k];
721 }
722 for (int l = 0; l < obstacles_num_; ++l) {
723 slack_result_(l, horizon_) = x[slack_index + l];
724 }
725
726 // memory deallocation of ADOL-C variables
727 delete[] obj_lam;
728 if (enable_jacobian_ad_) {
729 free(rind_g);
730 free(cind_g);
731 free(jacval);
732 }
733 free(rind_L);
734 free(cind_L);
735 free(hessval);
736}
apollo::planning::DistanceApproachIPOPTRelaxEndSlackInterface::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_relax_end_slack_interface.cc:1078

◆ generate_tapes()

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

Method to generate the required tapes by ADOL-C

在文件 distance_approach_ipopt_relax_end_slack_interface.cc 第 1160 行定义.

1160 {
1161 std::vector<double> xp(n);
1162 std::vector<double> lamp(m);
1163 std::vector<double> zl(m);
1164 std::vector<double> zu(m);
1165 std::vector<adouble> xa(n);
1166 std::vector<adouble> g(m);
1167 std::vector<double> lam(m);
1168
1169 double sig = 0.0;
1170 adouble obj_value;
1171 double dummy = 0.0;
1172 obj_lam = new double[m + 1];
1173 get_starting_point(n, 1, &xp[0], 0, &zl[0], &zu[0], m, 0, &lamp[0]);
1174
1175 trace_on(tag_f);
1176 for (int idx = 0; idx < n; idx++) {
1177 xa[idx] <<= xp[idx];
1178 }
1179 eval_obj(n, &xa[0], &obj_value);
1180 obj_value >>= dummy;
1181 trace_off();
1182
1183 trace_on(tag_g);
1184 for (int idx = 0; idx < n; idx++) {
1185 xa[idx] <<= xp[idx];
1186 }
1187 eval_constraints(n, &xa[0], m, &g[0]);
1188 for (int idx = 0; idx < m; idx++) {
1189 g[idx] >>= dummy;
1190 }
1191 trace_off();
1192
1193 trace_on(tag_L);
1194 for (int idx = 0; idx < n; idx++) {
1195 xa[idx] <<= xp[idx];
1196 }
1197 for (int idx = 0; idx < m; idx++) {
1198 lam[idx] = 1.0;
1199 }
1200 sig = 1.0;
1201 eval_obj(n, &xa[0], &obj_value);
1202 obj_value *= mkparam(sig);
1203 eval_constraints(n, &xa[0], m, &g[0]);
1204 for (int idx = 0; idx < m; idx++) {
1205 obj_value += g[idx] * mkparam(lam[idx]);
1206 }
1207 obj_value >>= dummy;
1208
1209 trace_off();
1210
1211 if (enable_jacobian_ad_) {
1212 rind_g = nullptr;
1213 cind_g = nullptr;
1214 jacval = nullptr;
1215 options_g[0] = 0; /* sparsity pattern by index domains (default) */
1216 options_g[1] = 0; /* safe mode (default) */
1217 options_g[2] = 0;
1218 options_g[3] = 0; /* column compression (default) */
1219
1220 sparse_jac(tag_g, m, n, 0, &xp[0], &nnz_jac, &rind_g, &cind_g, &jacval, options_g);
1221 *nnz_jac_g = nnz_jac;
1222 }
1223
1224 rind_L = nullptr;
1225 cind_L = nullptr;
1226 hessval = nullptr;
1227 options_L[0] = 0;
1228 options_L[1] = 1;
1229
1230 sparse_hess(tag_L, n, 0, &xp[0], &nnz_L, &rind_L, &cind_L, &hessval, options_L);
1231 *nnz_h_lag = nnz_L;
1232}
apollo::planning::DistanceApproachIPOPTRelaxEndSlackInterface::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_relax_end_slack_interface.cc:487

◆ get_bounds_info()

bool apollo::planning::DistanceApproachIPOPTRelaxEndSlackInterface::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_relax_end_slack_interface.cc 第 255 行定义.

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

◆ get_nlp_info()

bool apollo::planning::DistanceApproachIPOPTRelaxEndSlackInterface::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_relax_end_slack_interface.cc 第 205 行定义.

210 {
211 ADEBUG << "get_nlp_info";
212 // n1 : states variables, 4 * (N+1)
213 int n1 = 4 * (horizon_ + 1);
214 ADEBUG << "n1: " << n1;
215 // n2 : control inputs variables
216 int n2 = 2 * horizon_;
217 ADEBUG << "n2: " << n2;
218 // n3 : sampling time variables
219 int n3 = horizon_ + 1;
220 ADEBUG << "n3: " << n3;
221 // n4 : dual multiplier associated with obstacle shape
222 lambda_horizon_ = obstacles_edges_num_.sum() * (horizon_ + 1);
223 ADEBUG << "lambda_horizon_: " << lambda_horizon_;
224 // n5 : dual multipier associated with car shape, obstacles_num*4 * (N+1)
225 miu_horizon_ = obstacles_num_ * 4 * (horizon_ + 1);
226 ADEBUG << "miu_horizon_: " << miu_horizon_;
227 // n6 : slack variables related to safety min distance
228 slack_horizon_ = obstacles_num_ * (horizon_ + 1);
229
230 // m1 : dynamics constatins
231 int m1 = 4 * horizon_;
232 // m2 : control rate constraints (only steering)
233 int m2 = horizon_;
234 // m3 : sampling time equality constraints
235 int m3 = horizon_;
236 // m4 : obstacle constraints
237 int m4 = 4 * obstacles_num_ * (horizon_ + 1);
238
239 num_of_variables_ = n1 + n2 + n3 + lambda_horizon_ + miu_horizon_ + slack_horizon_;
240 num_of_constraints_ = m1 + m2 + m3 + m4 + (num_of_variables_ - (horizon_ + 1) + 2);
241
242 // number of variables
243 n = num_of_variables_;
244 ADEBUG << "num_of_variables_ " << num_of_variables_;
245 // number of constraints
246 m = num_of_constraints_;
247 ADEBUG << "num_of_constraints_ " << num_of_constraints_;
248
249 generate_tapes(n, m, &nnz_jac_g, &nnz_h_lag);
250
251 index_style = IndexStyleEnum::C_STYLE;
252 return true;
253}
apollo::planning::DistanceApproachIPOPTRelaxEndSlackInterface::generate_tapes
void generate_tapes(int n, int m, int *nnz_jac_g, int *nnz_h_lag)
Method to generate the required tapes by ADOL-C
Definition distance_approach_ipopt_relax_end_slack_interface.cc:1160

◆ get_optimization_results()

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

实现了 apollo::planning::DistanceApproachInterface.

在文件 distance_approach_ipopt_relax_end_slack_interface.cc 第 738 行定义.

743 {
744 // TODO(RHe): extract slack variables for further study
745 ADEBUG << "get_optimization_results";
746 *state_result = state_result_;
747 *control_result = control_result_;
748 *time_result = time_result_;
749 *dual_l_result = dual_l_result_;
750 *dual_n_result = dual_n_result_;
751
752 if (!distance_approach_config_.enable_initial_final_check()) {
753 return;
754 }
755 CHECK_EQ(state_result_.cols(), xWS_.cols());
756 CHECK_EQ(state_result_.rows(), xWS_.rows());
757 double state_diff_max = 0.0;
758 for (int i = 0; i < horizon_ + 1; ++i) {
759 for (int j = 0; j < 4; ++j) {
760 state_diff_max = std::max(std::abs(xWS_(j, i) - state_result_(j, i)), state_diff_max);
761 }
762 }
763
764 // 2. control variable initialization, 2 * horizon_
765 CHECK_EQ(control_result_.cols(), uWS_.cols());
766 CHECK_EQ(control_result_.rows(), uWS_.rows());
767 double control_diff_max = 0.0;
768 for (int i = 0; i < horizon_; ++i) {
769 control_diff_max = std::max(std::abs(uWS_(0, i) - control_result_(0, i)), control_diff_max);
770 control_diff_max = std::max(std::abs(uWS_(1, i) - control_result_(1, i)), control_diff_max);
771 }
772
773 // 2. time scale variable initialization, horizon_ + 1
774
775 // 3. lagrange constraint l, obstacles_edges_sum_ * (horizon_+1)
776 CHECK_EQ(dual_l_result_.cols(), l_warm_up_.cols());
777 CHECK_EQ(dual_l_result_.rows(), l_warm_up_.rows());
778 double l_diff_max = 0.0;
779 for (int i = 0; i < horizon_ + 1; ++i) {
780 for (int j = 0; j < obstacles_edges_sum_; ++j) {
781 l_diff_max = std::max(std::abs(l_warm_up_(j, i) - dual_l_result_(j, i)), l_diff_max);
782 }
783 }
784
785 // 4. lagrange constraint m, 4*obstacles_num * (horizon_+1)
786 CHECK_EQ(n_warm_up_.cols(), dual_n_result_.cols());
787 CHECK_EQ(n_warm_up_.rows(), dual_n_result_.rows());
788 double n_diff_max = 0.0;
789 for (int i = 0; i < horizon_ + 1; ++i) {
790 for (int j = 0; j < 4 * obstacles_num_; ++j) {
791 n_diff_max = std::max(std::abs(n_warm_up_(j, i) - dual_n_result_(j, i)), n_diff_max);
792 }
793 }
794
795 ADEBUG << "state_diff_max: " << state_diff_max;
796 ADEBUG << "control_diff_max: " << control_diff_max;
797 ADEBUG << "dual_l_diff_max: " << l_diff_max;
798 ADEBUG << "dual_n_diff_max: " << n_diff_max;
799}

◆ get_starting_point()

bool apollo::planning::DistanceApproachIPOPTRelaxEndSlackInterface::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_relax_end_slack_interface.cc 第 487 行定义.

496 {
497 ADEBUG << "get_starting_point";
498 ACHECK(init_x) << "Warm start init_x setting failed";
499
500 CHECK_EQ(horizon_, uWS_.cols());
501 CHECK_EQ(horizon_ + 1, xWS_.cols());
502
503 // 1. state variables 4 * (horizon_ + 1)
504 for (int i = 0; i < horizon_ + 1; ++i) {
505 int index = i * 4;
506 for (int j = 0; j < 4; ++j) {
507 x[index + j] = xWS_(j, i);
508 }
509 }
510
511 // 2. control variable initialization, 2 * horizon_
512 for (int i = 0; i < horizon_; ++i) {
513 int index = i * 2;
514 x[control_start_index_ + index] = uWS_(0, i);
515 x[control_start_index_ + index + 1] = uWS_(1, i);
516 }
517
518 // 2. time scale variable initialization, horizon_ + 1
519 for (int i = 0; i < horizon_ + 1; ++i) {
520 x[time_start_index_ + i] = 0.5 * (min_time_sample_scaling_ + max_time_sample_scaling_);
521 }
522
523 // 3. lagrange constraint l, obstacles_edges_sum_ * (horizon_+1)
524 for (int i = 0; i < horizon_ + 1; ++i) {
525 int index = i * obstacles_edges_sum_;
526 for (int j = 0; j < obstacles_edges_sum_; ++j) {
527 x[l_start_index_ + index + j] = l_warm_up_(j, i);
528 }
529 }
530
531 // 4. lagrange constraint m, 4*obstacles_num * (horizon_+1)
532 for (int i = 0; i < horizon_ + 1; ++i) {
533 int index = i * 4 * obstacles_num_;
534 for (int j = 0; j < 4 * obstacles_num_; ++j) {
535 x[n_start_index_ + index + j] = n_warm_up_(j, i);
536 }
537 }
538
539 // 5. slack constraint s, obstacle_num * (horizon_+1)
540 for (int i = 0; i < horizon_ + 1; ++i) {
541 int index = i * obstacles_num_;
542 for (int j = 0; j < obstacles_num_; ++j) {
543 x[slack_index_ + index + j] = slack_warm_up_(j, i);
544 }
545 }
546
547 ADEBUG << "get_starting_point out";
548 return true;
549}
该类的文档由以下文件生成: