#include <piecewise_jerk_speed_problem.h>

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

apollo::planning::PiecewiseJerkSpeedProblem 的协作图:

Public 成员函数
	PiecewiseJerkSpeedProblem (const size_t num_of_knots, const double delta_s, const std::array< double, 3 > &x_init)

virtual	~PiecewiseJerkSpeedProblem ()=default

void	set_dx_ref (const double weight_dx_ref, const double dx_ref)

void	set_dx_ref (const std::vector< double > &weight_dx_ref, const std::vector< double > &dx_ref)

void	set_penalty_dx (std::vector< double > penalty_dx)

Public 成员函数继承自 apollo::planning::PiecewiseJerkProblem
	PiecewiseJerkProblem (const size_t num_of_knots, const double delta_s, const std::array< double, 3 > &x_init)

virtual	~PiecewiseJerkProblem ()=default

void	set_x_bounds (std::vector< std::pair< double, double > > x_bounds)

void	set_x_bounds (const double x_lower_bound, const double x_upper_bound)

void	set_dx_bounds (std::vector< std::pair< double, double > > dx_bounds)

void	set_dx_bounds (const double dx_lower_bound, const double dx_upper_bound)

void	set_ddx_bounds (std::vector< std::pair< double, double > > ddx_bounds)

void	set_ddx_bounds (const double ddx_lower_bound, const double ddx_upper_bound)

void	set_dddx_bound (const double dddx_bound)

void	set_dddx_bound (const double dddx_lower_bound, const double dddx_upper_bound)

void	set_weight_x (const double weight_x)

void	set_weight_dx (const double weight_dx)

void	set_weight_ddx (const double weight_ddx)

void	set_weight_dddx (const double weight_dddx)

void	set_scale_factor (const std::array< double, 3 > &scale_factor)

void	set_x_ref (const double weight_x_ref, std::vector< double > x_ref)
	Set the x ref object and the uniform x_ref weighting

void	set_x_ref (std::vector< double > weight_x_ref_vec, std::vector< double > x_ref)
	Set the x ref object and piecewised x_ref weightings

void	set_towing_x_ref (const double weight_towing_x_ref, std::vector< double > towing_x_ref)
	Set towing x ref object and piecewised towing_x_ref weightings

void	set_towing_x_ref (std::vector< double > weight_towing_x_ref_vec, std::vector< double > towing_x_ref)

void	set_end_state_ref (const std::array< double, 3 > &weight_end_state, const std::array< double, 3 > &end_state_ref)

virtual bool	Optimize (const int max_iter=4000)

const std::vector< double > &	opt_x () const

const std::vector< double > &	opt_dx () const

const std::vector< double > &	opt_ddx () const

Protected 成员函数
void	CalculateKernel (std::vector< c_float > P_data, std::vector< c_int > P_indices, std::vector< c_int > *P_indptr) override

void	CalculateOffset (std::vector< c_float > *q) override

OSQPSettings *	SolverDefaultSettings () override

Protected 成员函数继承自 apollo::planning::PiecewiseJerkProblem
virtual void	CalculateAffineConstraint (std::vector< c_float > A_data, std::vector< c_int > A_indices, std::vector< c_int > A_indptr, std::vector< c_float > lower_bounds, std::vector< c_float > *upper_bounds)

bool	FormulateProblem (OSQPData *data)

void	FreeData (OSQPData *data)

bool	CheckLowUpperBound (const std::vector< c_float > &lower, const std::vector< c_float > &upper)

template<typename T >
T *	CopyData (const std::vector< T > &vec)

Protected 属性
bool	has_dx_ref_ = false

std::vector< double >	dx_ref_

std::vector< double >	weight_dx_ref_

std::vector< double >	penalty_dx_

Protected 属性继承自 apollo::planning::PiecewiseJerkProblem
size_t	num_of_knots_ = 0

std::vector< double >	x_

std::vector< double >	dx_

std::vector< double >	ddx_

std::array< double, 3 >	x_init_

std::array< double, 3 >	scale_factor_ = {{1.0, 1.0, 1.0}}

std::vector< std::pair< double, double > >	x_bounds_

std::vector< std::pair< double, double > >	dx_bounds_

std::vector< std::pair< double, double > >	ddx_bounds_

std::pair< double, double >	dddx_bound_

double	weight_x_ = 0.0

double	weight_dx_ = 0.0

double	weight_ddx_ = 0.0

double	weight_dddx_ = 0.0

double	delta_s_ = 1.0

bool	has_x_ref_ = false

double	weight_x_ref_ = 0.0

std::vector< double >	x_ref_

std::vector< double >	weight_x_ref_vec_

bool	has_towing_x_ref_ = false

std::vector< double >	towing_x_ref_

std::vector< double >	weight_towing_x_ref_vec_

bool	has_end_state_ref_ = false

std::array< double, 3 >	weight_end_state_ = {{0.0, 0.0, 0.0}}

std::array< double, 3 >	end_state_ref_

详细描述

在文件 piecewise_jerk_speed_problem.h 第 48 行定义.

构造及析构函数说明

◆ PiecewiseJerkSpeedProblem()

apollo::planning::PiecewiseJerkSpeedProblem::PiecewiseJerkSpeedProblem	(	const size_t	num_of_knots,
		const double	delta_s,
		const std::array< double, 3 > &	x_init
	)

在文件 piecewise_jerk_speed_problem.cc 第 25 行定义.

    : PiecewiseJerkProblem(num_of_knots, delta_s, x_init) {
  penalty_dx_.resize(num_of_knots_, 0.0);
  dx_ref_.resize(num_of_knots_, 0.0);
  weight_dx_ref_.resize(num_of_knots_, 0.0);
}

◆ ~PiecewiseJerkSpeedProblem()

virtual apollo::planning::PiecewiseJerkSpeedProblem::~PiecewiseJerkSpeedProblem ( )

virtualdefault

成员函数说明

◆ CalculateKernel()

void apollo::planning::PiecewiseJerkSpeedProblem::CalculateKernel	(	std::vector< c_float > *	P_data,
		std::vector< c_int > *	P_indices,
		std::vector< c_int > *	P_indptr
	)

overrideprotectedvirtual

实现了 apollo::planning::PiecewiseJerkProblem.

在文件 piecewise_jerk_speed_problem.cc 第 54 行定义.

                                                                            {
  const int n = static_cast<int>(num_of_knots_);
  const int kNumParam = 3 * n;
  const int kNumValue = 4 * n - 1;
  std::vector<std::vector<std::pair<c_int, c_float>>> columns;
  columns.resize(kNumParam);
  int value_index = 0;
 
  // x(i)^2 * w_x_ref
  for (int i = 0; i < n - 1; ++i) {
    columns[i].emplace_back(
        i, weight_x_ref_ / (scale_factor_[0] * scale_factor_[0]));
    ++value_index;
  }
  // x(n-1)^2 * (w_x_ref + w_end_x)
  columns[n - 1].emplace_back(n - 1, (weight_x_ref_ + weight_end_state_[0]) /
                                         (scale_factor_[0] * scale_factor_[0]));
  ++value_index;
 
  // x(i)'^2 * (w_dx_ref + penalty_dx)
  for (int i = 0; i < n - 1; ++i) {
    columns[n + i].emplace_back(n + i,
                                (weight_dx_ref_[i] + penalty_dx_[i]) /
                                    (scale_factor_[1] * scale_factor_[1]));
    ++value_index;
  }
  // x(n-1)'^2 * (w_dx_ref + penalty_dx + w_end_dx)
  columns[2 * n - 1].emplace_back(
      2 * n - 1,
      (weight_dx_ref_[n - 1] + penalty_dx_[n - 1] + weight_end_state_[1]) /
          (scale_factor_[1] * scale_factor_[1]));
  ++value_index;
 
  auto delta_s_square = delta_s_ * delta_s_;
  // x(i)''^2 * (w_ddx + 2 * w_dddx / delta_s^2)
  columns[2 * n].emplace_back(2 * n,
                              (weight_ddx_ + weight_dddx_ / delta_s_square) /
                                  (scale_factor_[2] * scale_factor_[2]));
  ++value_index;
 
  for (int i = 1; i < n - 1; ++i) {
    columns[2 * n + i].emplace_back(
        2 * n + i, (weight_ddx_ + 2.0 * weight_dddx_ / delta_s_square) /
                       (scale_factor_[2] * scale_factor_[2]));
    ++value_index;
  }
 
  columns[3 * n - 1].emplace_back(
      3 * n - 1,
      (weight_ddx_ + weight_dddx_ / delta_s_square + weight_end_state_[2]) /
          (scale_factor_[2] * scale_factor_[2]));
  ++value_index;
 
  // -2 * w_dddx / delta_s^2 * x(i)'' * x(i + 1)''
  for (int i = 0; i < n - 1; ++i) {
    columns[2 * n + i].emplace_back(2 * n + i + 1,
                                    -2.0 * weight_dddx_ / delta_s_square /
                                        (scale_factor_[2] * scale_factor_[2]));
    ++value_index;
  }
 
  CHECK_EQ(value_index, kNumValue);
 
  int ind_p = 0;
  for (int i = 0; i < kNumParam; ++i) {
    P_indptr->push_back(ind_p);
    for (const auto& row_data_pair : columns[i]) {
      P_data->push_back(row_data_pair.second * 2.0);
      P_indices->push_back(row_data_pair.first);
      ++ind_p;
    }
  }
  P_indptr->push_back(ind_p);
}

◆ CalculateOffset()

void apollo::planning::PiecewiseJerkSpeedProblem::CalculateOffset ( std::vector< c_float > * q )

overrideprotectedvirtual

实现了 apollo::planning::PiecewiseJerkProblem.

在文件 piecewise_jerk_speed_problem.cc 第 131 行定义.

                                                                     {
  CHECK_NOTNULL(q);
  const int n = static_cast<int>(num_of_knots_);
  const int kNumParam = 3 * n;
  q->resize(kNumParam);
  for (int i = 0; i < n; ++i) {
    if (has_x_ref_) {
      q->at(i) += -2.0 * weight_x_ref_ * x_ref_[i] / scale_factor_[0];
    }
    if (has_dx_ref_) {
      q->at(n + i) += -2.0 * weight_dx_ref_[i] * dx_ref_[i] / scale_factor_[1];
    }
  }
 
  if (has_end_state_ref_) {
    q->at(n - 1) +=
        -2.0 * weight_end_state_[0] * end_state_ref_[0] / scale_factor_[0];
    q->at(2 * n - 1) +=
        -2.0 * weight_end_state_[1] * end_state_ref_[1] / scale_factor_[1];
    q->at(3 * n - 1) +=
        -2.0 * weight_end_state_[2] * end_state_ref_[2] / scale_factor_[2];
  }
}

◆ set_dx_ref() [1/2]

void apollo::planning::PiecewiseJerkSpeedProblem::set_dx_ref	(	const double	weight_dx_ref,
		const double	dx_ref
	)

在文件 piecewise_jerk_speed_problem.cc 第 34 行定义.

                                                                {
  weight_dx_ref_.resize(num_of_knots_, weight_dx_ref);
  dx_ref_.resize(num_of_knots_, dx_ref);
  has_dx_ref_ = true;
}

◆ set_dx_ref() [2/2]

void apollo::planning::PiecewiseJerkSpeedProblem::set_dx_ref	(	const std::vector< double > &	weight_dx_ref,
		const std::vector< double > &	dx_ref
	)

在文件 piecewise_jerk_speed_problem.cc 第 41 行定义.

                                     {
  weight_dx_ref_ = weight_dx_ref;
  dx_ref_ = dx_ref;
  has_dx_ref_ = true;
}

◆ set_penalty_dx()

void apollo::planning::PiecewiseJerkSpeedProblem::set_penalty_dx ( std::vector< double > penalty_dx )

在文件 piecewise_jerk_speed_problem.cc 第 49 行定义.

                                                                           {
  CHECK_EQ(penalty_dx.size(), num_of_knots_);
  penalty_dx_ = std::move(penalty_dx);
}

◆ SolverDefaultSettings()

OSQPSettings * apollo::planning::PiecewiseJerkSpeedProblem::SolverDefaultSettings ( )

overrideprotectedvirtual

重载 apollo::planning::PiecewiseJerkProblem .

在文件 piecewise_jerk_speed_problem.cc 第 155 行定义.

                                                               {
  // Define Solver default settings
  OSQPSettings* settings =
      reinterpret_cast<OSQPSettings*>(c_malloc(sizeof(OSQPSettings)));
  osqp_set_default_settings(settings);
  settings->eps_abs = 1e-2;
  settings->eps_rel = 1e-2;
  settings->eps_prim_inf = 1e-5;
  settings->eps_dual_inf = 1e-5;
  settings->polish = true;
  settings->verbose = FLAGS_enable_osqp_debug;
  settings->scaled_termination = true;
  settings->time_limit = FLAGS_path_speed_osqp_setting_time_limit;
  return settings;
}

类成员变量说明

◆ dx_ref_

std::vector<double> apollo::planning::PiecewiseJerkSpeedProblem::dx_ref_

protected

在文件 piecewise_jerk_speed_problem.h 第 73 行定义.

◆ has_dx_ref_

bool apollo::planning::PiecewiseJerkSpeedProblem::has_dx_ref_ = false

protected

在文件 piecewise_jerk_speed_problem.h 第 72 行定义.

◆ penalty_dx_

std::vector<double> apollo::planning::PiecewiseJerkSpeedProblem::penalty_dx_

protected

在文件 piecewise_jerk_speed_problem.h 第 75 行定义.

◆ weight_dx_ref_

std::vector<double> apollo::planning::PiecewiseJerkSpeedProblem::weight_dx_ref_

protected

在文件 piecewise_jerk_speed_problem.h 第 74 行定义.

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

modules/planning/planning_base/math/piecewise_jerk/piecewise_jerk_speed_problem.h
modules/planning/planning_base/math/piecewise_jerk/piecewise_jerk_speed_problem.cc

Public 成员函数

Protected 成员函数

Protected 属性

详细描述

构造及析构函数说明

◆ PiecewiseJerkSpeedProblem()

◆ ~PiecewiseJerkSpeedProblem()

成员函数说明

◆ CalculateKernel()

◆ CalculateOffset()

◆ set_dx_ref() [1/2]

◆ set_dx_ref() [2/2]

◆ set_penalty_dx()

◆ SolverDefaultSettings()

类成员变量说明

◆ dx_ref_

◆ has_dx_ref_

◆ penalty_dx_

◆ weight_dx_ref_