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

#include <distance_approach_ipopt_fixed_dual_interface.h>

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

Public 成员函数

 DistanceApproachIPOPTFixedDualInterface (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 ~DistanceApproachIPOPTFixedDualInterface ()=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
 
 DistanceApproachIPOPTFixedDualInterface (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_fixed_dual_interface.h 第 58 行定义.

构造及析构函数说明

◆ DistanceApproachIPOPTFixedDualInterface() [1/2]

apollo::planning::DistanceApproachIPOPTFixedDualInterface::DistanceApproachIPOPTFixedDualInterface ( 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_fixed_dual_interface.cc 第 24 行定义.

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

◆ ~DistanceApproachIPOPTFixedDualInterface()

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

◆ DistanceApproachIPOPTFixedDualInterface() [2/2]

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

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

成员函数说明

◆ check_g()

bool apollo::planning::DistanceApproachIPOPTFixedDualInterface::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_fixed_dual_interface.cc 第 871 行定义.

871 {
872 int kN = n;
873 int kM = m;
874 double x_u_tmp[kN];
875 double x_l_tmp[kN];
876 double g_u_tmp[kM];
877 double g_l_tmp[kM];
878
879 get_bounds_info(n, x_l_tmp, x_u_tmp, m, g_l_tmp, g_u_tmp);
880
881 const double delta_v = 1e-4;
882 for (int idx = 0; idx < n; ++idx) {
883 x_u_tmp[idx] = x_u_tmp[idx] + delta_v;
884 x_l_tmp[idx] = x_l_tmp[idx] - delta_v;
885 if (x[idx] > x_u_tmp[idx] || x[idx] < x_l_tmp[idx]) {
886 AINFO << "x idx unfeasible: " << idx << ", x: " << x[idx] << ", lower: " << x_l_tmp[idx]
887 << ", upper: " << x_u_tmp[idx];
888 }
889 }
890
891 // m1 : dynamics constatins
892 int m1 = 4 * horizon_;
893
894 // m2 : control rate constraints (only steering)
895 int m2 = m1 + horizon_;
896
897 // m3 : sampling time equality constraints
898 int m3 = m2 + horizon_;
899
900 // m4 : obstacle constraints
901 // int m4 = m3 + 4 * obstacles_num_ * (horizon_ + 1);
902
903 // 5. load variable bounds as constraints
904 // start configuration
905 int m5 = m3 + 3 + 1;
906
907 // constraints on x,y,v
908 int m6 = m5 + 3 * (horizon_ - 1);
909
910 // end configuration
911 int m7 = m6 + 3 + 1;
912
913 // control variable bnd
914 int m8 = m7 + 2 * horizon_;
915
916 // time interval variable bnd
917 int m9 = m8 + (horizon_ + 1);
918
919 // lambda_horizon_
920 // int m10 = m9 + lambda_horizon_;
921
922 // miu_horizon_
923 // int m11 = m10 + miu_horizon_;
924
925 CHECK_EQ(m9, num_of_constraints_);
926
927 AINFO << "dynamics constatins to: " << m1;
928 AINFO << "control rate constraints (only steering) to: " << m2;
929 AINFO << "sampling time equality constraints to: " << m3;
930 AINFO << "start conf constraints to: " << m5;
931 AINFO << "constraints on x,y,v to: " << m6;
932 AINFO << "end constraints to: " << m7;
933 AINFO << "control bnd to: " << m8;
934 AINFO << "time interval constraints to: " << m9;
935 AINFO << "total constraints: " << num_of_constraints_;
936
937 for (int idx = 0; idx < m; ++idx) {
938 if (g[idx] > g_u_tmp[idx] + delta_v || g[idx] < g_l_tmp[idx] - delta_v) {
939 AINFO << "constratins idx unfeasible: " << idx << ", g: " << g[idx] << ", lower: " << g_l_tmp[idx]
940 << ", upper: " << g_u_tmp[idx];
941 }
942 }
943 return true;
944}
apollo::planning::DistanceApproachIPOPTFixedDualInterface::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_fixed_dual_interface.cc:232
AINFO
#define AINFO
Definition log.h:42

◆ eval_constraints()

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

Template to compute constraints

在文件 distance_approach_ipopt_fixed_dual_interface.cc 第 741 行定义.

741 {
742 // state start index
743 int state_index = state_start_index_;
744
745 // control start index.
746 int control_index = control_start_index_;
747
748 // time start index
749 int time_index = time_start_index_;
750
751 int constraint_index = 0;
752
753 // 1. state constraints 4 * [0, horizons-1]
754 for (int i = 0; i < horizon_; ++i) {
755 // x1
756 g[constraint_index] = x[state_index + 4]
757 - (x[state_index]
758 + ts_ * x[time_index] * (x[state_index + 3] + ts_ * x[time_index] * 0.5 * x[control_index + 1])
759 * cos(x[state_index + 2]
760 + ts_ * x[time_index] * 0.5 * x[state_index + 3] * tan(x[control_index])
761 / wheelbase_));
762
763 // x2
764 g[constraint_index + 1] = x[state_index + 5]
765 - (x[state_index + 1]
766 + ts_ * x[time_index] * (x[state_index + 3] + ts_ * x[time_index] * 0.5 * x[control_index + 1])
767 * sin(x[state_index + 2]
768 + ts_ * x[time_index] * 0.5 * x[state_index + 3] * tan(x[control_index])
769 / wheelbase_));
770
771 // x3
772 g[constraint_index + 2] = x[state_index + 6]
773 - (x[state_index + 2]
774 + ts_ * x[time_index] * (x[state_index + 3] + ts_ * x[time_index] * 0.5 * x[control_index + 1])
775 * tan(x[control_index]) / wheelbase_);
776
777 // x4
778 g[constraint_index + 3]
779 = x[state_index + 7] - (x[state_index + 3] + ts_ * x[time_index] * x[control_index + 1]);
780
781 control_index += 2;
782 constraint_index += 4;
783 time_index++;
784 state_index += 4;
785 }
786
787 ADEBUG << "constraint_index after adding Euler forward dynamics constraints "
788 "updated: "
789 << constraint_index;
790
791 // 2. Control rate limit constraints, 1 * [0, horizons-1], only apply
792 // steering rate as of now
793 control_index = control_start_index_;
794 time_index = time_start_index_;
795
796 // First rate is compare first with stitch point
797 g[constraint_index] = (x[control_index] - last_time_u_(0, 0)) / x[time_index] / ts_;
798 control_index += 2;
799 constraint_index++;
800 time_index++;
801
802 for (int i = 1; i < horizon_; ++i) {
803 g[constraint_index] = (x[control_index] - x[control_index - 2]) / x[time_index] / ts_;
804 constraint_index++;
805 control_index += 2;
806 time_index++;
807 }
808
809 // 3. Time constraints 1 * [0, horizons-1]
810 time_index = time_start_index_;
811 for (int i = 0; i < horizon_; ++i) {
812 g[constraint_index] = x[time_index + 1] - x[time_index];
813 constraint_index++;
814 time_index++;
815 }
816
817 ADEBUG << "constraint_index after adding time constraints "
818 "updated: "
819 << constraint_index;
820
821 // 4. Three obstacles related equal constraints, one equality constraints,
822 // [0, horizon_] * [0, obstacles_num_-1] * 4
823 state_index = state_start_index_;
824
825 // 5. load variable bounds as constraints
826 state_index = state_start_index_;
827 control_index = control_start_index_;
828 time_index = time_start_index_;
829
830 // start configuration
831 g[constraint_index] = x[state_index];
832 g[constraint_index + 1] = x[state_index + 1];
833 g[constraint_index + 2] = x[state_index + 2];
834 g[constraint_index + 3] = x[state_index + 3];
835 constraint_index += 4;
836 state_index += 4;
837
838 // constraints on x,y,v
839 for (int i = 1; i < horizon_; ++i) {
840 g[constraint_index] = x[state_index];
841 g[constraint_index + 1] = x[state_index + 1];
842 g[constraint_index + 2] = x[state_index + 3];
843 constraint_index += 3;
844 state_index += 4;
845 }
846
847 // end configuration
848 g[constraint_index] = x[state_index];
849 g[constraint_index + 1] = x[state_index + 1];
850 g[constraint_index + 2] = x[state_index + 2];
851 g[constraint_index + 3] = x[state_index + 3];
852 constraint_index += 4;
853 state_index += 4;
854
855 for (int i = 0; i < horizon_; ++i) {
856 g[constraint_index] = x[control_index];
857 g[constraint_index + 1] = x[control_index + 1];
858 constraint_index += 2;
859 control_index += 2;
860 }
861
862 for (int i = 0; i < horizon_ + 1; ++i) {
863 g[constraint_index] = x[time_index];
864 constraint_index++;
865 time_index++;
866 }
867
868 ADEBUG << "constraint_index after adding variable box constraints updated: " << constraint_index;
869}
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::DistanceApproachIPOPTFixedDualInterface::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_fixed_dual_interface.cc 第 444 行定义.

444 {
445 eval_obj(n, x, &obj_value);
446 return true;
447}
apollo::planning::DistanceApproachIPOPTFixedDualInterface::eval_obj
void eval_obj(int n, const T *x, T *obj_value)
Template to return the objective value
Definition distance_approach_ipopt_fixed_dual_interface.cc:680

◆ eval_g()

bool apollo::planning::DistanceApproachIPOPTFixedDualInterface::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_fixed_dual_interface.cc 第 454 行定义.

454 {
455 eval_constraints(n, x, m, g);
456 // if (enable_constraint_check_) check_g(n, x, m, g);
457 return true;
458}
apollo::planning::DistanceApproachIPOPTFixedDualInterface::eval_constraints
void eval_constraints(int n, const T *x, int m, T *g)
Template to compute constraints
Definition distance_approach_ipopt_fixed_dual_interface.cc:741

◆ eval_grad_f()

bool apollo::planning::DistanceApproachIPOPTFixedDualInterface::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_fixed_dual_interface.cc 第 449 行定义.

449 {
450 gradient(tag_f, n, x, grad_f);
451 return true;
452}
tag_f
#define tag_f
Definition cos_theta_ipopt_interface.h:36

◆ eval_h()

bool apollo::planning::DistanceApproachIPOPTFixedDualInterface::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_fixed_dual_interface.cc 第 499 行定义.

510 {
511 if (values == nullptr) {
512 // return the structure. This is a symmetric matrix, fill the lower left
513 // triangle only.
514
515 for (int idx = 0; idx < nnz_L; idx++) {
516 iRow[idx] = rind_L[idx];
517 jCol[idx] = cind_L[idx];
518 }
519 } else {
520 // return the values. This is a symmetric matrix, fill the lower left
521 // triangle only
522
523 obj_lam[0] = obj_factor;
524
525 for (int idx = 0; idx < m; idx++) {
526 obj_lam[1 + idx] = lambda[idx];
527 }
528
529 set_param_vec(tag_L, m + 1, obj_lam);
530 sparse_hess(tag_L, n, 1, const_cast<double*>(x), &nnz_L, &rind_L, &cind_L, &hessval, options_L);
531
532 for (int idx = 0; idx < nnz_L; idx++) {
533 values[idx] = hessval[idx];
534 }
535 }
536
537 return true;
538}
tag_L
#define tag_L
Definition cos_theta_ipopt_interface.h:38

◆ eval_jac_g()

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

468 {
469 if (values == nullptr) {
470 ADEBUG << "nnz_jac: " << nnz_jac;
471 // return the structure of the jacobian
472 for (int idx = 0; idx < nnz_jac; idx++) {
473 iRow[idx] = rind_g[idx];
474 jCol[idx] = cind_g[idx];
475 }
476 } else {
477 // return the values of the jacobian of the constraints
478 sparse_jac(tag_g, m, n, 1, x, &nnz_jac, &rind_g, &cind_g, &jacval, options_g);
479 for (int idx = 0; idx < nnz_jac; idx++) {
480 values[idx] = jacval[idx];
481 }
482 }
483 return true;
484}
tag_g
#define tag_g
Definition cos_theta_ipopt_interface.h:37

◆ eval_jac_g_ser()

bool apollo::planning::DistanceApproachIPOPTFixedDualInterface::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_fixed_dual_interface.cc 第 486 行定义.

494 {
495 AERROR << "not ready ATM!";
496 return false;
497}
AERROR
#define AERROR
Definition log.h:44

◆ eval_obj()

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

Template to return the objective value

在文件 distance_approach_ipopt_fixed_dual_interface.cc 第 680 行定义.

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

◆ finalize_solution()

void apollo::planning::DistanceApproachIPOPTFixedDualInterface::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_fixed_dual_interface.cc 第 540 行定义.

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

◆ generate_tapes()

void apollo::planning::DistanceApproachIPOPTFixedDualInterface::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_fixed_dual_interface.cc 第 946 行定义.

946 {
947 std::vector<double> xp(n);
948 std::vector<double> lamp(m);
949 std::vector<double> zl(m);
950 std::vector<double> zu(m);
951 std::vector<adouble> xa(n);
952 std::vector<adouble> g(m);
953 std::vector<double> lam(m);
954
955 double sig = 0.0;
956 adouble obj_value;
957 double dummy = 0.0;
958 obj_lam = new double[m + 1];
959 get_starting_point(n, 1, &xp[0], 0, &zl[0], &zu[0], m, 0, &lamp[0]);
960
961 trace_on(tag_f);
962 for (int idx = 0; idx < n; idx++) {
963 xa[idx] <<= xp[idx];
964 }
965 eval_obj(n, &xa[0], &obj_value);
966 obj_value >>= dummy;
967 trace_off();
968
969 trace_on(tag_g);
970 for (int idx = 0; idx < n; idx++) {
971 xa[idx] <<= xp[idx];
972 }
973 eval_constraints(n, &xa[0], m, &g[0]);
974 for (int idx = 0; idx < m; idx++) {
975 g[idx] >>= dummy;
976 }
977 trace_off();
978
979 trace_on(tag_L);
980 for (int idx = 0; idx < n; idx++) {
981 xa[idx] <<= xp[idx];
982 }
983 for (int idx = 0; idx < m; idx++) {
984 lam[idx] = 1.0;
985 }
986 sig = 1.0;
987 eval_obj(n, &xa[0], &obj_value);
988 obj_value *= mkparam(sig);
989 eval_constraints(n, &xa[0], m, &g[0]);
990 for (int idx = 0; idx < m; idx++) {
991 obj_value += g[idx] * mkparam(lam[idx]);
992 }
993 obj_value >>= dummy;
994
995 trace_off();
996
997 rind_g = nullptr;
998 cind_g = nullptr;
999 jacval = nullptr;
1000 options_g[0] = 0; /* sparsity pattern by index domains (default) */
1001 options_g[1] = 0; /* safe mode (default) */
1002 options_g[2] = 0;
1003 options_g[3] = 0; /* column compression (default) */
1004
1005 sparse_jac(tag_g, m, n, 0, &xp[0], &nnz_jac, &rind_g, &cind_g, &jacval, options_g);
1006 *nnz_jac_g = nnz_jac;
1007
1008 rind_L = nullptr;
1009 cind_L = nullptr;
1010 hessval = nullptr;
1011 options_L[0] = 0;
1012 options_L[1] = 1;
1013
1014 sparse_hess(tag_L, n, 0, &xp[0], &nnz_L, &rind_L, &cind_L, &hessval, options_L);
1015 *nnz_h_lag = nnz_L;
1016}
apollo::planning::DistanceApproachIPOPTFixedDualInterface::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_fixed_dual_interface.cc:396

◆ get_bounds_info()

bool apollo::planning::DistanceApproachIPOPTFixedDualInterface::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_fixed_dual_interface.cc 第 232 行定义.

238 {
239 ADEBUG << "get_bounds_info";
240 ACHECK(XYbounds_.size() == 4) << "XYbounds_ size is not 4, but" << XYbounds_.size();
241
242 // Variables: includes state, u, sample time and lagrange multipliers
243 // 1. state variables, 4 * [0, horizon]
244 // start point pose
245 int variable_index = 0;
246 for (int i = 0; i < 4; ++i) {
247 x_l[i] = -2e19;
248 x_u[i] = 2e19;
249 }
250 variable_index += 4;
251
252 // During horizons, 2 ~ N-1
253 for (int i = 1; i < horizon_; ++i) {
254 // x
255 x_l[variable_index] = -2e19;
256 x_u[variable_index] = 2e19;
257
258 // y
259 x_l[variable_index + 1] = -2e19;
260 x_u[variable_index + 1] = 2e19;
261
262 // phi
263 x_l[variable_index + 2] = -2e19;
264 x_u[variable_index + 2] = 2e19;
265
266 // v
267 x_l[variable_index + 3] = -2e19;
268 x_u[variable_index + 3] = 2e19;
269
270 variable_index += 4;
271 }
272
273 // end point pose
274 for (int i = 0; i < 4; ++i) {
275 x_l[variable_index + i] = -2e19;
276 x_u[variable_index + i] = 2e19;
277 }
278 variable_index += 4;
279 ADEBUG << "variable_index after adding state variables : " << variable_index;
280
281 // 2. control variables, 2 * [0, horizon_-1]
282 for (int i = 0; i < horizon_; ++i) {
283 // steer
284 x_l[variable_index] = -2e19;
285 x_u[variable_index] = 2e19;
286
287 // a
288 x_l[variable_index + 1] = -2e19;
289 x_u[variable_index + 1] = 2e19;
290
291 variable_index += 2;
292 }
293 ADEBUG << "variable_index after adding control variables : " << variable_index;
294
295 // 3. sampling time variables, 1 * [0, horizon_]
296 for (int i = 0; i < horizon_ + 1; ++i) {
297 x_l[variable_index] = -2e19;
298 x_u[variable_index] = 2e19;
299 ++variable_index;
300 }
301 ADEBUG << "variable_index after adding sample time : " << variable_index;
302 ADEBUG << "sample time fix time status is : " << use_fix_time_;
303
304 // Constraints: includes four state Euler forward constraints, three
305 // Obstacle related constraints
306
307 // 1. dynamics constraints 4 * [0, horizons-1]
308 int constraint_index = 0;
309 for (int i = 0; i < 4 * horizon_; ++i) {
310 g_l[i] = 0.0;
311 g_u[i] = 0.0;
312 }
313 constraint_index += 4 * horizon_;
314
315 ADEBUG << "constraint_index after adding Euler forward dynamics constraints: " << constraint_index;
316
317 // 2. Control rate limit constraints, 1 * [0, horizons-1], only apply
318 // steering rate as of now
319 for (int i = 0; i < horizon_; ++i) {
320 g_l[constraint_index] = -max_steer_rate_;
321 g_u[constraint_index] = max_steer_rate_;
322 ++constraint_index;
323 }
324
325 ADEBUG << "constraint_index after adding steering rate constraints: " << constraint_index;
326
327 // 3. Time constraints 1 * [0, horizons-1]
328 for (int i = 0; i < horizon_; ++i) {
329 g_l[constraint_index] = 0.0;
330 g_u[constraint_index] = 0.0;
331 ++constraint_index;
332 }
333
334 ADEBUG << "constraint_index after adding time constraints: " << constraint_index;
335
336 // 4. Three obstacles related equal constraints, one equality constraints,
337 // [0, horizon_] * [0, obstacles_num_-1] * 4
338
339 // 5. load variable bounds as constraints
340 // start configuration
341 g_l[constraint_index] = x0_(0, 0);
342 g_u[constraint_index] = x0_(0, 0);
343 g_l[constraint_index + 1] = x0_(1, 0);
344 g_u[constraint_index + 1] = x0_(1, 0);
345 g_l[constraint_index + 2] = x0_(2, 0);
346 g_u[constraint_index + 2] = x0_(2, 0);
347 g_l[constraint_index + 3] = x0_(3, 0);
348 g_u[constraint_index + 3] = x0_(3, 0);
349 constraint_index += 4;
350
351 for (int i = 1; i < horizon_; ++i) {
352 g_l[constraint_index] = XYbounds_[0];
353 g_u[constraint_index] = XYbounds_[1];
354 g_l[constraint_index + 1] = XYbounds_[2];
355 g_u[constraint_index + 1] = XYbounds_[3];
356 g_l[constraint_index + 2] = -max_speed_reverse_;
357 g_u[constraint_index + 2] = max_speed_forward_;
358 constraint_index += 3;
359 }
360
361 // end configuration
362 g_l[constraint_index] = xf_(0, 0);
363 g_u[constraint_index] = xf_(0, 0);
364 g_l[constraint_index + 1] = xf_(1, 0);
365 g_u[constraint_index + 1] = xf_(1, 0);
366 g_l[constraint_index + 2] = xf_(2, 0);
367 g_u[constraint_index + 2] = xf_(2, 0);
368 g_l[constraint_index + 3] = xf_(3, 0);
369 g_u[constraint_index + 3] = xf_(3, 0);
370 constraint_index += 4;
371
372 for (int i = 0; i < horizon_; ++i) {
373 g_l[constraint_index] = -max_steer_angle_;
374 g_u[constraint_index] = max_steer_angle_;
375 g_l[constraint_index + 1] = -max_acceleration_reverse_;
376 g_u[constraint_index + 1] = max_acceleration_forward_;
377 constraint_index += 2;
378 }
379
380 for (int i = 0; i < horizon_ + 1; ++i) {
381 if (!use_fix_time_) {
382 g_l[constraint_index] = min_time_sample_scaling_;
383 g_u[constraint_index] = max_time_sample_scaling_;
384 } else {
385 g_l[constraint_index] = 1.0;
386 g_u[constraint_index] = 1.0;
387 }
388 constraint_index++;
389 }
390
391 ADEBUG << "constraint_index after adding obstacles constraints: " << constraint_index;
392 ADEBUG << "get_bounds_info_ out";
393 return true;
394}

◆ get_nlp_info()

bool apollo::planning::DistanceApproachIPOPTFixedDualInterface::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_fixed_dual_interface.cc 第 186 行定义.

191 {
192 ADEBUG << "get_nlp_info";
193 // n1 : states variables, 4 * (N+1)
194 int n1 = 4 * (horizon_ + 1);
195 ADEBUG << "n1: " << n1;
196 // n2 : control inputs variables
197 int n2 = 2 * horizon_;
198 ADEBUG << "n2: " << n2;
199 // n3 : sampling time variables
200 int n3 = horizon_ + 1;
201 ADEBUG << "n3: " << n3;
202 // n4 : dual multiplier associated with obstacle shape
203 lambda_horizon_ = 0;
204 // n5 : dual multipier associated with car shape, obstacles_num*4 * (N+1)
205 miu_horizon_ = 0;
206
207 // m1 : dynamics constatins
208 int m1 = 4 * horizon_;
209 // m2 : control rate constraints (only steering)
210 int m2 = horizon_;
211 // m3 : sampling time equality constraints
212 int m3 = horizon_;
213 // m4 : obstacle constraints
214 // int m4 = 4 * obstacles_num_ * (horizon_ + 1);
215
216 num_of_variables_ = n1 + n2 + n3;
217 num_of_constraints_ = m1 + m2 + m3 + (num_of_variables_ - (horizon_ + 1) + 2);
218
219 // number of variables
220 n = num_of_variables_;
221 ADEBUG << "num_of_variables_ " << num_of_variables_;
222 // number of constraints
223 m = num_of_constraints_;
224 ADEBUG << "num_of_constraints_ " << num_of_constraints_;
225
226 generate_tapes(n, m, &nnz_jac_g, &nnz_h_lag);
227
228 index_style = IndexStyleEnum::C_STYLE;
229 return true;
230}
apollo::planning::DistanceApproachIPOPTFixedDualInterface::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_fixed_dual_interface.cc:946

◆ get_optimization_results()

void apollo::planning::DistanceApproachIPOPTFixedDualInterface::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_fixed_dual_interface.cc 第 616 行定义.

621 {
622 ADEBUG << "get_optimization_results";
623 *state_result = state_result_;
624 *control_result = control_result_;
625 *time_result = time_result_;
626 *dual_l_result = dual_l_result_;
627 *dual_n_result = dual_n_result_;
628
629 if (!distance_approach_config_.enable_initial_final_check()) {
630 return;
631 }
632 CHECK_EQ(state_result_.cols(), xWS_.cols());
633 CHECK_EQ(state_result_.rows(), xWS_.rows());
634 double state_diff_max = 0.0;
635 for (int i = 0; i < horizon_ + 1; ++i) {
636 for (int j = 0; j < 4; ++j) {
637 state_diff_max = std::max(std::abs(xWS_(j, i) - state_result_(j, i)), state_diff_max);
638 }
639 }
640
641 // 2. control variable initialization, 2 * horizon_
642 CHECK_EQ(control_result_.cols(), uWS_.cols());
643 CHECK_EQ(control_result_.rows(), uWS_.rows());
644 double control_diff_max = 0.0;
645 for (int i = 0; i < horizon_; ++i) {
646 control_diff_max = std::max(std::abs(uWS_(0, i) - control_result_(0, i)), control_diff_max);
647 control_diff_max = std::max(std::abs(uWS_(1, i) - control_result_(1, i)), control_diff_max);
648 }
649
650 // 2. time scale variable initialization, horizon_ + 1
651
652 // 3. lagrange constraint l, obstacles_edges_sum_ * (horizon_+1)
653 CHECK_EQ(dual_l_result_.cols(), l_warm_up_.cols());
654 CHECK_EQ(dual_l_result_.rows(), l_warm_up_.rows());
655 double l_diff_max = 0.0;
656 for (int i = 0; i < horizon_ + 1; ++i) {
657 for (int j = 0; j < obstacles_edges_sum_; ++j) {
658 l_diff_max = std::max(std::abs(l_warm_up_(j, i) - dual_l_result_(j, i)), l_diff_max);
659 }
660 }
661
662 // 4. lagrange constraint m, 4*obstacles_num * (horizon_+1)
663 CHECK_EQ(n_warm_up_.cols(), dual_n_result_.cols());
664 CHECK_EQ(n_warm_up_.rows(), dual_n_result_.rows());
665 double n_diff_max = 0.0;
666 for (int i = 0; i < horizon_ + 1; ++i) {
667 for (int j = 0; j < 4 * obstacles_num_; ++j) {
668 n_diff_max = std::max(std::abs(n_warm_up_(j, i) - dual_n_result_(j, i)), n_diff_max);
669 }
670 }
671
672 ADEBUG << "state_diff_max: " << state_diff_max;
673 ADEBUG << "control_diff_max: " << control_diff_max;
674 ADEBUG << "dual_l_diff_max: " << l_diff_max;
675 ADEBUG << "dual_n_diff_max: " << n_diff_max;
676}

◆ get_starting_point()

bool apollo::planning::DistanceApproachIPOPTFixedDualInterface::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_fixed_dual_interface.cc 第 396 行定义.

405 {
406 ADEBUG << "get_starting_point";
407 ACHECK(init_x) << "Warm start init_x setting failed";
408
409 CHECK_EQ(horizon_, uWS_.cols());
410 CHECK_EQ(horizon_ + 1, xWS_.cols());
411
412 // 1. state variables 4 * (horizon_ + 1)
413 for (int i = 0; i < horizon_ + 1; ++i) {
414 int index = i * 4;
415 for (int j = 0; j < 4; ++j) {
416 x[index + j] = xWS_(j, i);
417 }
418 }
419
420 // 2. control variable initialization, 2 * horizon_
421 for (int i = 0; i < horizon_; ++i) {
422 int index = i * 2;
423 x[control_start_index_ + index] = uWS_(0, i);
424 x[control_start_index_ + index + 1] = uWS_(1, i);
425 }
426
427 // 3. time scale variable initialization, horizon_ + 1
428 for (int i = 0; i < horizon_ + 1; ++i) {
429 x[time_start_index_ + i] = 0.5 * (min_time_sample_scaling_ + max_time_sample_scaling_);
430 }
431
432 // check initial states
433 if (distance_approach_config_.enable_check_initial_state()) {
434 int kM = m;
435 double g[kM];
436 eval_g(n, x, true, m, g);
437 check_g(n, x, m, g);
438 }
439
440 ADEBUG << "get_starting_point out";
441 return true;
442}
apollo::planning::DistanceApproachIPOPTFixedDualInterface::eval_g
bool eval_g(int n, const double *x, bool new_x, int m, double *g) override
Method to return the constraint residuals
Definition distance_approach_ipopt_fixed_dual_interface.cc:454
该类的文档由以下文件生成: