Planning: Added back all three version os osqp solver for lateral qp problem.

modules/planning/lattice/trajectory_generation/trajectory1d_generator.cc

@@ -142,7 +142,7 @@ void Trajectory1dGenerator::GenerateLateralTrajectoryBundle(
 
     // LateralTrajectoryOptimizer lateral_optimizer;
     std::unique_ptr<LateralQPOptimizer> lateral_optimizer(
-        new ActiverSetLateralQPOptimizer);
+        new ActiveSetLateralQPOptimizer);
 
     lateral_optimizer->optimize(init_lat_state_, delta_s, lateral_bounds);
 

modules/planning/math/finite_element_qp/BUILD

@@ -56,6 +56,21 @@ cc_library(
     ],
 )
 
+cc_library(
+    name = "osqp_lateral_linear_qp_optimizer",
+    srcs = [
+        "osqp_lateral_linear_qp_optimizer.cc",
+    ],
+    hdrs = [
+        "osqp_lateral_linear_qp_optimizer.h",
+    ],
+    deps = [
+        ":lateral_qp_optimizer",
+        "@eigen",
+        "@osqp",
+    ],
+)
+
 cc_library(
     name = "active_set_augmented_lateral_qp_optimizer",
     srcs = [

modules/planning/math/finite_element_qp/active_set_augmented_lateral_qp_optimizer.cc

@@ -24,7 +24,7 @@
 namespace apollo {
 namespace planning {
 
-bool ActiverSetAugmentedLateralQPOptimizer::optimize(
+bool ActiveSetAugmentedLateralQPOptimizer::optimize(
     const std::array<double, 3>& d_state, const double delta_s,
     const std::vector<std::pair<double, double>>& d_bounds) {
   opt_d_.clear();

modules/planning/math/finite_element_qp/active_set_augmented_lateral_qp_optimizer.h

@@ -31,11 +31,11 @@
 namespace apollo {
 namespace planning {
 
-class ActiverSetAugmentedLateralQPOptimizer : public LateralQPOptimizer {
+class ActiveSetAugmentedLateralQPOptimizer : public LateralQPOptimizer {
  public:
-  ActiverSetAugmentedLateralQPOptimizer() = default;
+  ActiveSetAugmentedLateralQPOptimizer() = default;
 
-  virtual ~ActiverSetAugmentedLateralQPOptimizer() = default;
+  virtual ~ActiveSetAugmentedLateralQPOptimizer() = default;
 
   bool optimize(
       const std::array<double, 3>& d_state, const double delta_s,

modules/planning/math/finite_element_qp/active_set_lateral_qp_optimizer.cc

@@ -24,7 +24,7 @@
 namespace apollo {
 namespace planning {
 
-bool ActiverSetLateralQPOptimizer::optimize(
+bool ActiveSetLateralQPOptimizer::optimize(
     const std::array<double, 3>& d_state, const double delta_s,
     const std::vector<std::pair<double, double>>& d_bounds) {
   delta_s_ = delta_s;

modules/planning/math/finite_element_qp/active_set_lateral_qp_optimizer.h

@@ -31,11 +31,11 @@
 namespace apollo {
 namespace planning {
 
-class ActiverSetLateralQPOptimizer : public LateralQPOptimizer {
+class ActiveSetLateralQPOptimizer : public LateralQPOptimizer {
  public:
-  ActiverSetLateralQPOptimizer() = default;
+  ActiveSetLateralQPOptimizer() = default;
 
-  virtual ~ActiverSetLateralQPOptimizer() = default;
+  virtual ~ActiveSetLateralQPOptimizer() = default;
 
   bool optimize(
       const std::array<double, 3>& d_state, const double delta_s,

modules/planning/math/finite_element_qp/osqp_lateral_linear_qp_optimizer.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/finite_element_qp/osqp_lateral_linear_qp_optimizer.h"
+
+#include <algorithm>
+
+#include "cybertron/common/log.h"
+#include "modules/common/math/matrix_operations.h"
+#include "modules/planning/common/planning_gflags.h"
+
+namespace apollo {
+namespace planning {
+
+using Eigen::MatrixXd;
+using apollo::common::math::DenseToCSCMatrix;
+
+bool OsqpLateralLinearQPOptimizer::optimize(
+    const std::array<double, 3>& d_state, const double delta_s,
+    const std::vector<std::pair<double, double>>& d_bounds) {
+  // clean up old results
+  opt_d_.clear();
+  opt_d_prime_.clear();
+  opt_d_pprime_.clear();
+
+  // kernel
+  std::vector<c_float> P_data_;
+  std::vector<c_int> P_indices_;
+  std::vector<c_int> P_indptr_;
+  CalcualteKernel(d_bounds, delta_s, &P_data_, &P_indices_, &P_indptr_);
+
+  const int kNumVariable = d_bounds.size();
+
+  delta_s_ = delta_s;
+  const int kNumConstraint = kNumVariable + (kNumVariable - 3);
+
+  MatrixXd affine_constraint = MatrixXd::Zero(kNumConstraint, kNumVariable);
+  c_float lower_bounds[kNumConstraint];
+  c_float upper_bounds[kNumConstraint];
+  std::fill(lower_bounds, lower_bounds + kNumConstraint, 0.0);
+  std::fill(upper_bounds, upper_bounds + kNumConstraint, 0.0);
+
+  int constraint_index = 0;
+
+  for (int i = 0; i < kNumVariable; ++i) {
+    affine_constraint(i, i) = 1.0;
+    lower_bounds[constraint_index] = d_bounds[i].first;
+    upper_bounds[constraint_index] = d_bounds[i].second;
+    ++constraint_index;
+  }
+
+  const double third_order_derivative_max_coff =
+      FLAGS_lateral_third_order_derivative_max * delta_s * delta_s * delta_s;
+
+  for (int i = 0; i + 3 < kNumVariable; ++i) {
+    affine_constraint(constraint_index, i) = -1.0;
+    affine_constraint(constraint_index, i + 1) = 3.0;
+    affine_constraint(constraint_index, i + 2) = -3.0;
+    affine_constraint(constraint_index, i + 3) = 1.0;
+    lower_bounds[constraint_index] = -third_order_derivative_max_coff;
+    upper_bounds[constraint_index] = third_order_derivative_max_coff;
+    ++constraint_index;
+  }
+
+  CHECK_EQ(constraint_index, kNumConstraint);
+
+  // change affine_constraint to CSC format
+  std::vector<c_float> A_data_;
+  std::vector<c_int> A_indices_;
+  std::vector<c_int> A_indptr_;
+  DenseToCSCMatrix(affine_constraint, &A_data_, &A_indices_, &A_indptr_);
+
+  // offset
+  double q[kNumVariable];
+  for (int i = 0; i < kNumVariable; ++i) {
+    q[i] = -2.0 * FLAGS_weight_lateral_obstacle_distance *
+           (d_bounds[i].first + d_bounds[i].second);
+  }
+
+  // Problem settings
+  OSQPSettings* settings =
+      reinterpret_cast<OSQPSettings*>(c_malloc(sizeof(OSQPSettings)));
+
+  // 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 = true;
+
+  // Populate data
+  OSQPData* data = reinterpret_cast<OSQPData*>(c_malloc(sizeof(OSQPData)));
+  data->n = kNumVariable;
+  data->m = kNumConstraint;
+  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 = lower_bounds;
+  data->u = upper_bounds;
+
+  // Workspace
+  OSQPWorkspace* work = osqp_setup(data, settings);
+
+  // Solve Problem
+  osqp_solve(work);
+
+  // extract primal results
+  for (int i = 0; i < kNumVariable; ++i) {
+    opt_d_.push_back(work->solution->x[i]);
+    if (i > 0) {
+      opt_d_prime_.push_back((work->solution->x[i] - work->solution->x[i - 1]) /
+                             delta_s);
+    }
+    if (i > 1) {
+      const double t = work->solution->x[i] - 2 * work->solution->x[i - 1] +
+                       work->solution->x[i - 2];
+      opt_d_pprime_.push_back(t / (delta_s * delta_s));
+    }
+  }
+  opt_d_prime_.push_back(0.0);
+  opt_d_pprime_.push_back(0.0);
+
+  // Cleanup
+  osqp_cleanup(work);
+  c_free(data->A);
+  c_free(data->P);
+  c_free(data);
+  c_free(settings);
+
+  return true;
+}
+
+void OsqpLateralLinearQPOptimizer::CalcualteKernel(
+    const std::vector<std::pair<double, double>>& d_bounds,
+    const double delta_s, std::vector<c_float>* P_data,
+    std::vector<c_int>* P_indices, std::vector<c_int>* P_indptr) {
+  const int kNumVariable = d_bounds.size();
+
+  // const int kNumOfMatrixElement = kNumVariable * kNumVariable;
+  MatrixXd kernel = MatrixXd::Zero(kNumVariable, kNumVariable);  // dense matrix
+
+  // pre-calculate some const
+  const double delta_s_sq = delta_s * delta_s;
+  const double delta_s_quad = delta_s_sq * delta_s_sq;
+  const double one_over_delta_s_sq_coeff =
+      1.0 / delta_s_sq * FLAGS_weight_lateral_derivative;
+  const double one_over_delta_s_quad_coeff =
+      1.0 / delta_s_quad * FLAGS_weight_lateral_second_order_derivative;
+
+  for (int i = 0; i < kNumVariable; ++i) {
+    kernel(i, i) += 2.0 * FLAGS_weight_lateral_offset;
+    kernel(i, i) += 2.0 * FLAGS_weight_lateral_obstacle_distance;
+
+    // first order derivative
+    if (i + 1 < kNumVariable) {
+      kernel(i + 1, i + 1) += 2.0 * one_over_delta_s_sq_coeff;
+      kernel(i, i) += 2.0 * one_over_delta_s_sq_coeff;
+      kernel(i, i + 1) += 2.0 * one_over_delta_s_sq_coeff;
+      kernel(i + 1, i) += 2.0 * one_over_delta_s_sq_coeff;
+    }
+
+    // second order derivative
+    if (i + 2 < kNumVariable) {
+      kernel(i + 2, i + 2) += 2.0 * one_over_delta_s_quad_coeff;
+      kernel(i + 1, i + 1) += 8.0 * one_over_delta_s_quad_coeff;
+      kernel(i, i) += 2.0 * one_over_delta_s_quad_coeff;
+
+      kernel(i, i + 1) += -4.0 * one_over_delta_s_quad_coeff;
+      kernel(i + 1, i) += -4.0 * one_over_delta_s_quad_coeff;
+
+      kernel(i + 1, i + 2) += -4.0 * one_over_delta_s_quad_coeff;
+      kernel(i + 2, i + 1) += -4.0 * one_over_delta_s_quad_coeff;
+
+      kernel(i, i + 2) += 2.0 * one_over_delta_s_quad_coeff;
+      kernel(i + 2, i) += 2.0 * one_over_delta_s_quad_coeff;
+    }
+  }
+
+  DenseToCSCMatrix(kernel, P_data, P_indices, P_indptr);
+}
+
+}  // namespace planning
+}  // namespace apollo

modules/planning/math/finite_element_qp/osqp_lateral_linear_qp_optimizer.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 <array>
+#include <utility>
+#include <vector>
+
+#include "Eigen/Core"
+#include "osqp/include/osqp.h"
+
+#include "modules/planning/lattice/trajectory1d/piecewise_jerk_trajectory1d.h"
+#include "modules/planning/math/finite_element_qp/lateral_qp_optimizer.h"
+
+namespace apollo {
+namespace planning {
+
+class OsqpLateralLinearQPOptimizer : public LateralQPOptimizer {
+ public:
+  OsqpLateralLinearQPOptimizer() = default;
+
+  virtual ~OsqpLateralLinearQPOptimizer() = default;
+
+  bool optimize(
+      const std::array<double, 3>& d_state, const double delta_s,
+      const std::vector<std::pair<double, double>>& d_bounds) override;
+
+ private:
+  void CalcualteKernel(const std::vector<std::pair<double, double>>& d_bounds,
+                       const double delta_s, std::vector<c_float>* P_data,
+                       std::vector<c_int>* P_indices,
+                       std::vector<c_int>* P_indptr);
+};
+
+}  // namespace planning
+}  // namespace apollo

modules/planning/math/finite_element_qp/osqp_lateral_qp_optimizer.cc

@@ -31,63 +31,108 @@ using apollo::common::math::DenseToCSCMatrix;
 bool OsqpLateralQPOptimizer::optimize(
     const std::array<double, 3>& d_state, const double delta_s,
     const std::vector<std::pair<double, double>>& d_bounds) {
-  // clean up old results
-  opt_d_.clear();
-  opt_d_prime_.clear();
-  opt_d_pprime_.clear();
-
-  // kernel
-  std::vector<c_float> P_data_;
-  std::vector<c_int> P_indices_;
-  std::vector<c_int> P_indptr_;
-  CalcualteKernel(d_bounds, delta_s, &P_data_, &P_indices_, &P_indptr_);
-
-  const int kNumVariable = d_bounds.size();
+  std::vector<c_float> P_data;
+  std::vector<c_int> P_indices;
+  std::vector<c_int> P_indptr;
+  CalcualteKernel(d_bounds, &P_data, &P_indices, &P_indptr);
 
   delta_s_ = delta_s;
-  const int kNumConstraint = kNumVariable + (kNumVariable - 3);
-
-  MatrixXd affine_constraint = MatrixXd::Zero(kNumConstraint, kNumVariable);
+  const int num_var = d_bounds.size();
+  const int kNumParam = 3 * d_bounds.size();
+  const int kNumConstraint = 3 * (num_var - 1) + 5;
+  MatrixXd affine_constraint = MatrixXd::Zero(kNumParam, kNumConstraint);
+  // const int kNumOfConstraint = kNumParam * kNumConstraint;
   c_float lower_bounds[kNumConstraint];
   c_float upper_bounds[kNumConstraint];
-  std::fill(lower_bounds, lower_bounds + kNumConstraint, 0.0);
-  std::fill(upper_bounds, upper_bounds + kNumConstraint, 0.0);
 
+  const int prime_offset = num_var;
+  const int pprime_offset = 2 * num_var;
   int constraint_index = 0;
 
-  for (int i = 0; i < kNumVariable; ++i) {
-    affine_constraint(i, i) = 1.0;
-    lower_bounds[constraint_index] = d_bounds[i].first;
-    upper_bounds[constraint_index] = d_bounds[i].second;
+  // d_i+1'' - d_i''
+  for (int i = 0; i + 1 < num_var; ++i) {
+    const int row = constraint_index;
+    const int col = pprime_offset + i;
+    affine_constraint(row, col) = -1.0;
+    affine_constraint(row, col + 1) = 1.0;
+
+    lower_bounds[constraint_index] =
+        -FLAGS_lateral_third_order_derivative_max * delta_s;
+    upper_bounds[constraint_index] =
+        FLAGS_lateral_third_order_derivative_max * delta_s;
     ++constraint_index;
   }
 
-  const double third_order_derivative_max_coff =
-      FLAGS_lateral_third_order_derivative_max * delta_s * delta_s * delta_s;
+  // d_i+1' - d_i' - 0.5 * ds * (d_i'' + d_i+1'')
+  for (int i = 0; i + 1 < num_var; ++i) {
+    affine_constraint(constraint_index, prime_offset + i) = -1.0;
+    affine_constraint(constraint_index, prime_offset + i + 1) = 1.0;
+
+    affine_constraint(constraint_index, pprime_offset + i) = -0.5 * delta_s;
+    affine_constraint(constraint_index, pprime_offset + i + 1) = -0.5 * delta_s;
+
+    lower_bounds[constraint_index] = 0.0;
+    upper_bounds[constraint_index] = 0.0;
+    ++constraint_index;
+  }
 
-  for (int i = 0; i + 3 < kNumVariable; ++i) {
+  // d_i+1 - d_i - d_i' * ds - 1/3 * d_i'' * ds^2 - 1/6 * d_i+1'' * ds^2
+  for (int i = 0; i + 1 < num_var; ++i) {
     affine_constraint(constraint_index, i) = -1.0;
-    affine_constraint(constraint_index, i + 1) = 3.0;
-    affine_constraint(constraint_index, i + 2) = -3.0;
-    affine_constraint(constraint_index, i + 3) = 1.0;
-    lower_bounds[constraint_index] = -third_order_derivative_max_coff;
-    upper_bounds[constraint_index] = third_order_derivative_max_coff;
+    affine_constraint(constraint_index, i + 1) = 1.0;
+
+    affine_constraint(constraint_index, prime_offset + i) = -delta_s;
+
+    affine_constraint(constraint_index, pprime_offset + i) =
+        -delta_s * delta_s / 3.0;
+    affine_constraint(constraint_index, pprime_offset + i + 1) =
+        -delta_s * delta_s / 6.0;
+
+    lower_bounds[constraint_index] = 0.0;
+    upper_bounds[constraint_index] = 0.0;
     ++constraint_index;
   }
 
-  CHECK_EQ(constraint_index, kNumConstraint);
+  affine_constraint(constraint_index, 0) = 1.0;
+  lower_bounds[constraint_index] = d_state[0];
+  upper_bounds[constraint_index] = d_state[0];
+  ++constraint_index;
+
+  affine_constraint(constraint_index, prime_offset) = 1.0;
+  lower_bounds[constraint_index] = d_state[1];
+  upper_bounds[constraint_index] = d_state[1];
+  ++constraint_index;
+
+  affine_constraint(constraint_index, pprime_offset) = 1.0;
+  lower_bounds[constraint_index] = d_state[2];
+  upper_bounds[constraint_index] = d_state[2];
+  ++constraint_index;
+
+  affine_constraint(constraint_index, prime_offset + num_var - 1) = 1.0;
+  lower_bounds[constraint_index] = 0.0;
+  upper_bounds[constraint_index] = 0.0;
+  ++constraint_index;
+
+  affine_constraint(constraint_index, pprime_offset + num_var - 1) = 1.0;
+  lower_bounds[constraint_index] = 0.0;
+  upper_bounds[constraint_index] = 0.0;
+  ++constraint_index;
 
   // change affine_constraint to CSC format
-  std::vector<c_float> A_data_;
-  std::vector<c_int> A_indices_;
-  std::vector<c_int> A_indptr_;
-  DenseToCSCMatrix(affine_constraint, &A_data_, &A_indices_, &A_indptr_);
+  std::vector<c_float> A_data;
+  std::vector<c_int> A_indices;
+  std::vector<c_int> A_indptr;
+  DenseToCSCMatrix(affine_constraint, &A_data, &A_indices, &A_indptr);
 
   // offset
-  double q[kNumVariable];
-  for (int i = 0; i < kNumVariable; ++i) {
-    q[i] = -2.0 * FLAGS_weight_lateral_obstacle_distance *
-           (d_bounds[i].first + d_bounds[i].second);
+  double q[kNumParam];
+  for (int i = 0; i < kNumParam; ++i) {
+    if (i < num_var) {
+      q[i] = -2.0 * FLAGS_weight_lateral_obstacle_distance *
+             (d_bounds[i].first + d_bounds[i].second);
+    } else {
+      q[i] = 0.0;
+    }
   }
 
   // Problem settings
@@ -101,17 +146,17 @@ bool OsqpLateralQPOptimizer::optimize(
   settings->eps_rel = 1.0e-05;
   settings->max_iter = 5000;
   settings->polish = true;
-  settings->verbose = true;
+  settings->verbose = FLAGS_enable_osqp_debug;
 
   // Populate data
   OSQPData* data = reinterpret_cast<OSQPData*>(c_malloc(sizeof(OSQPData)));
-  data->n = kNumVariable;
-  data->m = kNumConstraint;
-  data->P = csc_matrix(data->n, data->n, P_data_.size(), P_data_.data(),
-                       P_indices_.data(), P_indptr_.data());
+  data->n = kNumParam;
+  data->m = affine_constraint.rows();
+  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->A = csc_matrix(data->m, data->n, A_data.size(), A_data.data(),
+                       A_indices.data(), A_indptr.data());
   data->l = lower_bounds;
   data->u = upper_bounds;
 
@@ -122,20 +167,13 @@ bool OsqpLateralQPOptimizer::optimize(
   osqp_solve(work);
 
   // extract primal results
-  for (int i = 0; i < kNumVariable; ++i) {
+  for (int i = 0; i < num_var; ++i) {
     opt_d_.push_back(work->solution->x[i]);
-    if (i > 0) {
-      opt_d_prime_.push_back((work->solution->x[i] - work->solution->x[i - 1]) /
-                             delta_s);
-    }
-    if (i > 1) {
-      const double t = work->solution->x[i] - 2 * work->solution->x[i - 1] +
-                       work->solution->x[i - 2];
-      opt_d_pprime_.push_back(t / (delta_s * delta_s));
-    }
+    opt_d_prime_.push_back(work->solution->x[i + num_var]);
+    opt_d_pprime_.push_back(work->solution->x[i + 2 * num_var]);
   }
-  opt_d_prime_.push_back(0.0);
-  opt_d_pprime_.push_back(0.0);
+  opt_d_prime_[num_var - 1] = 0.0;
+  opt_d_pprime_[num_var - 1] = 0.0;
 
   // Cleanup
   osqp_cleanup(work);
@@ -146,50 +184,23 @@ bool OsqpLateralQPOptimizer::optimize(
 
   return true;
 }
-
 void OsqpLateralQPOptimizer::CalcualteKernel(
     const std::vector<std::pair<double, double>>& d_bounds,
-    const double delta_s, std::vector<c_float>* P_data,
-    std::vector<c_int>* P_indices, std::vector<c_int>* P_indptr) {
-  const int kNumVariable = d_bounds.size();
-
-  // const int kNumOfMatrixElement = kNumVariable * kNumVariable;
-  MatrixXd kernel = MatrixXd::Zero(kNumVariable, kNumVariable);  // dense matrix
-
-  // pre-calculate some const
-  const double delta_s_sq = delta_s * delta_s;
-  const double delta_s_quad = delta_s_sq * delta_s_sq;
-  const double one_over_delta_s_sq_coeff =
-      1.0 / delta_s_sq * FLAGS_weight_lateral_derivative;
-  const double one_over_delta_s_quad_coeff =
-      1.0 / delta_s_quad * FLAGS_weight_lateral_second_order_derivative;
-
-  for (int i = 0; i < kNumVariable; ++i) {
-    kernel(i, i) += 2.0 * FLAGS_weight_lateral_offset;
-    kernel(i, i) += 2.0 * FLAGS_weight_lateral_obstacle_distance;
-
-    // first order derivative
-    if (i + 1 < kNumVariable) {
-      kernel(i + 1, i + 1) += 2.0 * one_over_delta_s_sq_coeff;
-      kernel(i, i) += 2.0 * one_over_delta_s_sq_coeff;
-      kernel(i, i + 1) += 2.0 * one_over_delta_s_sq_coeff;
-      kernel(i + 1, i) += 2.0 * one_over_delta_s_sq_coeff;
-    }
-
-    // second order derivative
-    if (i + 2 < kNumVariable) {
-      kernel(i + 2, i + 2) += 2.0 * one_over_delta_s_quad_coeff;
-      kernel(i + 1, i + 1) += 8.0 * one_over_delta_s_quad_coeff;
-      kernel(i, i) += 2.0 * one_over_delta_s_quad_coeff;
-
-      kernel(i, i + 1) += -4.0 * one_over_delta_s_quad_coeff;
-      kernel(i + 1, i) += -4.0 * one_over_delta_s_quad_coeff;
-
-      kernel(i + 1, i + 2) += -4.0 * one_over_delta_s_quad_coeff;
-      kernel(i + 2, i + 1) += -4.0 * one_over_delta_s_quad_coeff;
-
-      kernel(i, i + 2) += 2.0 * one_over_delta_s_quad_coeff;
-      kernel(i + 2, i) += 2.0 * one_over_delta_s_quad_coeff;
+    std::vector<c_float>* P_data, std::vector<c_int>* P_indices,
+    std::vector<c_int>* P_indptr) {
+  const int kNumParam = 3 * d_bounds.size();
+
+  // const int kNumOfMatrixElement = kNumParam * kNumParam;
+  MatrixXd kernel = MatrixXd::Zero(kNumParam, kNumParam);  // dense matrix
+
+  for (int i = 0; i < kNumParam; ++i) {
+    if (i < static_cast<int>(d_bounds.size())) {
+      kernel(i, i) = 2.0 * FLAGS_weight_lateral_offset +
+                     2.0 * FLAGS_weight_lateral_obstacle_distance;
+    } else if (i < 2 * static_cast<int>(d_bounds.size())) {
+      kernel(i, i) = 2.0 * FLAGS_weight_lateral_derivative;
+    } else {
+      kernel(i, i) = 2.0 * FLAGS_weight_lateral_second_order_derivative;
     }
   }
 

modules/planning/math/finite_element_qp/osqp_lateral_qp_optimizer.h

@@ -45,9 +45,10 @@ class OsqpLateralQPOptimizer : public LateralQPOptimizer {
 
  private:
   void CalcualteKernel(const std::vector<std::pair<double, double>>& d_bounds,
-                       const double delta_s, std::vector<c_float>* P_data,
+                       std::vector<c_float>* P_data,
                        std::vector<c_int>* P_indices,
                        std::vector<c_int>* P_indptr);
+  double delta_s_ = 0.0;
 };
 
 }  // namespace planning

modules/planning/toolkits/optimizers/qp_piecewise_jerk_path/qp_piecewise_jerk_path_optimizer.cc

@@ -82,8 +82,8 @@ bool QpPiecewiseJerkPathOptimizer::Init(
     config_ = config.qp_piecewise_jerk_path_config();
   }
   // TODO(all): use gflags or config to turn on/off new algorithms
-  // lateral_qp_optimizer_.reset(new ActiverSetLateralQPOptimizer());
-  // lateral_qp_optimizer_.reset(new ActiverSetAugmentedLateralQPOptimizer());
+  // lateral_qp_optimizer_.reset(new ActiveSetLateralQPOptimizer());
+  // lateral_qp_optimizer_.reset(new ActiveSetAugmentedLateralQPOptimizer());
   // lateral_qp_optimizer_.reset(new OsqpLateralJerkQPOptimizer());
   lateral_qp_optimizer_.reset(new OsqpLateralQPOptimizer());