Planning: add path_time_qp problem

modules/planning/math/piecewise_jerk/BUILD

@@ -42,4 +42,17 @@ cc_library(
         ":piecewise_jerk_problem",
     ],
 )
+
+cc_library(
+    name = "path_time_qp_problem",
+    srcs = [
+        "path_time_qp_problem.cc",
+    ],
+    hdrs = [
+        "path_time_qp_problem.h",
+    ],
+    deps = [
+        ":piecewise_jerk_problem",
+    ],
+)
 cpplint()

modules/planning/math/piecewise_jerk/path_time_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/path_time_qp_problem.h"
+
+#include <algorithm>
+
+#include "cyber/common/log.h"
+
+#include "modules/planning/common/planning_gflags.h"
+
+namespace apollo {
+namespace planning {
+
+void PathTimeQpProblem::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 = 2 * N;
+  std::vector<std::vector<std::pair<c_int, c_float>>> columns;
+  columns.resize(kNumParam);
+  int value_index = 0;
+
+  // 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;
+  }
+
+  // x(n-1)^2 * w_x
+  columns[N - 1].emplace_back(N - 1, weight_.x_w);
+  ++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 PathTimeQpProblem::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) {
+    q->at(i) = 0.0;
+  }
+}
+
+}  // namespace planning
+}  // namespace apollo

modules/planning/math/piecewise_jerk/path_time_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:
+ * This class solve the path time optimization problem:
+ * s
+ * |
+ * |                       P(t1, s1)  P(t2, s2)
+ * |            P(t0, s0)                       ... P(t(k-1), s(k-1))
+ * |P(start)
+ * |
+ * |________________________________________________________ t
+ *
+ * we suppose t(k+1) - t(k) == t(k) - t(k-1)
+ *
+ * Given the s, s', s'' at P(start),  The goal is to find t0, t1, ... t(k-1)
+ * which makes the line P(start), P0, P(1) ... P(k-1) "smooth".
+ */
+
+class PathTimeQpProblem : public PiecewiseJerkProblem {
+ public:
+  PathTimeQpProblem() = default;
+
+  virtual ~PathTimeQpProblem() = 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