Planning: refactor piecewise_jerk math functions

modules/planning/math/finite_element_qp/BUILD → modules/planning/math/piecewise_jerk/BUILD

@@ -3,12 +3,12 @@ load("//tools:cpplint.bzl", "cpplint")
 package(default_visibility = ["//visibility:public"])
 
 cc_library(
-    name = "fem_1d_qp_problem",
+    name = "piecewise_jerk_problem",
     srcs = [
-        "fem_1d_qp_problem.cc",
+        "piecewise_jerk_problem.cc",
     ],
     hdrs = [
-        "fem_1d_qp_problem.h",
+        "piecewise_jerk_problem.h",
     ],
     deps = [
         "//cyber/common:log",
@@ -18,16 +18,28 @@ cc_library(
 )
 
 cc_test(
-    name = "fem_1d_qp_problem_test",
+    name = "piecewise_jerk_problem_test",
     size = "small",
     srcs = [
-        "fem_1d_qp_problem_test.cc",
+        "piecewise_jerk_problem.cc",
     ],
     deps = [
-        ":fem_1d_qp_problem",
+        ":piecewise_jerk_problem",
         "//cyber/common:log",
         "@gtest//:main",
     ],
 )
 
+cc_library(
+    name = "fem_1d_qp_problem",
+    srcs = [
+        "fem_1d_qp_problem.cc",
+    ],
+    hdrs = [
+        "fem_1d_qp_problem.h",
+    ],
+    deps = [
+        ":piecewise_jerk_problem",
+    ],
+)
 cpplint()

modules/planning/math/piecewise_jerk/fem_1d_qp_problem.cc

+/******************************************************************************
+ * Copyright 2018 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+#include "modules/planning/math/piecewise_jerk/fem_1d_qp_problem.h"
+
+#include <algorithm>
+
+#include "cyber/common/log.h"
+
+#include "modules/planning/common/planning_gflags.h"
+
+namespace apollo {
+namespace planning {
+
+void Fem1dQpProblem::CalculateKernel(std::vector<c_float>* P_data,
+                                     std::vector<c_int>* P_indices,
+                                     std::vector<c_int>* P_indptr) {
+  const int N = static_cast<int>(num_of_knots_);
+  const int kNumParam = 3 * N;
+  const int kNumValue = kNumParam + (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 + w_mid_line)
+  for (int i = 0; i < N; ++i) {
+    columns[i].emplace_back(i, (weight_.x_w + weight_.x_mid_line_w));
+    ++value_index;
+  }
+
+  // x(i)'^2 * w_dx
+  for (int i = 0; i < N; ++i) {
+    columns[N + i].emplace_back(N + i, weight_.x_derivative_w);
+    ++value_index;
+  }
+
+  // x(i)''^2 * (w_ddx + 2 * w_dddx / delta_s^2)
+  columns[2 * N].emplace_back(
+      2 * N, weight_.x_second_order_derivative_w +
+                 weight_.x_third_order_derivative_w / delta_s_sq_);
+  ++value_index;
+  for (int i = 1; i < N - 1; ++i) {
+    columns[2 * N + i].emplace_back(
+        2 * N + i, weight_.x_second_order_derivative_w +
+                       2.0 * weight_.x_third_order_derivative_w / delta_s_sq_);
+    ++value_index;
+  }
+  columns[3 * N - 1].emplace_back(
+      3 * N - 1, weight_.x_second_order_derivative_w +
+                     weight_.x_third_order_derivative_w / delta_s_sq_);
+  ++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_.x_third_order_derivative_w / delta_s_sq_);
+    ++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);
+}
+
+void Fem1dQpProblem::CalculateOffset(std::vector<c_float>* q) {
+  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 < kNumParam; ++i) {
+    if (i < N) {
+      q->at(i) = -2.0 * weight_.x_mid_line_w *
+                 (std::get<0>(x_bounds_[i]) + std::get<1>(x_bounds_[i]));
+    } else {
+      q->at(i) = 0.0;
+    }
+  }
+}
+
+}  // namespace planning
+}  // namespace apollo

modules/planning/math/piecewise_jerk/fem_1d_qp_problem.h

+/******************************************************************************
+ * Copyright 2018 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * @file
+ **/
+
+#pragma once
+
+#include <tuple>
+#include <utility>
+#include <vector>
+
+#include "modules/planning/math/piecewise_jerk/piecewise_jerk_problem.h"
+
+namespace apollo {
+namespace planning {
+
+/*
+ * @brief:
+ * FEM stands for finite element method.
+ * This class solve an optimization problem:
+ * x
+ * |
+ * |                       P(s1, x1)  P(s2, x2)
+ * |            P(s0, x0)                       ... P(s(k-1), x(k-1))
+ * |P(start)
+ * |
+ * |________________________________________________________ s
+ *
+ * we suppose s(k+1) - s(k) == s(k) - s(k-1)
+ *
+ * Given the x, x', x'' at P(start),  The goal is to find x0, x1, ... x(k-1)
+ * which makes the line P(start), P0, P(1) ... P(k-1) "smooth".
+ */
+
+class Fem1dQpProblem : public PiecewiseJerkProblem {
+ public:
+  Fem1dQpProblem() = default;
+
+  virtual ~Fem1dQpProblem() = default;
+
+ protected:
+  // naming convention follows osqp solver.
+  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;
+};
+
+}  // namespace planning
+}  // namespace apollo

modules/planning/math/finite_element_qp/fem_1d_qp_problem.cc → modules/planning/math/piecewise_jerk/piecewise_jerk_problem.cc

@@ -14,10 +14,9 @@
  * limitations under the License.
  *****************************************************************************/
 
-#include "modules/planning/math/finite_element_qp/fem_1d_qp_problem.h"
+#include "modules/planning/math/piecewise_jerk/piecewise_jerk_problem.h"
 
 #include <algorithm>
-#include <chrono>
 
 #include "cyber/common/log.h"
 
@@ -30,7 +29,7 @@ namespace {
 constexpr double kMaxVariableRange = 1e10;
 }  // namespace
 
-Fem1dQpProblem::Fem1dQpProblem(const size_t num_of_knots,
+void PiecewiseJerkProblem::InitProblem(const size_t num_of_knots,
                                const std::array<double, 3>& x_init,
                                const double delta_s,
                                const std::array<double, 5>& w,
@@ -61,7 +60,7 @@ Fem1dQpProblem::Fem1dQpProblem(const size_t num_of_knots,
                      std::make_pair(-kMaxVariableRange, kMaxVariableRange));
 }
 
-bool Fem1dQpProblem::OptimizeWithOsqp(
+bool PiecewiseJerkProblem::OptimizeWithOsqp(
     const size_t kernel_dim, const size_t num_affine_constraint,
     std::vector<c_float>& P_data, std::vector<c_int>& P_indices,    // NOLINT
     std::vector<c_int>& P_indptr, std::vector<c_float>& A_data,     // NOLINT
@@ -104,25 +103,25 @@ bool Fem1dQpProblem::OptimizeWithOsqp(
   return true;
 }
 
-void Fem1dQpProblem::SetZeroOrderBounds(
+void PiecewiseJerkProblem::SetZeroOrderBounds(
     std::vector<std::pair<double, double>> x_bounds) {
   CHECK_EQ(x_bounds.size(), num_of_knots_);
   x_bounds_ = std::move(x_bounds);
 }
 
-void Fem1dQpProblem::SetFirstOrderBounds(
+void PiecewiseJerkProblem::SetFirstOrderBounds(
     std::vector<std::pair<double, double>> dx_bounds) {
   CHECK_EQ(dx_bounds.size(), num_of_knots_);
   dx_bounds_ = std::move(dx_bounds);
 }
 
-void Fem1dQpProblem::SetSecondOrderBounds(
+void PiecewiseJerkProblem::SetSecondOrderBounds(
     std::vector<std::pair<double, double>> d2x_bounds) {
   CHECK_EQ(d2x_bounds.size(), num_of_knots_);
   ddx_bounds_ = std::move(d2x_bounds);
 }
 
-void Fem1dQpProblem::SetZeroOrderBounds(const double x_bound) {
+void PiecewiseJerkProblem::SetZeroOrderBounds(const double x_bound) {
   CHECK_GT(x_bound, 0.0);
   for (auto& x : x_bounds_) {
     x.first = -x_bound;
@@ -130,7 +129,7 @@ void Fem1dQpProblem::SetZeroOrderBounds(const double x_bound) {
   }
 }
 
-void Fem1dQpProblem::SetFirstOrderBounds(const double dx_bound) {
+void PiecewiseJerkProblem::SetFirstOrderBounds(const double dx_bound) {
   CHECK_GT(dx_bound, 0.0);
   for (auto& x : dx_bounds_) {
     x.first = -dx_bound;
@@ -138,7 +137,7 @@ void Fem1dQpProblem::SetFirstOrderBounds(const double dx_bound) {
   }
 }
 
-void Fem1dQpProblem::SetSecondOrderBounds(const double ddx_bound) {
+void PiecewiseJerkProblem::SetSecondOrderBounds(const double ddx_bound) {
   CHECK_GT(ddx_bound, 0.0);
   for (auto& x : ddx_bounds_) {
     x.first = -ddx_bound;
@@ -146,7 +145,7 @@ void Fem1dQpProblem::SetSecondOrderBounds(const double ddx_bound) {
   }
 }
 
-void Fem1dQpProblem::ProcessBound(
+void PiecewiseJerkProblem::ProcessBound(
     const std::vector<std::tuple<double, double, double>>& src,
     std::vector<std::pair<double, double>>* dst) {
   DCHECK_NOTNULL(dst);
@@ -166,33 +165,29 @@ void Fem1dQpProblem::ProcessBound(
 }
 
 // x_bounds: tuple(s, lower_bounds, upper_bounds)
-void Fem1dQpProblem::SetVariableBounds(
+void PiecewiseJerkProblem::SetVariableBounds(
     const std::vector<std::tuple<double, double, double>>& x_bounds) {
   ProcessBound(x_bounds, &x_bounds_);
 }
 
 // dx_bounds: tuple(s, lower_bounds, upper_bounds)
-void Fem1dQpProblem::SetVariableDerivativeBounds(
+void PiecewiseJerkProblem::SetVariableDerivativeBounds(
     const std::vector<std::tuple<double, double, double>>& dx_bounds) {
   ProcessBound(dx_bounds, &dx_bounds_);
 }
 
 // ddx_bounds: tuple(s, lower_bounds, upper_bounds)
-void Fem1dQpProblem::SetVariableSecondOrderDerivativeBounds(
+void PiecewiseJerkProblem::SetVariableSecondOrderDerivativeBounds(
     const std::vector<std::tuple<double, double, double>>& ddx_bounds) {
   ProcessBound(ddx_bounds, &ddx_bounds_);
 }
 
-bool Fem1dQpProblem::Optimize(const int max_iter) {
+bool PiecewiseJerkProblem::Optimize(const int max_iter) {
   // calculate kernel
   std::vector<c_float> P_data;
   std::vector<c_int> P_indices;
   std::vector<c_int> P_indptr;
-  auto start_time = std::chrono::system_clock::now();
   CalculateKernel(&P_data, &P_indices, &P_indptr);
-  auto end_time1 = std::chrono::system_clock::now();
-  std::chrono::duration<double> diff = end_time1 - start_time;
-  ADEBUG << "Set Kernel used time: " << diff.count() * 1000 << " ms.";
 
   // calculate affine constraints
   std::vector<c_float> A_data;
@@ -202,17 +197,10 @@ bool Fem1dQpProblem::Optimize(const int max_iter) {
   std::vector<c_float> upper_bounds;
   CalculateAffineConstraint(&A_data, &A_indices, &A_indptr, &lower_bounds,
                             &upper_bounds);
-  auto end_time2 = std::chrono::system_clock::now();
-  diff = end_time2 - end_time1;
-  ADEBUG << "CalculateAffineConstraint used time: " << diff.count() * 1000
-         << " ms.";
 
   // calculate offset
   std::vector<c_float> q;
   CalculateOffset(&q);
-  auto end_time3 = std::chrono::system_clock::now();
-  diff = end_time3 - end_time2;
-  ADEBUG << "CalculateOffset used time: " << diff.count() * 1000 << " ms.";
 
   OSQPData* data = reinterpret_cast<OSQPData*>(c_malloc(sizeof(OSQPData)));
   OSQPSettings* settings =
@@ -260,14 +248,11 @@ bool Fem1dQpProblem::Optimize(const int max_iter) {
   c_free(data->P);
   c_free(data);
   c_free(settings);
-  auto end_time4 = std::chrono::system_clock::now();
-  diff = end_time4 - end_time3;
-  ADEBUG << "Run OptimizeWithOsqp used time: " << diff.count() * 1000 << " ms.";
 
   return true;
 }
 
-void Fem1dQpProblem::CalculateKernel(std::vector<c_float>* P_data,
+void PiecewiseJerkProblem::CalculateKernel(std::vector<c_float>* P_data,
                                      std::vector<c_int>* P_indices,
                                      std::vector<c_int>* P_indptr) {
   const int N = static_cast<int>(num_of_knots_);
@@ -324,12 +309,9 @@ void Fem1dQpProblem::CalculateKernel(std::vector<c_float>* P_data,
     }
   }
   P_indptr->push_back(ind_p);
-  ADEBUG << "P_data.size()=" << P_data->size();
-  ADEBUG << "P_indices.size()=" << P_indices->size();
-  ADEBUG << "P_indptr.size()=" << P_indptr->size();
 }
 
-void Fem1dQpProblem::CalculateAffineConstraint(
+void PiecewiseJerkProblem::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) {
@@ -428,7 +410,7 @@ void Fem1dQpProblem::CalculateAffineConstraint(
   A_indptr->push_back(ind_p);
 }
 
-void Fem1dQpProblem::CalculateOffset(std::vector<c_float>* q) {
+void PiecewiseJerkProblem::CalculateOffset(std::vector<c_float>* q) {
   CHECK_NOTNULL(q);
   const int N = static_cast<int>(num_of_knots_);
   const int kNumParam = 3 * N;

modules/planning/math/finite_element_qp/fem_1d_qp_problem.h → modules/planning/math/piecewise_jerk/piecewise_jerk_problem.h

@@ -31,7 +31,6 @@ namespace planning {
 
 /*
  * @brief:
- * FEM stands for finite element method.
  * This class solve an optimization problem:
  * x
  * |
@@ -47,8 +46,12 @@ namespace planning {
  * which makes the line P(start), P0, P(1) ... P(k-1) "smooth".
  */
 
-class Fem1dQpProblem {
+class PiecewiseJerkProblem {
  public:
+  PiecewiseJerkProblem() = default;
+
+  virtual ~PiecewiseJerkProblem() = default;
+
   /*
    * @param
    * x_init: the init status of x, x', x''
@@ -62,14 +65,10 @@ class Fem1dQpProblem {
    * -- w[4]: default reference line weight, (x_bounds[k].first +
    * x_bounds[k].second)/2
    */
-  Fem1dQpProblem(const size_t num_var, const std::array<double, 3>& x_init,
-                 const double delta_s, const std::array<double, 5>& w,
-                 const double max_x_third_order_derivative);
-
-  virtual ~Fem1dQpProblem() = default;
-
-  virtual void AddReferenceLineKernel(const std::vector<double>& ref_line,
-                                      const double wweight) {}
+  virtual void InitProblem(const size_t num_var,
+                           const std::array<double, 3>& x_init,
+                           const double delta_s, const std::array<double, 5>& w,
+                           const double max_x_third_order_derivative);
 
   virtual void ResetInitConditions(const std::array<double, 3>& x_init) {
     x_init_ = x_init;
@@ -102,8 +101,6 @@ class Fem1dQpProblem {
   virtual void SetVariableSecondOrderDerivativeBounds(
       const std::vector<std::tuple<double, double, double>>& ddx_bounds);
 
-  virtual void PreSetKernel() {}
-
   virtual bool Optimize(const int max_iter = 4000);
 
   const std::vector<double>& x() const { return x_; }

modules/planning/math/finite_element_qp/fem_1d_qp_problem_test.cc → modules/planning/math/piecewise_jerk/piecewise_jerk_problem_test.cc

@@ -30,12 +30,12 @@
 
 #define private public
 #define protected public
-#include "modules/planning/math/finite_element_qp/fem_1d_qp_problem.h"
+#include "modules/planning/math/piecewise_jerk/piesewise_jerk_problem.h"
 
 namespace apollo {
 namespace planning {
 
-TEST(Fem1dQPProblemTest, basic_test) {
+TEST(PiecewiseJerkProblemTest, basic_test) {
   FLAGS_enable_osqp_debug = true;
   std::array<double, 3> x_init = {1.5, 0.01, 0.001};
   double delta_s = 0.5;
@@ -53,8 +53,8 @@ TEST(Fem1dQPProblemTest, basic_test) {
   std::array<double, 5> w = {1.0, 2.0, 3.0, 4.0, 1.45};
   double max_x_third_order_derivative = 1.25;
 
-  std::unique_ptr<Fem1dQpProblem> fem_qp(
-      new Fem1dQpProblem(n, x_init, delta_s, w, max_x_third_order_derivative));
+  std::unique_ptr<PiecewiseJerkProblem> fem_qp(
+      new PiecewiseJerkProblem(n, x_init, delta_s, w, max_x_third_order_derivative));
 
   fem_qp->SetVariableBounds(x_bounds);
   fem_qp->SetFirstOrderBounds(FLAGS_lateral_derivative_bound_default);
@@ -74,7 +74,7 @@ TEST(Fem1dQPProblemTest, basic_test) {
   }
 }
 
-TEST(Fem1dQPProblemTest, add_bounds_test) {
+TEST(PiecewiseJerkProblemTest, add_bounds_test) {
   FLAGS_enable_osqp_debug = false;
   std::array<double, 3> x_init = {1.5, 0.01, 0.001};
   double delta_s = 0.5;
@@ -82,8 +82,8 @@ TEST(Fem1dQPProblemTest, add_bounds_test) {
   std::array<double, 5> w = {1.0, 2.0, 3.0, 4.0, 1.45};
   double max_x_third_order_derivative = 0.25;
 
-  std::unique_ptr<Fem1dQpProblem> fem_qp(
-      new Fem1dQpProblem(n, x_init, delta_s, w, max_x_third_order_derivative));
+  std::unique_ptr<PiecewiseJerkProblem> fem_qp(
+      new PiecewiseJerkProblem(n, x_init, delta_s, w, max_x_third_order_derivative));
 
   std::vector<std::tuple<double, double, double>> x_bounds;
   for (size_t i = 10; i < 20; ++i) {
@@ -101,7 +101,7 @@ TEST(Fem1dQPProblemTest, add_bounds_test) {
   }
 }
 
-TEST(Fem1dJerkQpProblemTest, derivative_constraint_test) {
+TEST(PiecewiseJerkProblemTest, derivative_constraint_test) {
   FLAGS_enable_osqp_debug = true;
   std::array<double, 3> x_init = {4.5, 0.00, 0.0};
   double delta_s = 0.5;
@@ -113,8 +113,8 @@ TEST(Fem1dJerkQpProblemTest, derivative_constraint_test) {
   std::array<double, 5> w = {1.0, 100.0, 1000.0, 1000.0, 0.0};
   double max_x_third_order_derivative = 2.0;
 
-  std::unique_ptr<Fem1dQpProblem> fem_qp(
-      new Fem1dQpProblem(n, x_init, delta_s, w, max_x_third_order_derivative));
+  std::unique_ptr<PiecewiseJerkProblem> fem_qp(
+      new PiecewiseJerkProblem(n, x_init, delta_s, w, max_x_third_order_derivative));
 
   fem_qp->SetVariableBounds(x_bounds);
   fem_qp->SetFirstOrderBounds(FLAGS_lateral_derivative_bound_default);