apollo::planning::PiecewiseJerkSpeedNonlinearIpoptInterface类 参考

#include <piecewise_jerk_speed_nonlinear_ipopt_interface.h>

类 apollo::planning::PiecewiseJerkSpeedNonlinearIpoptInterface 继承关系图:

apollo::planning::PiecewiseJerkSpeedNonlinearIpoptInterface 的协作图:

Public 成员函数
	PiecewiseJerkSpeedNonlinearIpoptInterface (const double s_init, const double s_dot_init, const double s_ddot_init, const double delta_t, const int num_of_points, const double s_max_, const double s_dot_max, const double s_ddot_min, const double s_ddot_max, const double s_dddot_min, const double s_dddot_max)
virtual	~PiecewiseJerkSpeedNonlinearIpoptInterface ()=default
void	set_warm_start (const std::vector< std::vector< double > > &speed_profile)
void	set_curvature_curve (const PiecewiseJerkTrajectory1d &curvature_curve)
void	get_optimization_results (std::vector< double > *ptr_opt_s, std::vector< double > *ptr_opt_v, std::vector< double > *ptr_opt_a)
void	set_end_state_target (const double s_target, const double v_target, const double a_target)
void	set_reference_speed (const double s_dot_ref)
void	set_reference_spatial_distance (const std::vector< double > &s_ref)
void	set_speed_limit_curve (const PiecewiseJerkTrajectory1d &v_bound_f)
void	set_safety_bounds (const std::vector< std::pair< double, double > > &safety_bounds)
void	set_soft_safety_bounds (const std::vector< std::pair< double, double > > &soft_safety_bounds)
void	set_w_target_state (const double w_target_s, const double w_target_v, const double w_target_a)
void	set_w_overall_a (const double w_overall_a)
void	set_w_overall_j (const double w_overall_j)
void	set_w_overall_centripetal_acc (const double w_overall_centripetal_acc)
void	set_w_reference_speed (const double w_reference_speed)
void	set_w_reference_spatial_distance (const double w_ref_s)
void	set_w_soft_s_bound (const double w_soft_s_bound)
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	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_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)

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

详细描述

在文件 piecewise_jerk_speed_nonlinear_ipopt_interface.h 第 36 行定义.

构造及析构函数说明

PiecewiseJerkSpeedNonlinearIpoptInterface()

apollo::planning::PiecewiseJerkSpeedNonlinearIpoptInterface::PiecewiseJerkSpeedNonlinearIpoptInterface	(	const double	s_init,
		const double	s_dot_init,
		const double	s_ddot_init,
		const double	delta_t,
		const int	num_of_points,
		const double	s_max_,
		const double	s_dot_max,
		const double	s_ddot_min,
		const double	s_ddot_max,
		const double	s_dddot_min,
		const double	s_dddot_max
	)

在文件 piecewise_jerk_speed_nonlinear_ipopt_interface.cc 第 29 行定义.

    : curvature_curve_(0.0, 0.0, 0.0),
      v_bound_func_(0.0, 0.0, 0.0),
      s_init_(s_init),
      s_dot_init_(s_dot_init),
      s_ddot_init_(s_ddot_init),
      delta_t_(delta_t),
      num_of_points_(num_of_points),
      s_max_(s_max),
      s_dot_max_(s_dot_max),
      s_ddot_min_(-std::abs(s_ddot_min)),
      s_ddot_max_(s_ddot_max),
      s_dddot_min_(-std::abs(s_dddot_min)),
      s_dddot_max_(s_dddot_max),
      v_offset_(num_of_points),
      a_offset_(num_of_points * 2) {}

~PiecewiseJerkSpeedNonlinearIpoptInterface()

virtual apollo::planning::PiecewiseJerkSpeedNonlinearIpoptInterface::~PiecewiseJerkSpeedNonlinearIpoptInterface ( )

virtualdefault

成员函数说明

eval_f()

bool apollo::planning::PiecewiseJerkSpeedNonlinearIpoptInterface::eval_f ( int  n, const double *  x, bool  new_x, double &  obj_value  )

override

Method to return the objective value

在文件 piecewise_jerk_speed_nonlinear_ipopt_interface.cc 第 291 行定义.

                                                                          {
  obj_value = 0.0;
  // difference between ref spatial distace
  for (int i = 0; i < num_of_points_; ++i) {
    double s_diff = x[i] - s_ref_[i];
    obj_value += s_diff * s_diff * w_ref_s_;
  }
 
  // difference between ref speed
  for (int i = 0; i < num_of_points_; ++i) {
    double v_diff = x[v_offset_ + i] - v_ref_;
    obj_value += v_diff * v_diff * w_ref_v_;
  }
 
  // acceleration obj.
  for (int i = 0; i < num_of_points_; ++i) {
    double a = x[a_offset_ + i];
    obj_value += a * a * w_overall_a_;
  }
 
  // jerk obj.
  for (int i = 0; i + 1 < num_of_points_; ++i) {
    double j = (x[a_offset_ + i + 1] - x[a_offset_ + i]) / delta_t_;
    obj_value += j * j * w_overall_j_;
  }
 
  // centripetal acceleration obj.
  for (int i = 0; i < num_of_points_; ++i) {
    double v = x[v_offset_ + i];
    double s = x[i];
    double kappa = curvature_curve_.Evaluate(0, s);
    double a_lat = v * v * kappa;
    obj_value += a_lat * a_lat * w_overall_centripetal_acc_;
  }
 
  if (has_end_state_target_) {
    double s_diff = x[num_of_points_ - 1] - s_target_;
    obj_value += s_diff * s_diff * w_target_s_;
 
    double v_diff = x[v_offset_ + num_of_points_ - 1] - v_target_;
    obj_value += v_diff * v_diff * w_target_v_;
 
    double a_diff = x[a_offset_ + num_of_points_ - 1] - a_target_;
    obj_value += a_diff * a_diff * w_target_a_;
  }
 
  if (use_soft_safety_bound_) {
    for (int i = 0; i < num_of_points_; ++i) {
      obj_value += x[lower_s_slack_offset_ + i] * w_soft_s_bound_;
      obj_value += x[upper_s_slack_offset_ + i] * w_soft_s_bound_;
    }
  }
 
  return true;
}

eval_g()

bool apollo::planning::PiecewiseJerkSpeedNonlinearIpoptInterface::eval_g ( int  n, const double *  x, bool  new_x, int  m, double *  g  )

override

Method to return the constraint residuals

在文件 piecewise_jerk_speed_nonlinear_ipopt_interface.cc 第 420 行定义.

                                                                  {
  int offset = 0;
 
  // s monotone constraints s_i+1 - s_i
  for (int i = 0; i + 1 < num_of_points_; ++i) {
    g[i] = x[offset + i + 1] - x[i];
  }
 
  offset += num_of_points_ - 1;
 
  // jerk bound constraint, |s_ddot_i+1 - s_ddot_i| <= s_dddot_max
  // position equality constraints,
  // s_i+1 - s_i - s_dot_i * delta_t - 1/3 * s_ddot_i * delta_t^2 - 1/6 *
  // s_ddot_i+1 * delta_t^2
  // velocity equality constraints
  // s_dot_i+1 - s_dot_i - 0.5 * delta_t * (s_ddot_i + s_ddot_i+1)
  int coffset_jerk = offset;
  int coffset_position = coffset_jerk + num_of_points_ - 1;
  int coffset_velocity = coffset_position + num_of_points_ - 1;
 
  double t = delta_t_;
  double t2 = t * t;
  double t3 = t2 * t;
 
  for (int i = 0; i + 1 < num_of_points_; ++i) {
    double s0 = x[i];
    double s1 = x[i + 1];
 
    double v0 = x[v_offset_ + i];
    double v1 = x[v_offset_ + i + 1];
 
    double a0 = x[a_offset_ + i];
    double a1 = x[a_offset_ + i + 1];
 
    double j = (a1 - a0) / t;
 
    g[coffset_jerk + i] = j;
    g[coffset_position + i] =
        s1 - (s0 + v0 * t + 0.5 * a0 * t2 + 1.0 / 6.0 * j * t3);
    g[coffset_velocity + i] = v1 - (v0 + a0 * t + 0.5 * j * t2);
  }
 
  offset += 3 * (num_of_points_ - 1);
 
  if (use_v_bound_) {
    // speed limit constraints
    int coffset_speed_limit = offset;
    // s_dot_i - v_bound_func_(s_i) <= 0.0
    for (int i = 0; i < num_of_points_; ++i) {
      double s = x[i];
      double s_dot = x[v_offset_ + i];
      g[coffset_speed_limit + i] = s_dot - v_bound_func_.Evaluate(0, s);
    }
 
    offset += num_of_points_;
  }
 
  if (use_soft_safety_bound_) {
    // soft safety boundary constraints
    int coffset_soft_lower_s = offset;
    // s_i - soft_lower_s_i + lower_slack_i >= 0.0
    for (int i = 0; i < num_of_points_; ++i) {
      double s = x[i];
      double lower_s_slack = x[lower_s_slack_offset_ + i];
      g[coffset_soft_lower_s + i] =
          s - soft_safety_bounds_[i].first + lower_s_slack;
    }
    offset += num_of_points_;
 
    int coffset_soft_upper_s = offset;
    // s_i - soft_upper_s_i - upper_slack_i <= 0.0
    for (int i = 0; i < num_of_points_; ++i) {
      double s = x[i];
      double upper_s_slack = x[upper_s_slack_offset_ + i];
      g[coffset_soft_upper_s + i] =
          s - soft_safety_bounds_[i].second - upper_s_slack;
    }
 
    offset += num_of_points_;
  }
 
  return true;
}

eval_grad_f()

bool apollo::planning::PiecewiseJerkSpeedNonlinearIpoptInterface::eval_grad_f ( int  n, const double *  x, bool  new_x, double *  grad_f  )

override

Method to return the gradient of the objective

在文件 piecewise_jerk_speed_nonlinear_ipopt_interface.cc 第 349 行定义.

                                                                            {
  std::fill(grad_f, grad_f + n, 0.0);
 
  // ref. spatial distance objective
  for (int i = 0; i < num_of_points_; ++i) {
    auto s_diff = x[i] - s_ref_[i];
    grad_f[i] += 2.0 * s_diff * w_ref_s_;
  }
 
  // ref. speed objective
  for (int i = 0; i < num_of_points_; ++i) {
    auto v_diff = x[v_offset_ + i] - v_ref_;
    grad_f[v_offset_ + i] += 2.0 * v_diff * w_ref_v_;
  }
 
  // jerk objective
  double c = 2.0 / (delta_t_ * delta_t_) * w_overall_j_;
  grad_f[a_offset_] += -c * (x[a_offset_ + 1] - x[a_offset_]);
  for (int i = 1; i + 1 < num_of_points_; ++i) {
    grad_f[a_offset_ + i] += c * (2.0 * x[a_offset_ + i] -
                                  x[a_offset_ + i + 1] - x[a_offset_ + i - 1]);
  }
  grad_f[a_offset_ + num_of_points_ - 1] +=
      c *
      (x[a_offset_ + num_of_points_ - 1] - x[a_offset_ + num_of_points_ - 2]);
 
  // acceleration objective
  for (int i = 0; i < num_of_points_; ++i) {
    grad_f[a_offset_ + i] += 2.0 * x[a_offset_ + i] * w_overall_a_;
  }
 
  // centripetal acceleration objective
  for (int i = 0; i < num_of_points_; ++i) {
    double v = x[v_offset_ + i];
    double v2 = v * v;
    double v3 = v2 * v;
    double v4 = v3 * v;
 
    double s = x[i];
    double kappa = curvature_curve_.Evaluate(0, s);
    double kappa_dot = curvature_curve_.Evaluate(1, s);
 
    grad_f[i] += 2.0 * w_overall_centripetal_acc_ * v4 * kappa * kappa_dot;
    grad_f[v_offset_ + i] +=
        4.0 * w_overall_centripetal_acc_ * v3 * kappa * kappa;
  }
 
  if (has_end_state_target_) {
    double s_diff = x[num_of_points_ - 1] - s_target_;
    grad_f[num_of_points_ - 1] += 2.0 * s_diff * w_target_s_;
 
    double v_diff = x[v_offset_ + num_of_points_ - 1] - v_target_;
    grad_f[v_offset_ + num_of_points_ - 1] += 2.0 * v_diff * w_target_v_;
 
    double a_diff = x[a_offset_ + num_of_points_ - 1] - a_target_;
    grad_f[a_offset_ + num_of_points_ - 1] += 2.0 * a_diff * w_target_a_;
  }
 
  if (use_soft_safety_bound_) {
    for (int i = 0; i < num_of_points_; ++i) {
      grad_f[lower_s_slack_offset_ + i] += w_soft_s_bound_;
      grad_f[upper_s_slack_offset_ + i] += w_soft_s_bound_;
    }
  }
 
  return true;
}

eval_h()

bool apollo::planning::PiecewiseJerkSpeedNonlinearIpoptInterface::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)

在文件 piecewise_jerk_speed_nonlinear_ipopt_interface.cc 第 758 行定义.

                    {
  if (values == nullptr) {
    int nz_index = 0;
    for (int i = 0; i < num_of_points_; ++i) {
      iRow[nz_index] = i;
      jCol[nz_index] = i;
      ++nz_index;
    }
 
    for (int i = 0; i < num_of_points_; ++i) {
      iRow[nz_index] = i;
      jCol[nz_index] = v_offset_ + i;
      ++nz_index;
    }
 
    for (int i = 0; i < num_of_points_; ++i) {
      iRow[nz_index] = v_offset_ + i;
      jCol[nz_index] = v_offset_ + i;
      ++nz_index;
    }
 
    for (int i = 0; i < num_of_points_; ++i) {
      iRow[nz_index] = a_offset_ + i;
      jCol[nz_index] = a_offset_ + i;
      ++nz_index;
    }
 
    for (int i = 0; i + 1 < num_of_points_; ++i) {
      iRow[nz_index] = a_offset_ + i;
      jCol[nz_index] = a_offset_ + i + 1;
      ++nz_index;
    }
 
    for (int i = 0; i < nz_index; ++i) {
      int r = iRow[i];
      int c = jCol[i];
      hessian_mapper_[to_hash_key(r, c)] = i;
    }
 
  } else {
    std::fill(values, values + nele_hess, 0.0);
 
    // speed by curvature objective
    for (int i = 0; i < num_of_points_; ++i) {
      auto s_index = i;
      auto v_index = v_offset_ + i;
 
      auto s = x[s_index];
      auto v = x[v_index];
 
      auto kappa = curvature_curve_.Evaluate(0, s);
      auto kappa_dot = curvature_curve_.Evaluate(1, s);
      auto kappa_ddot = curvature_curve_.Evaluate(2, s);
 
      auto v2 = v * v;
      auto v3 = v2 * v;
      auto v4 = v3 * v;
 
      auto h_s_s_obj =
          2.0 * kappa_dot * kappa_dot * v4 + 2.0 * kappa * kappa_ddot * v4;
      auto h_s_s_index = hessian_mapper_[to_hash_key(s_index, s_index)];
      values[h_s_s_index] +=
          h_s_s_obj * w_overall_centripetal_acc_ * obj_factor;
 
      auto h_s_v_obj = 8.0 * kappa * kappa_dot * v3;
      auto h_s_v_index = hessian_mapper_[to_hash_key(s_index, v_index)];
      values[h_s_v_index] +=
          h_s_v_obj * w_overall_centripetal_acc_ * obj_factor;
 
      auto h_v_v_obj = 12.0 * kappa * kappa * v2;
      auto h_v_v_index = hessian_mapper_[to_hash_key(v_index, v_index)];
      values[h_v_v_index] +=
          h_v_v_obj * w_overall_centripetal_acc_ * obj_factor;
    }
 
    // spatial distance reference objective
    for (int i = 0; i < num_of_points_; ++i) {
      auto h_s_s_index = hessian_mapper_[to_hash_key(i, i)];
      values[h_s_s_index] += 2.0 * w_ref_s_ * obj_factor;
    }
 
    // speed limit constraint
    if (use_v_bound_) {
      int lambda_offset = 4 * (num_of_points_ - 1);
      for (int i = 0; i < num_of_points_; ++i) {
        auto s_index = i;
 
        auto s = x[s_index];
 
        auto v_bound_ddot = v_bound_func_.Evaluate(2, s);
 
        auto h_s_s_constr = -1.0 * v_bound_ddot;
        auto h_s_s_index = hessian_mapper_[to_hash_key(s_index, s_index)];
        values[h_s_s_index] += h_s_s_constr * lambda[lambda_offset + i];
      }
    }
 
    for (int i = 0; i < num_of_points_; ++i) {
      auto a_index = a_offset_ + i;
      auto h_index = hessian_mapper_[to_hash_key(a_index, a_index)];
      values[h_index] += 2.0 * w_overall_a_ * obj_factor;
    }
 
    auto c = 2.0 / delta_t_ / delta_t_ * w_overall_j_ * obj_factor;
    for (int i = 0; i + 1 < num_of_points_; ++i) {
      auto a0_index = a_offset_ + i;
      auto a1_index = a_offset_ + i + 1;
 
      auto h_a0_a0_index = hessian_mapper_[to_hash_key(a0_index, a0_index)];
      values[h_a0_a0_index] += c;
 
      auto h_a0_a1_index = hessian_mapper_[to_hash_key(a0_index, a1_index)];
      values[h_a0_a1_index] += -c;
 
      auto h_a1_a1_index = hessian_mapper_[to_hash_key(a1_index, a1_index)];
      values[h_a1_a1_index] += c;
    }
 
    for (int i = 0; i < num_of_points_; ++i) {
      auto v_index = i + v_offset_;
      auto key = to_hash_key(v_index, v_index);
      auto index = hessian_mapper_[key];
      values[index] += 2.0 * w_ref_v_ * obj_factor;
    }
 
    if (has_end_state_target_) {
      // target s
      auto s_end_index = num_of_points_ - 1;
      auto s_key = to_hash_key(s_end_index, s_end_index);
 
      auto s_index = hessian_mapper_[s_key];
 
      values[s_index] += 2.0 * w_target_s_ * obj_factor;
 
      // target v
      auto v_end_index = 2 * num_of_points_ - 1;
      auto v_key = to_hash_key(v_end_index, v_end_index);
 
      auto v_index = hessian_mapper_[v_key];
 
      values[v_index] += 2.0 * w_target_v_ * obj_factor;
 
      // target a
      auto a_end_index = 3 * num_of_points_ - 1;
      auto a_key = to_hash_key(a_end_index, a_end_index);
 
      auto a_index = hessian_mapper_[a_key];
 
      values[a_index] += 2.0 * w_target_a_ * obj_factor;
    }
  }
  return true;
}

eval_jac_g()

bool apollo::planning::PiecewiseJerkSpeedNonlinearIpoptInterface::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)

在文件 piecewise_jerk_speed_nonlinear_ipopt_interface.cc 第 506 行定义.

                               {
  if (values == nullptr) {
    int non_zero_index = 0;
    int constraint_index = 0;
 
    // s monotone constraints s_i+1 - s_i
    for (int i = 0; i + 1 < num_of_points_; ++i) {
      // s_i
      iRow[non_zero_index] = constraint_index;
      jCol[non_zero_index] = i;
      ++non_zero_index;
 
      // s_i+1
      iRow[non_zero_index] = constraint_index;
      jCol[non_zero_index] = i + 1;
      ++non_zero_index;
 
      ++constraint_index;
    }
 
    // jerk bound constraint, |s_ddot_i+1 - s_ddot_i| / delta_t <= s_dddot_max
    for (int i = 0; i + 1 < num_of_points_; ++i) {
      // a_i
      iRow[non_zero_index] = constraint_index;
      jCol[non_zero_index] = a_offset_ + i;
      ++non_zero_index;
 
      // a_i+1
      iRow[non_zero_index] = constraint_index;
      jCol[non_zero_index] = a_offset_ + i + 1;
      ++non_zero_index;
 
      ++constraint_index;
    }
 
    // position equality constraints
    for (int i = 0; i + 1 < num_of_points_; ++i) {
      // s_i
      iRow[non_zero_index] = constraint_index;
      jCol[non_zero_index] = i;
      ++non_zero_index;
 
      // v_i
      iRow[non_zero_index] = constraint_index;
      jCol[non_zero_index] = v_offset_ + i;
      ++non_zero_index;
 
      // a_i
      iRow[non_zero_index] = constraint_index;
      jCol[non_zero_index] = a_offset_ + i;
      ++non_zero_index;
 
      // s_i+1
      iRow[non_zero_index] = constraint_index;
      jCol[non_zero_index] = i + 1;
      ++non_zero_index;
 
      // a_i+1
      iRow[non_zero_index] = constraint_index;
      jCol[non_zero_index] = a_offset_ + i + 1;
      ++non_zero_index;
 
      ++constraint_index;
    }
 
    // velocity equality constraints
    for (int i = 0; i + 1 < num_of_points_; ++i) {
      // v_i
      iRow[non_zero_index] = constraint_index;
      jCol[non_zero_index] = v_offset_ + i;
      ++non_zero_index;
 
      // a_i
      iRow[non_zero_index] = constraint_index;
      jCol[non_zero_index] = a_offset_ + i;
      ++non_zero_index;
 
      // v_i+1
      iRow[non_zero_index] = constraint_index;
      jCol[non_zero_index] = v_offset_ + i + 1;
      ++non_zero_index;
 
      // a_i+1
      iRow[non_zero_index] = constraint_index;
      jCol[non_zero_index] = a_offset_ + i + 1;
      ++non_zero_index;
 
      ++constraint_index;
    }
 
    if (use_v_bound_) {
      // speed limit constraints
      // s_dot_i - v_bound_func_(s_i) <= 0.0
      for (int i = 0; i < num_of_points_; ++i) {
        // s_i
        iRow[non_zero_index] = constraint_index;
        jCol[non_zero_index] = i;
        ++non_zero_index;
 
        // v_i
        iRow[non_zero_index] = constraint_index;
        jCol[non_zero_index] = v_offset_ + i;
        ++non_zero_index;
 
        ++constraint_index;
      }
    }
 
    if (use_soft_safety_bound_) {
      // soft_s_bound constraints
      // s_i - soft_lower_s_i + lower_slack_i >= 0.0
      for (int i = 0; i < num_of_points_; ++i) {
        // s_i
        iRow[non_zero_index] = constraint_index;
        jCol[non_zero_index] = i;
        ++non_zero_index;
 
        // lower_slack_i
        iRow[non_zero_index] = constraint_index;
        jCol[non_zero_index] = lower_s_slack_offset_ + i;
        ++non_zero_index;
 
        ++constraint_index;
      }
      // s_i - soft_upper_s_i - upper_slack_i <= 0.0
 
      for (int i = 0; i < num_of_points_; ++i) {
        // s_i
        iRow[non_zero_index] = constraint_index;
        jCol[non_zero_index] = i;
        ++non_zero_index;
 
        // upper_slack_i
        iRow[non_zero_index] = constraint_index;
        jCol[non_zero_index] = upper_s_slack_offset_ + i;
        ++non_zero_index;
 
        ++constraint_index;
      }
    }
 
  } else {
    int non_zero_index = 0;
    // s monotone constraints s_i+1 - s_i
    for (int i = 0; i + 1 < num_of_points_; ++i) {
      // s_i
      values[non_zero_index] = -1.0;
      ++non_zero_index;
 
      // s_i+1
      values[non_zero_index] = 1.0;
      ++non_zero_index;
    }
 
    // jerk bound constraint, |s_ddot_i+1 - s_ddot_i| / delta_t <= s_dddot_max
    for (int i = 0; i + 1 < num_of_points_; ++i) {
      // a_i
      values[non_zero_index] = -1.0 / delta_t_;
      ++non_zero_index;
 
      // a_i+1
      values[non_zero_index] = 1.0 / delta_t_;
      ++non_zero_index;
    }
 
    // position equality constraints
    for (int i = 0; i + 1 < num_of_points_; ++i) {
      // s_i
      values[non_zero_index] = -1.0;
      ++non_zero_index;
 
      // v_i
      values[non_zero_index] = -delta_t_;
      ++non_zero_index;
 
      // a_i
      values[non_zero_index] = -1.0 / 3.0 * delta_t_ * delta_t_;
      ++non_zero_index;
 
      // s_i+1
      values[non_zero_index] = 1.0;
      ++non_zero_index;
 
      // a_i+1
      values[non_zero_index] = -1.0 / 6.0 * delta_t_ * delta_t_;
      ++non_zero_index;
    }
 
    // velocity equality constraints
    for (int i = 0; i + 1 < num_of_points_; ++i) {
      // v_i
      values[non_zero_index] = -1.0;
      ++non_zero_index;
 
      // a_i
      values[non_zero_index] = -0.5 * delta_t_;
      ++non_zero_index;
 
      // v_i+1
      values[non_zero_index] = 1.0;
      ++non_zero_index;
 
      // a_i+1
      values[non_zero_index] = -0.5 * delta_t_;
      ++non_zero_index;
    }
 
    if (use_v_bound_) {
      // speed limit constraints
      // s_dot_i - v_bound_func_(s_i) <= 0.0
      for (int i = 0; i < num_of_points_; ++i) {
        // s_i
        double s = x[i];
        values[non_zero_index] = -1.0 * v_bound_func_.Evaluate(1, s);
        ++non_zero_index;
 
        // v_i
        values[non_zero_index] = 1.0;
        ++non_zero_index;
      }
    }
 
    if (use_soft_safety_bound_) {
      // soft_s_bound constraints
      // s_i - soft_lower_s_i + lower_slack_i >= 0.0
      for (int i = 0; i < num_of_points_; ++i) {
        // s_i
        values[non_zero_index] = 1.0;
        ++non_zero_index;
 
        // lower_slack_i
        values[non_zero_index] = 1.0;
        ++non_zero_index;
      }
 
      // s_i - soft_upper_s_i - upper_slack_i <= 0.0
      for (int i = 0; i < num_of_points_; ++i) {
        // s_i
        values[non_zero_index] = 1.0;
        ++non_zero_index;
 
        // upper_slack_i
        values[non_zero_index] = -1.0;
        ++non_zero_index;
      }
    }
  }
  return true;
}

finalize_solution()

void apollo::planning::PiecewiseJerkSpeedNonlinearIpoptInterface::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

在文件 piecewise_jerk_speed_nonlinear_ipopt_interface.cc 第 923 行定义.

                                           {
  opt_s_.clear();
  opt_v_.clear();
  opt_a_.clear();
 
  for (int i = 0; i < num_of_points_; ++i) {
    double s = x[i];
    double v = x[v_offset_ + i];
    double a = x[a_offset_ + i];
 
    opt_s_.push_back(s);
    opt_v_.push_back(v);
    opt_a_.push_back(a);
  }
 
  if (use_soft_safety_bound_) {
    // statistic analysis on soft bound intrusion by inspecting slack variable
    double lower_s_mean_intrusion = 0.0;
    double lower_s_highest_intrusion = -std::numeric_limits<double>::infinity();
    int lower_s_highest_intrustion_index = 0.0;
    double upper_s_mean_intrusion = 0.0;
    double upper_s_highest_intrusion = -std::numeric_limits<double>::infinity();
    int upper_s_highest_intrustion_index = 0.0;
 
    for (int i = 0; i < num_of_points_; ++i) {
      double lower_s_slack = x[lower_s_slack_offset_ + i];
      double upper_s_slack = x[upper_s_slack_offset_ + i];
 
      lower_s_mean_intrusion += lower_s_slack;
      upper_s_mean_intrusion += upper_s_slack;
 
      if (lower_s_highest_intrusion < lower_s_slack) {
        lower_s_highest_intrusion = lower_s_slack;
        lower_s_highest_intrustion_index = i;
      }
 
      if (upper_s_highest_intrusion < upper_s_slack) {
        upper_s_highest_intrusion = upper_s_slack;
        upper_s_highest_intrustion_index = i;
      }
    }
 
    lower_s_mean_intrusion /= static_cast<double>(num_of_points_);
    upper_s_mean_intrusion /= static_cast<double>(num_of_points_);
 
    ADEBUG << "lower soft s boundary average intrustion is ["
           << lower_s_mean_intrusion << "] with highest value of ["
           << lower_s_highest_intrusion << "] at time ["
           << delta_t_ * static_cast<double>(lower_s_highest_intrustion_index)
           << "].";
    ADEBUG << "upper soft s boundary average intrustion is ["
           << upper_s_mean_intrusion << "] with highest value of ["
           << upper_s_highest_intrusion << "] at time ["
           << delta_t_ * static_cast<double>(upper_s_highest_intrustion_index)
           << "].";
  }
}

get_bounds_info()

bool apollo::planning::PiecewiseJerkSpeedNonlinearIpoptInterface::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

在文件 piecewise_jerk_speed_nonlinear_ipopt_interface.cc 第 137 行定义.

                                                                      {
  // default nlp_lower_bound_inf value in Ipopt
  double INF = 1.0e19;
  double LARGE_VELOCITY_VALUE = s_dot_max_;
 
  // bounds for variables
  // s
  x_l[0] = s_init_;
  x_u[0] = s_init_;
  for (int i = 1; i < num_of_points_; ++i) {
    x_l[i] = safety_bounds_[i].first;
    x_u[i] = safety_bounds_[i].second;
  }
 
  // s_dot
  x_l[v_offset_] = s_dot_init_;
  x_u[v_offset_] = s_dot_init_;
  for (int i = 1; i < num_of_points_; ++i) {
    x_l[v_offset_ + i] = 0.0;
    x_u[v_offset_ + i] = LARGE_VELOCITY_VALUE;
  }
 
  // s_ddot
  x_l[a_offset_] = s_ddot_init_;
  x_u[a_offset_] = s_ddot_init_;
  for (int i = 1; i < num_of_points_; ++i) {
    x_l[a_offset_ + i] = s_ddot_min_;
    x_u[a_offset_ + i] = s_ddot_max_;
  }
 
  if (use_soft_safety_bound_) {
    // lower_s_slack
    for (int i = 0; i < num_of_points_; ++i) {
      x_l[lower_s_slack_offset_ + i] = 0.0;
      x_u[lower_s_slack_offset_ + i] = INF;
    }
 
    // upper_s_slack
    for (int i = 0; i < num_of_points_; ++i) {
      x_l[upper_s_slack_offset_ + i] = 0.0;
      x_u[upper_s_slack_offset_ + i] = INF;
    }
  }
 
  // bounds for constraints
  // s monotone constraints s_i+1 - s_i
  for (int i = 0; i + 1 < num_of_points_; ++i) {
    g_l[i] = 0.0;
    g_u[i] = LARGE_VELOCITY_VALUE * delta_t_;
  }
 
  // jerk bound constraint
  // |s_ddot_i+1 - s_ddot_i| / delta_t <= s_dddot_max
  int offset = num_of_points_ - 1;
  for (int i = 0; i + 1 < num_of_points_; ++i) {
    g_l[offset + i] = s_dddot_min_;
    g_u[offset + i] = s_dddot_max_;
  }
 
  // position equality constraints,
  // s_i+1 - s_i - s_dot_i * delta_t - 1/3 * s_ddot_i * delta_t^2 - 1/6 *
  // s_ddot_i+1 * delta_t^2
  offset += num_of_points_ - 1;
  for (int i = 0; i + 1 < num_of_points_; ++i) {
    g_l[offset + i] = 0.0;
    g_u[offset + i] = 0.0;
  }
 
  // velocity equality constraints,
  // s_dot_i+1 - s_dot_i - 0.5 * delta_t * s_ddot_i - 0.5 * delta_t *
  // s_ddot_i+1
  offset += num_of_points_ - 1;
  for (int i = 0; i + 1 < num_of_points_; ++i) {
    g_l[offset + i] = 0.0;
    g_u[offset + i] = 0.0;
  }
 
  if (use_v_bound_) {
    // speed limit constraints
    // s_dot_i - v_bound_func_(s_i) <= 0.0
    offset += num_of_points_ - 1;
    for (int i = 0; i < num_of_points_; ++i) {
      g_l[offset + i] = -INF;
      g_u[offset + i] = 0.0;
    }
  }
 
  if (use_soft_safety_bound_) {
    // soft_s_bound constraints
    // s_i - soft_lower_s_i + lower_slack_i >= 0.0
    offset += num_of_points_;
    for (int i = 0; i < num_of_points_; ++i) {
      g_l[offset + i] = 0.0;
      g_u[offset + i] = INF;
    }
 
    // s_i - soft_upper_s_i - upper_slack_i <= 0.0
    offset += num_of_points_;
    for (int i = 0; i < num_of_points_; ++i) {
      g_l[offset + i] = -INF;
      g_u[offset + i] = 0.0;
    }
  }
 
  return true;
}

get_nlp_info()

bool apollo::planning::PiecewiseJerkSpeedNonlinearIpoptInterface::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

在文件 piecewise_jerk_speed_nonlinear_ipopt_interface.cc 第 52 行定义.

                                 {
  num_of_variables_ = num_of_points_ * 3;
 
  if (use_soft_safety_bound_) {
    // complementary slack variable for soft upper and lower s bound
    num_of_variables_ += num_of_points_ * 2;
 
    lower_s_slack_offset_ = num_of_points_ * 3;
 
    upper_s_slack_offset_ = num_of_points_ * 4;
  }
 
  n = num_of_variables_;
 
  // s monotone constraints s_i+1 - s_i >= 0.0
  num_of_constraints_ = num_of_points_ - 1;
 
  // jerk bound constraint
  // |s_ddot_i+1 - s_ddot_i| / delta_t <= s_dddot_max
  num_of_constraints_ += num_of_points_ - 1;
 
  // position equality constraints
  // s_i+1 - s_i - s_dot_i * delta_t - 1/3 * s_ddot_i * delta_t^2 - 1/6 *
  // s_ddot_i+1 * delta_t^2
  num_of_constraints_ += num_of_points_ - 1;
 
  // velocity equality constraints
  // s_dot_i+1 - s_dot_i - 0.5 * delta_t * s_ddot_i - 0.5 * delta_t * s_ddot_i+1
  num_of_constraints_ += num_of_points_ - 1;
 
  if (use_v_bound_) {
    // speed limit constraints
    // s_dot_i - v_bound_func_(s_i) <= 0.0
    num_of_constraints_ += num_of_points_;
  }
 
  if (use_soft_safety_bound_) {
    // soft safety boundary constraints
    // s_i - soft_lower_s_i + lower_slack_i >= 0.0
    num_of_constraints_ += num_of_points_;
 
    // s_i - soft_upper_s_i - upper_slack_i <= 0.0
    num_of_constraints_ += num_of_points_;
  }
 
  m = num_of_constraints_;
 
  nnz_jac_g = 0;
  // s_i+1 - s_i
  nnz_jac_g += (num_of_points_ - 1) * 2;
 
  // (s_ddot_i+1 - s_ddot_i) / delta_t
  nnz_jac_g += (num_of_points_ - 1) * 2;
 
  // s_i+1 - s_i - s_dot_i * delta_t - 1/3 * s_ddot_i * delta_t^2 - 1/6 *
  // s_ddot_i+1 * delta_t^2
  nnz_jac_g += (num_of_points_ - 1) * 5;
 
  // s_dot_i+1 - s_dot_i - 0.5 * s_ddot_i * delta_t - 0.5 * s_ddot_i+1 * delta_t
  nnz_jac_g += (num_of_points_ - 1) * 4;
 
  if (use_v_bound_) {
    // speed limit constraints
    // s_dot_i - v_bound_func_(s_i) <= 0.0
    nnz_jac_g += num_of_points_ * 2;
  }
 
  if (use_soft_safety_bound_) {
    // soft safety boundary constraints
    // s_i - soft_lower_s_i + lower_slack_i >= 0.0
    nnz_jac_g += num_of_points_ * 2;
 
    // s_i - soft_upper_s_i - upper_slack_i <= 0.0
    nnz_jac_g += num_of_points_ * 2;
  }
 
  nnz_h_lag = num_of_points_ * 5 - 1;
 
  index_style = IndexStyleEnum::C_STYLE;
 
  return true;
}

get_optimization_results()

void apollo::planning::PiecewiseJerkSpeedNonlinearIpoptInterface::get_optimization_results	(	std::vector< double > *	ptr_opt_s,
		std::vector< double > *	ptr_opt_v,
		std::vector< double > *	ptr_opt_a
	)

在文件 piecewise_jerk_speed_nonlinear_ipopt_interface.cc 第 915 行定义.

                                  {
  *ptr_opt_s = opt_s_;
  *ptr_opt_v = opt_v_;
  *ptr_opt_a = opt_a_;
}

get_starting_point()

bool apollo::planning::PiecewiseJerkSpeedNonlinearIpoptInterface::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

在文件 piecewise_jerk_speed_nonlinear_ipopt_interface.cc 第 245 行定义.

                                      {
  if (!x_warm_start_.empty()) {
    for (int i = 0; i < num_of_points_; ++i) {
      x[i] = x_warm_start_[i][0];
      x[v_offset_ + i] = x_warm_start_[i][1];
      x[a_offset_ + i] = x_warm_start_[i][2];
    }
  }
 
  if (use_soft_safety_bound_) {
    for (int i = 0; i < num_of_points_; ++i) {
      x[lower_s_slack_offset_ + i] = 0.0;
      x[upper_s_slack_offset_ + i] = 0.0;
    }
  }
 
  return true;
 
  // TODO(Jinyun): Implement better default warm start based on safety_bounds
  for (int i = 0; i < num_of_points_; ++i) {
    x[i] = std::min(5.0 * delta_t_ * i + s_init_, s_max_);
  }
  x[0] = s_init_;
 
  for (int i = 0; i < num_of_points_; ++i) {
    x[v_offset_ + i] = 5.0;
  }
  x[v_offset_] = s_dot_init_;
 
  for (int i = 0; i < num_of_points_; ++i) {
    x[a_offset_ + i] = 0.0;
  }
  x[a_offset_] = s_ddot_init_;
 
  if (use_soft_safety_bound_) {
    for (int i = 0; i < num_of_points_; ++i) {
      x[lower_s_slack_offset_ + i] = 0.0;
      x[upper_s_slack_offset_ + i] = 0.0;
    }
  }
 
  return true;
}

set_curvature_curve()

void apollo::planning::PiecewiseJerkSpeedNonlinearIpoptInterface::set_curvature_curve

(

const PiecewiseJerkTrajectory1d &

curvature_curve

)

在文件 piecewise_jerk_speed_nonlinear_ipopt_interface.cc 第 985 行定义.

                                                      {
  curvature_curve_ = curvature_curve;
}

set_end_state_target()

void apollo::planning::PiecewiseJerkSpeedNonlinearIpoptInterface::set_end_state_target	(	const double	s_target,
		const double	v_target,
		const double	a_target
	)

在文件 piecewise_jerk_speed_nonlinear_ipopt_interface.cc 第 1017 行定义.

                                                                         {
  s_target_ = s_target;
  v_target_ = v_target;
  a_target_ = a_target;
  has_end_state_target_ = true;
}

set_reference_spatial_distance()

void apollo::planning::PiecewiseJerkSpeedNonlinearIpoptInterface::set_reference_spatial_distance

(

const std::vector< double > &

s_ref

)

在文件 piecewise_jerk_speed_nonlinear_ipopt_interface.cc 第 1067 行定义.

                                    {
  s_ref_ = s_ref;
}

set_reference_speed()

void apollo::planning::PiecewiseJerkSpeedNonlinearIpoptInterface::set_reference_speed

(

const double

s_dot_ref

)

在文件 piecewise_jerk_speed_nonlinear_ipopt_interface.cc 第 996 行定义.

                        {
  v_ref_ = v_ref;
}

set_safety_bounds()

void apollo::planning::PiecewiseJerkSpeedNonlinearIpoptInterface::set_safety_bounds

(

const std::vector< std::pair< double, double > > &

safety_bounds

)

在文件 piecewise_jerk_speed_nonlinear_ipopt_interface.cc 第 1001 行定义.

                                                             {
  safety_bounds_ = safety_bounds;
}

set_soft_safety_bounds()

void apollo::planning::PiecewiseJerkSpeedNonlinearIpoptInterface::set_soft_safety_bounds

(

const std::vector< std::pair< double, double > > &

soft_safety_bounds

)

在文件 piecewise_jerk_speed_nonlinear_ipopt_interface.cc 第 1006 行定义.

                                                                  {
  soft_safety_bounds_ = soft_safety_bounds;
  use_soft_safety_bound_ = true;
}

set_speed_limit_curve()

void apollo::planning::PiecewiseJerkSpeedNonlinearIpoptInterface::set_speed_limit_curve

(

const PiecewiseJerkTrajectory1d &

v_bound_f

)

在文件 piecewise_jerk_speed_nonlinear_ipopt_interface.cc 第 990 行定义.

                                                {
  v_bound_func_ = v_bound_f;
  use_v_bound_ = true;
}

set_w_overall_a()

void apollo::planning::PiecewiseJerkSpeedNonlinearIpoptInterface::set_w_overall_a

(

const double

w_overall_a

)

在文件 piecewise_jerk_speed_nonlinear_ipopt_interface.cc 第 1032 行定义.

                              {
  w_overall_a_ = w_overall_a;
}

set_w_overall_centripetal_acc()

void apollo::planning::PiecewiseJerkSpeedNonlinearIpoptInterface::set_w_overall_centripetal_acc

(

const double

w_overall_centripetal_acc

)

在文件 piecewise_jerk_speed_nonlinear_ipopt_interface.cc 第 1042 行定义.

                                            {
  w_overall_centripetal_acc_ = w_overall_centripetal_acc;
}

set_w_overall_j()

void apollo::planning::PiecewiseJerkSpeedNonlinearIpoptInterface::set_w_overall_j

(

const double

w_overall_j

)

在文件 piecewise_jerk_speed_nonlinear_ipopt_interface.cc 第 1037 行定义.

                              {
  w_overall_j_ = w_overall_j;
}

set_w_reference_spatial_distance()

void apollo::planning::PiecewiseJerkSpeedNonlinearIpoptInterface::set_w_reference_spatial_distance

(

const double

w_ref_s

)

在文件 piecewise_jerk_speed_nonlinear_ipopt_interface.cc 第 1052 行定义.

                                                           {
  w_ref_s_ = w_ref_s;
}

set_w_reference_speed()

void apollo::planning::PiecewiseJerkSpeedNonlinearIpoptInterface::set_w_reference_speed

(

const double

w_reference_speed

)

在文件 piecewise_jerk_speed_nonlinear_ipopt_interface.cc 第 1047 行定义.

                                    {
  w_ref_v_ = w_reference_speed;
}

set_w_soft_s_bound()

void apollo::planning::PiecewiseJerkSpeedNonlinearIpoptInterface::set_w_soft_s_bound

(

const double

w_soft_s_bound

)

在文件 piecewise_jerk_speed_nonlinear_ipopt_interface.cc 第 1057 行定义.

                                 {
  w_soft_s_bound_ = w_soft_s_bound;
}

set_w_target_state()

void apollo::planning::PiecewiseJerkSpeedNonlinearIpoptInterface::set_w_target_state	(	const double	w_target_s,
		const double	w_target_v,
		const double	w_target_a
	)

在文件 piecewise_jerk_speed_nonlinear_ipopt_interface.cc 第 1025 行定义.

                                                                               {
  w_target_s_ = w_target_s;
  w_target_v_ = w_target_v;
  w_target_a_ = w_target_a;
}

set_warm_start()

void apollo::planning::PiecewiseJerkSpeedNonlinearIpoptInterface::set_warm_start

(

const std::vector< std::vector< double > > &

speed_profile

)

在文件 piecewise_jerk_speed_nonlinear_ipopt_interface.cc 第 1062 行定义.

                                                       {
  x_warm_start_ = speed_profile;
}

---

该类的文档由以下文件生成:

• modules/planning/tasks/piecewise_jerk_speed_nonlinear/piecewise_jerk_speed_nonlinear_ipopt_interface.h
• modules/planning/tasks/piecewise_jerk_speed_nonlinear/piecewise_jerk_speed_nonlinear_ipopt_interface.cc