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

#include <distance_approach_ipopt_fixed_ts_interface.h>

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

Public 成员函数

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

构造及析构函数说明

◆ DistanceApproachIPOPTFixedTsInterface() [1/2]

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

33 : ts_(ts),
34 ego_(ego),
35 xWS_(xWS),
36 uWS_(uWS),
37 l_warm_up_(l_warm_up),
38 n_warm_up_(n_warm_up),
39 x0_(x0),
40 xf_(xf),
41 last_time_u_(last_time_u),
42 XYbounds_(XYbounds),
43 obstacles_edges_num_(obstacles_edges_num),
44 obstacles_A_(obstacles_A),
45 obstacles_b_(obstacles_b) {
46 ACHECK(horizon < std::numeric_limits<int>::max())
47 << "Invalid cast on horizon in open space planner";
48 horizon_ = static_cast<int>(horizon);
49 ACHECK(obstacles_num < std::numeric_limits<int>::max())
50 << "Invalid cast on obstacles_num in open space planner";
51
52 obstacles_num_ = static_cast<int>(obstacles_num);
53 w_ev_ = ego_(1, 0) + ego_(3, 0);
54 l_ev_ = ego_(0, 0) + ego_(2, 0);
55 g_ = {l_ev_ / 2, w_ev_ / 2, l_ev_ / 2, w_ev_ / 2};
56 offset_ = (ego_(0, 0) + ego_(2, 0)) / 2 - ego_(2, 0);
57 obstacles_edges_sum_ = obstacles_edges_num_.sum();
58 state_result_ = Eigen::MatrixXd::Zero(4, horizon_ + 1);
59 dual_l_result_ = Eigen::MatrixXd::Zero(obstacles_edges_sum_, horizon_ + 1);
60 dual_n_result_ = Eigen::MatrixXd::Zero(4 * obstacles_num_, horizon_ + 1);
61 control_result_ = Eigen::MatrixXd::Zero(2, horizon_ + 1);
62 time_result_ = Eigen::MatrixXd::Zero(1, horizon_ + 1);
63 state_start_index_ = 0;
64 control_start_index_ = 4 * (horizon_ + 1);
65 time_start_index_ = control_start_index_ + 2 * horizon_;
66 l_start_index_ = time_start_index_;
67 n_start_index_ = l_start_index_ + obstacles_edges_sum_ * (horizon_ + 1);
68
69 planner_open_space_config_ = planner_open_space_config;
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 min_safety_distance_ = distance_approach_config_.min_safety_distance();
87 max_steer_angle_ =
88 vehicle_param_.max_steer_angle() / vehicle_param_.steer_ratio();
89 max_speed_forward_ = distance_approach_config_.max_speed_forward();
90 max_speed_reverse_ = distance_approach_config_.max_speed_reverse();
91 max_acceleration_forward_ =
92 distance_approach_config_.max_acceleration_forward();
93 max_acceleration_reverse_ =
94 distance_approach_config_.max_acceleration_reverse();
95 min_time_sample_scaling_ =
96 distance_approach_config_.min_time_sample_scaling();
97 max_time_sample_scaling_ =
98 distance_approach_config_.max_time_sample_scaling();
99 max_steer_rate_ =
100 vehicle_param_.max_steer_angle_rate() / vehicle_param_.steer_ratio();
101 use_fix_time_ = distance_approach_config_.use_fix_time();
102 wheelbase_ = vehicle_param_.wheel_base();
103 enable_constraint_check_ =
104 distance_approach_config_.enable_constraint_check();
105}
ACHECK
#define ACHECK(cond)
Definition log.h:80

◆ ~DistanceApproachIPOPTFixedTsInterface()

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

◆ DistanceApproachIPOPTFixedTsInterface() [2/2]

apollo::planning::DistanceApproachIPOPTFixedTsInterface::DistanceApproachIPOPTFixedTsInterface ( 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::DistanceApproachIPOPTFixedTsInterface::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_ts_interface.cc 第 880 行定义.

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

◆ eval_constraints()

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

Template to compute constraints

在文件 distance_approach_ipopt_fixed_ts_interface.cc 第 669 行定义.

670 {
671 // state start index
672 int state_index = state_start_index_;
673
674 // control start index.
675 int control_index = control_start_index_;
676
677 // time start index
678 // int time_index = time_start_index_;
679
680 int constraint_index = 0;
681
682 // 1. state constraints 4 * [0, horizons-1]
683 // commented implementation is linearized model with fixed ts
684 for (int i = 0; i < horizon_; ++i) {
685 // x1
686 /*
687 g[constraint_index] =
688 x[state_index + 4] -
689 ((xWS_(0, i) + ts_ * xWS_(3, i) * cos(xWS_(2, i))) +
690 (x[state_index] - xWS_(0, i)) +
691 (ts_ * cos(xWS_(2, i))) * (x[state_index + 3] - xWS_(3, i)) +
692 (-ts_ * xWS_(3, i) * sin(xWS_(2, i))) *
693 (x[state_index + 2] - xWS_(2, i)));
694 */
695
696 g[constraint_index] =
697 x[state_index + 4] -
698 (x[state_index] + ts_ * x[state_index + 3] * cos(x[state_index + 2]));
699
700 // x2
701 /*
702 g[constraint_index + 1] =
703 x[state_index + 5] -
704 ((xWS_(1, i) + ts_ * xWS_(3, i) * sin(xWS_(2, i))) +
705 (x[state_index + 1] - xWS_(1, i)) +
706 (ts_ * sin(xWS_(2, i))) * (x[state_index + 3] - xWS_(3, i)) +
707 (ts_ * xWS_(3, i) * cos(xWS_(2, i))) *
708 (x[state_index + 2] - xWS_(2, i)));
709 */
710
711 g[constraint_index + 1] =
712 x[state_index + 5] - (x[state_index + 1] + ts_ * x[state_index + 3] *
713 sin(x[state_index + 2]));
714
715 // x3
716 /*
717 g[constraint_index + 2] =
718 x[state_index + 6] -
719 ((xWS_(2, i) + ts_ * xWS_(3, i) * tan(uWS_(0, i)) / wheelbase_) +
720 (x[state_index + 2] - xWS_(2, i)) +
721 (ts_ * tan(uWS_(0, i)) / wheelbase_) *
722 (x[state_index + 3] - xWS_(3, i)) +
723 (ts_ * xWS_(3, i) / cos(uWS_(0, i)) / cos(uWS_(0, i)) / wheelbase_)
724 *
725 (x[control_index] - uWS_(0, i)));
726 */
727
728 g[constraint_index + 2] =
729 x[state_index + 6] -
730 (x[state_index + 2] +
731 ts_ * x[state_index + 3] * tan(x[control_index]) / wheelbase_);
732
733 // x4
734 /*
735 g[constraint_index + 3] =
736 x[state_index + 7] - ((x[state_index + 3] + ts_ * uWS_(1, i)) +
737 (ts_ * (x[control_index + 1] - uWS_(1, i))));
738 */
739 g[constraint_index + 3] =
740 x[state_index + 7] - (x[state_index + 3] + ts_ * x[control_index + 1]);
741
742 control_index += 2;
743 constraint_index += 4;
744 state_index += 4;
745 }
746
747 ADEBUG << "constraint_index after adding Euler forward dynamics constraints "
748 "updated: "
749 << constraint_index;
750
751 // 2. Control rate limit constraints, 1 * [0, horizons-1], only apply
752 // steering rate as of now
753 control_index = control_start_index_;
754
755 // First rate is compare first with stitch point
756 g[constraint_index] = (x[control_index] - last_time_u_(0, 0)) / ts_;
757 control_index += 2;
758 constraint_index++;
759
760 for (int i = 1; i < horizon_; ++i) {
761 g[constraint_index] = (x[control_index] - x[control_index - 2]) / ts_;
762 constraint_index++;
763 control_index += 2;
764 }
765
766 // 4. Three obstacles related equal constraints, one equality constraints,
767 // [0, horizon_] * [0, obstacles_num_-1] * 4
768 state_index = state_start_index_;
769 int l_index = l_start_index_;
770 int n_index = n_start_index_;
771
772 for (int i = 0; i < horizon_ + 1; ++i) {
773 int edges_counter = 0;
774 for (int j = 0; j < obstacles_num_; ++j) {
775 int current_edges_num = obstacles_edges_num_(j, 0);
776 Eigen::MatrixXd Aj =
777 obstacles_A_.block(edges_counter, 0, current_edges_num, 2);
778 Eigen::MatrixXd bj =
779 obstacles_b_.block(edges_counter, 0, current_edges_num, 1);
780
781 // norm(A* lambda) <= 1
782 T tmp1 = 0.0;
783 T tmp2 = 0.0;
784 for (int k = 0; k < current_edges_num; ++k) {
785 // TODO(QiL) : replace this one directly with x
786 tmp1 += Aj(k, 0) * x[l_index + k];
787 tmp2 += Aj(k, 1) * x[l_index + k];
788 }
789 g[constraint_index] = tmp1 * tmp1 + tmp2 * tmp2;
790
791 // G' * mu + R' * lambda == 0
792 g[constraint_index + 1] = x[n_index] - x[n_index + 2] +
793 cos(x[state_index + 2]) * tmp1 +
794 sin(x[state_index + 2]) * tmp2;
795
796 g[constraint_index + 2] = x[n_index + 1] - x[n_index + 3] -
797 sin(x[state_index + 2]) * tmp1 +
798 cos(x[state_index + 2]) * tmp2;
799
800 // -g'*mu + (A*t - b)*lambda > 0
801 T tmp3 = 0.0;
802 for (int k = 0; k < 4; ++k) {
803 tmp3 += -g_[k] * x[n_index + k];
804 }
805
806 T tmp4 = 0.0;
807 for (int k = 0; k < current_edges_num; ++k) {
808 tmp4 += bj(k, 0) * x[l_index + k];
809 }
810
811 g[constraint_index + 3] =
812 tmp3 + (x[state_index] + cos(x[state_index + 2]) * offset_) * tmp1 +
813 (x[state_index + 1] + sin(x[state_index + 2]) * offset_) * tmp2 -
814 tmp4;
815
816 // Update index
817 edges_counter += current_edges_num;
818 l_index += current_edges_num;
819 n_index += 4;
820 constraint_index += 4;
821 }
822 state_index += 4;
823 }
824 ADEBUG << "constraint_index after obstacles avoidance constraints "
825 "updated: "
826 << constraint_index;
827
828 // 5. load variable bounds as constraints
829 state_index = state_start_index_;
830 control_index = control_start_index_;
831 // time_index = time_start_index_;
832 l_index = l_start_index_;
833 n_index = n_start_index_;
834
835 // start configuration
836 g[constraint_index] = x[state_index];
837 g[constraint_index + 1] = x[state_index + 1];
838 g[constraint_index + 2] = x[state_index + 2];
839 g[constraint_index + 3] = x[state_index + 3];
840 constraint_index += 4;
841 state_index += 4;
842
843 // constraints on x,y,v
844 for (int i = 1; i < horizon_; ++i) {
845 g[constraint_index] = x[state_index];
846 g[constraint_index + 1] = x[state_index + 1];
847 g[constraint_index + 2] = x[state_index + 3];
848 constraint_index += 3;
849 state_index += 4;
850 }
851
852 // end configuration
853 g[constraint_index] = x[state_index];
854 g[constraint_index + 1] = x[state_index + 1];
855 g[constraint_index + 2] = x[state_index + 2];
856 g[constraint_index + 3] = x[state_index + 3];
857 constraint_index += 4;
858 state_index += 4;
859
860 for (int i = 0; i < horizon_; ++i) {
861 g[constraint_index] = x[control_index];
862 g[constraint_index + 1] = x[control_index + 1];
863 constraint_index += 2;
864 control_index += 2;
865 }
866
867 for (int i = 0; i < lambda_horizon_; ++i) {
868 g[constraint_index] = x[l_index];
869 constraint_index++;
870 l_index++;
871 }
872
873 for (int i = 0; i < miu_horizon_; ++i) {
874 g[constraint_index] = x[n_index];
875 constraint_index++;
876 n_index++;
877 }
878}
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::DistanceApproachIPOPTFixedTsInterface::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_ts_interface.cc 第 393 行定义.

395 {
396 eval_obj(n, x, &obj_value);
397 return true;
398}
apollo::planning::DistanceApproachIPOPTFixedTsInterface::eval_obj
void eval_obj(int n, const T *x, T *obj_value)
Template to return the objective value
Definition distance_approach_ipopt_fixed_ts_interface.cc:613

◆ eval_g()

bool apollo::planning::DistanceApproachIPOPTFixedTsInterface::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_ts_interface.cc 第 407 行定义.

409 {
410 eval_constraints(n, x, m, g);
411 if (enable_constraint_check_) {
412 check_g(n, x, m, g);
413 }
414 return true;
415}
apollo::planning::DistanceApproachIPOPTFixedTsInterface::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_ts_interface.cc:880
apollo::planning::DistanceApproachIPOPTFixedTsInterface::eval_constraints
void eval_constraints(int n, const T *x, int m, T *g)
Template to compute constraints
Definition distance_approach_ipopt_fixed_ts_interface.cc:669

◆ eval_grad_f()

bool apollo::planning::DistanceApproachIPOPTFixedTsInterface::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_ts_interface.cc 第 400 行定义.

402 {
403 gradient(tag_f, n, x, grad_f);
404 return true;
405}
tag_f
#define tag_f
Definition cos_theta_ipopt_interface.h:36

◆ eval_h()

bool apollo::planning::DistanceApproachIPOPTFixedTsInterface::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_ts_interface.cc 第 447 行定义.

450 {
451 if (values == nullptr) {
452 // return the structure. This is a symmetric matrix, fill the lower left
453 // triangle only.
454
455 for (int idx = 0; idx < nnz_L; idx++) {
456 iRow[idx] = rind_L[idx];
457 jCol[idx] = cind_L[idx];
458 }
459 } else {
460 // return the values. This is a symmetric matrix, fill the lower left
461 // triangle only
462
463 obj_lam[0] = obj_factor;
464
465 for (int idx = 0; idx < m; idx++) {
466 obj_lam[1 + idx] = lambda[idx];
467 }
468
469 set_param_vec(tag_L, m + 1, obj_lam);
470 sparse_hess(tag_L, n, 1, const_cast<double*>(x), &nnz_L, &rind_L, &cind_L,
471 &hessval, options_L);
472
473 for (int idx = 0; idx < nnz_L; idx++) {
474 values[idx] = hessval[idx];
475 }
476 }
477
478 return true;
479}
tag_L
#define tag_L
Definition cos_theta_ipopt_interface.h:38

◆ eval_jac_g()

bool apollo::planning::DistanceApproachIPOPTFixedTsInterface::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_ts_interface.cc 第 417 行定义.

421 {
422 if (values == nullptr) {
423 // return the structure of the jacobian
424 for (int idx = 0; idx < nnz_jac; idx++) {
425 iRow[idx] = rind_g[idx];
426 jCol[idx] = cind_g[idx];
427 }
428 } else {
429 // return the values of the jacobian of the constraints
430 sparse_jac(tag_g, m, n, 1, x, &nnz_jac, &rind_g, &cind_g, &jacval,
431 options_g);
432 for (int idx = 0; idx < nnz_jac; idx++) {
433 values[idx] = jacval[idx];
434 }
435 }
436 return true;
437 // return eval_jac_g_ser(n, x, new_x, m, nele_jac, iRow, jCol, values);
438}
tag_g
#define tag_g
Definition cos_theta_ipopt_interface.h:37

◆ eval_jac_g_ser()

bool apollo::planning::DistanceApproachIPOPTFixedTsInterface::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_ts_interface.cc 第 440 行定义.

442 {
443 AERROR << "NOT VALID NOW";
444 return false;
445} // NOLINT
AERROR
#define AERROR
Definition log.h:44

◆ eval_obj()

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

Template to return the objective value

在文件 distance_approach_ipopt_fixed_ts_interface.cc 第 613 行定义.

614 {
615 // Objective is :
616 // min control inputs
617 // min input rate
618 // min time (if the time step is not fixed)
619 // regularization wrt warm start trajectory
620 DCHECK(ts_ != 0) << "ts in distance_approach_ is 0";
621 int control_index = control_start_index_;
622 // int time_index = time_start_index_;
623 int state_index = state_start_index_;
624
625 // TODO(QiL): Initial implementation towards earlier understanding and debug
626 // purpose, later code refine towards improving efficiency
627
628 *obj_value = 0.0;
629 // 1. objective to minimize state diff to warm up
630 for (int i = 0; i < horizon_ + 1; ++i) {
631 T x1_diff = x[state_index] - xWS_(0, i);
632 T x2_diff = x[state_index + 1] - xWS_(1, i);
633 T x3_diff = x[state_index + 2] - xWS_(2, i);
634 T x4_abs = x[state_index + 3];
635 *obj_value += weight_state_x_ * x1_diff * x1_diff +
636 weight_state_y_ * x2_diff * x2_diff +
637 weight_state_phi_ * x3_diff * x3_diff +
638 weight_state_v_ * x4_abs * x4_abs;
639 state_index += 4;
640 }
641
642 // 2. objective to minimize u square
643 for (int i = 0; i < horizon_; ++i) {
644 *obj_value += weight_input_steer_ * x[control_index] * x[control_index] +
645 weight_input_a_ * x[control_index + 1] * x[control_index + 1];
646 control_index += 2;
647 }
648
649 // 3. objective to minimize input change rate for first horizon
650 control_index = control_start_index_;
651 T last_time_steer_rate = (x[control_index] - last_time_u_(0, 0)) / ts_;
652 T last_time_a_rate = (x[control_index + 1] - last_time_u_(1, 0)) / ts_;
653
654 *obj_value +=
655 weight_stitching_steer_ * last_time_steer_rate * last_time_steer_rate +
656 weight_stitching_a_ * last_time_a_rate * last_time_a_rate;
657
658 // 4. objective to minimize input change rates, [0- horizon_ -2]
659 for (int i = 0; i < horizon_ - 1; ++i) {
660 T steering_rate = (x[control_index + 2] - x[control_index]) / ts_;
661 T a_rate = (x[control_index + 3] - x[control_index + 1]) / ts_;
662 *obj_value += weight_rate_steer_ * steering_rate * steering_rate +
663 weight_rate_a_ * a_rate * a_rate;
664 control_index += 2;
665 }
666}

◆ finalize_solution()

void apollo::planning::DistanceApproachIPOPTFixedTsInterface::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_ts_interface.cc 第 481 行定义.

485 {
486 int state_index = state_start_index_;
487 int control_index = control_start_index_;
488 // int time_index = time_start_index_;
489 int dual_l_index = l_start_index_;
490 int dual_n_index = n_start_index_;
491
492 // enable_constraint_check_: for debug only
493 if (enable_constraint_check_) {
494 ADEBUG << "final resolution constraint checking";
495 check_g(n, x, m, g);
496 }
497 // 1. state variables, 4 * [0, horizon]
498 // 2. control variables, 2 * [0, horizon_-1]
499 // 3. sampling time variables, 1 * [0, horizon_]
500 // 4. dual_l obstacles_edges_sum_ * [0, horizon]
501 // 5. dual_n obstacles_num * [0, horizon]
502 for (int i = 0; i < horizon_; ++i) {
503 state_result_(0, i) = x[state_index];
504 state_result_(1, i) = x[state_index + 1];
505 state_result_(2, i) = x[state_index + 2];
506 state_result_(3, i) = x[state_index + 3];
507 control_result_(0, i) = x[control_index];
508 control_result_(1, i) = x[control_index + 1];
509 time_result_(0, i) = ts_;
510 for (int j = 0; j < obstacles_edges_sum_; ++j) {
511 dual_l_result_(j, i) = x[dual_l_index + j];
512 }
513 for (int k = 0; k < 4 * obstacles_num_; k++) {
514 dual_n_result_(k, i) = x[dual_n_index + k];
515 }
516 state_index += 4;
517 control_index += 2;
518 // time_index++;
519 dual_l_index += obstacles_edges_sum_;
520 dual_n_index += 4 * obstacles_num_;
521 }
522 state_result_(0, 0) = x0_(0, 0);
523 state_result_(1, 0) = x0_(1, 0);
524 state_result_(2, 0) = x0_(2, 0);
525 state_result_(3, 0) = x0_(3, 0);
526 // push back last horizon for state and time variables
527 state_result_(0, horizon_) = xf_(0, 0);
528 state_result_(1, horizon_) = xf_(1, 0);
529 state_result_(2, horizon_) = xf_(2, 0);
530 state_result_(3, horizon_) = xf_(3, 0);
531 time_result_(0, horizon_) = ts_;
532 // time_result_ = ts_ * time_result_;
533 for (int j = 0; j < obstacles_edges_sum_; ++j) {
534 dual_l_result_(j, horizon_) = x[dual_l_index + j];
535 }
536 for (int k = 0; k < 4 * obstacles_num_; k++) {
537 dual_n_result_(k, horizon_) = x[dual_n_index + k];
538 }
539 // memory deallocation of ADOL-C variables
540 delete[] obj_lam;
541 free(rind_g);
542 free(cind_g);
543 free(rind_L);
544 free(cind_L);
545 free(jacval);
546 free(hessval);
547}

◆ generate_tapes()

void apollo::planning::DistanceApproachIPOPTFixedTsInterface::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_ts_interface.cc 第 959 行定义.

961 {
962 std::vector<double> xp(n);
963 std::vector<double> lamp(m);
964 std::vector<double> zl(m);
965 std::vector<double> zu(m);
966 std::vector<adouble> xa(n);
967 std::vector<adouble> g(m);
968 std::vector<double> lam(m);
969
970 double sig = 0.0;
971 adouble obj_value;
972 double dummy = 0.0;
973 obj_lam = new double[m + 1];
974 get_starting_point(n, 1, &xp[0], 0, &zl[0], &zu[0], m, 0, &lamp[0]);
975
976 trace_on(tag_f);
977 for (int idx = 0; idx < n; idx++) {
978 xa[idx] <<= xp[idx];
979 }
980 eval_obj(n, &xa[0], &obj_value);
981 obj_value >>= dummy;
982 trace_off();
983
984 trace_on(tag_g);
985 for (int idx = 0; idx < n; idx++) {
986 xa[idx] <<= xp[idx];
987 }
988 eval_constraints(n, &xa[0], m, &g[0]);
989 for (int idx = 0; idx < m; idx++) {
990 g[idx] >>= dummy;
991 }
992 trace_off();
993
994 trace_on(tag_L);
995 for (int idx = 0; idx < n; idx++) {
996 xa[idx] <<= xp[idx];
997 }
998 for (int idx = 0; idx < m; idx++) {
999 lam[idx] = 1.0;
1000 }
1001 sig = 1.0;
1002 eval_obj(n, &xa[0], &obj_value);
1003 obj_value *= mkparam(sig);
1004 eval_constraints(n, &xa[0], m, &g[0]);
1005 for (int idx = 0; idx < m; idx++) {
1006 obj_value += g[idx] * mkparam(lam[idx]);
1007 }
1008 obj_value >>= dummy;
1009
1010 trace_off();
1011
1012 rind_g = nullptr;
1013 cind_g = nullptr;
1014 rind_L = nullptr;
1015 cind_L = nullptr;
1016 jacval = nullptr;
1017 hessval = nullptr;
1018
1019 options_g[0] = 0; /* sparsity pattern by index domains (default) */
1020 options_g[1] = 0; /* safe mode (default) */
1021 options_g[2] = 0;
1022 options_g[3] = 0; /* column compression (default) */
1023 sparse_jac(tag_g, m, n, 0, &xp[0], &nnz_jac, &rind_g, &cind_g, &jacval,
1024 options_g);
1025 *nnz_jac_g = nnz_jac;
1026
1027 options_L[0] = 0;
1028 options_L[1] = 1;
1029 sparse_hess(tag_L, n, 0, &xp[0], &nnz_L, &rind_L, &cind_L, &hessval,
1030 options_L);
1031 *nnz_h_lag = nnz_L;
1032}
apollo::planning::DistanceApproachIPOPTFixedTsInterface::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_ts_interface.cc:341

◆ get_bounds_info()

bool apollo::planning::DistanceApproachIPOPTFixedTsInterface::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_ts_interface.cc 第 150 行定义.

153 {
154 ADEBUG << "get_bounds_info";
155 ACHECK(XYbounds_.size() == 4)
156 << "XYbounds_ size is not 4, but" << XYbounds_.size();
157
158 // Variables: includes state, u, sample time and lagrange multipliers
159 // 1. state variables, 4 * [0, horizon]
160 // start point pose
161 int variable_index = 0;
162 for (int i = 0; i < 4; ++i) {
163 x_l[i] = -2e19;
164 x_u[i] = 2e19;
165 }
166 variable_index += 4;
167
168 // During horizons, 2 ~ N-1
169 for (int i = 1; i < horizon_; ++i) {
170 // x
171 x_l[variable_index] = -2e19;
172 x_u[variable_index] = 2e19;
173
174 // y
175 x_l[variable_index + 1] = -2e19;
176 x_u[variable_index + 1] = 2e19;
177
178 // phi
179 x_l[variable_index + 2] = -2e19;
180 x_u[variable_index + 2] = 2e19;
181
182 // v
183 x_l[variable_index + 3] = -2e19;
184 x_u[variable_index + 3] = 2e19;
185
186 variable_index += 4;
187 }
188
189 // end point pose
190 for (int i = 0; i < 4; ++i) {
191 x_l[variable_index + i] = -2e19;
192 x_u[variable_index + i] = 2e19;
193 }
194 variable_index += 4;
195 ADEBUG << "variable_index after adding state variables : " << variable_index;
196
197 // 2. control variables, 2 * [0, horizon_-1]
198 for (int i = 0; i < horizon_; ++i) {
199 // u1
200 x_l[variable_index] = -2e19;
201 x_u[variable_index] = 2e19;
202
203 // u2
204 x_l[variable_index + 1] = -2e19;
205 x_u[variable_index + 1] = 2e19;
206
207 variable_index += 2;
208 }
209 ADEBUG << "variable_index after adding control variables : "
210 << variable_index;
211
212 // 4. lagrange constraint l, [0, obstacles_edges_sum_ - 1] * [0,
213 // horizon_]
214 for (int i = 0; i < horizon_ + 1; ++i) {
215 for (int j = 0; j < obstacles_edges_sum_; ++j) {
216 x_l[variable_index] = 0.0;
217 x_u[variable_index] = 2e19; // nlp_upper_bound_limit
218 ++variable_index;
219 }
220 }
221 ADEBUG << "variable_index after adding lagrange l : " << variable_index;
222
223 // 5. lagrange constraint n, [0, 4*obstacles_num-1] * [0, horizon_]
224 for (int i = 0; i < horizon_ + 1; ++i) {
225 for (int j = 0; j < 4 * obstacles_num_; ++j) {
226 x_l[variable_index] = 0.0;
227 x_u[variable_index] = 2e19; // nlp_upper_bound_limit
228
229 ++variable_index;
230 }
231 }
232 ADEBUG << "variable_index after adding lagrange n : " << variable_index;
233
234 // Constraints: includes four state Euler forward constraints, three
235 // Obstacle related constraints
236
237 // 1. dynamics constraints 4 * [0, horizons-1]
238 int constraint_index = 0;
239 for (int i = 0; i < 4 * horizon_; ++i) {
240 g_l[i] = 0.0;
241 g_u[i] = 0.0;
242 }
243 constraint_index += 4 * horizon_;
244
245 ADEBUG << "constraint_index after adding Euler forward dynamics constraints: "
246 << constraint_index;
247
248 // 2. Control rate limit constraints, 1 * [0, horizons-1], only apply
249 // steering rate as of now
250 for (int i = 0; i < horizon_; ++i) {
251 g_l[constraint_index] = -max_steer_rate_;
252 g_u[constraint_index] = max_steer_rate_;
253 ++constraint_index;
254 }
255
256 ADEBUG << "constraint_index after adding steering rate constraints: "
257 << constraint_index;
258
259 // 4. Three obstacles related equal constraints, one equality constraints,
260 // [0, horizon_] * [0, obstacles_num_-1] * 4
261 for (int i = 0; i < horizon_ + 1; ++i) {
262 for (int j = 0; j < obstacles_num_; ++j) {
263 // a. norm(A'*lambda) <= 1
264 g_l[constraint_index] = -2e19;
265 g_u[constraint_index] = 1.0;
266
267 // b. G'*mu + R'*A*lambda = 0
268 g_l[constraint_index + 1] = 0.0;
269 g_u[constraint_index + 1] = 0.0;
270 g_l[constraint_index + 2] = 0.0;
271 g_u[constraint_index + 2] = 0.0;
272
273 // c. -g'*mu + (A*t - b)*lambda > min_safety_distance_
274 g_l[constraint_index + 3] = min_safety_distance_;
275 g_u[constraint_index + 3] = 2e19; // nlp_upper_bound_limit
276 constraint_index += 4;
277 }
278 }
279 ADEBUG << "constraints_index after adding obstacles related constraints: "
280 << constraint_index;
281
282 // 5. load variable bounds as constraints
283 // start configuration
284 g_l[constraint_index] = x0_(0, 0);
285 g_u[constraint_index] = x0_(0, 0);
286 g_l[constraint_index + 1] = x0_(1, 0);
287 g_u[constraint_index + 1] = x0_(1, 0);
288 g_l[constraint_index + 2] = x0_(2, 0);
289 g_u[constraint_index + 2] = x0_(2, 0);
290 g_l[constraint_index + 3] = x0_(3, 0);
291 g_u[constraint_index + 3] = x0_(3, 0);
292 constraint_index += 4;
293
294 for (int i = 1; i < horizon_; ++i) {
295 g_l[constraint_index] = XYbounds_[0];
296 g_u[constraint_index] = XYbounds_[1];
297 g_l[constraint_index + 1] = XYbounds_[2];
298 g_u[constraint_index + 1] = XYbounds_[3];
299 g_l[constraint_index + 2] = -max_speed_reverse_;
300 g_u[constraint_index + 2] = max_speed_forward_;
301 constraint_index += 3;
302 }
303
304 // end configuration
305 g_l[constraint_index] = xf_(0, 0);
306 g_u[constraint_index] = xf_(0, 0);
307 g_l[constraint_index + 1] = xf_(1, 0);
308 g_u[constraint_index + 1] = xf_(1, 0);
309 g_l[constraint_index + 2] = xf_(2, 0);
310 g_u[constraint_index + 2] = xf_(2, 0);
311 g_l[constraint_index + 3] = xf_(3, 0);
312 g_u[constraint_index + 3] = xf_(3, 0);
313 constraint_index += 4;
314
315 for (int i = 0; i < horizon_; ++i) {
316 g_l[constraint_index] = -max_steer_angle_;
317 g_u[constraint_index] = max_steer_angle_;
318 g_l[constraint_index + 1] = -max_acceleration_reverse_;
319 g_u[constraint_index + 1] = max_acceleration_forward_;
320 constraint_index += 2;
321 }
322
323 for (int i = 0; i < lambda_horizon_; ++i) {
324 g_l[constraint_index] = 0.0;
325 g_u[constraint_index] = 2e19;
326 constraint_index++;
327 }
328
329 for (int i = 0; i < miu_horizon_; ++i) {
330 g_l[constraint_index] = 0.0;
331 g_u[constraint_index] = 2e19;
332 constraint_index++;
333 }
334
335 ADEBUG << "constraint_index after adding obstacles constraints: "
336 << constraint_index;
337 ADEBUG << "get_bounds_info_ out";
338 return true;
339}

◆ get_nlp_info()

bool apollo::planning::DistanceApproachIPOPTFixedTsInterface::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_ts_interface.cc 第 107 行定义.

109 {
110 ADEBUG << "get_nlp_info";
111 // n1 : states variables, 4 * (N+1)
112 int n1 = 4 * (horizon_ + 1);
113 ADEBUG << "n1: " << n1;
114 // n2 : control inputs variables
115 int n2 = 2 * horizon_;
116 ADEBUG << "n2: " << n2;
117 // n4 : dual multiplier associated with obstacle shape
118 lambda_horizon_ = obstacles_edges_num_.sum() * (horizon_ + 1);
119 ADEBUG << "lambda_horizon_: " << lambda_horizon_;
120 // n5 : dual multipier associated with car shape, obstacles_num*4 * (N+1)
121 miu_horizon_ = obstacles_num_ * 4 * (horizon_ + 1);
122 ADEBUG << "miu_horizon_: " << miu_horizon_;
123
124 // m1 : dynamics constatins
125 int m1 = 4 * horizon_;
126 ADEBUG << "m1: " << m1;
127 // m2 : control rate constraints (only steering)
128 int m2 = horizon_;
129 ADEBUG << "m2: " << m2;
130 // m4 : obstacle constraints
131 int m4 = 4 * obstacles_num_ * (horizon_ + 1);
132 ADEBUG << "m4: " << m4;
133
134 num_of_variables_ = n1 + n2 + lambda_horizon_ + miu_horizon_;
135 num_of_constraints_ = m1 + m2 + m4 + (num_of_variables_ - (horizon_ + 1) + 2);
136
137 // number of variables
138 n = num_of_variables_;
139 ADEBUG << "num_of_variables_ " << num_of_variables_;
140 // number of constraints
141 m = num_of_constraints_;
142 ADEBUG << "num_of_constraints_ " << num_of_constraints_;
143
144 generate_tapes(n, m, &nnz_jac_g, &nnz_h_lag);
145
146 index_style = IndexStyleEnum::C_STYLE;
147 return true;
148}
apollo::planning::DistanceApproachIPOPTFixedTsInterface::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_ts_interface.cc:959

◆ get_optimization_results()

void apollo::planning::DistanceApproachIPOPTFixedTsInterface::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_ts_interface.cc 第 549 行定义.

552 {
553 ADEBUG << "get_optimization_results";
554 *state_result = state_result_;
555 *control_result = control_result_;
556 *time_result = time_result_;
557 *dual_l_result = dual_l_result_;
558 *dual_n_result = dual_n_result_;
559
560 if (!distance_approach_config_.enable_initial_final_check()) {
561 return;
562 }
563 CHECK_EQ(state_result_.cols(), xWS_.cols());
564 CHECK_EQ(state_result_.rows(), xWS_.rows());
565 double state_diff_max = 0.0;
566 for (int i = 0; i < horizon_ + 1; ++i) {
567 for (int j = 0; j < 4; ++j) {
568 state_diff_max =
569 std::max(std::abs(xWS_(j, i) - state_result_(j, i)), state_diff_max);
570 }
571 }
572
573 // 2. control variable initialization, 2 * horizon_
574 CHECK_EQ(control_result_.rows(), uWS_.rows());
575 double control_diff_max = 0.0;
576 for (int i = 0; i < horizon_; ++i) {
577 control_diff_max = std::max(std::abs(uWS_(0, i) - control_result_(0, i)),
578 control_diff_max);
579 control_diff_max = std::max(std::abs(uWS_(1, i) - control_result_(1, i)),
580 control_diff_max);
581 }
582
583 // 3. lagrange constraint l, obstacles_edges_sum_ * (horizon_+1)
584 CHECK_EQ(dual_l_result_.cols(), l_warm_up_.cols());
585 CHECK_EQ(dual_l_result_.rows(), l_warm_up_.rows());
586 double l_diff_max = 0.0;
587 for (int i = 0; i < horizon_ + 1; ++i) {
588 for (int j = 0; j < obstacles_edges_sum_; ++j) {
589 l_diff_max = std::max(std::abs(l_warm_up_(j, i) - dual_l_result_(j, i)),
590 l_diff_max);
591 }
592 }
593
594 // 4. lagrange constraint m, 4*obstacles_num * (horizon_+1)
595 CHECK_EQ(n_warm_up_.cols(), dual_n_result_.cols());
596 CHECK_EQ(n_warm_up_.rows(), dual_n_result_.rows());
597 double n_diff_max = 0.0;
598 for (int i = 0; i < horizon_ + 1; ++i) {
599 for (int j = 0; j < 4 * obstacles_num_; ++j) {
600 n_diff_max = std::max(std::abs(n_warm_up_(j, i) - dual_n_result_(j, i)),
601 n_diff_max);
602 }
603 }
604
605 ADEBUG << "state_diff_max: " << state_diff_max;
606 ADEBUG << "control_diff_max: " << control_diff_max;
607 ADEBUG << "dual_l_diff_max: " << l_diff_max;
608 ADEBUG << "dual_n_diff_max: " << n_diff_max;
609}

◆ get_starting_point()

bool apollo::planning::DistanceApproachIPOPTFixedTsInterface::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_ts_interface.cc 第 341 行定义.

343 {
344 ADEBUG << "get_starting_point";
345 ACHECK(init_x) << "Warm start init_x setting failed";
346
347 CHECK_EQ(horizon_, uWS_.cols());
348 CHECK_EQ(horizon_ + 1, xWS_.cols());
349
350 // 1. state variables 4 * (horizon_ + 1)
351 for (int i = 0; i < horizon_ + 1; ++i) {
352 int index = i * 4;
353 for (int j = 0; j < 4; ++j) {
354 x[index + j] = xWS_(j, i);
355 }
356 }
357
358 // 2. control variable initialization, 2 * horizon_
359 for (int i = 0; i < horizon_; ++i) {
360 int index = i * 2;
361 x[control_start_index_ + index] = uWS_(0, i);
362 x[control_start_index_ + index + 1] = uWS_(1, i);
363 }
364
365 // 3. lagrange constraint l, obstacles_edges_sum_ * (horizon_+1)
366 for (int i = 0; i < horizon_ + 1; ++i) {
367 int index = i * obstacles_edges_sum_;
368 for (int j = 0; j < obstacles_edges_sum_; ++j) {
369 x[l_start_index_ + index + j] = l_warm_up_(j, i);
370 }
371 }
372
373 // 4. lagrange constraint m, 4*obstacles_num * (horizon_+1)
374 for (int i = 0; i < horizon_ + 1; ++i) {
375 int index = i * 4 * obstacles_num_;
376 for (int j = 0; j < 4 * obstacles_num_; ++j) {
377 x[n_start_index_ + index + j] = n_warm_up_(j, i);
378 }
379 }
380
381 if (enable_constraint_check_) {
382 int kM = m;
383 double g[kM];
384 ADEBUG << "initial points constraint checking";
385 eval_constraints(n, x, m, g);
386 check_g(n, x, m, g);
387 }
388
389 ADEBUG << "get_starting_point out";
390 return true;
391}
该类的文档由以下文件生成: