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

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

成员函数说明

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

952 {
953 int kN = n;
954 int kM = m;
955 double x_u_tmp[kN];
956 double x_l_tmp[kN];
957 double g_u_tmp[kM];
958 double g_l_tmp[kM];
959
960 get_bounds_info(n, x_l_tmp, x_u_tmp, m, g_l_tmp, g_u_tmp);
961
962 const double delta_v = 1e-4;
963 for (int idx = 0; idx < n; ++idx) {
964 x_u_tmp[idx] = x_u_tmp[idx] + delta_v;
965 x_l_tmp[idx] = x_l_tmp[idx] - delta_v;
966 if (x[idx] > x_u_tmp[idx] || x[idx] < x_l_tmp[idx]) {
967 AINFO << "x idx unfeasible: " << idx << ", x: " << x[idx]
968 << ", lower: " << x_l_tmp[idx] << ", upper: " << x_u_tmp[idx];
969 }
970 }
971
972 // m1 : dynamics constatins
973 int m1 = 4 * horizon_;
974
975 // m2 : control rate constraints (only steering)
976 int m2 = m1 + horizon_;
977
978 // m3 : sampling time equality constraints
979 int m3 = m2 + horizon_;
980
981 // m4 : obstacle constraints
982 int m4 = m3 + 4 * obstacles_num_ * (horizon_ + 1);
983
984 // 5. load variable bounds as constraints
985 // start configuration
986 int m5 = m4 + 3 + 1;
987
988 // constraints on x,y,v
989 int m6 = m5 + 3 * (horizon_ - 1);
990
991 // end configuration
992 int m7 = m6 + 3 + 1;
993
994 // control variable bnd
995 int m8 = m7 + 2 * horizon_;
996
997 // time interval variable bnd
998 int m9 = m8 + (horizon_ + 1);
999
1000 // lambda_horizon_
1001 int m10 = m9 + lambda_horizon_;
1002
1003 // miu_horizon_
1004 int m11 = m10 + miu_horizon_;
1005
1006 CHECK_EQ(m11, num_of_constraints_);
1007
1008 AINFO << "dynamics constatins to: " << m1;
1009 AINFO << "control rate constraints (only steering) to: " << m2;
1010 AINFO << "sampling time equality constraints to: " << m3;
1011 AINFO << "obstacle constraints to: " << m4;
1012 AINFO << "start conf constraints to: " << m5;
1013 AINFO << "constraints on x,y,v to: " << m6;
1014 AINFO << "end constraints to: " << m7;
1015 AINFO << "control bnd to: " << m8;
1016 AINFO << "time interval constraints to: " << m9;
1017 AINFO << "lambda constraints to: " << m10;
1018 AINFO << "miu constraints to: " << m11;
1019 AINFO << "total constraints: " << num_of_constraints_;
1020
1021 for (int idx = 0; idx < m; ++idx) {
1022 if (g[idx] > g_u_tmp[idx] + delta_v || g[idx] < g_l_tmp[idx] - delta_v) {
1023 AINFO << "constratins idx unfeasible: " << idx << ", g: " << g[idx]
1024 << ", lower: " << g_l_tmp[idx] << ", upper: " << g_u_tmp[idx];
1025 }
1026 }
1027 return true;
1028}
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:167
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 第 738 行定义.

