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

34 : ts_(ts),
35 ego_(ego),
36 xWS_(xWS),
37 uWS_(uWS),
38 l_warm_up_(l_warm_up),
39 n_warm_up_(n_warm_up),
40 x0_(x0),
41 xf_(xf),
42 last_time_u_(last_time_u),
43 XYbounds_(XYbounds),
44 obstacles_edges_num_(obstacles_edges_num),
45 obstacles_A_(obstacles_A),
46 obstacles_b_(obstacles_b) {
47 ACHECK(horizon < std::numeric_limits<int>::max())
48 << "Invalid cast on horizon in open space planner";
49 horizon_ = static_cast<int>(horizon);
50 ACHECK(obstacles_num < std::numeric_limits<int>::max())
51 << "Invalid cast on obstacles_num in open space planner";
52
53 obstacles_num_ = static_cast<int>(obstacles_num);
54 w_ev_ = ego_(1, 0) + ego_(3, 0);
55 l_ev_ = ego_(0, 0) + ego_(2, 0);
56 g_ = {l_ev_ / 2, w_ev_ / 2, l_ev_ / 2, w_ev_ / 2};
57 offset_ = (ego_(0, 0) + ego_(2, 0)) / 2 - ego_(2, 0);
58 obstacles_edges_sum_ = obstacles_edges_num_.sum();
59 state_result_ = Eigen::MatrixXd::Zero(4, horizon_ + 1);
60 dual_l_result_ = Eigen::MatrixXd::Zero(obstacles_edges_sum_, horizon_ + 1);
61 dual_n_result_ = Eigen::MatrixXd::Zero(4 * obstacles_num_, horizon_ + 1);
62 control_result_ = Eigen::MatrixXd::Zero(2, horizon_);
63 time_result_ = Eigen::MatrixXd::Zero(1, horizon_);
64 state_start_index_ = 0;
65 control_start_index_ = 4 * (horizon_ + 1);
66 time_start_index_ = control_start_index_ + 2 * horizon_;
67 l_start_index_ = time_start_index_ + (horizon_ + 1);
68 n_start_index_ = l_start_index_ + obstacles_edges_sum_ * (horizon_ + 1);
69
70 distance_approach_config_ =
71 planner_open_space_config.distance_approach_config();
72 weight_state_x_ = distance_approach_config_.weight_x();
73 weight_state_y_ = distance_approach_config_.weight_y();
74 weight_state_phi_ = distance_approach_config_.weight_phi();
75 weight_state_v_ = distance_approach_config_.weight_v();
76 weight_input_steer_ = distance_approach_config_.weight_steer();
77 weight_input_a_ = distance_approach_config_.weight_a();
78 weight_rate_steer_ = distance_approach_config_.weight_steer_rate();
79 weight_rate_a_ = distance_approach_config_.weight_a_rate();
80 weight_stitching_steer_ = distance_approach_config_.weight_steer_stitching();
81 weight_stitching_a_ = distance_approach_config_.weight_a_stitching();
82 weight_first_order_time_ =
83 distance_approach_config_.weight_first_order_time();
84 weight_second_order_time_ =
85 distance_approach_config_.weight_second_order_time();
86 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 enable_jacobian_ad_ = distance_approach_config_.enable_jacobian_ad();
106}
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 
)

成员函数说明

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

1808 {
1809 int kN = n;
1810 int kM = m;
1811 double x_u_tmp[kN];
1812 double x_l_tmp[kN];
1813 double g_u_tmp[kM];
1814 double g_l_tmp[kM];
1815
1816 get_bounds_info(n, x_l_tmp, x_u_tmp, m, g_l_tmp, g_u_tmp);
1817
1818 const double delta_v = 1e-4;
1819 for (int idx = 0; idx < n; ++idx) {
1820 x_u_tmp[idx] = x_u_tmp[idx] + delta_v;
1821 x_l_tmp[idx] = x_l_tmp[idx] - delta_v;
1822 if (x[idx] > x_u_tmp[idx] || x[idx] < x_l_tmp[idx]) {
1823 AINFO << "x idx unfeasible: " << idx << ", x: " << x[idx]
1824 << ", lower: " << x_l_tmp[idx] << ", upper: " << x_u_tmp[idx];
1825 }
1826 }
1827
1828 // m1 : dynamics constatins
1829 int m1 = 4 * horizon_;
1830
1831 // m2 : control rate constraints (only steering)
1832 int m2 = m1 + horizon_;
1833
1834 // m3 : sampling time equality constraints
1835 int m3 = m2 + horizon_;
1836
1837 // m4 : obstacle constraints
1838 int m4 = m3 + 4 * obstacles_num_ * (horizon_ + 1);
1839
1840 // 5. load variable bounds as constraints
1841 // start configuration
1842 int m5 = m4 + 3 + 1;
1843
1844 // constraints on x,y,v
1845 int m6 = m5 + 3 * (horizon_ - 1);
1846
1847 // end configuration
1848 int m7 = m6 + 3 + 1;
1849
1850 // control variable bnd
1851 int m8 = m7 + 2 * horizon_;
1852
1853 // time interval variable bnd
1854 int m9 = m8 + (horizon_ + 1);
1855
1856 // lambda_horizon_
1857 int m10 = m9 + lambda_horizon_;
1858
1859 // miu_horizon_
1860 int m11 = m10 + miu_horizon_;
1861
1862 CHECK_EQ(m11, num_of_constraints_);
1863
1864 AINFO << "dynamics constatins to: " << m1;
1865 AINFO << "control rate constraints (only steering) to: " << m2;
1866 AINFO << "sampling time equality constraints to: " << m3;
1867 AINFO << "obstacle constraints to: " << m4;
1868 AINFO << "start conf constraints to: " << m5;
1869 AINFO << "constraints on x,y,v to: " << m6;
1870 AINFO << "end constraints to: " << m7;
1871 AINFO << "control bnd to: " << m8;
1872 AINFO << "time interval constraints to: " << m9;
1873 AINFO << "lambda constraints to: " << m10;
1874 AINFO << "miu constraints to: " << m11;
1875 AINFO << "total constraints: " << num_of_constraints_;
1876
1877 for (int idx = 0; idx < m; ++idx) {
1878 if (g[idx] > g_u_tmp[idx] + delta_v || g[idx] < g_l_tmp[idx] - delta_v) {
1879 AINFO << "constrains idx unfeasible: " << idx << ", g: " << g[idx]
1880 << ", lower: " << g_l_tmp[idx] << ", upper: " << g_u_tmp[idx];
1881 }
1882 }
1883 return true;
1884}
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:167
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 第 1548 行定义.

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

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

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

447 {
448 eval_constraints(n, x, m, g);
449 // if (enable_constraint_check_) check_g(n, x, m, g);
450 return true;
451}
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:1548

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

441 {
442 gradient(tag_f, n, x, grad_f);
443 return true;
444}
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 第 1306 行定义.

1311 {
1312 if (values == nullptr) {
1313 // return the structure. This is a symmetric matrix, fill the lower left
1314 // triangle only.
1315
1316 for (int idx = 0; idx < nnz_L; idx++) {
1317 iRow[idx] = rind_L[idx];
1318 jCol[idx] = cind_L[idx];
1319 }
1320 } else {
1321 // return the values. This is a symmetric matrix, fill the lower left
1322 // triangle only
1323
1324 obj_lam[0] = obj_factor;
1325
1326 for (int idx = 0; idx < m; idx++) {
1327 obj_lam[1 + idx] = lambda[idx];
1328 }
1329
1330 set_param_vec(tag_L, m + 1, obj_lam);
1331 sparse_hess(tag_L, n, 1, const_cast<double*>(x), &nnz_L, &rind_L, &cind_L,
1332 &hessval, options_L);
1333
1334 for (int idx = 0; idx < nnz_L; idx++) {
1335 values[idx] = hessval[idx];
1336 }
1337 }
1338
1339 return true;
1340}
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 第 453 行定义.

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

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

1478 {
1479 // Objective is :
1480 // min control inputs
1481 // min input rate
1482 // min time (if the time step is not fixed)
1483 // regularization wrt warm start trajectory
1484 DCHECK(ts_ != 0) << "ts in distance_approach_ is 0";
1485 int control_index = control_start_index_;
1486 int time_index = time_start_index_;
1487 int state_index = state_start_index_;
1488
1489 // TODO(QiL): Initial implementation towards earlier understanding and debug
1490 // purpose, later code refine towards improving efficiency
1491
1492 *obj_value = 0.0;
1493 // 1. objective to minimize state diff to warm up
1494 for (int i = 0; i < horizon_ + 1; ++i) {
1495 T x1_diff = x[state_index] - xWS_(0, i);
1496 T x2_diff = x[state_index + 1] - xWS_(1, i);
1497 T x3_diff = x[state_index + 2] - xWS_(2, i);
1498 T x4_abs = x[state_index + 3];
1499 *obj_value += weight_state_x_ * x1_diff * x1_diff +
1500 weight_state_y_ * x2_diff * x2_diff +
1501 weight_state_phi_ * x3_diff * x3_diff +
1502 weight_state_v_ * x4_abs * x4_abs;
1503 state_index += 4;
1504 }
1505
1506 // 2. objective to minimize u square
1507 for (int i = 0; i < horizon_; ++i) {
1508 *obj_value += weight_input_steer_ * x[control_index] * x[control_index] +
1509 weight_input_a_ * x[control_index + 1] * x[control_index + 1];
1510 control_index += 2;
1511 }
1512
1513 // 3. objective to minimize input change rate for first horizon
1514 control_index = control_start_index_;
1515 T last_time_steer_rate =
1516 (x[control_index] - last_time_u_(0, 0)) / x[time_index] / ts_;
1517 T last_time_a_rate =
1518 (x[control_index + 1] - last_time_u_(1, 0)) / x[time_index] / ts_;
1519 *obj_value +=
1520 weight_stitching_steer_ * last_time_steer_rate * last_time_steer_rate +
1521 weight_stitching_a_ * last_time_a_rate * last_time_a_rate;
1522
1523 // 4. objective to minimize input change rates, [0- horizon_ -2]
1524 time_index++;
1525 for (int i = 0; i < horizon_ - 1; ++i) {
1526 T steering_rate =
1527 (x[control_index + 2] - x[control_index]) / x[time_index] / ts_;
1528 T a_rate =
1529 (x[control_index + 3] - x[control_index + 1]) / x[time_index] / ts_;
1530 *obj_value += weight_rate_steer_ * steering_rate * steering_rate +
1531 weight_rate_a_ * a_rate * a_rate;
1532 control_index += 2;
1533 time_index++;
1534 }
1535
1536 // 5. objective to minimize total time [0, horizon_]
1537 time_index = time_start_index_;
1538 for (int i = 0; i < horizon_ + 1; ++i) {
1539 T first_order_penalty = weight_first_order_time_ * x[time_index];
1540 T second_order_penalty =
1541 weight_second_order_time_ * x[time_index] * x[time_index];
1542 *obj_value += first_order_penalty + second_order_penalty;
1543 time_index++;
1544 }
1545}

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

