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

#include <distance_approach_ipopt_interface.h>

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

Public 成员函数

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

构造及析构函数说明

◆ DistanceApproachIPOPTInterface() [1/2]

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

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

◆ ~DistanceApproachIPOPTInterface()

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

◆ DistanceApproachIPOPTInterface() [2/2]

apollo::planning::DistanceApproachIPOPTInterface::DistanceApproachIPOPTInterface ( 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_interface.cc 第 104 行定义.

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

成员函数说明

◆ check_g()

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

1831 {
1832 int kN = n;
1833 int kM = m;
1834 double x_u_tmp[kN];
1835 double x_l_tmp[kN];
1836 double g_u_tmp[kM];
1837 double g_l_tmp[kM];
1838
1839 get_bounds_info(n, x_l_tmp, x_u_tmp, m, g_l_tmp, g_u_tmp);
1840
1841 const double delta_v = 1e-4;
1842 for (int idx = 0; idx < n; ++idx) {
1843 x_u_tmp[idx] = x_u_tmp[idx] + delta_v;
1844 x_l_tmp[idx] = x_l_tmp[idx] - delta_v;
1845 if (x[idx] > x_u_tmp[idx] || x[idx] < x_l_tmp[idx]) {
1846 AINFO << "x idx unfeasible: " << idx << ", x: " << x[idx] << ", lower: " << x_l_tmp[idx]
1847 << ", upper: " << x_u_tmp[idx];
1848 }
1849 }
1850
1851 // m1 : dynamics constatins
1852 int m1 = 4 * horizon_;
1853
1854 // m2 : control rate constraints (only steering)
1855 int m2 = m1 + horizon_;
1856
1857 // m3 : sampling time equality constraints
1858 int m3 = m2 + horizon_;
1859
1860 // m4 : obstacle constraints
1861 int m4 = m3 + 4 * obstacles_num_ * (horizon_ + 1);
1862
1863 // 5. load variable bounds as constraints
1864 // start configuration
1865 int m5 = m4 + 3 + 1;
1866
1867 // constraints on x,y,v
1868 int m6 = m5 + 3 * (horizon_ - 1);
1869
1870 // end configuration
1871 int m7 = m6 + 3 + 1;
1872
1873 // control variable bnd
1874 int m8 = m7 + 2 * horizon_;
1875
1876 // time interval variable bnd
1877 int m9 = m8 + (horizon_ + 1);
1878
1879 // lambda_horizon_
1880 int m10 = m9 + lambda_horizon_;
1881
1882 // miu_horizon_
1883 int m11 = m10 + miu_horizon_;
1884
1885 CHECK_EQ(m11, num_of_constraints_);
1886
1887 AINFO << "dynamics constatins to: " << m1;
1888 AINFO << "control rate constraints (only steering) to: " << m2;
1889 AINFO << "sampling time equality constraints to: " << m3;
1890 AINFO << "obstacle constraints to: " << m4;
1891 AINFO << "start conf constraints to: " << m5;
1892 AINFO << "constraints on x,y,v to: " << m6;
1893 AINFO << "end constraints to: " << m7;
1894 AINFO << "control bnd to: " << m8;
1895 AINFO << "time interval constraints to: " << m9;
1896 AINFO << "lambda constraints to: " << m10;
1897 AINFO << "miu constraints to: " << m11;
1898 AINFO << "total constraints: " << num_of_constraints_;
1899
1900 for (int idx = 0; idx < m; ++idx) {
1901 if (g[idx] > g_u_tmp[idx] + delta_v || g[idx] < g_l_tmp[idx] - delta_v) {
1902 AINFO << "constrains idx unfeasible: " << idx << ", g: " << g[idx] << ", lower: " << g_l_tmp[idx]
1903 << ", upper: " << g_u_tmp[idx];
1904 }
1905 }
1906 return true;
1907}
apollo::planning::DistanceApproachIPOPTInterface::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_interface.cc:242
AINFO
#define AINFO
Definition log.h:42

◆ eval_constraints()

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

Template to compute constraints

在文件 distance_approach_ipopt_interface.cc 第 1591 行定义.

1591 {
1592 // state start index
1593 int state_index = state_start_index_;
1594
1595 // control start index.
1596 int control_index = control_start_index_;
1597
1598 // time start index
1599 int time_index = time_start_index_;
1600
1601 int constraint_index = 0;
1602
1603 // // 1. state constraints 4 * [0, horizons-1]
1604 for (int i = 0; i < horizon_; ++i) {
1605 // x1
1606 g[constraint_index] = x[state_index + 4]
1607 - (x[state_index]
1608 + ts_ * x[time_index] * (x[state_index + 3] + ts_ * x[time_index] * 0.5 * x[control_index + 1])
1609 * cos(x[state_index + 2]
1610 + ts_ * x[time_index] * 0.5 * x[state_index + 3] * tan(x[control_index])
1611 / wheelbase_));
1612 // TODO(Jinyun): evaluate performance of different models
1613 // g[constraint_index] =
1614 // x[state_index + 4] -
1615 // (x[state_index] +
1616 // ts_ * x[time_index] * x[state_index + 3] * cos(x[state_index + 2]));
1617 // g[constraint_index] =
1618 // x[state_index + 4] -
1619 // ((xWS_(0, i) + ts_ * xWS_(3, i) * cos(xWS_(2, i))) +
1620 // (x[state_index] - xWS_(0, i)) +
1621 // (xWS_(3, i) * cos(xWS_(2, i))) * (ts_ * x[time_index] - ts_) +
1622 // (ts_ * cos(xWS_(2, i))) * (x[state_index + 3] - xWS_(3, i)) +
1623 // (-ts_ * xWS_(3, i) * sin(xWS_(2, i))) *
1624 // (x[state_index + 2] - xWS_(2, i)));
1625
1626 // x2
1627 g[constraint_index + 1] = x[state_index + 5]
1628 - (x[state_index + 1]
1629 + ts_ * x[time_index] * (x[state_index + 3] + ts_ * x[time_index] * 0.5 * x[control_index + 1])
1630 * sin(x[state_index + 2]
1631 + ts_ * x[time_index] * 0.5 * x[state_index + 3] * tan(x[control_index])
1632 / wheelbase_));
1633 // g[constraint_index + 1] =
1634 // x[state_index + 5] -
1635 // (x[state_index + 1] +
1636 // ts_ * x[time_index] * x[state_index + 3] * sin(x[state_index + 2]));
1637 // g[constraint_index + 1] =
1638 // x[state_index + 5] -
1639 // ((xWS_(1, i) + ts_ * xWS_(3, i) * sin(xWS_(2, i))) +
1640 // (x[state_index + 1] - xWS_(1, i)) +
1641 // (xWS_(3, i) * sin(xWS_(2, i))) * (ts_ * x[time_index] - ts_) +
1642 // (ts_ * sin(xWS_(2, i))) * (x[state_index + 3] - xWS_(3, i)) +
1643 // (ts_ * xWS_(3, i) * cos(xWS_(2, i))) *
1644 // (x[state_index + 2] - xWS_(2, i)));
1645
1646 // x3
1647 g[constraint_index + 2] = x[state_index + 6]
1648 - (x[state_index + 2]
1649 + ts_ * x[time_index] * (x[state_index + 3] + ts_ * x[time_index] * 0.5 * x[control_index + 1])
1650 * tan(x[control_index]) / wheelbase_);
1651 // g[constraint_index + 2] =
1652 // x[state_index + 6] -
1653 // (x[state_index + 2] + ts_ * x[time_index] * x[state_index + 3] *
1654 // tan(x[control_index]) / wheelbase_);
1655 // g[constraint_index + 2] =
1656 // x[state_index + 6] -
1657 // ((xWS_(2, i) + ts_ * xWS_(3, i) * tan(uWS_(0, i)) / wheelbase_) +
1658 // (x[state_index + 2] - xWS_(2, i)) +
1659 // (xWS_(3, i) * tan(uWS_(0, i)) / wheelbase_) *
1660 // (ts_ * x[time_index] - ts_) +
1661 // (ts_ * tan(uWS_(0, i)) / wheelbase_) *
1662 // (x[state_index + 3] - xWS_(3, i)) +
1663 // (ts_ * xWS_(3, i) / cos(uWS_(0, i)) / cos(uWS_(0, i)) / wheelbase_)
1664 // *
1665 // (x[control_index] - uWS_(0, i)));
1666
1667 // x4
1668 g[constraint_index + 3]
1669 = x[state_index + 7] - (x[state_index + 3] + ts_ * x[time_index] * x[control_index + 1]);
1670 // g[constraint_index + 3] =
1671 // x[state_index + 7] -
1672 // ((xWS_(3, i) + ts_ * uWS_(1, i)) + (x[state_index + 3] - xWS_(3, i))
1673 // +
1674 // uWS_(1, i) * (ts_ * x[time_index] - ts_) +
1675 // ts_ * (x[control_index + 1] - uWS_(1, i)));
1676
1677 control_index += 2;
1678 constraint_index += 4;
1679 time_index++;
1680 state_index += 4;
1681 }
1682
1683 ADEBUG << "constraint_index after adding Euler forward dynamics constraints "
1684 "updated: "
1685 << constraint_index;
1686
1687 // 2. Control rate limit constraints, 1 * [0, horizons-1], only apply
1688 // steering rate as of now
1689 control_index = control_start_index_;
1690 time_index = time_start_index_;
1691
1692 // First rate is compare first with stitch point
1693 g[constraint_index] = (x[control_index] - last_time_u_(0, 0)) / x[time_index] / ts_;
1694 control_index += 2;
1695 constraint_index++;
1696 time_index++;
1697
1698 for (int i = 1; i < horizon_; ++i) {
1699 g[constraint_index] = (x[control_index] - x[control_index - 2]) / x[time_index] / ts_;
1700 constraint_index++;
1701 control_index += 2;
1702 time_index++;
1703 }
1704
1705 // 3. Time constraints 1 * [0, horizons-1]
1706 time_index = time_start_index_;
1707 for (int i = 0; i < horizon_; ++i) {
1708 g[constraint_index] = x[time_index + 1] - x[time_index];
1709 constraint_index++;
1710 time_index++;
1711 }
1712
1713 ADEBUG << "constraint_index after adding time constraints "
1714 "updated: "
1715 << constraint_index;
1716
1717 // 4. Three obstacles related equal constraints, one equality constraints,
1718 // [0, horizon_] * [0, obstacles_num_-1] * 4
1719 state_index = state_start_index_;
1720 int l_index = l_start_index_;
1721 int n_index = n_start_index_;
1722
1723 for (int i = 0; i < horizon_ + 1; ++i) {
1724 int edges_counter = 0;
1725 for (int j = 0; j < obstacles_num_; ++j) {
1726 int current_edges_num = obstacles_edges_num_(j, 0);
1727 Eigen::MatrixXd Aj = obstacles_A_.block(edges_counter, 0, current_edges_num, 2);
1728 Eigen::MatrixXd bj = obstacles_b_.block(edges_counter, 0, current_edges_num, 1);
1729
1730 // norm(A* lambda) <= 1
1731 T tmp1 = 0.0;
1732 T tmp2 = 0.0;
1733 for (int k = 0; k < current_edges_num; ++k) {
1734 // TODO(QiL) : replace this one directly with x
1735 tmp1 += Aj(k, 0) * x[l_index + k];
1736 tmp2 += Aj(k, 1) * x[l_index + k];
1737 }
1738 g[constraint_index] = tmp1 * tmp1 + tmp2 * tmp2;
1739
1740 // G' * mu + R' * lambda == 0
1741 g[constraint_index + 1]
1742 = x[n_index] - x[n_index + 2] + cos(x[state_index + 2]) * tmp1 + sin(x[state_index + 2]) * tmp2;
1743
1744 g[constraint_index + 2]
1745 = x[n_index + 1] - x[n_index + 3] - sin(x[state_index + 2]) * tmp1 + cos(x[state_index + 2]) * tmp2;
1746
1747 // -g'*mu + (A*t - b)*lambda > 0
1748 T tmp3 = 0.0;
1749 for (int k = 0; k < 4; ++k) {
1750 tmp3 += -g_[k] * x[n_index + k];
1751 }
1752
1753 T tmp4 = 0.0;
1754 for (int k = 0; k < current_edges_num; ++k) {
1755 tmp4 += bj(k, 0) * x[l_index + k];
1756 }
1757
1758 g[constraint_index + 3] = tmp3 + (x[state_index] + cos(x[state_index + 2]) * offset_) * tmp1
1759 + (x[state_index + 1] + sin(x[state_index + 2]) * offset_) * tmp2 - tmp4;
1760
1761 // Update index
1762 edges_counter += current_edges_num;
1763 l_index += current_edges_num;
1764 n_index += 4;
1765 constraint_index += 4;
1766 }
1767 state_index += 4;
1768 }
1769 ADEBUG << "constraint_index after obstacles avoidance constraints "
1770 "updated: "
1771 << constraint_index;
1772
1773 // 5. load variable bounds as constraints
1774 state_index = state_start_index_;
1775 control_index = control_start_index_;
1776 time_index = time_start_index_;
1777 l_index = l_start_index_;
1778 n_index = n_start_index_;
1779
1780 // start configuration
1781 g[constraint_index] = x[state_index];
1782 g[constraint_index + 1] = x[state_index + 1];
1783 g[constraint_index + 2] = x[state_index + 2];
1784 g[constraint_index + 3] = x[state_index + 3];
1785 constraint_index += 4;
1786 state_index += 4;
1787
1788 // constraints on x,y,v
1789 for (int i = 1; i < horizon_; ++i) {
1790 g[constraint_index] = x[state_index];
1791 g[constraint_index + 1] = x[state_index + 1];
1792 g[constraint_index + 2] = x[state_index + 3];
1793 constraint_index += 3;
1794 state_index += 4;
1795 }
1796
1797 // end configuration
1798 g[constraint_index] = x[state_index];
1799 g[constraint_index + 1] = x[state_index + 1];
1800 g[constraint_index + 2] = x[state_index + 2];
1801 g[constraint_index + 3] = x[state_index + 3];
1802 constraint_index += 4;
1803 state_index += 4;
1804
1805 for (int i = 0; i < horizon_; ++i) {
1806 g[constraint_index] = x[control_index];
1807 g[constraint_index + 1] = x[control_index + 1];
1808 constraint_index += 2;
1809 control_index += 2;
1810 }
1811
1812 for (int i = 0; i < horizon_ + 1; ++i) {
1813 g[constraint_index] = x[time_index];
1814 constraint_index++;
1815 time_index++;
1816 }
1817
1818 for (int i = 0; i < lambda_horizon_; ++i) {
1819 g[constraint_index] = x[l_index];
1820 constraint_index++;
1821 l_index++;
1822 }
1823
1824 for (int i = 0; i < miu_horizon_; ++i) {
1825 g[constraint_index] = x[n_index];
1826 constraint_index++;
1827 n_index++;
1828 }
1829}
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::DistanceApproachIPOPTInterface::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_interface.cc 第 507 行定义.

507 {
508 eval_obj(n, x, &obj_value);
509 return true;
510}
apollo::planning::DistanceApproachIPOPTInterface::eval_obj
void eval_obj(int n, const T *x, T *obj_value)
Template to return the objective value
Definition distance_approach_ipopt_interface.cc:1530

◆ eval_g()

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

517 {
518 eval_constraints(n, x, m, g);
519 // if (enable_constraint_check_) check_g(n, x, m, g);
520 return true;
521}
apollo::planning::DistanceApproachIPOPTInterface::eval_constraints
void eval_constraints(int n, const T *x, int m, T *g)
Template to compute constraints
Definition distance_approach_ipopt_interface.cc:1591

◆ eval_grad_f()

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

512 {
513 gradient(tag_f, n, x, grad_f);
514 return true;
515}
tag_f
#define tag_f
Definition cos_theta_ipopt_interface.h:36

◆ eval_h()

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

1360 {
1361 if (values == nullptr) {
1362 // return the structure. This is a symmetric matrix, fill the lower left
1363 // triangle only.
1364
1365 for (int idx = 0; idx < nnz_L; idx++) {
1366 iRow[idx] = rind_L[idx];
1367 jCol[idx] = cind_L[idx];
1368 }
1369 } else {
1370 // return the values. This is a symmetric matrix, fill the lower left
1371 // triangle only
1372
1373 obj_lam[0] = obj_factor;
1374
1375 for (int idx = 0; idx < m; idx++) {
1376 obj_lam[1 + idx] = lambda[idx];
1377 }
1378
1379 set_param_vec(tag_L, m + 1, obj_lam);
1380 sparse_hess(tag_L, n, 1, const_cast<double*>(x), &nnz_L, &rind_L, &cind_L, &hessval, options_L);
1381
1382 for (int idx = 0; idx < nnz_L; idx++) {
1383 values[idx] = hessval[idx];
1384 }
1385 }
1386
1387 return true;
1388}
tag_L
#define tag_L
Definition cos_theta_ipopt_interface.h:38

◆ eval_jac_g()

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

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

◆ eval_jac_g_ser()

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

560 {
561 ADEBUG << "eval_jac_g";
562 CHECK_EQ(n, num_of_variables_) << "No. of variables wrong in eval_jac_g. n : " << n;
563 CHECK_EQ(m, num_of_constraints_) << "No. of constraints wrong in eval_jac_g. n : " << m;
564
565 if (values == nullptr) {
566 int nz_index = 0;
567 int constraint_index = 0;
568 int state_index = state_start_index_;
569 int control_index = control_start_index_;
570 int time_index = time_start_index_;
571
572 // 1. State Constraint with respect to variables
573 for (int i = 0; i < horizon_; ++i) {
574 // g(0)' with respect to x0 ~ x7
575 iRow[nz_index] = state_index;
576 jCol[nz_index] = state_index;
577 ++nz_index;
578
579 iRow[nz_index] = state_index;
580 jCol[nz_index] = state_index + 2;
581 ++nz_index;
582
583 iRow[nz_index] = state_index;
584 jCol[nz_index] = state_index + 3;
585 ++nz_index;
586
587 iRow[nz_index] = state_index;
588 jCol[nz_index] = state_index + 4;
589 ++nz_index;
590
591 // g(0)' with respect to u0 ~ u1'
592 iRow[nz_index] = state_index;
593 jCol[nz_index] = control_index;
594 ++nz_index;
595
596 iRow[nz_index] = state_index;
597 jCol[nz_index] = control_index + 1;
598 ++nz_index;
599
600 // g(0)' with respect to time
601 iRow[nz_index] = state_index;
602 jCol[nz_index] = time_index;
603 ++nz_index;
604
605 // g(1)' with respect to x0 ~ x7
606 iRow[nz_index] = state_index + 1;
607 jCol[nz_index] = state_index + 1;
608 ++nz_index;
609
610 iRow[nz_index] = state_index + 1;
611 jCol[nz_index] = state_index + 2;
612 ++nz_index;
613
614 iRow[nz_index] = state_index + 1;
615 jCol[nz_index] = state_index + 3;
616 ++nz_index;
617
618 iRow[nz_index] = state_index + 1;
619 jCol[nz_index] = state_index + 5;
620 ++nz_index;
621
622 // g(1)' with respect to u0 ~ u1'
623 iRow[nz_index] = state_index + 1;
624 jCol[nz_index] = control_index;
625 ++nz_index;
626
627 iRow[nz_index] = state_index + 1;
628 jCol[nz_index] = control_index + 1;
629 ++nz_index;
630
631 // g(1)' with respect to time
632 iRow[nz_index] = state_index + 1;
633 jCol[nz_index] = time_index;
634 ++nz_index;
635
636 // g(2)' with respect to x0 ~ x7
637 iRow[nz_index] = state_index + 2;
638 jCol[nz_index] = state_index + 2;
639 ++nz_index;
640
641 iRow[nz_index] = state_index + 2;
642 jCol[nz_index] = state_index + 3;
643 ++nz_index;
644
645 iRow[nz_index] = state_index + 2;
646 jCol[nz_index] = state_index + 6;
647 ++nz_index;
648
649 // g(2)' with respect to u0 ~ u1'
650 iRow[nz_index] = state_index + 2;
651 jCol[nz_index] = control_index;
652 ++nz_index;
653
654 iRow[nz_index] = state_index + 2;
655 jCol[nz_index] = control_index + 1;
656 ++nz_index;
657
658 // g(2)' with respect to time
659 iRow[nz_index] = state_index + 2;
660 jCol[nz_index] = time_index;
661 ++nz_index;
662
663 // g(3)' with x0 ~ x7
664 iRow[nz_index] = state_index + 3;
665 jCol[nz_index] = state_index + 3;
666 ++nz_index;
667
668 iRow[nz_index] = state_index + 3;
669 jCol[nz_index] = state_index + 7;
670 ++nz_index;
671
672 // g(3)' with respect to u0 ~ u1'
673 iRow[nz_index] = state_index + 3;
674 jCol[nz_index] = control_index + 1;
675 ++nz_index;
676
677 // g(3)' with respect to time
678 iRow[nz_index] = state_index + 3;
679 jCol[nz_index] = time_index;
680 ++nz_index;
681
682 state_index += 4;
683 control_index += 2;
684 time_index++;
685 constraint_index += 4;
686 }
687
688 // 2. only have control rate constraints on u0 , range [0, horizon_-1]
689 control_index = control_start_index_;
690 state_index = state_start_index_;
691 time_index = time_start_index_;
692
693 // First one, with respect to u(0, 0)
694 iRow[nz_index] = constraint_index;
695 jCol[nz_index] = control_index;
696 ++nz_index;
697
698 // First element, with respect to time
699 iRow[nz_index] = constraint_index;
700 jCol[nz_index] = time_index;
701 ++nz_index;
702
703 control_index += 2;
704 time_index++;
705 constraint_index++;
706
707 for (int i = 1; i < horizon_; ++i) {
708 // with respect to u(0, i-1)
709 iRow[nz_index] = constraint_index;
710 jCol[nz_index] = control_index - 2;
711 ++nz_index;
712
713 // with respect to u(0, i)
714 iRow[nz_index] = constraint_index;
715 jCol[nz_index] = control_index;
716 ++nz_index;
717
718 // with respect to time
719 iRow[nz_index] = constraint_index;
720 jCol[nz_index] = time_index;
721 ++nz_index;
722
723 // only consider rate limits on u0
724 control_index += 2;
725 constraint_index++;
726 time_index++;
727 }
728
729 // 3. Time constraints [0, horizon_ -1]
730 time_index = time_start_index_;
731
732 for (int i = 0; i < horizon_; ++i) {
733 // with respect to timescale(0, i-1)
734 iRow[nz_index] = constraint_index;
735 jCol[nz_index] = time_index;
736 ++nz_index;
737
738 // with respect to timescale(0, i)
739 iRow[nz_index] = constraint_index;
740 jCol[nz_index] = time_index + 1;
741 ++nz_index;
742
743 time_index++;
744 constraint_index++;
745 }
746
747 // 4. Three obstacles related equal constraints, one equality constraints,
748 // [0, horizon_] * [0, obstacles_num_-1] * 4
749 state_index = state_start_index_;
750 int l_index = l_start_index_;
751 int n_index = n_start_index_;
752 for (int i = 0; i < horizon_ + 1; ++i) {
753 for (int j = 0; j < obstacles_num_; ++j) {
754 int current_edges_num = obstacles_edges_num_(j, 0);
755
756 // 1. norm(A* lambda == 1)
757 for (int k = 0; k < current_edges_num; ++k) {
758 // with respect to l
759 iRow[nz_index] = constraint_index;
760 jCol[nz_index] = l_index + k;
761 ++nz_index;
762 }
763
764 // 2. G' * mu + R' * lambda == 0, part 1
765 // With respect to x
766 iRow[nz_index] = constraint_index + 1;
767 jCol[nz_index] = state_index + 2;
768 ++nz_index;
769
770 // with respect to l
771 for (int k = 0; k < current_edges_num; ++k) {
772 iRow[nz_index] = constraint_index + 1;
773 jCol[nz_index] = l_index + k;
774 ++nz_index;
775 }
776
777 // With respect to n
778 iRow[nz_index] = constraint_index + 1;
779 jCol[nz_index] = n_index;
780 ++nz_index;
781
782 iRow[nz_index] = constraint_index + 1;
783 jCol[nz_index] = n_index + 2;
784 ++nz_index;
785
786 // 2. G' * mu + R' * lambda == 0, part 2
787 // With respect to x
788 iRow[nz_index] = constraint_index + 2;
789 jCol[nz_index] = state_index + 2;
790 ++nz_index;
791
792 // with respect to l
793 for (int k = 0; k < current_edges_num; ++k) {
794 iRow[nz_index] = constraint_index + 2;
795 jCol[nz_index] = l_index + k;
796 ++nz_index;
797 }
798
799 // With respect to n
800 iRow[nz_index] = constraint_index + 2;
801 jCol[nz_index] = n_index + 1;
802 ++nz_index;
803
804 iRow[nz_index] = constraint_index + 2;
805 jCol[nz_index] = n_index + 3;
806 ++nz_index;
807
808 // -g'*mu + (A*t - b)*lambda > 0
809 // With respect to x
810 iRow[nz_index] = constraint_index + 3;
811 jCol[nz_index] = state_index;
812 ++nz_index;
813
814 iRow[nz_index] = constraint_index + 3;
815 jCol[nz_index] = state_index + 1;
816 ++nz_index;
817
818 iRow[nz_index] = constraint_index + 3;
819 jCol[nz_index] = state_index + 2;
820 ++nz_index;
821
822 // with respect to l
823 for (int k = 0; k < current_edges_num; ++k) {
824 iRow[nz_index] = constraint_index + 3;
825 jCol[nz_index] = l_index + k;
826 ++nz_index;
827 }
828
829 // with respect to n
830 for (int k = 0; k < 4; ++k) {
831 iRow[nz_index] = constraint_index + 3;
832 jCol[nz_index] = n_index + k;
833 ++nz_index;
834 }
835
836 // Update index
837 l_index += current_edges_num;
838 n_index += 4;
839 constraint_index += 4;
840 }
841 state_index += 4;
842 }
843
844 // 5. load variable bounds as constraints
845 state_index = state_start_index_;
846 control_index = control_start_index_;
847 time_index = time_start_index_;
848 l_index = l_start_index_;
849 n_index = n_start_index_;
850
851 // start configuration
852 iRow[nz_index] = constraint_index;
853 jCol[nz_index] = state_index;
854 nz_index++;
855 iRow[nz_index] = constraint_index + 1;
856 jCol[nz_index] = state_index + 1;
857 nz_index++;
858 iRow[nz_index] = constraint_index + 2;
859 jCol[nz_index] = state_index + 2;
860 nz_index++;
861 iRow[nz_index] = constraint_index + 3;
862 jCol[nz_index] = state_index + 3;
863 nz_index++;
864 constraint_index += 4;
865 state_index += 4;
866
867 for (int i = 1; i < horizon_; ++i) {
868 iRow[nz_index] = constraint_index;
869 jCol[nz_index] = state_index;
870 nz_index++;
871 iRow[nz_index] = constraint_index + 1;
872 jCol[nz_index] = state_index + 1;
873 nz_index++;
874 iRow[nz_index] = constraint_index + 2;
875 jCol[nz_index] = state_index + 3;
876 nz_index++;
877 constraint_index += 3;
878 state_index += 4;
879 }
880
881 // end configuration
882 iRow[nz_index] = constraint_index;
883 jCol[nz_index] = state_index;
884 nz_index++;
885 iRow[nz_index] = constraint_index + 1;
886 jCol[nz_index] = state_index + 1;
887 nz_index++;
888 iRow[nz_index] = constraint_index + 2;
889 jCol[nz_index] = state_index + 2;
890 nz_index++;
891 iRow[nz_index] = constraint_index + 3;
892 jCol[nz_index] = state_index + 3;
893 nz_index++;
894 constraint_index += 4;
895 state_index += 4;
896
897 for (int i = 0; i < horizon_; ++i) {
898 iRow[nz_index] = constraint_index;
899 jCol[nz_index] = control_index;
900 nz_index++;
901 iRow[nz_index] = constraint_index + 1;
902 jCol[nz_index] = control_index + 1;
903 nz_index++;
904 constraint_index += 2;
905 control_index += 2;
906 }
907
908 for (int i = 0; i < horizon_ + 1; ++i) {
909 iRow[nz_index] = constraint_index;
910 jCol[nz_index] = time_index;
911 nz_index++;
912 constraint_index++;
913 time_index++;
914 }
915
916 for (int i = 0; i < lambda_horizon_; ++i) {
917 iRow[nz_index] = constraint_index;
918 jCol[nz_index] = l_index;
919 nz_index++;
920 constraint_index++;
921 l_index++;
922 }
923
924 for (int i = 0; i < miu_horizon_; ++i) {
925 iRow[nz_index] = constraint_index;
926 jCol[nz_index] = n_index;
927 nz_index++;
928 constraint_index++;
929 n_index++;
930 }
931
932 CHECK_EQ(nz_index, static_cast<int>(nele_jac));
933 CHECK_EQ(constraint_index, static_cast<int>(m));
934 } else {
935 std::fill(values, values + nele_jac, 0.0);
936 int nz_index = 0;
937
938 int time_index = time_start_index_;
939 int state_index = state_start_index_;
940 int control_index = control_start_index_;
941
942 // TODO(QiL) : initially implemented to be debug friendly, later iterate
943 // towards better efficiency
944 // 1. state constraints 4 * [0, horizons-1]
945 for (int i = 0; i < horizon_; ++i) {
946 values[nz_index] = -1.0;
947 ++nz_index;
948
949 values[nz_index] = x[time_index] * ts_
950 * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
951 * std::sin(x[state_index + 2]
952 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
953 / wheelbase_); // a.
954 ++nz_index;
955
956 values[nz_index] = -1.0
957 * (x[time_index] * ts_
958 * std::cos(
959 x[state_index + 2]
960 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
961 / wheelbase_)
962 + x[time_index] * ts_ * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
963 * (-1) * x[time_index] * ts_ * 0.5 * std::tan(x[control_index]) / wheelbase_
964 * std::sin(
965 x[state_index + 2]
966 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
967 / wheelbase_)); // b
968 ++nz_index;
969
970 values[nz_index] = 1.0;
971 ++nz_index;
972
973 values[nz_index] = x[time_index] * ts_
974 * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
975 * std::sin(x[state_index + 2]
976 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
977 / wheelbase_)
978 * x[time_index] * ts_ * 0.5 * x[state_index + 3]
979 / (std::cos(x[control_index]) * std::cos(x[control_index])) / wheelbase_; // c
980 ++nz_index;
981
982 values[nz_index] = -1.0
983 * (x[time_index] * ts_ * x[time_index] * ts_ * 0.5
984 * std::cos(
985 x[state_index + 2]
986 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
987 / wheelbase_)); // d
988 ++nz_index;
989
990 values[nz_index] = -1.0
991 * (ts_ * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
992 * std::cos(
993 x[state_index + 2]
994 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
995 / wheelbase_)
996 + x[time_index] * ts_ * ts_ * 0.5 * x[control_index + 1]
997 * std::cos(
998 x[state_index + 2]
999 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1000 / wheelbase_)
1001 - x[time_index] * ts_ * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
1002 * x[state_index + 3] * ts_ * 0.5 * std::tan(x[control_index]) / wheelbase_
1003 * std::sin(
1004 x[state_index + 2]
1005 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1006 / wheelbase_)); // e
1007 ++nz_index;
1008
1009 values[nz_index] = -1.0;
1010 ++nz_index;
1011
1012 values[nz_index] = -1.0
1013 * (x[time_index] * ts_ * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
1014 * std::cos(
1015 x[state_index + 2]
1016 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1017 / wheelbase_)); // f.
1018 ++nz_index;
1019
1020 values[nz_index] = -1.0
1021 * (x[time_index] * ts_
1022 * std::sin(
1023 x[state_index + 2]
1024 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1025 / wheelbase_)
1026 + x[time_index] * ts_ * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
1027 * x[time_index] * ts_ * 0.5 * std::tan(x[control_index]) / wheelbase_
1028 * std::cos(
1029 x[state_index + 2]
1030 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1031 / wheelbase_)); // g
1032 ++nz_index;
1033
1034 values[nz_index] = 1.0;
1035 ++nz_index;
1036
1037 values[nz_index] = -1.0
1038 * (x[time_index] * ts_ * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
1039 * std::cos(
1040 x[state_index + 2]
1041 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1042 / wheelbase_)
1043 * x[time_index] * ts_ * 0.5 * x[state_index + 3]
1044 / (std::cos(x[control_index]) * std::cos(x[control_index])) / wheelbase_); // h
1045 ++nz_index;
1046
1047 values[nz_index] = -1.0
1048 * (x[time_index] * ts_ * x[time_index] * ts_ * 0.5
1049 * std::sin(
1050 x[state_index + 2]
1051 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1052 / wheelbase_)); // i
1053 ++nz_index;
1054
1055 values[nz_index] = -1.0
1056 * (ts_ * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
1057 * std::sin(
1058 x[state_index + 2]
1059 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1060 / wheelbase_)
1061 + x[time_index] * ts_ * ts_ * 0.5 * x[control_index + 1]
1062 * std::sin(
1063 x[state_index + 2]
1064 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1065 / wheelbase_)
1066 + x[time_index] * ts_ * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
1067 * x[state_index + 3] * ts_ * 0.5 * std::tan(x[control_index]) / wheelbase_
1068 * std::cos(
1069 x[state_index + 2]
1070 + x[time_index] * ts_ * 0.5 * x[state_index + 3] * std::tan(x[control_index])
1071 / wheelbase_)); // j
1072 ++nz_index;
1073
1074 values[nz_index] = -1.0;
1075 ++nz_index;
1076
1077 values[nz_index] = -1.0 * x[time_index] * ts_ * std::tan(x[control_index]) / wheelbase_; // k.
1078 ++nz_index;
1079
1080 values[nz_index] = 1.0;
1081 ++nz_index;
1082
1083 values[nz_index] = -1.0
1084 * (x[time_index] * ts_ * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
1085 / (std::cos(x[control_index]) * std::cos(x[control_index])) / wheelbase_); // l.
1086 ++nz_index;
1087
1088 values[nz_index] = -1.0
1089 * (x[time_index] * ts_ * x[time_index] * ts_ * 0.5 * std::tan(x[control_index])
1090 / wheelbase_); // m.
1091 ++nz_index;
1092
1093 values[nz_index] = -1.0
1094 * (ts_ * (x[state_index + 3] + x[time_index] * ts_ * 0.5 * x[control_index + 1])
1095 * std::tan(x[control_index]) / wheelbase_
1096 + x[time_index] * ts_ * ts_ * 0.5 * x[control_index + 1] * std::tan(x[control_index])
1097 / wheelbase_); // n.
1098 ++nz_index;
1099
1100 values[nz_index] = -1.0;
1101 ++nz_index;
1102
1103 values[nz_index] = 1.0;
1104 ++nz_index;
1105
1106 values[nz_index] = -1.0 * ts_ * x[time_index]; // o.
1107 ++nz_index;
1108
1109 values[nz_index] = -1.0 * ts_ * x[control_index + 1]; // p.
1110 ++nz_index;
1111
1112 state_index += 4;
1113 control_index += 2;
1114 time_index++;
1115 }
1116
1117 // 2. control rate constraints 1 * [0, horizons-1]
1118 control_index = control_start_index_;
1119 state_index = state_start_index_;
1120 time_index = time_start_index_;
1121
1122 // First horizon
1123
1124 // with respect to u(0, 0)
1125 values[nz_index] = 1.0 / x[time_index] / ts_; // q
1126 ++nz_index;
1127
1128 // with respect to time
1129 values[nz_index] = -1.0 * (x[control_index] - last_time_u_(0, 0)) / x[time_index] / x[time_index] / ts_;
1130 ++nz_index;
1131 time_index++;
1132 control_index += 2;
1133
1134 for (int i = 1; i < horizon_; ++i) {
1135 // with respect to u(0, i-1)
1136
1137 values[nz_index] = -1.0 / x[time_index] / ts_;
1138 ++nz_index;
1139
1140 // with respect to u(0, i)
1141 values[nz_index] = 1.0 / x[time_index] / ts_;
1142 ++nz_index;
1143
1144 // with respect to time
1145 values[nz_index] = -1.0 * (x[control_index] - x[control_index - 2]) / x[time_index] / x[time_index] / ts_;
1146 ++nz_index;
1147
1148 control_index += 2;
1149 time_index++;
1150 }
1151
1152 ADEBUG << "After fulfilled control rate constraints derivative, nz_index : " << nz_index
1153 << " nele_jac : " << nele_jac;
1154
1155 // 3. Time constraints [0, horizon_ -1]
1156 time_index = time_start_index_;
1157 for (int i = 0; i < horizon_; ++i) {
1158 // with respect to timescale(0, i-1)
1159 values[nz_index] = -1.0;
1160 ++nz_index;
1161
1162 // with respect to timescale(0, i)
1163 values[nz_index] = 1.0;
1164 ++nz_index;
1165
1166 time_index++;
1167 }
1168
1169 ADEBUG << "After fulfilled time constraints derivative, nz_index : " << nz_index << " nele_jac : " << nele_jac;
1170
1171 // 4. Three obstacles related equal constraints, one equality constraints,
1172 // [0, horizon_] * [0, obstacles_num_-1] * 4
1173
1174 state_index = state_start_index_;
1175 int l_index = l_start_index_;
1176 int n_index = n_start_index_;
1177
1178 for (int i = 0; i < horizon_ + 1; ++i) {
1179 int edges_counter = 0;
1180 for (int j = 0; j < obstacles_num_; ++j) {
1181 int current_edges_num = obstacles_edges_num_(j, 0);
1182 Eigen::MatrixXd Aj = obstacles_A_.block(edges_counter, 0, current_edges_num, 2);
1183 Eigen::MatrixXd bj = obstacles_b_.block(edges_counter, 0, current_edges_num, 1);
1184
1185 // TODO(QiL) : Remove redundant calculation
1186 double tmp1 = 0;
1187 double tmp2 = 0;
1188 for (int k = 0; k < current_edges_num; ++k) {
1189 // TODO(QiL) : replace this one directly with x
1190 tmp1 += Aj(k, 0) * x[l_index + k];
1191 tmp2 += Aj(k, 1) * x[l_index + k];
1192 }
1193
1194 // 1. norm(A* lambda == 1)
1195 for (int k = 0; k < current_edges_num; ++k) {
1196 // with respect to l
1197 values[nz_index] = 2 * tmp1 * Aj(k, 0) + 2 * tmp2 * Aj(k, 1); // t0~tk
1198 ++nz_index;
1199 }
1200
1201 // 2. G' * mu + R' * lambda == 0, part 1
1202 // With respect to x
1203 values[nz_index] = -std::sin(x[state_index + 2]) * tmp1 + std::cos(x[state_index + 2]) * tmp2; // u
1204 ++nz_index;
1205
1206 // with respect to l
1207 for (int k = 0; k < current_edges_num; ++k) {
1208 values[nz_index] = std::cos(x[state_index + 2]) * Aj(k, 0)
1209 + std::sin(x[state_index + 2]) * Aj(k, 1); // v0~vn
1210 ++nz_index;
1211 }
1212
1213 // With respect to n
1214 values[nz_index] = 1.0; // w0
1215 ++nz_index;
1216
1217 values[nz_index] = -1.0; // w2
1218 ++nz_index;
1219
1220 // 3. G' * mu + R' * lambda == 0, part 2
1221 // With respect to x
1222 values[nz_index] = -std::cos(x[state_index + 2]) * tmp1 - std::sin(x[state_index + 2]) * tmp2; // x
1223 ++nz_index;
1224
1225 // with respect to l
1226 for (int k = 0; k < current_edges_num; ++k) {
1227 values[nz_index] = -std::sin(x[state_index + 2]) * Aj(k, 0)
1228 + std::cos(x[state_index + 2]) * Aj(k, 1); // y0~yn
1229 ++nz_index;
1230 }
1231
1232 // With respect to n
1233 values[nz_index] = 1.0; // z1
1234 ++nz_index;
1235
1236 values[nz_index] = -1.0; // z3
1237 ++nz_index;
1238
1239 // 3. -g'*mu + (A*t - b)*lambda > 0
1240 double tmp3 = 0.0;
1241 double tmp4 = 0.0;
1242 for (int k = 0; k < 4; ++k) {
1243 tmp3 += -g_[k] * x[n_index + k];
1244 }
1245
1246 for (int k = 0; k < current_edges_num; ++k) {
1247 tmp4 += bj(k, 0) * x[l_index + k];
1248 }
1249
1250 // With respect to x
1251 values[nz_index] = tmp1; // aa1
1252 ++nz_index;
1253
1254 values[nz_index] = tmp2; // bb1
1255 ++nz_index;
1256
1257 values[nz_index] = -std::sin(x[state_index + 2]) * offset_ * tmp1
1258 + std::cos(x[state_index + 2]) * offset_ * tmp2; // cc1
1259 ++nz_index;
1260
1261 // with respect to l
1262 for (int k = 0; k < current_edges_num; ++k) {
1263 values[nz_index] = (x[state_index] + std::cos(x[state_index + 2]) * offset_) * Aj(k, 0)
1264 + (x[state_index + 1] + std::sin(x[state_index + 2]) * offset_) * Aj(k, 1)
1265 - bj(k, 0); // ddk
1266 ++nz_index;
1267 }
1268
1269 // with respect to n
1270 for (int k = 0; k < 4; ++k) {
1271 values[nz_index] = -g_[k]; // eek
1272 ++nz_index;
1273 }
1274
1275 // Update index
1276 edges_counter += current_edges_num;
1277 l_index += current_edges_num;
1278 n_index += 4;
1279 }
1280 state_index += 4;
1281 }
1282
1283 // 5. load variable bounds as constraints
1284 state_index = state_start_index_;
1285 control_index = control_start_index_;
1286 time_index = time_start_index_;
1287 l_index = l_start_index_;
1288 n_index = n_start_index_;
1289
1290 // start configuration
1291 values[nz_index] = 1.0;
1292 nz_index++;
1293 values[nz_index] = 1.0;
1294 nz_index++;
1295 values[nz_index] = 1.0;
1296 nz_index++;
1297 values[nz_index] = 1.0;
1298 nz_index++;
1299
1300 for (int i = 1; i < horizon_; ++i) {
1301 values[nz_index] = 1.0;
1302 nz_index++;
1303 values[nz_index] = 1.0;
1304 nz_index++;
1305 values[nz_index] = 1.0;
1306 nz_index++;
1307 }
1308
1309 // end configuration
1310 values[nz_index] = 1.0;
1311 nz_index++;
1312 values[nz_index] = 1.0;
1313 nz_index++;
1314 values[nz_index] = 1.0;
1315 nz_index++;
1316 values[nz_index] = 1.0;
1317 nz_index++;
1318
1319 for (int i = 0; i < horizon_; ++i) {
1320 values[nz_index] = 1.0;
1321 nz_index++;
1322 values[nz_index] = 1.0;
1323 nz_index++;
1324 }
1325
1326 for (int i = 0; i < horizon_ + 1; ++i) {
1327 values[nz_index] = 1.0;
1328 nz_index++;
1329 }
1330
1331 for (int i = 0; i < lambda_horizon_; ++i) {
1332 values[nz_index] = 1.0;
1333 nz_index++;
1334 }
1335
1336 for (int i = 0; i < miu_horizon_; ++i) {
1337 values[nz_index] = 1.0;
1338 nz_index++;
1339 }
1340
1341 ADEBUG << "eval_jac_g, fulfilled obstacle constraint values";
1342 CHECK_EQ(nz_index, static_cast<int>(nele_jac));
1343 }
1344
1345 ADEBUG << "eval_jac_g done";
1346 return true;
1347} // NOLINT

◆ eval_obj()

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

Template to return the objective value

在文件 distance_approach_ipopt_interface.cc 第 1530 行定义.

1530 {
1531 // Objective is :
1532 // min control inputs
1533 // min input rate
1534 // min time (if the time step is not fixed)
1535 // regularization wrt warm start trajectory
1536 DCHECK(ts_ != 0) << "ts in distance_approach_ is 0";
1537 int control_index = control_start_index_;
1538 int time_index = time_start_index_;
1539 int state_index = state_start_index_;
1540
1541 // TODO(QiL): Initial implementation towards earlier understanding and debug
1542 // purpose, later code refine towards improving efficiency
1543
1544 *obj_value = 0.0;
1545 // 1. objective to minimize state diff to warm up
1546 for (int i = 0; i < horizon_ + 1; ++i) {
1547 T x1_diff = x[state_index] - xWS_(0, i);
1548 T x2_diff = x[state_index + 1] - xWS_(1, i);
1549 T x3_diff = x[state_index + 2] - xWS_(2, i);
1550 T x4_abs = x[state_index + 3];
1551 *obj_value += weight_state_x_ * x1_diff * x1_diff + weight_state_y_ * x2_diff * x2_diff
1552 + weight_state_phi_ * x3_diff * x3_diff + weight_state_v_ * x4_abs * x4_abs;
1553 state_index += 4;
1554 }
1555
1556 // 2. objective to minimize u square
1557 for (int i = 0; i < horizon_; ++i) {
1558 *obj_value += weight_input_steer_ * x[control_index] * x[control_index]
1559 + weight_input_a_ * x[control_index + 1] * x[control_index + 1];
1560 control_index += 2;
1561 }
1562
1563 // 3. objective to minimize input change rate for first horizon
1564 control_index = control_start_index_;
1565 T last_time_steer_rate = (x[control_index] - last_time_u_(0, 0)) / x[time_index] / ts_;
1566 T last_time_a_rate = (x[control_index + 1] - last_time_u_(1, 0)) / x[time_index] / ts_;
1567 *obj_value += weight_stitching_steer_ * last_time_steer_rate * last_time_steer_rate
1568 + weight_stitching_a_ * last_time_a_rate * last_time_a_rate;
1569
1570 // 4. objective to minimize input change rates, [0- horizon_ -2]
1571 time_index++;
1572 for (int i = 0; i < horizon_ - 1; ++i) {
1573 T steering_rate = (x[control_index + 2] - x[control_index]) / x[time_index] / ts_;
1574 T a_rate = (x[control_index + 3] - x[control_index + 1]) / x[time_index] / ts_;
1575 *obj_value += weight_rate_steer_ * steering_rate * steering_rate + weight_rate_a_ * a_rate * a_rate;
1576 control_index += 2;
1577 time_index++;
1578 }
1579
1580 // 5. objective to minimize total time [0, horizon_]
1581 time_index = time_start_index_;
1582 for (int i = 0; i < horizon_ + 1; ++i) {
1583 T first_order_penalty = weight_first_order_time_ * x[time_index];
1584 T second_order_penalty = weight_second_order_time_ * x[time_index] * x[time_index];
1585 *obj_value += first_order_penalty + second_order_penalty;
1586 time_index++;
1587 }
1588}

◆ finalize_solution()

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

1401 {
1402 int state_index = state_start_index_;
1403 int control_index = control_start_index_;
1404 int time_index = time_start_index_;
1405 int dual_l_index = l_start_index_;
1406 int dual_n_index = n_start_index_;
1407
1408 // enable_constraint_check_: for debug only
1409 if (enable_constraint_check_) {
1410 ADEBUG << "final resolution constraint checking";
1411 check_g(n, x, m, g);
1412 }
1413 // 1. state variables, 4 * [0, horizon]
1414 // 2. control variables, 2 * [0, horizon_-1]
1415 // 3. sampling time variables, 1 * [0, horizon_]
1416 // 4. dual_l obstacles_edges_sum_ * [0, horizon]
1417 // 5. dual_n obstacles_num * [0, horizon]
1418 for (int i = 0; i < horizon_; ++i) {
1419 state_result_(0, i) = x[state_index];
1420 state_result_(1, i) = x[state_index + 1];
1421 state_result_(2, i) = x[state_index + 2];
1422 state_result_(3, i) = x[state_index + 3];
1423 control_result_(0, i) = x[control_index];
1424 control_result_(1, i) = x[control_index + 1];
1425 time_result_(0, i) = ts_ * x[time_index];
1426 for (int j = 0; j < obstacles_edges_sum_; ++j) {
1427 dual_l_result_(j, i) = x[dual_l_index + j];
1428 }
1429 for (int k = 0; k < 4 * obstacles_num_; k++) {
1430 dual_n_result_(k, i) = x[dual_n_index + k];
1431 }
1432 state_index += 4;
1433 control_index += 2;
1434 time_index++;
1435 dual_l_index += obstacles_edges_sum_;
1436 dual_n_index += 4 * obstacles_num_;
1437 }
1438 state_result_(0, 0) = x0_(0, 0);
1439 state_result_(1, 0) = x0_(1, 0);
1440 state_result_(2, 0) = x0_(2, 0);
1441 state_result_(3, 0) = x0_(3, 0);
1442 // push back last horizon for states
1443 state_result_(0, horizon_) = xf_(0, 0);
1444 state_result_(1, horizon_) = xf_(1, 0);
1445 state_result_(2, horizon_) = xf_(2, 0);
1446 state_result_(3, horizon_) = xf_(3, 0);
1447
1448 for (int j = 0; j < obstacles_edges_sum_; ++j) {
1449 dual_l_result_(j, horizon_) = x[dual_l_index + j];
1450 }
1451 for (int k = 0; k < 4 * obstacles_num_; k++) {
1452 dual_n_result_(k, horizon_) = x[dual_n_index + k];
1453 }
1454 // memory deallocation of ADOL-C variables
1455 delete[] obj_lam;
1456 if (enable_jacobian_ad_) {
1457 free(rind_g);
1458 free(cind_g);
1459 free(jacval);
1460 }
1461 free(rind_L);
1462 free(cind_L);
1463 free(hessval);
1464}
apollo::planning::DistanceApproachIPOPTInterface::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_interface.cc:1831

◆ generate_tapes()

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

1909 {
1910 std::vector<double> xp(n);
1911 std::vector<double> lamp(m);
1912 std::vector<double> zl(m);
1913 std::vector<double> zu(m);
1914 std::vector<adouble> xa(n);
1915 std::vector<adouble> g(m);
1916 std::vector<double> lam(m);
1917
1918 double sig = 0.0;
1919 adouble obj_value;
1920 double dummy = 0.0;
1921 obj_lam = new double[m + 1];
1922 get_starting_point(n, 1, &xp[0], 0, &zl[0], &zu[0], m, 0, &lamp[0]);
1923
1924 trace_on(tag_f);
1925 for (int idx = 0; idx < n; idx++) {
1926 xa[idx] <<= xp[idx];
1927 }
1928 eval_obj(n, &xa[0], &obj_value);
1929 obj_value >>= dummy;
1930 trace_off();
1931
1932 trace_on(tag_g);
1933 for (int idx = 0; idx < n; idx++) {
1934 xa[idx] <<= xp[idx];
1935 }
1936 eval_constraints(n, &xa[0], m, &g[0]);
1937 for (int idx = 0; idx < m; idx++) {
1938 g[idx] >>= dummy;
1939 }
1940 trace_off();
1941
1942 trace_on(tag_L);
1943 for (int idx = 0; idx < n; idx++) {
1944 xa[idx] <<= xp[idx];
1945 }
1946 for (int idx = 0; idx < m; idx++) {
1947 lam[idx] = 1.0;
1948 }
1949 sig = 1.0;
1950 eval_obj(n, &xa[0], &obj_value);
1951 obj_value *= mkparam(sig);
1952 eval_constraints(n, &xa[0], m, &g[0]);
1953 for (int idx = 0; idx < m; idx++) {
1954 obj_value += g[idx] * mkparam(lam[idx]);
1955 }
1956 obj_value >>= dummy;
1957
1958 trace_off();
1959
1960 if (enable_jacobian_ad_) {
1961 rind_g = nullptr;
1962 cind_g = nullptr;
1963 jacval = nullptr;
1964 options_g[0] = 0; /* sparsity pattern by index domains (default) */
1965 options_g[1] = 0; /* safe mode (default) */
1966 options_g[2] = 0;
1967 options_g[3] = 0; /* column compression (default) */
1968
1969 sparse_jac(tag_g, m, n, 0, &xp[0], &nnz_jac, &rind_g, &cind_g, &jacval, options_g);
1970 *nnz_jac_g = nnz_jac;
1971 }
1972
1973 rind_L = nullptr;
1974 cind_L = nullptr;
1975 hessval = nullptr;
1976 options_L[0] = 0;
1977 options_L[1] = 1;
1978
1979 sparse_hess(tag_L, n, 0, &xp[0], &nnz_L, &rind_L, &cind_L, &hessval, options_L);
1980 *nnz_h_lag = nnz_L;
1981}
apollo::planning::DistanceApproachIPOPTInterface::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_interface.cc:452

◆ get_bounds_info()

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

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

◆ get_nlp_info()

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

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

◆ get_optimization_results()

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

1471 {
1472 ADEBUG << "get_optimization_results";
1473 *state_result = state_result_;
1474 *control_result = control_result_;
1475 *time_result = time_result_;
1476 *dual_l_result = dual_l_result_;
1477 *dual_n_result = dual_n_result_;
1478
1479 if (!distance_approach_config_.enable_initial_final_check()) {
1480 return;
1481 }
1482 CHECK_EQ(state_result_.cols(), xWS_.cols());
1483 CHECK_EQ(state_result_.rows(), xWS_.rows());
1484 double state_diff_max = 0.0;
1485 for (int i = 0; i < horizon_ + 1; ++i) {
1486 for (int j = 0; j < 4; ++j) {
1487 state_diff_max = std::max(std::abs(xWS_(j, i) - state_result_(j, i)), state_diff_max);
1488 }
1489 }
1490
1491 // 2. control variable initialization, 2 * horizon_
1492 CHECK_EQ(control_result_.cols(), uWS_.cols());
1493 CHECK_EQ(control_result_.rows(), uWS_.rows());
1494 double control_diff_max = 0.0;
1495 for (int i = 0; i < horizon_; ++i) {
1496 control_diff_max = std::max(std::abs(uWS_(0, i) - control_result_(0, i)), control_diff_max);
1497 control_diff_max = std::max(std::abs(uWS_(1, i) - control_result_(1, i)), control_diff_max);
1498 }
1499
1500 // 2. time scale variable initialization, horizon_ + 1
1501
1502 // 3. lagrange constraint l, obstacles_edges_sum_ * (horizon_+1)
1503 CHECK_EQ(dual_l_result_.cols(), l_warm_up_.cols());
1504 CHECK_EQ(dual_l_result_.rows(), l_warm_up_.rows());
1505 double l_diff_max = 0.0;
1506 for (int i = 0; i < horizon_ + 1; ++i) {
1507 for (int j = 0; j < obstacles_edges_sum_; ++j) {
1508 l_diff_max = std::max(std::abs(l_warm_up_(j, i) - dual_l_result_(j, i)), l_diff_max);
1509 }
1510 }
1511
1512 // 4. lagrange constraint m, 4*obstacles_num * (horizon_+1)
1513 CHECK_EQ(n_warm_up_.cols(), dual_n_result_.cols());
1514 CHECK_EQ(n_warm_up_.rows(), dual_n_result_.rows());
1515 double n_diff_max = 0.0;
1516 for (int i = 0; i < horizon_ + 1; ++i) {
1517 for (int j = 0; j < 4 * obstacles_num_; ++j) {
1518 n_diff_max = std::max(std::abs(n_warm_up_(j, i) - dual_n_result_(j, i)), n_diff_max);
1519 }
1520 }
1521
1522 ADEBUG << "state_diff_max: " << state_diff_max;
1523 ADEBUG << "control_diff_max: " << control_diff_max;
1524 ADEBUG << "dual_l_diff_max: " << l_diff_max;
1525 ADEBUG << "dual_n_diff_max: " << n_diff_max;
1526}

◆ get_starting_point()

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

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