739 {
740 // state start index
741 int state_index = state_start_index_;
742
743 // control start index.
744 int control_index = control_start_index_;
745
746 // time start index
747 int time_index = time_start_index_;
748
749 int constraint_index = 0;
750
751 // // 1. state constraints 4 * [0, horizons-1]
752 for (int i = 0; i < horizon_; ++i) {
753 // x1
754 g[constraint_index] =
755 x[state_index + 4] -
756 (x[state_index] +
757 ts_ * x[time_index] *
758 (x[state_index + 3] +
759 ts_ * x[time_index] * 0.5 * x[control_index + 1]) *
760 cos(x[state_index + 2] + ts_ * x[time_index] * 0.5 *
761 x[state_index + 3] *
762 tan(x[control_index]) / wheelbase_));
763
764 // x2
765 g[constraint_index + 1] =
766 x[state_index + 5] -
767 (x[state_index + 1] +
768 ts_ * x[time_index] *
769 (x[state_index + 3] +
770 ts_ * x[time_index] * 0.5 * x[control_index + 1]) *
771 sin(x[state_index + 2] + ts_ * x[time_index] * 0.5 *
772 x[state_index + 3] *
773 tan(x[control_index]) / wheelbase_));
774
775 // x3
776 g[constraint_index + 2] =
777 x[state_index + 6] -
778 (x[state_index + 2] +
779 ts_ * x[time_index] *
780 (x[state_index + 3] +
781 ts_ * x[time_index] * 0.5 * x[control_index + 1]) *
782 tan(x[control_index]) / wheelbase_);
783
784 // x4
785 g[constraint_index + 3] =
786 x[state_index + 7] -
787 (x[state_index + 3] + ts_ * x[time_index] * x[control_index + 1]);
788
789 control_index += 2;
790 constraint_index += 4;
791 time_index++;
792 state_index += 4;
793 }
794
795 ADEBUG << "constraint_index after adding Euler forward dynamics constraints "
796 "updated: "
797 << constraint_index;
798
799 // 2. Control rate limit constraints, 1 * [0, horizons-1], only apply
800 // steering rate as of now
801 control_index = control_start_index_;
802 time_index = time_start_index_;
803
804 // First rate is compare first with stitch point
805 g[constraint_index] =
806 (x[control_index] - last_time_u_(0, 0)) / x[time_index] / ts_;
807 control_index += 2;
808 constraint_index++;
809 time_index++;
810
811 for (int i = 1; i < horizon_; ++i) {
812 g[constraint_index] =
813 (x[control_index] - x[control_index - 2]) / x[time_index] / ts_;
814 constraint_index++;
815 control_index += 2;
816 time_index++;
817 }
818
819 // 3. Time constraints 1 * [0, horizons-1]
820 time_index = time_start_index_;
821 for (int i = 0; i < horizon_; ++i) {
822 g[constraint_index] = x[time_index + 1] - x[time_index];
823 constraint_index++;
824 time_index++;
825 }
826
827 ADEBUG << "constraint_index after adding time constraints "
828 "updated: "
829 << constraint_index;
830
831 // 4. Three obstacles related equal constraints, one equality constraints,
832 // [0, horizon_] * [0, obstacles_num_-1] * 4
833 state_index = state_start_index_;
834 int l_index = l_start_index_;
835 int n_index = n_start_index_;
836
837 for (int i = 0; i < horizon_ + 1; ++i) {
838 int edges_counter = 0;
839 for (int j = 0; j < obstacles_num_; ++j) {
840 int current_edges_num = obstacles_edges_num_(j, 0);
841 Eigen::MatrixXd Aj =
842 obstacles_A_.block(edges_counter, 0, current_edges_num, 2);
843 Eigen::MatrixXd bj =
844 obstacles_b_.block(edges_counter, 0, current_edges_num, 1);
845
846 // norm(A* lambda) <= 1
847 T tmp1 = 0.0;
848 T tmp2 = 0.0;
849 for (int k = 0; k < current_edges_num; ++k) {
850 // TODO(QiL) : replace this one directly with x
851 tmp1 += Aj(k, 0) * x[l_index + k];
852 tmp2 += Aj(k, 1) * x[l_index + k];
853 }
854 g[constraint_index] = tmp1 * tmp1 + tmp2 * tmp2;
855
856 // G' * mu + R' * lambda == 0
857 g[constraint_index + 1] = x[n_index] - x[n_index + 2] +
858 cos(x[state_index + 2]) * tmp1 +
859 sin(x[state_index + 2]) * tmp2;
860
861 g[constraint_index + 2] = x[n_index + 1] - x[n_index + 3] -
862 sin(x[state_index + 2]) * tmp1 +
863 cos(x[state_index + 2]) * tmp2;
864
865 // -g'*mu + (A*t - b)*lambda > 0
866 T tmp3 = 0.0;
867 for (int k = 0; k < 4; ++k) {
868 tmp3 += -g_[k] * x[n_index + k];
869 }
870
871 T tmp4 = 0.0;
872 for (int k = 0; k < current_edges_num; ++k) {
873 tmp4 += bj(k, 0) * x[l_index + k];
874 }
875
876 g[constraint_index + 3] =
877 tmp3 + (x[state_index] + cos(x[state_index + 2]) * offset_) * tmp1 +
878 (x[state_index + 1] + sin(x[state_index + 2]) * offset_) * tmp2 -
879 tmp4;
880
881 // Update index
882 edges_counter += current_edges_num;
883 l_index += current_edges_num;
884 n_index += 4;
885 constraint_index += 4;
886 }
887 state_index += 4;
888 }
889 ADEBUG << "constraint_index after obstacles avoidance constraints "
890 "updated: "
891 << constraint_index;
892
893 // 5. load variable bounds as constraints
894 state_index = state_start_index_;
895 control_index = control_start_index_;
896 time_index = time_start_index_;
897 l_index = l_start_index_;
898 n_index = n_start_index_;
899
900 // start configuration
901 g[constraint_index] = x[state_index];
902 g[constraint_index + 1] = x[state_index + 1];
903 g[constraint_index + 2] = x[state_index + 2];
904 g[constraint_index + 3] = x[state_index + 3];
905 constraint_index += 4;
906 state_index += 4;
907
908 // constraints on x,y,v
909 for (int i = 1; i < horizon_; ++i) {
910 g[constraint_index] = x[state_index];
911 g[constraint_index + 1] = x[state_index + 1];
912 g[constraint_index + 2] = x[state_index + 3];
913 constraint_index += 3;
914 state_index += 4;
915 }
916
917 // end configuration
918 g[constraint_index] = x[state_index];
919 g[constraint_index + 1] = x[state_index + 1];
920 g[constraint_index + 2] = x[state_index + 2];
921 g[constraint_index + 3] = x[state_index + 3];
922 constraint_index += 4;
923 state_index += 4;
924
925 for (int i = 0; i < horizon_; ++i) {
926 g[constraint_index] = x[control_index];
927 g[constraint_index + 1] = x[control_index + 1];
928 constraint_index += 2;
929 control_index += 2;
930 }
931
932 for (int i = 0; i < horizon_ + 1; ++i) {
933 g[constraint_index] = x[time_index];
934 constraint_index++;
935 time_index++;
936 }
937
938 for (int i = 0; i < lambda_horizon_; ++i) {
939 g[constraint_index] = x[l_index];
940 constraint_index++;
941 l_index++;
942 }
943
944 for (int i = 0; i < miu_horizon_; ++i) {
945 g[constraint_index] = x[n_index];
946 constraint_index++;
947 n_index++;
948 }
949}
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 第 435 行定义.

437 {
438 eval_obj(n, x, &obj_value);
439 return true;
440}
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:660

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

451 {
452 eval_constraints(n, x, m, g);
453 // if (enable_constraint_check_) check_g(n, x, m, g);
454 return true;
455}
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:738

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

444 {
445 gradient(tag_f, n, x, grad_f);
446 return true;
447}
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 第 490 行定义.

493 {
494 if (values == nullptr) {
495 // return the structure. This is a symmetric matrix, fill the lower left
496 // triangle only.
497
498 for (int idx = 0; idx < nnz_L; idx++) {
499 iRow[idx] = rind_L[idx];
500 jCol[idx] = cind_L[idx];
501 }
502 } else {
503 // return the values. This is a symmetric matrix, fill the lower left
504 // triangle only
505
506 obj_lam[0] = obj_factor;
507
508 for (int idx = 0; idx < m; idx++) {
509 obj_lam[1 + idx] = lambda[idx];
510 }
511
512 set_param_vec(tag_L, m + 1, obj_lam);
513 sparse_hess(tag_L, n, 1, const_cast<double*>(x), &nnz_L, &rind_L, &cind_L,
514 &hessval, options_L);
515
516 for (int idx = 0; idx < nnz_L; idx++) {
517 values[idx] = hessval[idx];
518 }
519 }
520
521 return true;
522}
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 第 457 行定义.

461 {
462 if (enable_jacobian_ad_) {
463 if (values == nullptr) {
464 // return the structure of the jacobian
465 for (int idx = 0; idx < nnz_jac; idx++) {
466 iRow[idx] = rind_g[idx];
467 jCol[idx] = cind_g[idx];
468 }
469 } else {
470 // return the values of the jacobian of the constraints
471 sparse_jac(tag_g, m, n, 1, x, &nnz_jac, &rind_g, &cind_g, &jacval,
472 options_g);
473 for (int idx = 0; idx < nnz_jac; idx++) {
474 values[idx] = jacval[idx];
475 }
476 }
477 return true;
478 } else {
479 return eval_jac_g_ser(n, x, new_x, m, nele_jac, iRow, jCol, values);
480 }
481}
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:483
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 第 483 行定义.

485 {
486 AERROR << "NOT READY";
487 return false;
488}
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 第 660 行定义.

661 {
662 // Objective is :
663 // min control inputs
664 // min input rate
665 // min time (if the time step is not fixed)
666 // regularization wrt warm start trajectory
667 DCHECK(ts_ != 0) << "ts in distance_approach_ is 0";
668 int control_index = control_start_index_;
669 int time_index = time_start_index_;
670 int state_index = state_start_index_;
671
672 // TODO(QiL): Initial implementation towards earlier understanding and debug
673 // purpose, later code refine towards improving efficiency
674
675 *obj_value = 0.0;
676 // 1. objective to minimize state diff to warm up
677 for (int i = 0; i < horizon_ + 1; ++i) {
678 T x1_diff = x[state_index] - xWS_(0, i);
679 T x2_diff = x[state_index + 1] - xWS_(1, i);
680 T x3_diff = x[state_index + 2] - xWS_(2, i);
681 T x4_abs = x[state_index + 3];
682 *obj_value += weight_state_x_ * x1_diff * x1_diff +
683 weight_state_y_ * x2_diff * x2_diff +
684 weight_state_phi_ * x3_diff * x3_diff +
685 weight_state_v_ * x4_abs * x4_abs;
686 state_index += 4;
687 }
688
689 // 2. objective to minimize u square
690 for (int i = 0; i < horizon_; ++i) {
691 *obj_value += weight_input_steer_ * x[control_index] * x[control_index] +
692 weight_input_a_ * x[control_index + 1] * x[control_index + 1];
693 control_index += 2;
694 }
695
696 // 3. objective to minimize input change rate for first horizon
697 control_index = control_start_index_;
698 T last_time_steer_rate =
699 (x[control_index] - last_time_u_(0, 0)) / x[time_index] / ts_;
700 T last_time_a_rate =
701 (x[control_index + 1] - last_time_u_(1, 0)) / x[time_index] / ts_;
702 *obj_value +=
703 weight_stitching_steer_ * last_time_steer_rate * last_time_steer_rate +
704 weight_stitching_a_ * last_time_a_rate * last_time_a_rate;
705
706 // 4. objective to minimize input change rates, [0- horizon_ -2]
707 time_index++;
708 for (int i = 0; i < horizon_ - 1; ++i) {
709 T steering_rate =
710 (x[control_index + 2] - x[control_index]) / x[time_index] / ts_;
711 T a_rate =
712 (x[control_index + 3] - x[control_index + 1]) / x[time_index] / ts_;
713 *obj_value += weight_rate_steer_ * steering_rate * steering_rate +
714 weight_rate_a_ * a_rate * a_rate;
715 control_index += 2;
716 time_index++;
717 }
718
719 // 5. objective to minimize total time [0, horizon_]
720 time_index = time_start_index_;
721 for (int i = 0; i < horizon_ + 1; ++i) {
722 T first_order_penalty = weight_first_order_time_ * x[time_index];
723 T second_order_penalty =
724 weight_second_order_time_ * x[time_index] * x[time_index];
725 *obj_value += first_order_penalty + second_order_penalty;
726 time_index++;
727 }
728
729 // 6. end state constraints
730 for (int i = 0; i < 4; ++i) {
731 *obj_value += weight_end_state_ *
732 (x[state_start_index_ + 4 * horizon_ + i] - xf_(i, 0)) *
733 (x[state_start_index_ + 4 * horizon_ + i] - xf_(i, 0));
734 }
735}

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

528 {
529 int state_index = state_start_index_;
530 int control_index = control_start_index_;
531 int time_index = time_start_index_;
532 int dual_l_index = l_start_index_;
533 int dual_n_index = n_start_index_;
534
535 // enable_constraint_check_: for debug only
536 if (enable_constraint_check_) {
537 ADEBUG << "final resolution constraint checking";
538 check_g(n, x, m, g);
539 }
540 // 1. state variables, 4 * [0, horizon]
541 // 2. control variables, 2 * [0, horizon_-1]
542 // 3. sampling time variables, 1 * [0, horizon_]
543 // 4. dual_l obstacles_edges_sum_ * [0, horizon]
544 // 5. dual_n obstacles_num * [0, horizon]
545 for (int i = 0; i < horizon_; ++i) {
546 state_result_(0, i) = x[state_index];
547 state_result_(1, i) = x[state_index + 1];
548 state_result_(2, i) = x[state_index + 2];
549 state_result_(3, i) = x[state_index + 3];
550 control_result_(0, i) = x[control_index];
551 control_result_(1, i) = x[control_index + 1];
552 time_result_(0, i) = ts_ * x[time_index];
553 for (int j = 0; j < obstacles_edges_sum_; ++j) {
554 dual_l_result_(j, i) = x[dual_l_index + j];
555 }
556 for (int k = 0; k < 4 * obstacles_num_; k++) {
557 dual_n_result_(k, i) = x[dual_n_index + k];
558 }
559 state_index += 4;
560 control_index += 2;
561 time_index++;
562 dual_l_index += obstacles_edges_sum_;
563 dual_n_index += 4 * obstacles_num_;
564 }
565 state_result_(0, 0) = x0_(0, 0);
566 state_result_(1, 0) = x0_(1, 0);
567 state_result_(2, 0) = x0_(2, 0);
568 state_result_(3, 0) = x0_(3, 0);
569 // push back last horizon for states
570 state_result_(0, horizon_) = xf_(0, 0);
571 state_result_(1, horizon_) = xf_(1, 0);
572 state_result_(2, horizon_) = xf_(2, 0);
573 state_result_(3, horizon_) = xf_(3, 0);
574
575 for (int j = 0; j < obstacles_edges_sum_; ++j) {
576 dual_l_result_(j, horizon_) = x[dual_l_index + j];
577 }
578 for (int k = 0; k < 4 * obstacles_num_; k++) {
579 dual_n_result_(k, horizon_) = x[dual_n_index + k];
580 }
581 // memory deallocation of ADOL-C variables
582 delete[] obj_lam;
583 if (enable_jacobian_ad_) {
584 free(rind_g);
585 free(cind_g);
586 free(jacval);
587 }
588 free(rind_L);
589 free(cind_L);
590 free(hessval);
591}
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:951

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

1032 {
1033 std::vector<double> xp(n);
1034 std::vector<double> lamp(m);
1035 std::vector<double> zl(m);
1036 std::vector<double> zu(m);
1037 std::vector<adouble> xa(n);
1038 std::vector<adouble> g(m);
1039 std::vector<double> lam(m);
1040
1041 double sig = 0.0;
1042 adouble obj_value;
1043 double dummy = 0.0;
1044 obj_lam = new double[m + 1];
1045 get_starting_point(n, 1, &xp[0], 0, &zl[0], &zu[0], m, 0, &lamp[0]);
1046
1047 trace_on(tag_f);
1048 for (int idx = 0; idx < n; idx++) {
1049 xa[idx] <<= xp[idx];
1050 }
1051 eval_obj(n, &xa[0], &obj_value);
1052 obj_value >>= dummy;
1053 trace_off();
1054
1055 trace_on(tag_g);
1056 for (int idx = 0; idx < n; idx++) {
1057 xa[idx] <<= xp[idx];
1058 }
1059 eval_constraints(n, &xa[0], m, &g[0]);
1060 for (int idx = 0; idx < m; idx++) {
1061 g[idx] >>= dummy;
1062 }
1063 trace_off();
1064
1065 trace_on(tag_L);
1066 for (int idx = 0; idx < n; idx++) {
1067 xa[idx] <<= xp[idx];
1068 }
1069 for (int idx = 0; idx < m; idx++) {
1070 lam[idx] = 1.0;
1071 }
1072 sig = 1.0;
1073 eval_obj(n, &xa[0], &obj_value);
1074 obj_value *= mkparam(sig);
1075 eval_constraints(n, &xa[0], m, &g[0]);
1076 for (int idx = 0; idx < m; idx++) {
1077 obj_value += g[idx] * mkparam(lam[idx]);
1078 }
1079 obj_value >>= dummy;
1080
1081 trace_off();
1082
1083 if (enable_jacobian_ad_) {
1084 rind_g = nullptr;
1085 cind_g = nullptr;
1086 jacval = nullptr;
1087 options_g[0] = 0; /* sparsity pattern by index domains (default) */
1088 options_g[1] = 0; /* safe mode (default) */
1089 options_g[2] = 0;
1090 options_g[3] = 0; /* column compression (default) */
1091
1092 sparse_jac(tag_g, m, n, 0, &xp[0], &nnz_jac, &rind_g, &cind_g, &jacval,
1093 options_g);
1094 *nnz_jac_g = nnz_jac;
1095 }
1096
1097 rind_L = nullptr;
1098 cind_L = nullptr;
1099 hessval = nullptr;
1100 options_L[0] = 0;
1101 options_L[1] = 1;
1102
1103 sparse_hess(tag_L, n, 0, &xp[0], &nnz_L, &rind_L, &cind_L, &hessval,
1104 options_L);
1105 *nnz_h_lag = nnz_L;
1106}
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:386

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

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 // Constraints: includes four state Euler forward constraints, three
261 // Obstacle related constraints
262
263 // 1. dynamics constraints 4 * [0, horizons-1]
264 int constraint_index = 0;
265 for (int i = 0; i < 4 * horizon_; ++i) {
266 g_l[i] = 0.0;
267 g_u[i] = 0.0;
268 }
269 constraint_index += 4 * horizon_;
270
271 ADEBUG << "constraint_index after adding Euler forward dynamics constraints: "
272 << constraint_index;
273
274 // 2. Control rate limit constraints, 1 * [0, horizons-1], only apply
275 // steering rate as of now
276 for (int i = 0; i < horizon_; ++i) {
277 g_l[constraint_index] = -max_steer_rate_;
278 g_u[constraint_index] = max_steer_rate_;
279 ++constraint_index;
280 }
281
282 ADEBUG << "constraint_index after adding steering rate constraints: "
283 << constraint_index;
284
285 // 3. Time constraints 1 * [0, horizons-1]
286 for (int i = 0; i < horizon_; ++i) {
287 g_l[constraint_index] = 0.0;
288 g_u[constraint_index] = 0.0;
289 ++constraint_index;
290 }
291
292 ADEBUG << "constraint_index after adding time constraints: "
293 << constraint_index;
294
295 // 4. Three obstacles related equal constraints, one equality constraints,
296 // [0, horizon_] * [0, obstacles_num_-1] * 4
297 for (int i = 0; i < horizon_ + 1; ++i) {
298 for (int j = 0; j < obstacles_num_; ++j) {
299 // a. norm(A'*lambda) <= 1
300 g_l[constraint_index] = -2e19;
301 g_u[constraint_index] = 1.0;
302
303 // b. G'*mu + R'*A*lambda = 0
304 g_l[constraint_index + 1] = 0.0;
305 g_u[constraint_index + 1] = 0.0;
306 g_l[constraint_index + 2] = 0.0;
307 g_u[constraint_index + 2] = 0.0;
308
309 // c. -g'*mu + (A*t - b)*lambda > min_safety_distance_
310 g_l[constraint_index + 3] = min_safety_distance_;
311 g_u[constraint_index + 3] = 2e19; // nlp_upper_bound_limit
312 constraint_index += 4;
313 }
314 }
315
316 // 5. load variable bounds as constraints
317 // start configuration
318 g_l[constraint_index] = x0_(0, 0);
319 g_u[constraint_index] = x0_(0, 0);
320 g_l[constraint_index + 1] = x0_(1, 0);
321 g_u[constraint_index + 1] = x0_(1, 0);
322 g_l[constraint_index + 2] = x0_(2, 0);
323 g_u[constraint_index + 2] = x0_(2, 0);
324 g_l[constraint_index + 3] = x0_(3, 0);
325 g_u[constraint_index + 3] = x0_(3, 0);
326 constraint_index += 4;
327
328 for (int i = 1; i < horizon_; ++i) {
329 g_l[constraint_index] = XYbounds_[0];
330 g_u[constraint_index] = XYbounds_[1];
331 g_l[constraint_index + 1] = XYbounds_[2];
332 g_u[constraint_index + 1] = XYbounds_[3];
333 g_l[constraint_index + 2] = -max_speed_reverse_;
334 g_u[constraint_index + 2] = max_speed_forward_;
335 constraint_index += 3;
336 }
337
338 // end configuration
339 g_l[constraint_index] = -2e19;
340 g_u[constraint_index] = 2e19;
341 g_l[constraint_index + 1] = -2e19;
342 g_u[constraint_index + 1] = 2e19;
343 g_l[constraint_index + 2] = -2e19;
344 g_u[constraint_index + 2] = 2e19;
345 g_l[constraint_index + 3] = -2e19;
346 g_u[constraint_index + 3] = 2e19;
347 constraint_index += 4;
348
349 for (int i = 0; i < horizon_; ++i) {
350 g_l[constraint_index] = -max_steer_angle_;
351 g_u[constraint_index] = max_steer_angle_;
352 g_l[constraint_index + 1] = -max_acceleration_reverse_;
353 g_u[constraint_index + 1] = max_acceleration_forward_;
354 constraint_index += 2;
355 }
356
357 for (int i = 0; i < horizon_ + 1; ++i) {
358 if (!use_fix_time_) {
359 g_l[constraint_index] = min_time_sample_scaling_;
360 g_u[constraint_index] = max_time_sample_scaling_;
361 } else {
362 g_l[constraint_index] = 1.0;
363 g_u[constraint_index] = 1.0;
364 }
365 constraint_index++;
366 }
367
368 for (int i = 0; i < lambda_horizon_; ++i) {
369 g_l[constraint_index] = 0.0;
370 g_u[constraint_index] = 2e19;
371 constraint_index++;
372 }
373
374 for (int i = 0; i < miu_horizon_; ++i) {
375 g_l[constraint_index] = 0.0;
376 g_u[constraint_index] = 2e19;
377 constraint_index++;
378 }
379
380 ADEBUG << "constraint_index after adding obstacles constraints: "
381 << constraint_index;
382 ADEBUG << "get_bounds_info_ out";
383 return true;
384}

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