1346 {
1347 int state_index = state_start_index_;
1348 int control_index = control_start_index_;
1349 int time_index = time_start_index_;
1350 int dual_l_index = l_start_index_;
1351 int dual_n_index = n_start_index_;
1352
1353 // enable_constraint_check_: for debug only
1354 if (enable_constraint_check_) {
1355 ADEBUG << "final resolution constraint checking";
1356 check_g(n, x, m, g);
1357 }
1358 // 1. state variables, 4 * [0, horizon]
1359 // 2. control variables, 2 * [0, horizon_-1]
1360 // 3. sampling time variables, 1 * [0, horizon_]
1361 // 4. dual_l obstacles_edges_sum_ * [0, horizon]
1362 // 5. dual_n obstacles_num * [0, horizon]
1363 for (int i = 0; i < horizon_; ++i) {
1364 state_result_(0, i) = x[state_index];
1365 state_result_(1, i) = x[state_index + 1];
1366 state_result_(2, i) = x[state_index + 2];
1367 state_result_(3, i) = x[state_index + 3];
1368 control_result_(0, i) = x[control_index];
1369 control_result_(1, i) = x[control_index + 1];
1370 time_result_(0, i) = ts_ * x[time_index];
1371 for (int j = 0; j < obstacles_edges_sum_; ++j) {
1372 dual_l_result_(j, i) = x[dual_l_index + j];
1373 }
1374 for (int k = 0; k < 4 * obstacles_num_; k++) {
1375 dual_n_result_(k, i) = x[dual_n_index + k];
1376 }
1377 state_index += 4;
1378 control_index += 2;
1379 time_index++;
1380 dual_l_index += obstacles_edges_sum_;
1381 dual_n_index += 4 * obstacles_num_;
1382 }
1383 state_result_(0, 0) = x0_(0, 0);
1384 state_result_(1, 0) = x0_(1, 0);
1385 state_result_(2, 0) = x0_(2, 0);
1386 state_result_(3, 0) = x0_(3, 0);
1387 // push back last horizon for states
1388 state_result_(0, horizon_) = xf_(0, 0);
1389 state_result_(1, horizon_) = xf_(1, 0);
1390 state_result_(2, horizon_) = xf_(2, 0);
1391 state_result_(3, horizon_) = xf_(3, 0);
1392
1393 for (int j = 0; j < obstacles_edges_sum_; ++j) {
1394 dual_l_result_(j, horizon_) = x[dual_l_index + j];
1395 }
1396 for (int k = 0; k < 4 * obstacles_num_; k++) {
1397 dual_n_result_(k, horizon_) = x[dual_n_index + k];
1398 }
1399 // memory deallocation of ADOL-C variables
1400 delete[] obj_lam;
1401 if (enable_jacobian_ad_) {
1402 free(rind_g);
1403 free(cind_g);
1404 free(jacval);
1405 }
1406 free(rind_L);
1407 free(cind_L);
1408 free(hessval);
1409}
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:1807

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

1888 {
1889 std::vector<double> xp(n);
1890 std::vector<double> lamp(m);
1891 std::vector<double> zl(m);
1892 std::vector<double> zu(m);
1893 std::vector<adouble> xa(n);
1894 std::vector<adouble> g(m);
1895 std::vector<double> lam(m);
1896
1897 double sig = 0.0;
1898 adouble obj_value;
1899 double dummy = 0.0;
1900 obj_lam = new double[m + 1];
1901 get_starting_point(n, 1, &xp[0], 0, &zl[0], &zu[0], m, 0, &lamp[0]);
1902
1903 trace_on(tag_f);
1904 for (int idx = 0; idx < n; idx++) {
1905 xa[idx] <<= xp[idx];
1906 }
1907 eval_obj(n, &xa[0], &obj_value);
1908 obj_value >>= dummy;
1909 trace_off();
1910
1911 trace_on(tag_g);
1912 for (int idx = 0; idx < n; idx++) {
1913 xa[idx] <<= xp[idx];
1914 }
1915 eval_constraints(n, &xa[0], m, &g[0]);
1916 for (int idx = 0; idx < m; idx++) {
1917 g[idx] >>= dummy;
1918 }
1919 trace_off();
1920
1921 trace_on(tag_L);
1922 for (int idx = 0; idx < n; idx++) {
1923 xa[idx] <<= xp[idx];
1924 }
1925 for (int idx = 0; idx < m; idx++) {
1926 lam[idx] = 1.0;
1927 }
1928 sig = 1.0;
1929 eval_obj(n, &xa[0], &obj_value);
1930 obj_value *= mkparam(sig);
1931 eval_constraints(n, &xa[0], m, &g[0]);
1932 for (int idx = 0; idx < m; idx++) {
1933 obj_value += g[idx] * mkparam(lam[idx]);
1934 }
1935 obj_value >>= dummy;
1936
1937 trace_off();
1938
1939 if (enable_jacobian_ad_) {
1940 rind_g = nullptr;
1941 cind_g = nullptr;
1942 jacval = nullptr;
1943 options_g[0] = 0; /* sparsity pattern by index domains (default) */
1944 options_g[1] = 0; /* safe mode (default) */
1945 options_g[2] = 0;
1946 options_g[3] = 0; /* column compression (default) */
1947
1948 sparse_jac(tag_g, m, n, 0, &xp[0], &nnz_jac, &rind_g, &cind_g, &jacval,
1949 options_g);
1950 *nnz_jac_g = nnz_jac;
1951 }
1952
1953 rind_L = nullptr;
1954 cind_L = nullptr;
1955 hessval = nullptr;
1956 options_L[0] = 0;
1957 options_L[1] = 1;
1958
1959 sparse_hess(tag_L, n, 0, &xp[0], &nnz_L, &rind_L, &cind_L, &hessval,
1960 options_L);
1961 *nnz_h_lag = nnz_L;
1962}
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:385

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

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

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

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

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

1414 {
1415 ADEBUG << "get_optimization_results";
1416 *state_result = state_result_;
1417 *control_result = control_result_;
1418 *time_result = time_result_;
1419 *dual_l_result = dual_l_result_;
1420 *dual_n_result = dual_n_result_;
1421
1422 if (!distance_approach_config_.enable_initial_final_check()) {
1423 return;
1424 }
1425 CHECK_EQ(state_result_.cols(), xWS_.cols());
1426 CHECK_EQ(state_result_.rows(), xWS_.rows());
1427 double state_diff_max = 0.0;
1428 for (int i = 0; i < horizon_ + 1; ++i) {
1429 for (int j = 0; j < 4; ++j) {
1430 state_diff_max =
1431 std::max(std::abs(xWS_(j, i) - state_result_(j, i)), state_diff_max);
1432 }
1433 }
1434
1435 // 2. control variable initialization, 2 * horizon_
1436 CHECK_EQ(control_result_.cols(), uWS_.cols());
1437 CHECK_EQ(control_result_.rows(), uWS_.rows());
1438 double control_diff_max = 0.0;
1439 for (int i = 0; i < horizon_; ++i) {
1440 control_diff_max = std::max(std::abs(uWS_(0, i) - control_result_(0, i)),
1441 control_diff_max);
1442 control_diff_max = std::max(std::abs(uWS_(1, i) - control_result_(1, i)),
1443 control_diff_max);
1444 }
1445
1446 // 2. time scale variable initialization, horizon_ + 1
1447
1448 // 3. lagrange constraint l, obstacles_edges_sum_ * (horizon_+1)
1449 CHECK_EQ(dual_l_result_.cols(), l_warm_up_.cols());
1450 CHECK_EQ(dual_l_result_.rows(), l_warm_up_.rows());
1451 double l_diff_max = 0.0;
1452 for (int i = 0; i < horizon_ + 1; ++i) {
1453 for (int j = 0; j < obstacles_edges_sum_; ++j) {
1454 l_diff_max = std::max(std::abs(l_warm_up_(j, i) - dual_l_result_(j, i)),
1455 l_diff_max);
1456 }
1457 }
1458
1459 // 4. lagrange constraint m, 4*obstacles_num * (horizon_+1)
1460 CHECK_EQ(n_warm_up_.cols(), dual_n_result_.cols());
1461 CHECK_EQ(n_warm_up_.rows(), dual_n_result_.rows());
1462 double n_diff_max = 0.0;
1463 for (int i = 0; i < horizon_ + 1; ++i) {
1464 for (int j = 0; j < 4 * obstacles_num_; ++j) {
1465 n_diff_max = std::max(std::abs(n_warm_up_(j, i) - dual_n_result_(j, i)),
1466 n_diff_max);
1467 }
1468 }
1469
1470 ADEBUG << "state_diff_max: " << state_diff_max;
1471 ADEBUG << "control_diff_max: " << control_diff_max;
1472 ADEBUG << "dual_l_diff_max: " << l_diff_max;
1473 ADEBUG << "dual_n_diff_max: " << n_diff_max;
1474}

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

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