apollo::planning::OsqpSpline2dSolver类 参考

#include <osqp_spline_2d_solver.h>

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

apollo::planning::OsqpSpline2dSolver 的协作图:

Public 成员函数
	OsqpSpline2dSolver (const std::vector< double > &t_knots, const uint32_t order)
void	Reset (const std::vector< double > &t_knots, const uint32_t order) override
Spline2dConstraint *	mutable_constraint () override
Spline2dKernel *	mutable_kernel () override
Spline2d *	mutable_spline () override
bool	Solve () override
const Spline2d &	spline () const override
Public 成员函数 继承自 apollo::planning::Spline2dSolver
	Spline2dSolver (const std::vector< double > &t_knots, const uint32_t order)
virtual	~Spline2dSolver ()=default

额外继承的成员函数
Protected 属性 继承自 apollo::planning::Spline2dSolver
Spline2d	spline_
Spline2dKernel	kernel_
Spline2dConstraint	constraint_

详细描述

在文件 osqp_spline_2d_solver.h 第 35 行定义.

构造及析构函数说明

OsqpSpline2dSolver()

apollo::planning::OsqpSpline2dSolver::OsqpSpline2dSolver	(	const std::vector< double > &	t_knots,
		const uint32_t	order
	)

在文件 osqp_spline_2d_solver.cc 第 36 行定义.

    : Spline2dSolver(t_knots, order) {}

成员函数说明

mutable_constraint()

Spline2dConstraint * apollo::planning::OsqpSpline2dSolver::mutable_constraint ( )

overridevirtual

实现了 apollo::planning::Spline2dSolver.

在文件 osqp_spline_2d_solver.cc 第 48 行定义.

                                                           {
  return &constraint_;
}

mutable_kernel()

Spline2dKernel * apollo::planning::OsqpSpline2dSolver::mutable_kernel ( )

overridevirtual

实现了 apollo::planning::Spline2dSolver.

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

{ return &kernel_; }

mutable_spline()

Spline2d * apollo::planning::OsqpSpline2dSolver::mutable_spline ( )

overridevirtual

实现了 apollo::planning::Spline2dSolver.

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

{ return &spline_; }

Reset()

void apollo::planning::OsqpSpline2dSolver::Reset ( const std::vector< double > &  t_knots, const uint32_t  order  )

overridevirtual

实现了 apollo::planning::Spline2dSolver.

在文件 osqp_spline_2d_solver.cc 第 40 行定义.

                                                     {
  spline_ = Spline2d(t_knots, order);
  kernel_ = Spline2dKernel(t_knots, order);
  constraint_ = Spline2dConstraint(t_knots, order);
}

Solve()

bool apollo::planning::OsqpSpline2dSolver::Solve ( )

overridevirtual

实现了 apollo::planning::Spline2dSolver.

在文件 osqp_spline_2d_solver.cc 第 56 行定义.

                               {
  // Namings here are following osqp convention.
  // For details, visit: https://osqp.org/docs/examples/demo.html
 
  // change P to csc format
  const MatrixXd& P = kernel_.kernel_matrix();
  ADEBUG << "P: " << P.rows() << ", " << P.cols();
  if (P.rows() == 0) {
    return false;
  }
 
  std::vector<c_float> P_data;
  std::vector<c_int> P_indices;
  std::vector<c_int> P_indptr;
  DenseToCSCMatrix(P, &P_data, &P_indices, &P_indptr);
 
  // change A to csc format
  const MatrixXd& inequality_constraint_matrix =
      constraint_.inequality_constraint().constraint_matrix();
  const MatrixXd& equality_constraint_matrix =
      constraint_.equality_constraint().constraint_matrix();
  MatrixXd A(
      inequality_constraint_matrix.rows() + equality_constraint_matrix.rows(),
      inequality_constraint_matrix.cols());
  A << inequality_constraint_matrix, equality_constraint_matrix;
  ADEBUG << "A: " << A.rows() << ", " << A.cols();
  if (A.rows() == 0) {
    return false;
  }
 
  std::vector<c_float> A_data;
  std::vector<c_int> A_indices;
  std::vector<c_int> A_indptr;
  DenseToCSCMatrix(A, &A_data, &A_indices, &A_indptr);
 
  // set q, l, u: l < A < u
  const MatrixXd& q_eigen = kernel_.offset();
  c_float q[q_eigen.rows()];  // NOLINT
  for (int i = 0; i < q_eigen.size(); ++i) {
    q[i] = q_eigen(i);
  }
 
  const MatrixXd& inequality_constraint_boundary =
      constraint_.inequality_constraint().constraint_boundary();
  const MatrixXd& equality_constraint_boundary =
      constraint_.equality_constraint().constraint_boundary();
 
  auto constraint_num = inequality_constraint_boundary.rows() +
                        equality_constraint_boundary.rows();
 
  static constexpr float kEpsilon = 1e-9f;
  static constexpr float kUpperLimit = 1e9f;
  c_float l[constraint_num];  // NOLINT
  c_float u[constraint_num];  // NOLINT
  for (int i = 0; i < constraint_num; ++i) {
    if (i < inequality_constraint_boundary.rows()) {
      l[i] = inequality_constraint_boundary(i, 0);
      u[i] = kUpperLimit;
    } else {
      const auto idx = i - inequality_constraint_boundary.rows();
      l[i] = equality_constraint_boundary(idx, 0) - kEpsilon;
      u[i] = equality_constraint_boundary(idx, 0) + kEpsilon;
    }
  }
 
  // Problem settings
  OSQPSettings* settings =
      reinterpret_cast<OSQPSettings*>(c_malloc(sizeof(OSQPSettings)));
 
  // Populate data
  OSQPData* data = reinterpret_cast<OSQPData*>(c_malloc(sizeof(OSQPData)));
  data->n = P.rows();
  data->m = constraint_num;
  data->P = csc_matrix(data->n, data->n, P_data.size(), P_data.data(),
                       P_indices.data(), P_indptr.data());
  data->q = q;
  data->A = csc_matrix(data->m, data->n, A_data.size(), A_data.data(),
                       A_indices.data(), A_indptr.data());
  data->l = l;
  data->u = u;
 
  // Define Solver settings as default
  osqp_set_default_settings(settings);
  settings->alpha = 1.0;  // Change alpha parameter
  settings->eps_abs = 1.0e-05;
  settings->eps_rel = 1.0e-05;
  settings->max_iter = 5000;
  settings->polish = true;
  settings->verbose = FLAGS_enable_osqp_debug;
 
  // Setup workspace
  OSQPWorkspace* work = nullptr;
  work = osqp_setup(data, settings);
  // osqp_setup(&work, data, settings);
 
  // Solve Problem
  osqp_solve(work);
 
  MatrixXd solved_params = MatrixXd::Zero(P.rows(), 1);
  for (int i = 0; i < P.rows(); ++i) {
    solved_params(i, 0) = work->solution->x[i];
  }
 
  last_num_param_ = static_cast<int>(P.rows());
  last_num_constraint_ = static_cast<int>(constraint_num);
 
  // Cleanup
  osqp_cleanup(work);
  c_free(data->A);
  c_free(data->P);
  c_free(data);
  c_free(settings);
 
  return spline_.set_splines(solved_params, spline_.spline_order());
}

spline()

const Spline2d & apollo::planning::OsqpSpline2dSolver::spline ( ) const

overridevirtual

实现了 apollo::planning::Spline2dSolver.

在文件 osqp_spline_2d_solver.cc 第 173 行定义.

{ return spline_; }

---

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

• modules/planning/planning_base/math/smoothing_spline/osqp_spline_2d_solver.h
• modules/planning/planning_base/math/smoothing_spline/osqp_spline_2d_solver.cc