111 {
112 ADEBUG << "get_nlp_info";
113 // n1 : states variables, 4 * (N+1)
114 int n1 = 4 * (horizon_ + 1);
115 ADEBUG << "n1: " << n1;
116 // n2 : control inputs variables
117 int n2 = 2 * horizon_;
118 ADEBUG << "n2: " << n2;
119 // n3 : sampling time variables
120 int n3 = horizon_ + 1;
121 ADEBUG << "n3: " << n3;
122 // n4 : dual multiplier associated with obstacle shape
123 lambda_horizon_ = obstacles_edges_num_.sum() * (horizon_ + 1);
124 ADEBUG << "lambda_horizon_: " << lambda_horizon_;
125 // n5 : dual multipier associated with car shape, obstacles_num*4 * (N+1)
126 miu_horizon_ = obstacles_num_ * 4 * (horizon_ + 1);
127 ADEBUG << "miu_horizon_: " << miu_horizon_;
128
129 // m1 : dynamics constatins
130 int m1 = 4 * horizon_;
131 // m2 : control rate constraints (only steering)
132 int m2 = horizon_;
133 // m3 : sampling time equality constraints
134 int m3 = horizon_;
135 // m4 : obstacle constraints
136 int m4 = 4 * obstacles_num_ * (horizon_ + 1);
137
138 num_of_variables_ = n1 + n2 + n3 + lambda_horizon_ + miu_horizon_;
139 num_of_constraints_ =
140 m1 + m2 + m3 + m4 + (num_of_variables_ - (horizon_ + 1) + 2);
141
142 // number of variables
143 n = num_of_variables_;
144 ADEBUG << "num_of_variables_ " << num_of_variables_;
145 // number of constraints
146 m = num_of_constraints_;
147 ADEBUG << "num_of_constraints_ " << num_of_constraints_;
148
149 generate_tapes(n, m, &nnz_jac_g, &nnz_h_lag);
150 // number of nonzero in Jacobian.
151 if (!enable_jacobian_ad_) {
152 int tmp = 0;
153 for (int i = 0; i < horizon_ + 1; ++i) {
154 for (int j = 0; j < obstacles_num_; ++j) {
155 int current_edges_num = obstacles_edges_num_(j, 0);
156 tmp += current_edges_num * 4 + 9 + 4;
157 }
158 }
159 nnz_jac_g = 24 * horizon_ + 3 * horizon_ + 2 * horizon_ + tmp - 1 +
160 (num_of_variables_ - (horizon_ + 1) + 2);
161 }
162
163 index_style = IndexStyleEnum::C_STYLE;
164 return true;
165}
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:1030

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

