planning: wrap open space osqp warm up config into proto

modules/planning/conf/planner_open_space_config.pb.txt

@@ -42,6 +42,13 @@ dual_variable_warm_start_config {
   min_safety_distance: 0.0
   debug_osqp: false
   beta: 1.0
+  osqp_config {
+    alpha: 1.0
+    eps_abs: 1.0e-3
+    eps_rel: 1.0e-3
+    max_iter: 10000
+    polish: true
+  }
 }
 distance_approach_config {
   weight_steer: 0.3

modules/planning/conf/planning_config.pb.txt

@@ -159,6 +159,13 @@ default_task_config: {
           }
           qp_format: OSQP
           min_safety_distance: 0.01
+          osqp_config {
+            alpha: 1.0
+            eps_abs: 1.0e-3
+            eps_rel: 1.0e-3
+            max_iter: 10000
+            polish: true
+          }
         }
         distance_approach_config {
           weight_steer: 0.3

modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_osqp_interface.cc

@@ -72,6 +72,8 @@ DualVariableWarmStartOSQPInterface::DualVariableWarmStartOSQPInterface(
           .min_safety_distance();
   check_mode_ =
       planner_open_space_config.dual_variable_warm_start_config().debug_osqp();
+  osqp_config_ = planner_open_space_config.
+      dual_variable_warm_start_config().osqp_config();
 }
 
 void printMatrix(const int r, const int c, const std::vector<c_float>& P_data,
@@ -170,12 +172,11 @@ bool DualVariableWarmStartOSQPInterface::optimize() {
 
   // Define Solver settings as default
   osqp_set_default_settings(settings);
-  settings->alpha = 1.0;  // Change alpha parameter
-  settings->eps_abs = 1.0e-03;
-  settings->eps_rel = 1.0e-03;
-  settings->max_iter = 10000;
-  settings->polish = true;
-  settings->verbose = FLAGS_enable_osqp_debug;
+  settings->alpha = osqp_config_.alpha();  // Change alpha parameter
+  settings->eps_abs = osqp_config_.eps_abs();
+  settings->eps_rel = osqp_config_.eps_rel();
+  settings->max_iter = osqp_config_.max_iter();
+  settings->polish = osqp_config_.polish();
 
   // Populate data
   OSQPData* data = reinterpret_cast<OSQPData*>(c_malloc(sizeof(OSQPData)));

modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_osqp_interface.h

@@ -65,6 +65,7 @@ class DualVariableWarmStartOSQPInterface {
                       const Eigen::MatrixXd& n_warm_up);
 
  private:
+  OSQPConfig osqp_config_;
   int num_of_variables_;
   int num_of_constraints_;
   int horizon_;

modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_problem.cc

@@ -45,15 +45,14 @@ bool DualVariableWarmStartProblem::Solve(
 
   if (planner_open_space_config_.dual_variable_warm_start_config()
           .qp_format() == OSQP) {
-    DualVariableWarmStartOSQPInterface* ptop =
-        new DualVariableWarmStartOSQPInterface(
+    DualVariableWarmStartOSQPInterface ptop =
+        DualVariableWarmStartOSQPInterface(
             horizon, ts, ego, obstacles_edges_num, obstacles_num, obstacles_A,
             obstacles_b, xWS, planner_open_space_config_);
-    bool succ = ptop->optimize();
 
-    if (succ) {
+    if (ptop.optimize()) {
       ADEBUG << "dual warm up done.";
-      ptop->get_optimization_results(l_warm_up, n_warm_up);
+      ptop.get_optimization_results(l_warm_up, n_warm_up);
 
       auto t_end = cyber::Time::Now().ToSecond();
       ADEBUG << "Dual variable warm start solving time in second : "
@@ -62,20 +61,19 @@ bool DualVariableWarmStartProblem::Solve(
       solver_flag = true;
     } else {
       AWARN << "dual warm up fail.";
-      ptop->get_optimization_results(l_warm_up, n_warm_up);
+      ptop.get_optimization_results(l_warm_up, n_warm_up);
       solver_flag = false;
     }
   } else if (planner_open_space_config_.dual_variable_warm_start_config()
           .qp_format() == SLACKQP) {
-    DualVariableWarmStartSlackOSQPInterface* ptop =
-        new DualVariableWarmStartSlackOSQPInterface(
+    DualVariableWarmStartSlackOSQPInterface ptop =
+        DualVariableWarmStartSlackOSQPInterface(
             horizon, ts, ego, obstacles_edges_num, obstacles_num, obstacles_A,
             obstacles_b, xWS, planner_open_space_config_);
-    bool succ = ptop->optimize();
 
-    if (succ) {
+    if (ptop.optimize()) {
       ADEBUG << "dual warm up done.";
-      ptop->get_optimization_results(l_warm_up, n_warm_up);
+      ptop.get_optimization_results(l_warm_up, n_warm_up);
 
       auto t_end = cyber::Time::Now().ToSecond();
       ADEBUG << "Dual variable warm start solving time in second : "
@@ -84,7 +82,7 @@ bool DualVariableWarmStartProblem::Solve(
       solver_flag = true;
     } else {
       AWARN << "dual warm up fail.";
-      ptop->get_optimization_results(l_warm_up, n_warm_up);
+      ptop.get_optimization_results(l_warm_up, n_warm_up);
       solver_flag = false;
     }
   } else if (planner_open_space_config_.dual_variable_warm_start_config()

modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_slack_osqp_interface.cc

@@ -17,9 +17,9 @@
 /*
  * @file
  */
-#include <algorithm>
 #include "modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_slack_osqp_interface.h"
 
+#include <algorithm>
 #include "cyber/common/log.h"
 #include "modules/common/configs/vehicle_config_helper.h"
 #include "modules/common/math/math_utils.h"
@@ -78,6 +78,8 @@ DualVariableWarmStartSlackOSQPInterface::
   check_mode_ =
       planner_open_space_config.dual_variable_warm_start_config().debug_osqp();
   beta_ = planner_open_space_config.dual_variable_warm_start_config().beta();
+  osqp_config_ = planner_open_space_config.
+      dual_variable_warm_start_config().osqp_config();
 }
 
 void DualVariableWarmStartSlackOSQPInterface::printMatrix(
@@ -125,22 +127,23 @@ void DualVariableWarmStartSlackOSQPInterface::assembleA(
 }
 
 bool DualVariableWarmStartSlackOSQPInterface::optimize() {
-  int kNumParam = num_of_variables_;
-  int kNumConst = num_of_constraints_;
+  // int kNumParam = num_of_variables_;
+  // int kNumConst = num_of_constraints_;
 
   bool succ = true;
   // assemble P, quadratic term in objective
   std::vector<c_float> P_data;
   std::vector<c_int> P_indices;
   std::vector<c_int> P_indptr;
-  assemble_P(&P_data, &P_indices, &P_indptr);
+  assembleP(&P_data, &P_indices, &P_indptr);
   if (check_mode_) {
     AINFO << "print P_data in whole: ";
-    printMatrix(kNumParam, kNumParam, P_data, P_indices, P_indptr);
+    printMatrix(num_of_variables_, num_of_variables_,
+        P_data, P_indices, P_indptr);
   }
   // assemble q, linear term in objective, \sum{beta * slacks}
-  c_float q[kNumParam];
-  for (int i = 0; i < kNumParam; ++i) {
+  c_float q[num_of_variables_];  // NOLINT
+  for (int i = 0; i < num_of_variables_; ++i) {
     q[i] = 0.0;
     if (i >= s_start_index_) {
       q[i] = beta_;
@@ -151,18 +154,20 @@ bool DualVariableWarmStartSlackOSQPInterface::optimize() {
   std::vector<c_float> A_data;
   std::vector<c_int> A_indices;
   std::vector<c_int> A_indptr;
-  assemble_constraint(&A_data, &A_indices, &A_indptr);
+  assembleConstraint(&A_data, &A_indices, &A_indptr);
   if (check_mode_) {
     AINFO << "print A_data in whole: ";
-    printMatrix(kNumConst, kNumParam, A_data, A_indices, A_indptr);
-    assembleA(kNumConst, kNumParam, A_data, A_indices, A_indptr);
+    printMatrix(
+        num_of_constraints_, num_of_variables_, A_data, A_indices, A_indptr);
+    assembleA(
+        num_of_constraints_, num_of_variables_, A_data, A_indices, A_indptr);
   }
 
   // assemble lb & ub, slack_variable <= 0
-  c_float lb[kNumConst];
-  c_float ub[kNumConst];
-  int slack_indx = kNumConst - slack_horizon_;
-  for (int i = 0; i < kNumConst; ++i) {
+  c_float lb[num_of_constraints_];  // NOLINT
+  c_float ub[num_of_constraints_];  // NOLINT
+  int slack_indx = num_of_constraints_ - slack_horizon_;
+  for (int i = 0; i < num_of_constraints_; ++i) {
     lb[i] = 0.0;
     if (i >= slack_indx) {
       lb[i] = -2e19;
@@ -183,17 +188,16 @@ bool DualVariableWarmStartSlackOSQPInterface::optimize() {
 
   // Define Solver settings as default
   osqp_set_default_settings(settings);
-  settings->alpha = 1.0;  // Change alpha parameter
-  settings->eps_abs = 1.0e-03;
-  settings->eps_rel = 1.0e-03;
-  settings->max_iter = 10000;
-  settings->polish = true;
-  settings->verbose = FLAGS_enable_osqp_debug;
+  settings->alpha = osqp_config_.alpha();  // Change alpha parameter
+  settings->eps_abs = osqp_config_.eps_abs();
+  settings->eps_rel = osqp_config_.eps_rel();
+  settings->max_iter = osqp_config_.max_iter();
+  settings->polish = osqp_config_.polish();
 
   // Populate data
   OSQPData* data = reinterpret_cast<OSQPData*>(c_malloc(sizeof(OSQPData)));
-  data->n = kNumParam;
-  data->m = kNumConst;
+  data->n = num_of_variables_;
+  data->m = num_of_constraints_;
   data->P = csc_matrix(data->n, data->n, P_data.size(), P_data.data(),
                        P_indices.data(), P_indptr.data());
   data->q = q;
@@ -254,13 +258,13 @@ bool DualVariableWarmStartSlackOSQPInterface::optimize() {
   return succ;
 }
 
-void DualVariableWarmStartSlackOSQPInterface::check_solution(
+void DualVariableWarmStartSlackOSQPInterface::checkSolution(
     const Eigen::MatrixXd& l_warm_up, const Eigen::MatrixXd& n_warm_up) {
   // TODO(Runxin): extend
   return;
 }
 
-void DualVariableWarmStartSlackOSQPInterface::assemble_P(
+void DualVariableWarmStartSlackOSQPInterface::assembleP(
     std::vector<c_float>* P_data, std::vector<c_int>* P_indices,
     std::vector<c_int>* P_indptr) {
   // the objective function is norm(A' * lambda)
@@ -323,7 +327,7 @@ void DualVariableWarmStartSlackOSQPInterface::assemble_P(
   CHECK_EQ(P_indptr->size(), num_of_variables_ + 1);
 }
 
-void DualVariableWarmStartSlackOSQPInterface::assemble_constraint(
+void DualVariableWarmStartSlackOSQPInterface::assembleConstraint(
     std::vector<c_float>* A_data, std::vector<c_int>* A_indices,
     std::vector<c_int>* A_indptr) {
   /*
@@ -468,8 +472,8 @@ void DualVariableWarmStartSlackOSQPInterface::get_optimization_results(
     }
   }
 
-  LOG(INFO) << "max_s: " << std::to_string(max_s);
-  LOG(INFO) << "min_s: " << std::to_string(min_s);
+  ADEBUG << "max_s: " << std::to_string(max_s);
+  ADEBUG << "min_s: " << std::to_string(min_s);
 }
 }  // namespace planning
 }  // namespace apollo

modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_slack_osqp_interface.h

@@ -50,18 +50,18 @@ class DualVariableWarmStartSlackOSQPInterface {
 
   bool optimize();
 
-  void assemble_P(std::vector<c_float>* P_data, std::vector<c_int>* P_indices,
-                  std::vector<c_int>* P_indptr);
+  void assembleP(std::vector<c_float>* P_data, std::vector<c_int>* P_indices,
+                 std::vector<c_int>* P_indptr);
 
-  void assemble_constraint(std::vector<c_float>* A_data,
-                           std::vector<c_int>* A_indices,
-                           std::vector<c_int>* A_indptr);
+  void assembleConstraint(std::vector<c_float>* A_data,
+                          std::vector<c_int>* A_indices,
+                          std::vector<c_int>* A_indptr);
 
   void assembleA(const int r, const int c, const std::vector<c_float>& P_data,
                  const std::vector<c_int>& P_indices,
                  const std::vector<c_int>& P_indptr);
 
-  void check_solution(const Eigen::MatrixXd& l_warm_up,
+  void checkSolution(const Eigen::MatrixXd& l_warm_up,
                       const Eigen::MatrixXd& n_warm_up);
 
   void printMatrix(const int r, const int c,
@@ -70,6 +70,8 @@ class DualVariableWarmStartSlackOSQPInterface {
       const std::vector<c_int>& P_indptr);
 
  private:
+  OSQPConfig osqp_config_;
+
   int num_of_variables_;
   int num_of_constraints_;
   int horizon_;

modules/planning/proto/planner_open_space_config.proto

@@ -75,6 +75,7 @@ message DualVariableWarmStartConfig {
   optional double min_safety_distance = 4 [default = 0.0];
   optional bool debug_osqp = 5 [default = false];
   optional double beta = 6 [default = 1.0];
+  optional OSQPConfig osqp_config = 7;
 }
 
 message DistanceApproachConfig {
@@ -129,6 +130,15 @@ message IpoptConfig {
   // ipopt barrier parameter, default 0.1
 }
 
+// Dual variable configs for OSQP
+message OSQPConfig {
+  optional double alpha = 1 [default = 1.0];
+  optional double eps_abs = 2 [default = 1.0e-3];
+  optional double eps_rel = 3 [default = 1.0e-3];
+  optional int32 max_iter = 4 [default = 10000];
+  optional bool polish = 5 [default = true];
+}
+
 message IterativeAnchoringConfig {
   // Ipopt configs
   optional double interpolated_delta_s = 1 [default = 0.1];