提交 5e36ebe7 编写于 作者: Z Zhang Liangliang 提交者: Calvin Miao

Planning: cleaned up function names in planning/math

上级 60075c0a
......@@ -29,18 +29,13 @@ using ::apollo::common::math::ActiveSetQpSolver;
// discrete linear predictive control solver, with control format
// x(i + 1) = A * x(i) + B * u (i) + C
void SolveLinearMPC(const Matrix &matrix_a,
const Matrix &matrix_b,
const Matrix &matrix_c,
const Matrix &matrix_q,
const Matrix &matrix_r,
const Matrix &matrix_lower,
void SolveLinearMPC(const Matrix &matrix_a, const Matrix &matrix_b,
const Matrix &matrix_c, const Matrix &matrix_q,
const Matrix &matrix_r, const Matrix &matrix_lower,
const Matrix &matrix_upper,
const Matrix &matrix_initial_state,
const std::vector<Matrix> &reference,
const double eps,
const int max_iter,
std::vector<Matrix> *control) {
const std::vector<Matrix> &reference, const double eps,
const int max_iter, std::vector<Matrix> *control) {
if (matrix_a.rows() != matrix_a.cols() ||
matrix_b.rows() != matrix_a.rows() ||
matrix_lower.rows() != matrix_upper.rows()) {
......@@ -67,7 +62,7 @@ void SolveLinearMPC(const Matrix &matrix_a,
std::vector<Matrix> matrix_a_power(horizon);
matrix_a_power[0] = matrix_a;
for (unsigned int i = 1; i < matrix_a_power.size(); ++i) {
matrix_a_power[i] = matrix_a * matrix_a_power[i-1];
matrix_a_power[i] = matrix_a * matrix_a_power[i - 1];
}
Matrix matrix_k = Matrix::Zero(matrix_b.rows() * horizon,
......@@ -75,8 +70,8 @@ void SolveLinearMPC(const Matrix &matrix_a,
for (unsigned int r = 0; r < horizon; ++r) {
for (unsigned int c = 0; c <= r; ++c) {
matrix_k.block(r * matrix_b.rows(), c * matrix_b.cols(), matrix_b.rows(),
matrix_b.cols()) = matrix_a_power[r-c] * matrix_b;
}
matrix_b.cols()) = matrix_a_power[r - c] * matrix_b;
}
}
// Initialize matrix_k, matrix_m, matrix_t and matrix_v, matrix_qq, matrix_rr,
......@@ -93,7 +88,7 @@ void SolveLinearMPC(const Matrix &matrix_a,
for (unsigned int i = 1; i < horizon; ++i) {
matrix_m.block(i * matrix_a.rows(), 0, matrix_a.rows(), 1) =
matrix_a *
matrix_m.block((i-1) * matrix_a.rows(), 0, matrix_a.rows(), 1) +
matrix_m.block((i - 1) * matrix_a.rows(), 0, matrix_a.rows(), 1) +
matrix_c;
}
......@@ -103,10 +98,10 @@ void SolveLinearMPC(const Matrix &matrix_a,
matrix_lower;
matrix_uu.block(i * (*control)[0].rows(), 0, (*control)[0].rows(), 1) =
matrix_upper;
matrix_qq.block(i * matrix_q.rows(), i * matrix_q.rows(),
matrix_q.rows(), matrix_q.rows()) = matrix_q;
matrix_rr.block(i * matrix_r.rows(), i * matrix_r.rows(),
matrix_r.rows(), matrix_r.rows()) = matrix_r;
matrix_qq.block(i * matrix_q.rows(), i * matrix_q.rows(), matrix_q.rows(),
matrix_q.rows()) = matrix_q;
matrix_rr.block(i * matrix_r.rows(), i * matrix_r.rows(), matrix_r.rows(),
matrix_r.rows()) = matrix_r;
}
// Update matrix_m1, matrix_m2, convert MPC problem to QP problem done
......@@ -114,29 +109,27 @@ void SolveLinearMPC(const Matrix &matrix_a,
Matrix matrix_m2 = matrix_k.transpose() * matrix_qq * (matrix_m - matrix_t);
// Method 1: QPOASES
Eigen::MatrixXd matrix_inequality_constrain_ll = - Eigen::MatrixXd::Identity(
matrix_ll.rows(), matrix_ll.rows());
Eigen::MatrixXd matrix_inequality_constrain_uu = Eigen::MatrixXd::Identity(
matrix_uu.rows(), matrix_uu.rows());
Eigen::MatrixXd matrix_inequality_constrain_ll =
-Eigen::MatrixXd::Identity(matrix_ll.rows(), matrix_ll.rows());
Eigen::MatrixXd matrix_inequality_constrain_uu =
Eigen::MatrixXd::Identity(matrix_uu.rows(), matrix_uu.rows());
Eigen::MatrixXd matrix_inequality_constrain = Eigen::MatrixXd::Zero(
matrix_ll.rows() + matrix_uu.rows(), matrix_ll.rows());
matrix_inequality_constrain << - matrix_inequality_constrain_ll,
- matrix_inequality_constrain_uu;
matrix_inequality_constrain << -matrix_inequality_constrain_ll,
-matrix_inequality_constrain_uu;
Eigen::MatrixXd matrix_inequality_boundary = Eigen::MatrixXd::Zero(
matrix_ll.rows() + matrix_uu.rows(), matrix_ll.cols());
matrix_inequality_boundary << matrix_ll, - matrix_uu;
matrix_inequality_boundary << matrix_ll, -matrix_uu;
Eigen::MatrixXd matrix_equality_constrain = Eigen::MatrixXd::Zero(
matrix_ll.rows() + matrix_uu.rows(), matrix_ll.rows());
Eigen::MatrixXd matrix_equality_boundary = Eigen::MatrixXd::Zero(
matrix_ll.rows() + matrix_uu.rows(), matrix_ll.cols());
std::unique_ptr<QpSolver> qp_solver(new ActiveSetQpSolver(matrix_m1,
matrix_m2,
matrix_inequality_constrain,
matrix_inequality_boundary,
matrix_equality_constrain,
matrix_equality_boundary));
qp_solver->solve();
std::unique_ptr<QpSolver> qp_solver(new ActiveSetQpSolver(
matrix_m1, matrix_m2, matrix_inequality_constrain,
matrix_inequality_boundary, matrix_equality_constrain,
matrix_equality_boundary));
qp_solver->Solve();
matrix_v = qp_solver->params();
for (unsigned int i = 0; i < horizon; ++i) {
......
......@@ -48,7 +48,7 @@ ActiveSetQpSolver::ActiveSetQpSolver(
debug_info_ = FLAGS_default_enable_active_set_debug_info;
}
bool ActiveSetQpSolver::solve() {
bool ActiveSetQpSolver::Solve() {
::qpOASES::QProblem qp_problem(num_param_, num_constraint_);
::qpOASES::Options my_options;
my_options.epsNum = qp_eps_num_;
......
......@@ -38,7 +38,7 @@ class ActiveSetQpSolver : public QpSolver {
const Eigen::MatrixXd& affine_equality_matrix,
const Eigen::MatrixXd& affine_equality_boundary);
bool solve() override;
bool Solve() override;
void set_qp_eps_num(const double eps);
void set_qp_eps_den(const double eps);
......
......@@ -41,7 +41,7 @@ class QpSolver {
const Eigen::MatrixXd& affine_equality_matrix,
const Eigen::MatrixXd& affine_equality_boundary);
virtual bool solve() = 0;
virtual bool Solve() = 0;
const Eigen::MatrixXd& params() const;
const Eigen::MatrixXd& kernel_matrix() const;
const Eigen::MatrixXd& offset() const;
......
......@@ -40,10 +40,10 @@ class FrenetFramePath {
double Length() const;
const common::FrenetFramePoint &PointAt(const std::uint32_t index) const;
common::FrenetFramePoint &PointAt(const std::uint32_t index);
common::FrenetFramePoint EvaluateByS(const double s) const;
virtual void Clear();
private:
std::vector<common::FrenetFramePoint> points_;
};
......
......@@ -42,17 +42,17 @@ Spline1dKernel* Spline1dGenerator::mutable_spline_kernel() {
return &spline_kernel_;
}
void Spline1dGenerator::setup_init_qp_point(const Eigen::MatrixXd& x,
const Eigen::MatrixXd& y,
const Eigen::MatrixXd& z,
const Eigen::MatrixXd& s) {
void Spline1dGenerator::SetupInitQpPoint(const Eigen::MatrixXd& x,
const Eigen::MatrixXd& y,
const Eigen::MatrixXd& z,
const Eigen::MatrixXd& s) {
init_x_ = x;
init_y_ = y;
init_z_ = z;
init_s_ = s;
}
bool Spline1dGenerator::solve() {
bool Spline1dGenerator::Solve() {
const Eigen::MatrixXd& kernel_matrix = spline_kernel_.kernel_matrix();
const Eigen::MatrixXd& offset = spline_kernel_.offset();
const Eigen::MatrixXd& inequality_constraint_matrix =
......@@ -69,7 +69,7 @@ bool Spline1dGenerator::solve() {
inequality_constraint_boundary, equality_constraint_matrix,
equality_constraint_boundary));
if (!qp_solver_->solve()) {
if (!qp_solver_->Solve()) {
return false;
}
......
......@@ -74,11 +74,11 @@ class Spline1dGenerator {
// setup initialize point, if not,
// will use the default primal-dual parameter
void setup_init_qp_point(const Eigen::MatrixXd& x, const Eigen::MatrixXd& y,
const Eigen::MatrixXd& z, const Eigen::MatrixXd& s);
void SetupInitQpPoint(const Eigen::MatrixXd& x, const Eigen::MatrixXd& y,
const Eigen::MatrixXd& z, const Eigen::MatrixXd& s);
// solve
bool solve();
bool Solve();
// output
const Spline1d& spline() const;
......
......@@ -46,13 +46,13 @@ Spline1dKernel::Spline1dKernel(const std::vector<double>& x_knots,
offset_ = Eigen::MatrixXd::Zero(total_params_, 1);
}
void Spline1dKernel::add_regularization(const double regularized_param) {
void Spline1dKernel::AddRegularization(const double regularized_param) {
Eigen::MatrixXd id_matrix =
Eigen::MatrixXd::Identity(kernel_matrix_.rows(), kernel_matrix_.cols());
kernel_matrix_ += id_matrix * regularized_param;
}
bool Spline1dKernel::add_kernel(const Eigen::MatrixXd& kernel,
bool Spline1dKernel::AddKernel(const Eigen::MatrixXd& kernel,
const Eigen::MatrixXd& offset,
const double weight) {
if (kernel.rows() != kernel.cols() ||
......@@ -65,10 +65,10 @@ bool Spline1dKernel::add_kernel(const Eigen::MatrixXd& kernel,
return true;
}
bool Spline1dKernel::add_kernel(const Eigen::MatrixXd& kernel,
bool Spline1dKernel::AddKernel(const Eigen::MatrixXd& kernel,
const double weight) {
Eigen::MatrixXd offset = Eigen::MatrixXd::Zero(kernel.rows(), 1);
return add_kernel(kernel, offset, weight);
return AddKernel(kernel, offset, weight);
}
Eigen::MatrixXd* Spline1dKernel::mutable_kernel_matrix() {
......
......@@ -38,10 +38,10 @@ class Spline1dKernel {
const std::uint32_t spline_order);
// customized input / output method
void add_regularization(const double regularized_param);
bool add_kernel(const Eigen::MatrixXd& kernel, const Eigen::MatrixXd& offset,
void AddRegularization(const double regularized_param);
bool AddKernel(const Eigen::MatrixXd& kernel, const Eigen::MatrixXd& offset,
const double weight);
bool add_kernel(const Eigen::MatrixXd& kernel, const double weight);
bool AddKernel(const Eigen::MatrixXd& kernel, const double weight);
Eigen::MatrixXd* mutable_kernel_matrix();
Eigen::MatrixXd* mutable_offset();
......
......@@ -60,13 +60,13 @@ double Spline2d::y(const double t) const {
return splines_[index].y(t - t_knots_[index]);
}
double Spline2d::derivative_x(const double t) const {
double Spline2d::DerivativeX(const double t) const {
// zero order spline
if (splines_.size() == 0) {
return 0.0;
}
std::uint32_t index = find_index(t);
return splines_[index].derivative_x(t - t_knots_[index]);
return splines_[index].DerivativeX(t - t_knots_[index]);
}
double Spline2d::derivative_y(const double t) const {
......@@ -78,12 +78,12 @@ double Spline2d::derivative_y(const double t) const {
return splines_[index].derivative_y(t - t_knots_[index]);
}
double Spline2d::second_derivative_x(const double t) const {
double Spline2d::SecondDerivativeX(const double t) const {
if (splines_.size() == 0) {
return 0.0;
}
std::uint32_t index = find_index(t);
return splines_[index].second_derivative_x(t - t_knots_[index]);
return splines_[index].SecondDerivativeX(t - t_knots_[index]);
}
double Spline2d::second_derivative_y(const double t) const {
......@@ -94,12 +94,12 @@ double Spline2d::second_derivative_y(const double t) const {
return splines_[index].second_derivative_y(t - t_knots_[index]);
}
double Spline2d::third_derivative_x(const double t) const {
double Spline2d::ThirdDerivativeX(const double t) const {
if (splines_.size() == 0) {
return 0.0;
}
std::uint32_t index = find_index(t);
return splines_[index].third_derivative_x(t - t_knots_[index]);
return splines_[index].ThirdDerivativeX(t - t_knots_[index]);
}
double Spline2d::third_derivative_y(const double t) const {
......
......@@ -39,11 +39,11 @@ class Spline2d {
std::pair<double, double> operator()(const double t) const;
double x(const double t) const;
double y(const double t) const;
double derivative_x(const double t) const;
double DerivativeX(const double t) const;
double derivative_y(const double t) const;
double second_derivative_x(const double t) const;
double SecondDerivativeX(const double t) const;
double second_derivative_y(const double t) const;
double third_derivative_x(const double t) const;
double ThirdDerivativeX(const double t) const;
double third_derivative_y(const double t) const;
bool set_splines(const Eigen::MatrixXd& params, const std::uint32_t order);
Spline2dSeg* mutable_smoothing_spline(const std::uint32_t index);
......
......@@ -56,7 +56,7 @@ bool Spline2dConstraint::AddEqualityConstraint(
* @brief: inequality boundary constraints
* if no boundary, do specify either by std::infinity or let vector.size() = 0
**/
bool Spline2dConstraint::add_2d_boundary(
bool Spline2dConstraint::Add2dBoundary(
const std::vector<double>& t_coord, const std::vector<double>& angle,
const std::vector<Vec2d>& ref_point,
const std::vector<double>& longitidinal_bound,
......
......@@ -48,7 +48,7 @@ class Spline2dConstraint {
* if no boundary, do specify either by std::infinity or let vector.size() =
*0
**/
bool add_2d_boundary(
bool Add2dBoundary(
const std::vector<double>& t_coord, const std::vector<double>& angle,
const std::vector<apollo::common::math::Vec2d>& ref_point,
const std::vector<double>& longitidinal_bound,
......
......@@ -39,13 +39,13 @@ Spline2dKernel::Spline2dKernel(const std::vector<double>& t_knots,
}
// customized input output
void Spline2dKernel::add_regularization(const double regularization_param) {
void Spline2dKernel::AddRegularization(const double regularization_param) {
Eigen::MatrixXd id_matrix =
Eigen::MatrixXd::Identity(kernel_matrix_.rows(), kernel_matrix_.cols());
kernel_matrix_ += id_matrix * regularization_param;
}
bool Spline2dKernel::add_kernel(const Eigen::MatrixXd& kernel,
bool Spline2dKernel::AddKernel(const Eigen::MatrixXd& kernel,
const Eigen::MatrixXd& offset,
const double weight) {
if (kernel.rows() != kernel.cols() ||
......@@ -58,10 +58,10 @@ bool Spline2dKernel::add_kernel(const Eigen::MatrixXd& kernel,
return true;
}
bool Spline2dKernel::add_kernel(const Eigen::MatrixXd& kernel,
bool Spline2dKernel::AddKernel(const Eigen::MatrixXd& kernel,
const double weight) {
Eigen::MatrixXd offset = Eigen::MatrixXd::Zero(kernel.rows(), 1);
return add_kernel(kernel, offset, weight);
return AddKernel(kernel, offset, weight);
}
Eigen::MatrixXd* Spline2dKernel::mutable_kernel_matrix() {
......
......@@ -36,10 +36,10 @@ class Spline2dKernel {
const std::uint32_t spline_order);
// customized input output
void add_regularization(const double regularization_param);
bool add_kernel(const Eigen::MatrixXd& kernel, const Eigen::MatrixXd& offset,
void AddRegularization(const double regularization_param);
bool AddKernel(const Eigen::MatrixXd& kernel, const Eigen::MatrixXd& offset,
const double weight);
bool add_kernel(const Eigen::MatrixXd& kernel, const double weight);
bool AddKernel(const Eigen::MatrixXd& kernel, const double weight);
Eigen::MatrixXd* mutable_kernel_matrix();
Eigen::MatrixXd* mutable_offset();
......
......@@ -70,7 +70,7 @@ double Spline2dSeg::x(const double t) const { return spline_func_x_(t); }
double Spline2dSeg::y(const double t) const { return spline_func_y_(t); }
double Spline2dSeg::derivative_x(const double t) const {
double Spline2dSeg::DerivativeX(const double t) const {
return derivative_x_(t);
}
......@@ -78,7 +78,7 @@ double Spline2dSeg::derivative_y(const double t) const {
return derivative_y_(t);
}
double Spline2dSeg::second_derivative_x(const double t) const {
double Spline2dSeg::SecondDerivativeX(const double t) const {
return second_derivative_x_(t);
}
......@@ -86,7 +86,7 @@ double Spline2dSeg::second_derivative_y(const double t) const {
return second_derivative_y_(t);
}
double Spline2dSeg::third_derivative_x(const double t) const {
double Spline2dSeg::ThirdDerivativeX(const double t) const {
return third_derivative_x_(t);
}
......@@ -102,11 +102,11 @@ const PolynomialXd& Spline2dSeg::spline_func_y() const {
return spline_func_y_;
}
const PolynomialXd& Spline2dSeg::derivative_x() const { return derivative_x_; }
const PolynomialXd& Spline2dSeg::DerivativeX() const { return derivative_x_; }
const PolynomialXd& Spline2dSeg::derivative_y() const { return derivative_y_; }
const PolynomialXd& Spline2dSeg::second_derivative_x() const {
const PolynomialXd& Spline2dSeg::SecondDerivativeX() const {
return second_derivative_x_;
}
......@@ -114,7 +114,7 @@ const PolynomialXd& Spline2dSeg::second_derivative_y() const {
return second_derivative_y_;
}
const PolynomialXd& Spline2dSeg::third_derivative_x() const {
const PolynomialXd& Spline2dSeg::ThirdDerivativeX() const {
return third_derivative_x_;
}
......
......@@ -43,20 +43,20 @@ class Spline2dSeg {
std::pair<double, double> operator()(const double t) const;
double x(const double t) const;
double y(const double t) const;
double derivative_x(const double t) const;
double DerivativeX(const double t) const;
double derivative_y(const double t) const;
double second_derivative_x(const double t) const;
double SecondDerivativeX(const double t) const;
double second_derivative_y(const double t) const;
double third_derivative_x(const double t) const;
double ThirdDerivativeX(const double t) const;
double third_derivative_y(const double t) const;
const PolynomialXd& spline_func_x() const;
const PolynomialXd& spline_func_y() const;
const PolynomialXd& derivative_x() const;
const PolynomialXd& DerivativeX() const;
const PolynomialXd& derivative_y() const;
const PolynomialXd& second_derivative_x() const;
const PolynomialXd& SecondDerivativeX() const;
const PolynomialXd& second_derivative_y() const;
const PolynomialXd& third_derivative_x() const;
const PolynomialXd& ThirdDerivativeX() const;
const PolynomialXd& third_derivative_y() const;
private:
......
......@@ -40,7 +40,7 @@ Spline2dKernel* Spline2dSolver::mutable_kernel() { return &kernel_; }
Spline2d* Spline2dSolver::mutable_spline() { return &spline_; }
// solve
bool Spline2dSolver::solve() {
bool Spline2dSolver::Solve() {
const Eigen::MatrixXd& kernel_matrix = kernel_.kernel_matrix();
const Eigen::MatrixXd& offset = kernel_.offset();
const Eigen::MatrixXd& inequality_constraint_matrix =
......@@ -56,7 +56,7 @@ bool Spline2dSolver::solve() {
kernel_matrix, offset, inequality_constraint_matrix,
inequality_constraint_boundary, equality_constraint_matrix,
equality_constraint_boundary));
if (!qp_solver_->solve()) {
if (!qp_solver_->Solve()) {
return false;
}
......
......@@ -42,7 +42,7 @@ class Spline2dSolver {
Spline2d* mutable_spline();
// solve
bool solve();
bool Solve();
// extract
const Spline2d& spline() const;
......
......@@ -282,7 +282,7 @@ bool QpSplinePathGenerator::AddKernel() {
Spline1dKernel* spline_kernel = spline_generator_->mutable_spline_kernel();
if (qp_spline_path_config_.regularization_weight() > 0) {
spline_kernel->add_regularization(
spline_kernel->AddRegularization(
qp_spline_path_config_.regularization_weight());
}
......@@ -311,7 +311,7 @@ bool QpSplinePathGenerator::AddKernel() {
}
bool QpSplinePathGenerator::Solve() {
if (!spline_generator_->solve()) {
if (!spline_generator_->Solve()) {
AERROR << "Could not solve the qp problem in spline path generator.";
return false;
}
......
......@@ -245,7 +245,7 @@ Status QpSplineStGraph::ApplyKernel(
}
Status QpSplineStGraph::Solve() {
return spline_generator_->solve()
return spline_generator_->Solve()
? Status::OK()
: Status(ErrorCode::PLANNING_ERROR, "QpSplineStGraph::solve");
}
......
......@@ -75,7 +75,7 @@ bool ReferenceLineSmoother::smooth(
return false;
}
if (!solve()) {
if (!Solve()) {
AERROR << "Solve spline smoother problem failed";
}
......@@ -90,16 +90,16 @@ bool ReferenceLineSmoother::smooth(
i < smoother_config_.num_of_total_points() && t < end_t; ++i) {
std::pair<double, double> xy = spline_solver_->spline()(t);
const double heading = std::atan2(spline_solver_->spline().derivative_y(t),
spline_solver_->spline().derivative_x(t));
spline_solver_->spline().DerivativeX(t));
const double kappa = CurveMath::ComputeCurvature(
spline_solver_->spline().derivative_x(t),
spline_solver_->spline().second_derivative_x(t),
spline_solver_->spline().DerivativeX(t),
spline_solver_->spline().SecondDerivativeX(t),
spline_solver_->spline().derivative_y(t),
spline_solver_->spline().second_derivative_y(t));
const double dkappa = CurveMath::ComputeCurvatureDerivative(
spline_solver_->spline().derivative_x(t),
spline_solver_->spline().second_derivative_x(t),
spline_solver_->spline().third_derivative_x(t),
spline_solver_->spline().DerivativeX(t),
spline_solver_->spline().SecondDerivativeX(t),
spline_solver_->spline().ThirdDerivativeX(t),
spline_solver_->spline().derivative_y(t),
spline_solver_->spline().second_derivative_y(t),
spline_solver_->spline().third_derivative_y(t));
......@@ -162,7 +162,7 @@ bool ReferenceLineSmoother::apply_constraint(
xy_points.emplace_back(path_points[i].x(), path_points[i].y());
}
if (!spline_solver_->mutable_constraint()->add_2d_boundary(
if (!spline_solver_->mutable_constraint()->Add2dBoundary(
evaluated_t, angles, xy_points, longitidinal_bound, lateral_bound)) {
AERROR << "Add 2d boundary constraint failed";
return false;
......@@ -196,11 +196,11 @@ bool ReferenceLineSmoother::ApplyKernel() {
}
// TODO(fanhaoyang): change to a configurable param
kernel->add_regularization(0.01);
kernel->AddRegularization(0.01);
return true;
}
bool ReferenceLineSmoother::solve() { return spline_solver_->solve(); }
bool ReferenceLineSmoother::Solve() { return spline_solver_->Solve(); }
bool ReferenceLineSmoother::extract_evaluated_points(
const ReferenceLine& raw_reference_line, const std::vector<double>& vec_t,
......
......@@ -51,7 +51,7 @@ class ReferenceLineSmoother {
bool ApplyKernel();
bool solve();
bool Solve();
bool extract_evaluated_points(
const ReferenceLine& raw_reference_line, const std::vector<double>& vec_t,
......
Markdown is supported
0% .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑!
想要评论请 注册