596 {
597 ADEBUG << "get_optimization_results";
598 *state_result = state_result_;
599 *control_result = control_result_;
600 *time_result = time_result_;
601 *dual_l_result = dual_l_result_;
602 *dual_n_result = dual_n_result_;
603
604 if (!distance_approach_config_.enable_initial_final_check()) {
605 return;
606 }
607 CHECK_EQ(state_result_.cols(), xWS_.cols());
608 CHECK_EQ(state_result_.rows(), xWS_.rows());
609 double state_diff_max = 0.0;
610 for (int i = 0; i < horizon_ + 1; ++i) {
611 for (int j = 0; j < 4; ++j) {
612 state_diff_max =
613 std::max(std::abs(xWS_(j, i) - state_result_(j, i)), state_diff_max);
614 }
615 }
616
617 // 2. control variable initialization, 2 * horizon_
618 CHECK_EQ(control_result_.cols(), uWS_.cols());
619 CHECK_EQ(control_result_.rows(), uWS_.rows());
620 double control_diff_max = 0.0;
621 for (int i = 0; i < horizon_; ++i) {
622 control_diff_max = std::max(std::abs(uWS_(0, i) - control_result_(0, i)),
623 control_diff_max);
624 control_diff_max = std::max(std::abs(uWS_(1, i) - control_result_(1, i)),
625 control_diff_max);
626 }
627
628 // 2. time scale variable initialization, horizon_ + 1
629
630 // 3. lagrange constraint l, obstacles_edges_sum_ * (horizon_+1)
631 CHECK_EQ(dual_l_result_.cols(), l_warm_up_.cols());
632 CHECK_EQ(dual_l_result_.rows(), l_warm_up_.rows());
633 double l_diff_max = 0.0;
634 for (int i = 0; i < horizon_ + 1; ++i) {
635 for (int j = 0; j < obstacles_edges_sum_; ++j) {
636 l_diff_max = std::max(std::abs(l_warm_up_(j, i) - dual_l_result_(j, i)),
637 l_diff_max);
638 }
639 }
640
641 // 4. lagrange constraint m, 4*obstacles_num * (horizon_+1)
642 CHECK_EQ(n_warm_up_.cols(), dual_n_result_.cols());
643 CHECK_EQ(n_warm_up_.rows(), dual_n_result_.rows());
644 double n_diff_max = 0.0;
645 for (int i = 0; i < horizon_ + 1; ++i) {
646 for (int j = 0; j < 4 * obstacles_num_; ++j) {
647 n_diff_max = std::max(std::abs(n_warm_up_(j, i) - dual_n_result_(j, i)),
648 n_diff_max);
649 }
650 }
651
652 ADEBUG << "state_diff_max: " << state_diff_max;
653 ADEBUG << "control_diff_max: " << control_diff_max;
654 ADEBUG << "dual_l_diff_max: " << l_diff_max;
655 ADEBUG << "dual_n_diff_max: " << n_diff_max;
656}

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

388 {
389 ADEBUG << "get_starting_point";
390 ACHECK(init_x) << "Warm start init_x setting failed";
391
392 CHECK_EQ(horizon_, uWS_.cols());
393 CHECK_EQ(horizon_ + 1, xWS_.cols());
394
395 // 1. state variables 4 * (horizon_ + 1)
396 for (int i = 0; i < horizon_ + 1; ++i) {
397 int index = i * 4;
398 for (int j = 0; j < 4; ++j) {
399 x[index + j] = xWS_(j, i);
400 }
401 }
402
403 // 2. control variable initialization, 2 * horizon_
404 for (int i = 0; i < horizon_; ++i) {
405 int index = i * 2;
406 x[control_start_index_ + index] = uWS_(0, i);
407 x[control_start_index_ + index + 1] = uWS_(1, i);
408 }
409
410 // 2. time scale variable initialization, horizon_ + 1
411 for (int i = 0; i < horizon_ + 1; ++i) {
412 x[time_start_index_ + i] =
413 0.5 * (min_time_sample_scaling_ + max_time_sample_scaling_);
414 }
415
416 // 3. lagrange constraint l, obstacles_edges_sum_ * (horizon_+1)
417 for (int i = 0; i < horizon_ + 1; ++i) {
418 int index = i * obstacles_edges_sum_;
419 for (int j = 0; j < obstacles_edges_sum_; ++j) {
420 x[l_start_index_ + index + j] = l_warm_up_(j, i);
421 }
422 }
423
424 // 4. lagrange constraint m, 4*obstacles_num * (horizon_+1)
425 for (int i = 0; i < horizon_ + 1; ++i) {
426 int index = i * 4 * obstacles_num_;
427 for (int j = 0; j < 4 * obstacles_num_; ++j) {
428 x[n_start_index_ + index + j] = n_warm_up_(j, i);
429 }
430 }
431 ADEBUG << "get_starting_point out";
432 return true;
433}
该类的文档由以下文件生成: