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

#include <distance_approach_ipopt_relax_end_interface.h>

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

Public 成员函数

 DistanceApproachIPOPTRelaxEndInterface (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 ~DistanceApproachIPOPTRelaxEndInterface ()=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
 
 DistanceApproachIPOPTRelaxEndInterface (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_relax_end_interface.h 第 57 行定义.

构造及析构函数说明

◆ DistanceApproachIPOPTRelaxEndInterface() [1/2]

apollo::planning::DistanceApproachIPOPTRelaxEndInterface::DistanceApproachIPOPTRelaxEndInterface ( 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_relax_end_interface.cc 第 25 行定义.

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

◆ ~DistanceApproachIPOPTRelaxEndInterface()

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

◆ DistanceApproachIPOPTRelaxEndInterface() [2/2]

apollo::planning::DistanceApproachIPOPTRelaxEndInterface::DistanceApproachIPOPTRelaxEndInterface ( 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_relax_end_interface.cc 第 105 行定义.

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

成员函数说明

◆ check_g()

bool apollo::planning::DistanceApproachIPOPTRelaxEndInterface::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_interface.cc 第 1015 行定义.

1015 {
1016 int kN = n;
1017 int kM = m;
1018 double x_u_tmp[kN];
1019 double x_l_tmp[kN];
1020 double g_u_tmp[kM];
1021 double g_l_tmp[kM];
1022
1023 get_bounds_info(n, x_l_tmp, x_u_tmp, m, g_l_tmp, g_u_tmp);
1024
1025 const double delta_v = 1e-4;
1026 for (int idx = 0; idx < n; ++idx) {
1027 x_u_tmp[idx] = x_u_tmp[idx] + delta_v;
1028 x_l_tmp[idx] = x_l_tmp[idx] - delta_v;
1029 if (x[idx] > x_u_tmp[idx] || x[idx] < x_l_tmp[idx]) {
1030 AINFO << "x idx unfeasible: " << idx << ", x: " << x[idx] << ", lower: " << x_l_tmp[idx]
1031 << ", upper: " << x_u_tmp[idx];
1032 }
1033 }
1034
1035 // m1 : dynamics constatins
1036 int m1 = 4 * horizon_;
1037
1038 // m2 : control rate constraints (only steering)
1039 int m2 = m1 + horizon_;
1040
1041 // m3 : sampling time equality constraints
1042 int m3 = m2 + horizon_;
1043
1044 // m4 : obstacle constraints
1045 int m4 = m3 + 4 * obstacles_num_ * (horizon_ + 1);
1046
1047 // 5. load variable bounds as constraints
1048 // start configuration
1049 int m5 = m4 + 3 + 1;
1050
1051 // constraints on x,y,v
1052 int m6 = m5 + 3 * (horizon_ - 1);
1053
1054 // end configuration
1055 int m7 = m6 + 3 + 1;
1056
1057 // control variable bnd
1058 int m8 = m7 + 2 * horizon_;
1059
1060 // time interval variable bnd
1061 int m9 = m8 + (horizon_ + 1);
1062
1063 // lambda_horizon_
1064 int m10 = m9 + lambda_horizon_;
1065
1066 // miu_horizon_
1067 int m11 = m10 + miu_horizon_;
1068
1069 CHECK_EQ(m11, num_of_constraints_);
1070
1071 AINFO << "dynamics constatins to: " << m1;
1072 AINFO << "control rate constraints (only steering) to: " << m2;
1073 AINFO << "sampling time equality constraints to: " << m3;
1074 AINFO << "obstacle constraints to: " << m4;
1075 AINFO << "start conf constraints to: " << m5;
1076 AINFO << "constraints on x,y,v to: " << m6;
1077 AINFO << "end constraints to: " << m7;
1078 AINFO << "control bnd to: " << m8;
1079 AINFO << "time interval constraints to: " << m9;
1080 AINFO << "lambda constraints to: " << m10;
1081 AINFO << "miu constraints to: " << m11;
1082 AINFO << "total constraints: " << num_of_constraints_;
1083
1084 for (int idx = 0; idx < m; ++idx) {
1085 if (g[idx] > g_u_tmp[idx] + delta_v || g[idx] < g_l_tmp[idx] - delta_v) {
1086 AINFO << "constratins idx unfeasible: " << idx << ", g: " << g[idx] << ", lower: " << g_l_tmp[idx]
1087 << ", upper: " << g_u_tmp[idx];
1088 }
1089 }
1090 return true;
1091}
apollo::planning::DistanceApproachIPOPTRelaxEndInterface::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_interface.cc:244
AINFO
#define AINFO
Definition log.h:42

◆ eval_constraints()

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

Template to compute constraints

在文件 distance_approach_ipopt_relax_end_interface.cc 第 821 行定义.

821 {
822 // state start index
823 int state_index = state_start_index_;
824
825 // control start index.
826 int control_index = control_start_index_;
827
828 // time start index
829 int time_index = time_start_index_;
830
831 int constraint_index = 0;
832
833 // // 1. state constraints 4 * [0, horizons-1]
834 for (int i = 0; i < horizon_; ++i) {
835 // x1
836 g[constraint_index] = x[state_index + 4]
837 - (x[state_index]
838 + ts_ * x[time_index] * (x[state_index + 3] + ts_ * x[time_index] * 0.5 * x[control_index + 1])
839 * cos(x[state_index + 2]
840 + ts_ * x[time_index] * 0.5 * x[state_index + 3] * tan(x[control_index])
841 / wheelbase_));
842
843 // x2
844 g[constraint_index + 1] = x[state_index + 5]
845 - (x[state_index + 1]
846 + ts_ * x[time_index] * (x[state_index + 3] + ts_ * x[time_index] * 0.5 * x[control_index + 1])
847 * sin(x[state_index + 2]
848 + ts_ * x[time_index] * 0.5 * x[state_index + 3] * tan(x[control_index])
849 / wheelbase_));
850
851 // x3
852 g[constraint_index + 2] = x[state_index + 6]
853 - (x[state_index + 2]
854 + ts_ * x[time_index] * (x[state_index + 3] + ts_ * x[time_index] * 0.5 * x[control_index + 1])
855 * tan(x[control_index]) / wheelbase_);
856
857 // x4
858 g[constraint_index + 3]
859 = x[state_index + 7] - (x[state_index + 3] + ts_ * x[time_index] * x[control_index + 1]);
860
861 control_index += 2;
862 constraint_index += 4;
863 time_index++;
864 state_index += 4;
865 }
866
867 ADEBUG << "constraint_index after adding Euler forward dynamics constraints "
868 "updated: "
869 << constraint_index;
870
871 // 2. Control rate limit constraints, 1 * [0, horizons-1], only apply
872 // steering rate as of now
873 control_index = control_start_index_;
874 time_index = time_start_index_;
875
876 // First rate is compare first with stitch point
877 g[constraint_index] = (x[control_index] - last_time_u_(0, 0)) / x[time_index] / ts_;
878 control_index += 2;
879 constraint_index++;
880 time_index++;
881
882 for (int i = 1; i < horizon_; ++i) {
883 g[constraint_index] = (x[control_index] - x[control_index - 2]) / x[time_index] / ts_;
884 constraint_index++;
885 control_index += 2;
886 time_index++;
887 }
888
889 // 3. Time constraints 1 * [0, horizons-1]
890 time_index = time_start_index_;
891 for (int i = 0; i < horizon_; ++i) {
892 g[constraint_index] = x[time_index + 1] - x[time_index];
893 constraint_index++;
894 time_index++;
895 }
896
897 ADEBUG << "constraint_index after adding time constraints "
898 "updated: "
899 << constraint_index;
900
901 // 4. Three obstacles related equal constraints, one equality constraints,
902 // [0, horizon_] * [0, obstacles_num_-1] * 4
903 state_index = state_start_index_;
904 int l_index = l_start_index_;
905 int n_index = n_start_index_;
906
907 for (int i = 0; i < horizon_ + 1; ++i) {
908 int edges_counter = 0;
909 for (int j = 0; j < obstacles_num_; ++j) {
910 int current_edges_num = obstacles_edges_num_(j, 0);
911 Eigen::MatrixXd Aj = obstacles_A_.block(edges_counter, 0, current_edges_num, 2);
912 Eigen::MatrixXd bj = obstacles_b_.block(edges_counter, 0, current_edges_num, 1);
913
914 // norm(A* lambda) <= 1
915 T tmp1 = 0.0;
916 T tmp2 = 0.0;
917 for (int k = 0; k < current_edges_num; ++k) {
918 // TODO(QiL) : replace this one directly with x
919 tmp1 += Aj(k, 0) * x[l_index + k];
920 tmp2 += Aj(k, 1) * x[l_index + k];
921 }
922 g[constraint_index] = tmp1 * tmp1 + tmp2 * tmp2;
923
924 // G' * mu + R' * lambda == 0
925 g[constraint_index + 1]
926 = x[n_index] - x[n_index + 2] + cos(x[state_index + 2]) * tmp1 + sin(x[state_index + 2]) * tmp2;
927
928 g[constraint_index + 2]
929 = x[n_index + 1] - x[n_index + 3] - sin(x[state_index + 2]) * tmp1 + cos(x[state_index + 2]) * tmp2;
930
931 // -g'*mu + (A*t - b)*lambda > 0
932 T tmp3 = 0.0;
933 for (int k = 0; k < 4; ++k) {
934 tmp3 += -g_[k] * x[n_index + k];
935 }
936
937 T tmp4 = 0.0;
938 for (int k = 0; k < current_edges_num; ++k) {
939 tmp4 += bj(k, 0) * x[l_index + k];
940 }
941
942 g[constraint_index + 3] = tmp3 + (x[state_index] + cos(x[state_index + 2]) * offset_) * tmp1
943 + (x[state_index + 1] + sin(x[state_index + 2]) * offset_) * tmp2 - tmp4;
944
945 // Update index
946 edges_counter += current_edges_num;
947 l_index += current_edges_num;
948 n_index += 4;
949 constraint_index += 4;
950 }
951 state_index += 4;
952 }
953 ADEBUG << "constraint_index after obstacles avoidance constraints "
954 "updated: "
955 << constraint_index;
956
957 // 5. load variable bounds as constraints
958 state_index = state_start_index_;
959 control_index = control_start_index_;
960 time_index = time_start_index_;
961 l_index = l_start_index_;
962 n_index = n_start_index_;
963
964 // start configuration
965 g[constraint_index] = x[state_index];
966 g[constraint_index + 1] = x[state_index + 1];
967 g[constraint_index + 2] = x[state_index + 2];
968 g[constraint_index + 3] = x[state_index + 3];
969 constraint_index += 4;
970 state_index += 4;
971
972 // constraints on x,y,v
973 for (int i = 1; i < horizon_; ++i) {
974 g[constraint_index] = x[state_index];
975 g[constraint_index + 1] = x[state_index + 1];
976 g[constraint_index + 2] = x[state_index + 3];
977 constraint_index += 3;
978 state_index += 4;
979 }
980
981 // end configuration
982 g[constraint_index] = x[state_index];
983 g[constraint_index + 1] = x[state_index + 1];
984 g[constraint_index + 2] = x[state_index + 2];
985 g[constraint_index + 3] = x[state_index + 3];
986 constraint_index += 4;
987 state_index += 4;
988
989 for (int i = 0; i < horizon_; ++i) {
990 g[constraint_index] = x[control_index];
991 g[constraint_index + 1] = x[control_index + 1];
992 constraint_index += 2;
993 control_index += 2;
994 }
995
996 for (int i = 0; i < horizon_ + 1; ++i) {
997 g[constraint_index] = x[time_index];
998 constraint_index++;
999 time_index++;
1000 }
1001
1002 for (int i = 0; i < lambda_horizon_; ++i) {
1003 g[constraint_index] = x[l_index];
1004 constraint_index++;
1005 l_index++;
1006 }
1007
1008 for (int i = 0; i < miu_horizon_; ++i) {
1009 g[constraint_index] = x[n_index];
1010 constraint_index++;
1011 n_index++;
1012 }
1013}
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::DistanceApproachIPOPTRelaxEndInterface::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_interface.cc 第 515 行定义.

515 {
516 eval_obj(n, x, &obj_value);
517 return true;
518}
apollo::planning::DistanceApproachIPOPTRelaxEndInterface::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_interface.cc:754

◆ eval_g()

bool apollo::planning::DistanceApproachIPOPTRelaxEndInterface::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_interface.cc 第 525 行定义.

525 {
526 eval_constraints(n, x, m, g);
527 // if (enable_constraint_check_) check_g(n, x, m, g);
528 return true;
529}
apollo::planning::DistanceApproachIPOPTRelaxEndInterface::eval_constraints
void eval_constraints(int n, const T *x, int m, T *g)
Template to compute constraints
Definition distance_approach_ipopt_relax_end_interface.cc:821

◆ eval_grad_f()

bool apollo::planning::DistanceApproachIPOPTRelaxEndInterface::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_interface.cc 第 520 行定义.

520 {
521 gradient(tag_f, n, x, grad_f);
522 return true;
523}
tag_f
#define tag_f
Definition cos_theta_ipopt_interface.h:36

◆ eval_h()

bool apollo::planning::DistanceApproachIPOPTRelaxEndInterface::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_interface.cc 第 573 行定义.

584 {
585 if (values == nullptr) {
586 // return the structure. This is a symmetric matrix, fill the lower left
587 // triangle only.
588
589 for (int idx = 0; idx < nnz_L; idx++) {
590 iRow[idx] = rind_L[idx];
591 jCol[idx] = cind_L[idx];
592 }
593 } else {
594 // return the values. This is a symmetric matrix, fill the lower left
595 // triangle only
596
597 obj_lam[0] = obj_factor;
598
599 for (int idx = 0; idx < m; idx++) {
600 obj_lam[1 + idx] = lambda[idx];
601 }
602
603 set_param_vec(tag_L, m + 1, obj_lam);
604 sparse_hess(tag_L, n, 1, const_cast<double*>(x), &nnz_L, &rind_L, &cind_L, &hessval, options_L);
605
606 for (int idx = 0; idx < nnz_L; idx++) {
607 values[idx] = hessval[idx];
608 }
609 }
610
611 return true;
612}
tag_L
#define tag_L
Definition cos_theta_ipopt_interface.h:38

◆ eval_jac_g()

bool apollo::planning::DistanceApproachIPOPTRelaxEndInterface::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_interface.cc 第 531 行定义.

539 {
540 if (enable_jacobian_ad_) {
541 if (values == nullptr) {
542 // return the structure of the jacobian
543 for (int idx = 0; idx < nnz_jac; idx++) {
544 iRow[idx] = rind_g[idx];
545 jCol[idx] = cind_g[idx];
546 }
547 } else {
548 // return the values of the jacobian of the constraints
549 sparse_jac(tag_g, m, n, 1, x, &nnz_jac, &rind_g, &cind_g, &jacval, options_g);
550 for (int idx = 0; idx < nnz_jac; idx++) {
551 values[idx] = jacval[idx];
552 }
553 }
554 return true;
555 } else {
556 return eval_jac_g_ser(n, x, new_x, m, nele_jac, iRow, jCol, values);
557 }
558}
apollo::planning::DistanceApproachIPOPTRelaxEndInterface::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_interface.cc:560
tag_g
#define tag_g
Definition cos_theta_ipopt_interface.h:37

◆ eval_jac_g_ser()

bool apollo::planning::DistanceApproachIPOPTRelaxEndInterface::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_interface.cc 第 560 行定义.

568 {
569 AERROR << "NOT READY";
570 return false;
571}
AERROR
#define AERROR
Definition log.h:44

◆ eval_obj()

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

Template to return the objective value

在文件 distance_approach_ipopt_relax_end_interface.cc 第 754 行定义.

754 {
755 // Objective is :
756 // min control inputs
757 // min input rate
758 // min time (if the time step is not fixed)
759 // regularization wrt warm start trajectory
760 DCHECK(ts_ != 0) << "ts in distance_approach_ is 0";
761 int control_index = control_start_index_;
762 int time_index = time_start_index_;
763 int state_index = state_start_index_;
764
765 // TODO(QiL): Initial implementation towards earlier understanding and debug
766 // purpose, later code refine towards improving efficiency
767
768 *obj_value = 0.0;
769 // 1. objective to minimize state diff to warm up
770 for (int i = 0; i < horizon_ + 1; ++i) {
771 T x1_diff = x[state_index] - xWS_(0, i);
772 T x2_diff = x[state_index + 1] - xWS_(1, i);
773 T x3_diff = x[state_index + 2] - xWS_(2, i);
774 T x4_abs = x[state_index + 3];
775 *obj_value += weight_state_x_ * x1_diff * x1_diff + weight_state_y_ * x2_diff * x2_diff
776 + weight_state_phi_ * x3_diff * x3_diff + weight_state_v_ * x4_abs * x4_abs;
777 state_index += 4;
778 }
779
780 // 2. objective to minimize u square
781 for (int i = 0; i < horizon_; ++i) {
782 *obj_value += weight_input_steer_ * x[control_index] * x[control_index]
783 + weight_input_a_ * x[control_index + 1] * x[control_index + 1];
784 control_index += 2;
785 }
786
787 // 3. objective to minimize input change rate for first horizon
788 control_index = control_start_index_;
789 T last_time_steer_rate = (x[control_index] - last_time_u_(0, 0)) / x[time_index] / ts_;
790 T last_time_a_rate = (x[control_index + 1] - last_time_u_(1, 0)) / x[time_index] / ts_;
791 *obj_value += weight_stitching_steer_ * last_time_steer_rate * last_time_steer_rate
792 + weight_stitching_a_ * last_time_a_rate * last_time_a_rate;
793
794 // 4. objective to minimize input change rates, [0- horizon_ -2]
795 time_index++;
796 for (int i = 0; i < horizon_ - 1; ++i) {
797 T steering_rate = (x[control_index + 2] - x[control_index]) / x[time_index] / ts_;
798 T a_rate = (x[control_index + 3] - x[control_index + 1]) / x[time_index] / ts_;
799 *obj_value += weight_rate_steer_ * steering_rate * steering_rate + weight_rate_a_ * a_rate * a_rate;
800 control_index += 2;
801 time_index++;
802 }
803
804 // 5. objective to minimize total time [0, horizon_]
805 time_index = time_start_index_;
806 for (int i = 0; i < horizon_ + 1; ++i) {
807 T first_order_penalty = weight_first_order_time_ * x[time_index];
808 T second_order_penalty = weight_second_order_time_ * x[time_index] * x[time_index];
809 *obj_value += first_order_penalty + second_order_penalty;
810 time_index++;
811 }
812
813 // 6. end state constraints
814 for (int i = 0; i < 4; ++i) {
815 *obj_value += weight_end_state_ * (x[state_start_index_ + 4 * horizon_ + i] - xf_(i, 0))
816 * (x[state_start_index_ + 4 * horizon_ + i] - xf_(i, 0));
817 }
818}

◆ finalize_solution()

void apollo::planning::DistanceApproachIPOPTRelaxEndInterface::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_interface.cc 第 614 行定义.

625 {
626 int state_index = state_start_index_;
627 int control_index = control_start_index_;
628 int time_index = time_start_index_;
629 int dual_l_index = l_start_index_;
630 int dual_n_index = n_start_index_;
631
632 // enable_constraint_check_: for debug only
633 if (enable_constraint_check_) {
634 ADEBUG << "final resolution constraint checking";
635 check_g(n, x, m, g);
636 }
637 // 1. state variables, 4 * [0, horizon]
638 // 2. control variables, 2 * [0, horizon_-1]
639 // 3. sampling time variables, 1 * [0, horizon_]
640 // 4. dual_l obstacles_edges_sum_ * [0, horizon]
641 // 5. dual_n obstacles_num * [0, horizon]
642 for (int i = 0; i < horizon_; ++i) {
643 state_result_(0, i) = x[state_index];
644 state_result_(1, i) = x[state_index + 1];
645 state_result_(2, i) = x[state_index + 2];
646 state_result_(3, i) = x[state_index + 3];
647 control_result_(0, i) = x[control_index];
648 control_result_(1, i) = x[control_index + 1];
649 time_result_(0, i) = ts_ * x[time_index];
650 for (int j = 0; j < obstacles_edges_sum_; ++j) {
651 dual_l_result_(j, i) = x[dual_l_index + j];
652 }
653 for (int k = 0; k < 4 * obstacles_num_; k++) {
654 dual_n_result_(k, i) = x[dual_n_index + k];
655 }
656 state_index += 4;
657 control_index += 2;
658 time_index++;
659 dual_l_index += obstacles_edges_sum_;
660 dual_n_index += 4 * obstacles_num_;
661 }
662 state_result_(0, 0) = x0_(0, 0);
663 state_result_(1, 0) = x0_(1, 0);
664 state_result_(2, 0) = x0_(2, 0);
665 state_result_(3, 0) = x0_(3, 0);
666 // push back last horizon for states
667 state_result_(0, horizon_) = xf_(0, 0);
668 state_result_(1, horizon_) = xf_(1, 0);
669 state_result_(2, horizon_) = xf_(2, 0);
670 state_result_(3, horizon_) = xf_(3, 0);
671
672 for (int j = 0; j < obstacles_edges_sum_; ++j) {
673 dual_l_result_(j, horizon_) = x[dual_l_index + j];
674 }
675 for (int k = 0; k < 4 * obstacles_num_; k++) {
676 dual_n_result_(k, horizon_) = x[dual_n_index + k];
677 }
678 // memory deallocation of ADOL-C variables
679 delete[] obj_lam;
680 if (enable_jacobian_ad_) {
681 free(rind_g);
682 free(cind_g);
683 free(jacval);
684 }
685 free(rind_L);
686 free(cind_L);
687 free(hessval);
688}
apollo::planning::DistanceApproachIPOPTRelaxEndInterface::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_interface.cc:1015

◆ generate_tapes()

void apollo::planning::DistanceApproachIPOPTRelaxEndInterface::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_interface.cc 第 1093 行定义.

1093 {
1094 std::vector<double> xp(n);
1095 std::vector<double> lamp(m);
1096 std::vector<double> zl(m);
1097 std::vector<double> zu(m);
1098 std::vector<adouble> xa(n);
1099 std::vector<adouble> g(m);
1100 std::vector<double> lam(m);
1101
1102 double sig = 0.0;
1103 adouble obj_value;
1104 double dummy = 0.0;
1105 obj_lam = new double[m + 1];
1106 get_starting_point(n, 1, &xp[0], 0, &zl[0], &zu[0], m, 0, &lamp[0]);
1107
1108 trace_on(tag_f);
1109 for (int idx = 0; idx < n; idx++) {
1110 xa[idx] <<= xp[idx];
1111 }
1112 eval_obj(n, &xa[0], &obj_value);
1113 obj_value >>= dummy;
1114 trace_off();
1115
1116 trace_on(tag_g);
1117 for (int idx = 0; idx < n; idx++) {
1118 xa[idx] <<= xp[idx];
1119 }
1120 eval_constraints(n, &xa[0], m, &g[0]);
1121 for (int idx = 0; idx < m; idx++) {
1122 g[idx] >>= dummy;
1123 }
1124 trace_off();
1125
1126 trace_on(tag_L);
1127 for (int idx = 0; idx < n; idx++) {
1128 xa[idx] <<= xp[idx];
1129 }
1130 for (int idx = 0; idx < m; idx++) {
1131 lam[idx] = 1.0;
1132 }
1133 sig = 1.0;
1134 eval_obj(n, &xa[0], &obj_value);
1135 obj_value *= mkparam(sig);
1136 eval_constraints(n, &xa[0], m, &g[0]);
1137 for (int idx = 0; idx < m; idx++) {
1138 obj_value += g[idx] * mkparam(lam[idx]);
1139 }
1140 obj_value >>= dummy;
1141
1142 trace_off();
1143
1144 if (enable_jacobian_ad_) {
1145 rind_g = nullptr;
1146 cind_g = nullptr;
1147 jacval = nullptr;
1148 options_g[0] = 0; /* sparsity pattern by index domains (default) */
1149 options_g[1] = 0; /* safe mode (default) */
1150 options_g[2] = 0;
1151 options_g[3] = 0; /* column compression (default) */
1152
1153 sparse_jac(tag_g, m, n, 0, &xp[0], &nnz_jac, &rind_g, &cind_g, &jacval, options_g);
1154 *nnz_jac_g = nnz_jac;
1155 }
1156
1157 rind_L = nullptr;
1158 cind_L = nullptr;
1159 hessval = nullptr;
1160 options_L[0] = 0;
1161 options_L[1] = 1;
1162
1163 sparse_hess(tag_L, n, 0, &xp[0], &nnz_L, &rind_L, &cind_L, &hessval, options_L);
1164 *nnz_h_lag = nnz_L;
1165}
apollo::planning::DistanceApproachIPOPTRelaxEndInterface::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_interface.cc:460

◆ get_bounds_info()

bool apollo::planning::DistanceApproachIPOPTRelaxEndInterface::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_interface.cc 第 244 行定义.

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

◆ get_nlp_info()

bool apollo::planning::DistanceApproachIPOPTRelaxEndInterface::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_interface.cc 第 185 行定义.

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

◆ get_optimization_results()

void apollo::planning::DistanceApproachIPOPTRelaxEndInterface::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_interface.cc 第 690 行定义.

695 {
696 ADEBUG << "get_optimization_results";
697 *state_result = state_result_;
698 *control_result = control_result_;
699 *time_result = time_result_;
700 *dual_l_result = dual_l_result_;
701 *dual_n_result = dual_n_result_;
702
703 if (!distance_approach_config_.enable_initial_final_check()) {
704 return;
705 }
706 CHECK_EQ(state_result_.cols(), xWS_.cols());
707 CHECK_EQ(state_result_.rows(), xWS_.rows());
708 double state_diff_max = 0.0;
709 for (int i = 0; i < horizon_ + 1; ++i) {
710 for (int j = 0; j < 4; ++j) {
711 state_diff_max = std::max(std::abs(xWS_(j, i) - state_result_(j, i)), state_diff_max);
712 }
713 }
714
715 // 2. control variable initialization, 2 * horizon_
716 CHECK_EQ(control_result_.cols(), uWS_.cols());
717 CHECK_EQ(control_result_.rows(), uWS_.rows());
718 double control_diff_max = 0.0;
719 for (int i = 0; i < horizon_; ++i) {
720 control_diff_max = std::max(std::abs(uWS_(0, i) - control_result_(0, i)), control_diff_max);
721 control_diff_max = std::max(std::abs(uWS_(1, i) - control_result_(1, i)), control_diff_max);
722 }
723
724 // 2. time scale variable initialization, horizon_ + 1
725
726 // 3. lagrange constraint l, obstacles_edges_sum_ * (horizon_+1)
727 CHECK_EQ(dual_l_result_.cols(), l_warm_up_.cols());
728 CHECK_EQ(dual_l_result_.rows(), l_warm_up_.rows());
729 double l_diff_max = 0.0;
730 for (int i = 0; i < horizon_ + 1; ++i) {
731 for (int j = 0; j < obstacles_edges_sum_; ++j) {
732 l_diff_max = std::max(std::abs(l_warm_up_(j, i) - dual_l_result_(j, i)), l_diff_max);
733 }
734 }
735
736 // 4. lagrange constraint m, 4*obstacles_num * (horizon_+1)
737 CHECK_EQ(n_warm_up_.cols(), dual_n_result_.cols());
738 CHECK_EQ(n_warm_up_.rows(), dual_n_result_.rows());
739 double n_diff_max = 0.0;
740 for (int i = 0; i < horizon_ + 1; ++i) {
741 for (int j = 0; j < 4 * obstacles_num_; ++j) {
742 n_diff_max = std::max(std::abs(n_warm_up_(j, i) - dual_n_result_(j, i)), n_diff_max);
743 }
744 }
745
746 ADEBUG << "state_diff_max: " << state_diff_max;
747 ADEBUG << "control_diff_max: " << control_diff_max;
748 ADEBUG << "dual_l_diff_max: " << l_diff_max;
749 ADEBUG << "dual_n_diff_max: " << n_diff_max;
750}

◆ get_starting_point()

bool apollo::planning::DistanceApproachIPOPTRelaxEndInterface::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_interface.cc 第 460 行定义.

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