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

36 : ts_(ts),
37 ego_(ego),
38 xWS_(xWS),
39 uWS_(uWS),
40 l_warm_up_(l_warm_up),
41 n_warm_up_(n_warm_up),
42 slack_warm_up_(s_warm_up),
43 x0_(x0),
44 xf_(xf),
45 last_time_u_(last_time_u),
46 XYbounds_(XYbounds),
47 obstacles_edges_num_(obstacles_edges_num),
48 obstacles_A_(obstacles_A),
49 obstacles_b_(obstacles_b) {
50 ACHECK(horizon < std::numeric_limits<int>::max())
51 << "Invalid cast on horizon in open space planner";
52 horizon_ = static_cast<int>(horizon);
53 ACHECK(obstacles_num < std::numeric_limits<int>::max())
54 << "Invalid cast on obstacles_num in open space planner";
55
56 // TODO(RHe): use relax slacks from dual warm up
57 // slack_warm_up_ = Eigen::MatrixXd::Zero(obstacles_num, horizon_ + 1);
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 slack_result_ = Eigen::MatrixXd::Zero(obstacles_num_, horizon_ + 1);
69 control_result_ = Eigen::MatrixXd::Zero(2, horizon_);
70 time_result_ = Eigen::MatrixXd::Zero(1, horizon_);
71
72 state_start_index_ = 0;
73 control_start_index_ = 4 * (horizon_ + 1);
74 time_start_index_ = control_start_index_ + 2 * horizon_;
75 l_start_index_ = time_start_index_ + (horizon_ + 1);
76 n_start_index_ = l_start_index_ + obstacles_edges_sum_ * (horizon_ + 1);
77 slack_index_ = n_start_index_ + 4 * obstacles_num_ * (horizon_ + 1);
78
79 distance_approach_config_ =
80 planner_open_space_config.distance_approach_config();
81 weight_state_x_ = distance_approach_config_.weight_x();
82 weight_state_y_ = distance_approach_config_.weight_y();
83 weight_state_phi_ = distance_approach_config_.weight_phi();
84 weight_state_v_ = distance_approach_config_.weight_v();
85 weight_input_steer_ = distance_approach_config_.weight_steer();
86 weight_input_a_ = distance_approach_config_.weight_a();
87 weight_rate_steer_ = distance_approach_config_.weight_steer_rate();
88 weight_rate_a_ = distance_approach_config_.weight_a_rate();
89 weight_stitching_steer_ = distance_approach_config_.weight_steer_stitching();
90 weight_stitching_a_ = distance_approach_config_.weight_a_stitching();
91 weight_first_order_time_ =
92 distance_approach_config_.weight_first_order_time();
93 weight_second_order_time_ =
94 distance_approach_config_.weight_second_order_time();
95 weight_end_state_ = distance_approach_config_.weight_end_state();
96 weight_slack_ = distance_approach_config_.weight_slack();
97
98 min_safety_distance_ = distance_approach_config_.min_safety_distance();
99 max_steer_angle_ =
100 vehicle_param_.max_steer_angle() / vehicle_param_.steer_ratio();
101 max_speed_forward_ = distance_approach_config_.max_speed_forward();
102 max_speed_reverse_ = distance_approach_config_.max_speed_reverse();
103 max_acceleration_forward_ =
104 distance_approach_config_.max_acceleration_forward();
105 max_acceleration_reverse_ =
106 distance_approach_config_.max_acceleration_reverse();
107 min_time_sample_scaling_ =
108 distance_approach_config_.min_time_sample_scaling();
109 max_time_sample_scaling_ =
110 distance_approach_config_.max_time_sample_scaling();
111 max_steer_rate_ =
112 vehicle_param_.max_steer_angle_rate() / vehicle_param_.steer_ratio();
113 use_fix_time_ = distance_approach_config_.use_fix_time();
114 wheelbase_ = vehicle_param_.wheel_base();
115 enable_constraint_check_ =
116 distance_approach_config_.enable_constraint_check();
117 enable_jacobian_ad_ = true;
118}
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 
)

成员函数说明

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

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

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

462 {
463 eval_obj(n, x, &obj_value);
464 return true;
465}
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:697

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

477 {
478 eval_constraints(n, x, m, g);
479 // if (enable_constraint_check_) check_g(n, x, m, g);
480 return true;
481}
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:780

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

470 {
471 gradient(tag_f, n, x, grad_f);
472 return true;
473}
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 第 514 行定义.

517 {
518 if (values == nullptr) {
519 // return the structure. This is a symmetric matrix, fill the lower left
520 // triangle only.
521
522 for (int idx = 0; idx < nnz_L; idx++) {
523 iRow[idx] = rind_L[idx];
524 jCol[idx] = cind_L[idx];
525 }
526 } else {
527 // return the values. This is a symmetric matrix, fill the lower left
528 // triangle only
529
530 obj_lam[0] = obj_factor;
531
532 for (int idx = 0; idx < m; idx++) {
533 obj_lam[1 + idx] = lambda[idx];
534 }
535
536 set_param_vec(tag_L, m + 1, obj_lam);
537 sparse_hess(tag_L, n, 1, const_cast<double*>(x), &nnz_L, &rind_L, &cind_L,
538 &hessval, options_L);
539
540 for (int idx = 0; idx < nnz_L; idx++) {
541 values[idx] = hessval[idx];
542 }
543 }
544
545 return true;
546}
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 第 483 行定义.

485 {
486 if (enable_jacobian_ad_) {
487 if (values == nullptr) {
488 // return the structure of the jacobian
489 for (int idx = 0; idx < nnz_jac; idx++) {
490 iRow[idx] = rind_g[idx];
491 jCol[idx] = cind_g[idx];
492 }
493 } else {
494 // return the values of the jacobian of the constraints
495 sparse_jac(tag_g, m, n, 1, x, &nnz_jac, &rind_g, &cind_g, &jacval,
496 options_g);
497 for (int idx = 0; idx < nnz_jac; idx++) {
498 values[idx] = jacval[idx];
499 }
500 }
501 return true;
502 } else {
503 return eval_jac_g_ser(n, x, new_x, m, nele_jac, iRow, jCol, values);
504 }
505}
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:507
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 第 507 行定义.

509 {
510 AERROR << "NOT READY";
511 return false;
512}
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 第 697 行定义.

698 {
699 // Objective is :
700 // min control inputs
701 // min input rate
702 // min time (if the time step is not fixed)
703 // regularization wrt warm start trajectory
704 DCHECK(ts_ != 0) << "ts in distance_approach_ is 0";
705 int control_index = control_start_index_;
706 int time_index = time_start_index_;
707 int state_index = state_start_index_;
708
709 // TODO(QiL): Initial implementation towards earlier understanding and debug
710 // purpose, later code refine towards improving efficiency
711
712 *obj_value = 0.0;
713 // 1. objective to minimize state diff to warm up
714 for (int i = 0; i < horizon_ + 1; ++i) {
715 T x1_diff = x[state_index] - xWS_(0, i);
716 T x2_diff = x[state_index + 1] - xWS_(1, i);
717 T x3_diff = x[state_index + 2] - xWS_(2, i);
718 T x4_abs = x[state_index + 3];
719 *obj_value += weight_state_x_ * x1_diff * x1_diff +
720 weight_state_y_ * x2_diff * x2_diff +
721 weight_state_phi_ * x3_diff * x3_diff +
722 weight_state_v_ * x4_abs * x4_abs;
723 state_index += 4;
724 }
725
726 // 2. objective to minimize u square
727 for (int i = 0; i < horizon_; ++i) {
728 *obj_value += weight_input_steer_ * x[control_index] * x[control_index] +
729 weight_input_a_ * x[control_index + 1] * x[control_index + 1];
730 control_index += 2;
731 }
732
733 // 3. objective to minimize input change rate for first horizon
734 control_index = control_start_index_;
735 T last_time_steer_rate =
736 (x[control_index] - last_time_u_(0, 0)) / x[time_index] / ts_;
737 T last_time_a_rate =
738 (x[control_index + 1] - last_time_u_(1, 0)) / x[time_index] / ts_;
739 *obj_value +=
740 weight_stitching_steer_ * last_time_steer_rate * last_time_steer_rate +
741 weight_stitching_a_ * last_time_a_rate * last_time_a_rate;
742
743 // 4. objective to minimize input change rates, [0- horizon_ -2]
744 time_index++;
745 for (int i = 0; i < horizon_ - 1; ++i) {
746 T steering_rate =
747 (x[control_index + 2] - x[control_index]) / x[time_index] / ts_;
748 T a_rate =
749 (x[control_index + 3] - x[control_index + 1]) / x[time_index] / ts_;
750 *obj_value += weight_rate_steer_ * steering_rate * steering_rate +
751 weight_rate_a_ * a_rate * a_rate;
752 control_index += 2;
753 time_index++;
754 }
755
756 // 5. objective to minimize total time [0, horizon_]
757 time_index = time_start_index_;
758 for (int i = 0; i < horizon_ + 1; ++i) {
759 T first_order_penalty = weight_first_order_time_ * x[time_index];
760 T second_order_penalty =
761 weight_second_order_time_ * x[time_index] * x[time_index];
762 *obj_value += first_order_penalty + second_order_penalty;
763 time_index++;
764 }
765
766 // 6. end state constraints
767 for (int i = 0; i < 4; ++i) {
768 *obj_value += weight_end_state_ *
769 (x[state_start_index_ + 4 * horizon_ + i] - xf_(i, 0)) *
770 (x[state_start_index_ + 4 * horizon_ + i] - xf_(i, 0));
771 }
772
773 // 7. slack variables
774 for (int i = 0; i < slack_horizon_; ++i) {
775 *obj_value += weight_slack_ * x[slack_index_ + i];
776 }
777}

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

552 {
553 int state_index = state_start_index_;
554 int control_index = control_start_index_;
555 int time_index = time_start_index_;
556 int dual_l_index = l_start_index_;
557 int dual_n_index = n_start_index_;
558 int slack_index = slack_index_;
559
560 // enable_constraint_check_: for debug only
561 if (enable_constraint_check_) {
562 ADEBUG << "final resolution constraint checking";
563 check_g(n, x, m, g);
564 }
565 // 1. state variables, 4 * [0, horizon]
566 // 2. control variables, 2 * [0, horizon_-1]
567 // 3. sampling time variables, 1 * [0, horizon_]
568 // 4. dual_l, obstacles_edges_sum_ * [0, horizon]
569 // 5. dual_n, obstacles_num * [0, horizon]
570 // 6. slack_, obstacles_num * [0, horizon]
571 for (int i = 0; i < horizon_; ++i) {
572 state_result_(0, i) = x[state_index];
573 state_result_(1, i) = x[state_index + 1];
574 state_result_(2, i) = x[state_index + 2];
575 state_result_(3, i) = x[state_index + 3];
576 control_result_(0, i) = x[control_index];
577 control_result_(1, i) = x[control_index + 1];
578 time_result_(0, i) = ts_ * x[time_index];
579
580 for (int j = 0; j < obstacles_edges_sum_; ++j) {
581 dual_l_result_(j, i) = x[dual_l_index + j];
582 }
583 for (int k = 0; k < 4 * obstacles_num_; ++k) {
584 dual_n_result_(k, i) = x[dual_n_index + k];
585 }
586 for (int l = 0; l < obstacles_num_; ++l) {
587 slack_result_(l, i) = x[slack_index + l];
588 }
589
590 state_index += 4;
591 control_index += 2;
592 time_index++;
593 dual_l_index += obstacles_edges_sum_;
594 dual_n_index += 4 * obstacles_num_;
595 slack_index += obstacles_num_;
596 }
597 state_result_(0, 0) = x0_(0, 0);
598 state_result_(1, 0) = x0_(1, 0);
599 state_result_(2, 0) = x0_(2, 0);
600 state_result_(3, 0) = x0_(3, 0);
601 // push back last horizon for states
602 state_result_(0, horizon_) = xf_(0, 0);
603 state_result_(1, horizon_) = xf_(1, 0);
604 state_result_(2, horizon_) = xf_(2, 0);
605 state_result_(3, horizon_) = xf_(3, 0);
606
607 for (int j = 0; j < obstacles_edges_sum_; ++j) {
608 dual_l_result_(j, horizon_) = x[dual_l_index + j];
609 }
610 for (int k = 0; k < 4 * obstacles_num_; ++k) {
611 dual_n_result_(k, horizon_) = x[dual_n_index + k];
612 }
613 for (int l = 0; l < obstacles_num_; ++l) {
614 slack_result_(l, horizon_) = x[slack_index + l];
615 }
616
617 // memory deallocation of ADOL-C variables
618 delete[] obj_lam;
619 if (enable_jacobian_ad_) {
620 free(rind_g);
621 free(cind_g);
622 free(jacval);
623 }
624 free(rind_L);
625 free(cind_L);
626 free(hessval);
627}
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:1004

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

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

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

170 {
171 ADEBUG << "get_bounds_info";
172 ACHECK(XYbounds_.size() == 4)
173 << "XYbounds_ size is not 4, but" << XYbounds_.size();
174
175 // Variables: includes state, u, sample time and lagrange multipliers
176 // 1. state variables, 4 * [0, horizon]
177 // start point pose
178 int variable_index = 0;
179 for (int i = 0; i < 4; ++i) {
180 x_l[i] = -2e19;
181 x_u[i] = 2e19;
182 }
183 variable_index += 4;
184
185 // During horizons, 2 ~ N-1
186 for (int i = 1; i < horizon_; ++i) {
187 // x
188 x_l[variable_index] = -2e19;
189 x_u[variable_index] = 2e19;
190
191 // y
192 x_l[variable_index + 1] = -2e19;
193 x_u[variable_index + 1] = 2e19;
194
195 // phi
196 x_l[variable_index + 2] = -2e19;
197 x_u[variable_index + 2] = 2e19;
198
199 // v
200 x_l[variable_index + 3] = -2e19;
201 x_u[variable_index + 3] = 2e19;
202
203 variable_index += 4;
204 }
205
206 // end point pose
207 for (int i = 0; i < 4; ++i) {
208 x_l[variable_index + i] = -2e19;
209 x_u[variable_index + i] = 2e19;
210 }
211 variable_index += 4;
212 ADEBUG << "variable_index after adding state variables : " << variable_index;
213
214 // 2. control variables, 2 * [0, horizon_-1]
215 for (int i = 0; i < horizon_; ++i) {
216 // steer
217 x_l[variable_index] = -2e19;
218 x_u[variable_index] = 2e19;
219
220 // a
221 x_l[variable_index + 1] = -2e19;
222 x_u[variable_index + 1] = 2e19;
223
224 variable_index += 2;
225 }
226 ADEBUG << "variable_index after adding control variables : "
227 << variable_index;
228
229 // 3. sampling time variables, 1 * [0, horizon_]
230 for (int i = 0; i < horizon_ + 1; ++i) {
231 x_l[variable_index] = -2e19;
232 x_u[variable_index] = 2e19;
233 ++variable_index;
234 }
235 ADEBUG << "variable_index after adding sample time : " << variable_index;
236 ADEBUG << "sample time fix time status is : " << use_fix_time_;
237
238 // 4. lagrange constraint l, [0, obstacles_edges_sum_ - 1] * [0,
239 // horizon_]
240 for (int i = 0; i < horizon_ + 1; ++i) {
241 for (int j = 0; j < obstacles_edges_sum_; ++j) {
242 x_l[variable_index] = 0.0;
243 x_u[variable_index] = 2e19; // nlp_upper_bound_limit
244 ++variable_index;
245 }
246 }
247 ADEBUG << "variable_index after adding lagrange l : " << variable_index;
248
249 // 5. lagrange constraint n, [0, 4*obstacles_num-1] * [0, horizon_]
250 for (int i = 0; i < horizon_ + 1; ++i) {
251 for (int j = 0; j < 4 * obstacles_num_; ++j) {
252 x_l[variable_index] = 0.0;
253 x_u[variable_index] = 2e19; // nlp_upper_bound_limit
254
255 ++variable_index;
256 }
257 }
258 ADEBUG << "variable_index after adding lagrange n : " << variable_index;
259
260 // 6. slack variable constraint s, obstacles_num * (horizon_ + 1)
261 for (int i = 0; i < horizon_ + 1; ++i) {
262 for (int j = 0; j < obstacles_num_; ++j) {
263 x_l[variable_index] = 0.0;
264 x_u[variable_index] = 2e19;
265 ++variable_index;
266 }
267 }
268 ADEBUG << "variable_index after adding slack s: " << variable_index;
269
270 // Constraints: includes four state Euler forward constraints, three
271 // Obstacle related constraints
272
273 // 1. dynamics constraints 4 * [0, horizons-1]
274 int constraint_index = 0;
275 for (int i = 0; i < 4 * horizon_; ++i) {
276 g_l[i] = 0.0;
277 g_u[i] = 0.0;
278 }
279 constraint_index += 4 * horizon_;
280
281 ADEBUG << "constraint_index after adding Euler forward dynamics constraints: "
282 << constraint_index;
283
284 // 2. Control rate limit constraints, 1 * [0, horizons-1], only apply
285 // steering rate as of now
286 for (int i = 0; i < horizon_; ++i) {
287 g_l[constraint_index] = -max_steer_rate_;
288 g_u[constraint_index] = max_steer_rate_;
289 ++constraint_index;
290 }
291
292 ADEBUG << "constraint_index after adding steering rate constraints: "
293 << constraint_index;
294
295 // 3. Time constraints 1 * [0, horizons-1]
296 for (int i = 0; i < horizon_; ++i) {
297 g_l[constraint_index] = 0.0;
298 g_u[constraint_index] = 0.0;
299 ++constraint_index;
300 }
301
302 ADEBUG << "constraint_index after adding time constraints: "
303 << constraint_index;
304
305 // 4. Three obstacles related equal constraints, one equality constraints,
306 // [0, horizon_] * [0, obstacles_num_-1] * 4
307 for (int i = 0; i < horizon_ + 1; ++i) {
308 for (int j = 0; j < obstacles_num_; ++j) {
309 // a. norm(A'*lambda) <= 1
310 g_l[constraint_index] = -2e19;
311 g_u[constraint_index] = 1.0;
312
313 // b. G'*mu + R'*A*lambda = 0
314 g_l[constraint_index + 1] = 0.0;
315 g_u[constraint_index + 1] = 0.0;
316 g_l[constraint_index + 2] = 0.0;
317 g_u[constraint_index + 2] = 0.0;
318
319 // c. -g'*mu + (A*t - b)*lambda + s > 0
320 g_l[constraint_index + 3] = 0.0;
321 g_u[constraint_index + 3] = 2e19; // nlp_upper_bound_limit
322 constraint_index += 4;
323 }
324 }
325
326 // 5. load variable bounds as constraints
327 // start configuration
328 g_l[constraint_index] = x0_(0, 0);
329 g_u[constraint_index] = x0_(0, 0);
330 g_l[constraint_index + 1] = x0_(1, 0);
331 g_u[constraint_index + 1] = x0_(1, 0);
332 g_l[constraint_index + 2] = x0_(2, 0);
333 g_u[constraint_index + 2] = x0_(2, 0);
334 g_l[constraint_index + 3] = x0_(3, 0);
335 g_u[constraint_index + 3] = x0_(3, 0);
336 constraint_index += 4;
337
338 for (int i = 1; i < horizon_; ++i) {
339 g_l[constraint_index] = XYbounds_[0];
340 g_u[constraint_index] = XYbounds_[1];
341 g_l[constraint_index + 1] = XYbounds_[2];
342 g_u[constraint_index + 1] = XYbounds_[3];
343 g_l[constraint_index + 2] = -max_speed_reverse_;
344 g_u[constraint_index + 2] = max_speed_forward_;
345 constraint_index += 3;
346 }
347
348 // end configuration
349 g_l[constraint_index] = -2e19;
350 g_u[constraint_index] = 2e19;
351 g_l[constraint_index + 1] = -2e19;
352 g_u[constraint_index + 1] = 2e19;
353 g_l[constraint_index + 2] = -2e19;
354 g_u[constraint_index + 2] = 2e19;
355 g_l[constraint_index + 3] = -2e19;
356 g_u[constraint_index + 3] = 2e19;
357 constraint_index += 4;
358
359 for (int i = 0; i < horizon_; ++i) {
360 g_l[constraint_index] = -max_steer_angle_;
361 g_u[constraint_index] = max_steer_angle_;
362 g_l[constraint_index + 1] = -max_acceleration_reverse_;
363 g_u[constraint_index + 1] = max_acceleration_forward_;
364 constraint_index += 2;
365 }
366
367 for (int i = 0; i < horizon_ + 1; ++i) {
368 if (!use_fix_time_) {
369 g_l[constraint_index] = min_time_sample_scaling_;
370 g_u[constraint_index] = max_time_sample_scaling_;
371 } else {
372 g_l[constraint_index] = 1.0;
373 g_u[constraint_index] = 1.0;
374 }
375 constraint_index++;
376 }
377
378 for (int i = 0; i < lambda_horizon_; ++i) {
379 g_l[constraint_index] = 0.0;
380 g_u[constraint_index] = 2e19;
381 constraint_index++;
382 }
383
384 for (int i = 0; i < miu_horizon_; ++i) {
385 g_l[constraint_index] = 0.0;
386 g_u[constraint_index] = 2e19;
387 constraint_index++;
388 }
389
390 for (int i = 0; i < slack_horizon_; ++i) {
391 g_l[constraint_index] = 0.0;
392 g_u[constraint_index] = 2e19;
393 ++constraint_index;
394 }
395
396 ADEBUG << "constraint_index after adding obstacles constraints: "
397 << constraint_index;
398 ADEBUG << "get_bounds_info_ out";
399 return true;
400}

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

122 {
123 ADEBUG << "get_nlp_info";
124 // n1 : states variables, 4 * (N+1)
125 int n1 = 4 * (horizon_ + 1);
126 ADEBUG << "n1: " << n1;
127 // n2 : control inputs variables
128 int n2 = 2 * horizon_;
129 ADEBUG << "n2: " << n2;
130 // n3 : sampling time variables
131 int n3 = horizon_ + 1;
132 ADEBUG << "n3: " << n3;
133 // n4 : dual multiplier associated with obstacle shape
134 lambda_horizon_ = obstacles_edges_num_.sum() * (horizon_ + 1);
135 ADEBUG << "lambda_horizon_: " << lambda_horizon_;
136 // n5 : dual multipier associated with car shape, obstacles_num*4 * (N+1)
137 miu_horizon_ = obstacles_num_ * 4 * (horizon_ + 1);
138 ADEBUG << "miu_horizon_: " << miu_horizon_;
139 // n6 : slack variables related to safety min distance
140 slack_horizon_ = obstacles_num_ * (horizon_ + 1);
141
142 // m1 : dynamics constatins
143 int m1 = 4 * horizon_;
144 // m2 : control rate constraints (only steering)
145 int m2 = horizon_;
146 // m3 : sampling time equality constraints
147 int m3 = horizon_;
148 // m4 : obstacle constraints
149 int m4 = 4 * obstacles_num_ * (horizon_ + 1);
150
151 num_of_variables_ =
152 n1 + n2 + n3 + lambda_horizon_ + miu_horizon_ + slack_horizon_;
153 num_of_constraints_ =
154 m1 + m2 + m3 + m4 + (num_of_variables_ - (horizon_ + 1) + 2);
155
156 // number of variables
157 n = num_of_variables_;
158 ADEBUG << "num_of_variables_ " << num_of_variables_;
159 // number of constraints
160 m = num_of_constraints_;
161 ADEBUG << "num_of_constraints_ " << num_of_constraints_;
162
163 generate_tapes(n, m, &nnz_jac_g, &nnz_h_lag);
164
165 index_style = IndexStyleEnum::C_STYLE;
166 return true;
167}
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:1089

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

632 {
633 // TODO(RHe): extract slack variables for further study
634 ADEBUG << "get_optimization_results";
635 *state_result = state_result_;
636 *control_result = control_result_;
637 *time_result = time_result_;
638 *dual_l_result = dual_l_result_;
639 *dual_n_result = dual_n_result_;
640
641 if (!distance_approach_config_.enable_initial_final_check()) {
642 return;
643 }
644 CHECK_EQ(state_result_.cols(), xWS_.cols());
645 CHECK_EQ(state_result_.rows(), xWS_.rows());
646 double state_diff_max = 0.0;
647 for (int i = 0; i < horizon_ + 1; ++i) {
648 for (int j = 0; j < 4; ++j) {
649 state_diff_max =
650 std::max(std::abs(xWS_(j, i) - state_result_(j, i)), state_diff_max);
651 }
652 }
653
654 // 2. control variable initialization, 2 * horizon_
655 CHECK_EQ(control_result_.cols(), uWS_.cols());
656 CHECK_EQ(control_result_.rows(), uWS_.rows());
657 double control_diff_max = 0.0;
658 for (int i = 0; i < horizon_; ++i) {
659 control_diff_max = std::max(std::abs(uWS_(0, i) - control_result_(0, i)),
660 control_diff_max);
661 control_diff_max = std::max(std::abs(uWS_(1, i) - control_result_(1, i)),
662 control_diff_max);
663 }
664
665 // 2. time scale variable initialization, horizon_ + 1
666
667 // 3. lagrange constraint l, obstacles_edges_sum_ * (horizon_+1)
668 CHECK_EQ(dual_l_result_.cols(), l_warm_up_.cols());
669 CHECK_EQ(dual_l_result_.rows(), l_warm_up_.rows());
670 double l_diff_max = 0.0;
671 for (int i = 0; i < horizon_ + 1; ++i) {
672 for (int j = 0; j < obstacles_edges_sum_; ++j) {
673 l_diff_max = std::max(std::abs(l_warm_up_(j, i) - dual_l_result_(j, i)),
674 l_diff_max);
675 }
676 }
677
678 // 4. lagrange constraint m, 4*obstacles_num * (horizon_+1)
679 CHECK_EQ(n_warm_up_.cols(), dual_n_result_.cols());
680 CHECK_EQ(n_warm_up_.rows(), dual_n_result_.rows());
681 double n_diff_max = 0.0;
682 for (int i = 0; i < horizon_ + 1; ++i) {
683 for (int j = 0; j < 4 * obstacles_num_; ++j) {
684 n_diff_max = std::max(std::abs(n_warm_up_(j, i) - dual_n_result_(j, i)),
685 n_diff_max);
686 }
687 }
688
689 ADEBUG << "state_diff_max: " << state_diff_max;
690 ADEBUG << "control_diff_max: " << control_diff_max;
691 ADEBUG << "dual_l_diff_max: " << l_diff_max;
692 ADEBUG << "dual_n_diff_max: " << n_diff_max;
693}

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

404 {
405 ADEBUG << "get_starting_point";
406 ACHECK(init_x) << "Warm start init_x setting failed";
407
408 CHECK_EQ(horizon_, uWS_.cols());
409 CHECK_EQ(horizon_ + 1, xWS_.cols());
410
411 // 1. state variables 4 * (horizon_ + 1)
412 for (int i = 0; i < horizon_ + 1; ++i) {
413 int index = i * 4;
414 for (int j = 0; j < 4; ++j) {
415 x[index + j] = xWS_(j, i);
416 }
417 }
418
419 // 2. control variable initialization, 2 * horizon_
420 for (int i = 0; i < horizon_; ++i) {
421 int index = i * 2;
422 x[control_start_index_ + index] = uWS_(0, i);
423 x[control_start_index_ + index + 1] = uWS_(1, i);
424 }
425
426 // 2. time scale variable initialization, horizon_ + 1
427 for (int i = 0; i < horizon_ + 1; ++i) {
428 x[time_start_index_ + i] =
429 0.5 * (min_time_sample_scaling_ + max_time_sample_scaling_);
430 }
431
432 // 3. lagrange constraint l, obstacles_edges_sum_ * (horizon_+1)
433 for (int i = 0; i < horizon_ + 1; ++i) {
434 int index = i * obstacles_edges_sum_;
435 for (int j = 0; j < obstacles_edges_sum_; ++j) {
436 x[l_start_index_ + index + j] = l_warm_up_(j, i);
437 }
438 }
439
440 // 4. lagrange constraint m, 4*obstacles_num * (horizon_+1)
441 for (int i = 0; i < horizon_ + 1; ++i) {
442 int index = i * 4 * obstacles_num_;
443 for (int j = 0; j < 4 * obstacles_num_; ++j) {
444 x[n_start_index_ + index + j] = n_warm_up_(j, i);
445 }
446 }
447
448 // 5. slack constraint s, obstacle_num * (horizon_+1)
449 for (int i = 0; i < horizon_ + 1; ++i) {
450 int index = i * obstacles_num_;
451 for (int j = 0; j < obstacles_num_; ++j) {
452 x[slack_index_ + index + j] = slack_warm_up_(j, i);
453 }
454 }
455
456 ADEBUG << "get_starting_point out";
457 return true;
458}
该类的文档由以下文件生成: