planning: introduce dual warm up debug mode to compare three warm ups in open space

modules/planning/conf/planner_open_space_config.pb.txt

@@ -32,6 +32,7 @@ dual_variable_warm_start_config {
     ipopt_recalc_y: "yes"
   }
   qp_format: OSQP
+  min_safety_distance: 0.0
 }
 distance_approach_config {
   weight_steer: 0.3

modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_ipopt_interface.cc

@@ -115,7 +115,7 @@ bool DualVariableWarmStartIPOPTInterface::get_starting_point(
   // 1. lagrange constraint l, obstacles_edges_sum_ * (horizon_+1)
   for (int i = 0; i < horizon_ + 1; ++i) {
     for (int j = 0; j < obstacles_edges_sum_; ++j) {
-      x[l_index] = 0.5;
+      x[l_index] = 0.0;
       ++l_index;
     }
   }
@@ -123,7 +123,7 @@ bool DualVariableWarmStartIPOPTInterface::get_starting_point(
   // 2. lagrange constraint n, 4*obstacles_num * (horizon_+1)
   for (int i = 0; i < horizon_ + 1; ++i) {
     for (int j = 0; j < 4 * obstacles_num_; ++j) {
-      x[n_index] = 1.0;
+      x[n_index] = 0.0;
       ++n_index;
     }
   }

modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_osqp_interface.cc

@@ -66,11 +66,16 @@ DualVariableWarmStartOSQPInterface::DualVariableWarmStartOSQPInterface(
   num_of_variables_ = lambda_horizon_ + miu_horizon_;
   // number of constraints
   num_of_constraints_ = 3 * obstacles_num_ * (horizon_ + 1) + num_of_variables_;
+
+  min_safety_distance_ = planner_open_space_config.
+      dual_variable_warm_start_config().min_safety_distance();
 }
 
 bool DualVariableWarmStartOSQPInterface::optimize() {
   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;
@@ -96,7 +101,7 @@ bool DualVariableWarmStartOSQPInterface::optimize() {
     lb[i] = 0.0;
     if (i >= 2 * obstacles_num_ * (horizon_ + 1) &&
         i < 3 * obstacles_num_ * (horizon_ + 1)) {
-      lb[i] = 0.0;
+      lb[i] = min_safety_distance_;
     }
     if (i < 2 * obstacles_num_ * (horizon_ + 1)) {
       ub[i] = 0.0;
@@ -141,7 +146,7 @@ bool DualVariableWarmStartOSQPInterface::optimize() {
       AERROR << "OSQP dual warm up unsuccess, "
           << "return status: "
           << work->info->status;
-      return false;
+      succ = false;
   }
 
   // extract primal results
@@ -164,6 +169,8 @@ bool DualVariableWarmStartOSQPInterface::optimize() {
     }
   }
 
+  succ = succ & (work->info->obj_val <= 1.0);
+
   // Cleanup
   osqp_cleanup(work);
   c_free(data->A);
@@ -171,7 +178,7 @@ bool DualVariableWarmStartOSQPInterface::optimize() {
   c_free(data);
   c_free(settings);
 
-  return true;
+  return succ;
 }
 
 void DualVariableWarmStartOSQPInterface::assemble_P(

modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_osqp_interface.h

@@ -79,6 +79,8 @@ class DualVariableWarmStartOSQPInterface {
   int obstacles_num_;
   int obstacles_edges_sum_;
 
+  double min_safety_distance_;
+
   // lagrangian l start index
   int l_start_index_ = 0;
 

modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_problem.cc

@@ -135,7 +135,8 @@ bool DualVariableWarmStartProblem::Solve(
 
     return status == Ipopt::Solve_Succeeded ||
            status == Ipopt::Solved_To_Acceptable_Level;
-  } else {
+  } else if (planner_open_space_config_.
+      dual_variable_warm_start_config().qp_format() == IPOPT) {
     DualVariableWarmStartIPOPTInterface* ptop =
         new DualVariableWarmStartIPOPTInterface(
             horizon, ts, ego, obstacles_edges_num, obstacles_num, obstacles_A,
@@ -203,6 +204,175 @@ bool DualVariableWarmStartProblem::Solve(
 
     return status == Ipopt::Solve_Succeeded ||
            status == Ipopt::Solved_To_Acceptable_Level;
+  } else {  // debug mode
+    DualVariableWarmStartOSQPInterface* ptop_osqp =
+        new DualVariableWarmStartOSQPInterface(
+            horizon, ts, ego, obstacles_edges_num, obstacles_num, obstacles_A,
+            obstacles_b, xWS, planner_open_space_config_);
+    bool succ = ptop_osqp->optimize();
+
+    if (!succ) {
+      AERROR << "dual warm up fail.";
+      ptop_osqp->get_optimization_results(l_warm_up, n_warm_up);
+    }
+
+    AINFO << "dual warm up done.";
+    ptop_osqp->get_optimization_results(l_warm_up, n_warm_up);
+
+    auto t_end = cyber::Time::Now().ToSecond();
+    AINFO << "Dual vairable warm start solving time in second : "
+        << t_end - t_start;
+
+    // ipoptqp result
+    Eigen::MatrixXd l_warm_up_ipoptqp(l_warm_up->rows(), l_warm_up->cols());
+    Eigen::MatrixXd n_warm_up_ipoptqp(n_warm_up->rows(), n_warm_up->cols());
+
+    DualVariableWarmStartIPOPTQPInterface* ptop_ipoptqp =
+        new DualVariableWarmStartIPOPTQPInterface(
+            horizon, ts, ego, obstacles_edges_num, obstacles_num, obstacles_A,
+            obstacles_b, xWS, planner_open_space_config_);
+
+    Ipopt::SmartPtr<Ipopt::TNLP> problem = ptop_ipoptqp;
+    // Create an instance of the IpoptApplication
+    Ipopt::SmartPtr<Ipopt::IpoptApplication> app = IpoptApplicationFactory();
+
+    app->Options()->SetIntegerValue("print_level", planner_open_space_config_.
+        dual_variable_warm_start_config().ipopt_config().ipopt_print_level());
+    app->Options()->SetIntegerValue("mumps_mem_percent",
+        planner_open_space_config_.dual_variable_warm_start_config().
+        ipopt_config().mumps_mem_percent());
+    app->Options()->SetNumericValue("mumps_pivtol", planner_open_space_config_.
+        dual_variable_warm_start_config().ipopt_config().mumps_pivtol());
+    app->Options()->SetIntegerValue("max_iter", planner_open_space_config_.
+        dual_variable_warm_start_config().ipopt_config().ipopt_max_iter());
+    app->Options()->SetNumericValue("tol", planner_open_space_config_.
+        dual_variable_warm_start_config().ipopt_config().ipopt_tol());
+    app->Options()->SetNumericValue("acceptable_constr_viol_tol",
+        planner_open_space_config_.dual_variable_warm_start_config().
+        ipopt_config().ipopt_acceptable_constr_viol_tol());
+    app->Options()->SetNumericValue("min_hessian_perturbation",
+        planner_open_space_config_.dual_variable_warm_start_config().
+        ipopt_config().ipopt_min_hessian_perturbation());
+    app->Options()->SetNumericValue("jacobian_regularization_value",
+        planner_open_space_config_.dual_variable_warm_start_config().
+        ipopt_config().ipopt_jacobian_regularization_value());
+    app->Options()->SetStringValue("print_timing_statistics",
+        planner_open_space_config_.dual_variable_warm_start_config().
+        ipopt_config().ipopt_print_timing_statistics());
+    app->Options()->SetStringValue("alpha_for_y",
+        planner_open_space_config_.dual_variable_warm_start_config().
+        ipopt_config().ipopt_alpha_for_y());
+    app->Options()->SetStringValue("recalc_y", planner_open_space_config_.
+        dual_variable_warm_start_config().ipopt_config().ipopt_recalc_y());
+    // for qp problem speed up
+    app->Options()->SetStringValue("mehrotra_algorithm", "yes");
+
+    Ipopt::ApplicationReturnStatus status = app->Initialize();
+    if (status != Ipopt::Solve_Succeeded) {
+      AERROR
+        << "*** Dual variable wart start problem error during initialization!";
+      return false;
+    }
+
+    status = app->OptimizeTNLP(problem);
+
+    if (status == Ipopt::Solve_Succeeded ||
+        status == Ipopt::Solved_To_Acceptable_Level) {
+      // Retrieve some statistics about the solve
+      Ipopt::Index iter_count = app->Statistics()->IterationCount();
+      AINFO << "*** IPOPTQP: The problem solved in "
+          << iter_count << " iterations!";
+      Ipopt::Number final_obj = app->Statistics()->FinalObjective();
+      AINFO << "*** IPOPTQP: The final value of the objective function is "
+          << final_obj << '.';
+    } else {
+      AINFO << "Solve not succeeding, return status: " << int(status);
+    }
+
+    ptop_ipoptqp->get_optimization_results(
+        &l_warm_up_ipoptqp, &n_warm_up_ipoptqp);
+
+    // ipopt result
+    Eigen::MatrixXd l_warm_up_ipopt(l_warm_up->rows(), l_warm_up->cols());
+    Eigen::MatrixXd n_warm_up_ipopt(n_warm_up->rows(), n_warm_up->cols());
+
+    DualVariableWarmStartIPOPTInterface* ptop_ipopt =
+        new DualVariableWarmStartIPOPTInterface(
+            horizon, ts, ego, obstacles_edges_num, obstacles_num, obstacles_A,
+            obstacles_b, xWS, planner_open_space_config_);
+
+    problem = ptop_ipopt;
+    // Create an instance of the IpoptApplication
+
+    status = app->Initialize();
+    if (status != Ipopt::Solve_Succeeded) {
+      AERROR
+        << "*** Dual variable wart start problem error during initialization!";
+      return false;
+    }
+
+    status = app->OptimizeTNLP(problem);
+    if (status == Ipopt::Solve_Succeeded ||
+        status == Ipopt::Solved_To_Acceptable_Level) {
+      // Retrieve some statistics about the solve
+      Ipopt::Index iter_count = app->Statistics()->IterationCount();
+      AINFO << "*** IPOPT: The problem solved in "
+          << iter_count << " iterations!";
+      Ipopt::Number final_obj = app->Statistics()->FinalObjective();
+      AINFO << "*** IPOPT: The final value of the objective function is "
+          << final_obj << '.';
+    } else {
+      AINFO << "Solve not succeeding, return status: " << int(status);
+    }
+
+    ptop_ipopt->get_optimization_results(
+        &l_warm_up_ipopt, &n_warm_up_ipopt);
+
+    // compare three results
+    double l_max_diff1 = 0.0;
+    double l_max_diff2 = 0.0;
+    double l_max_diff3 = 0.0;
+    for (int c = 0; c < l_warm_up->cols(); ++c) {
+      for (int r = 0; r < l_warm_up->rows(); ++r) {
+        l_max_diff1 = std::max(l_max_diff1,
+            std::abs(l_warm_up->coeff(r, c) - l_warm_up_ipopt(r, c)));
+         l_max_diff2 = std::max(l_max_diff2,
+            std::abs(l_warm_up->coeff(r, c) - l_warm_up_ipoptqp(r, c)));
+        l_max_diff3 = std::max(l_max_diff3,
+            std::abs(l_warm_up_ipoptqp(r, c) - l_warm_up_ipopt(r, c)));
+      }
+    }
+    AINFO << "max l warm up diff between osqp & ipopt: "
+        << l_max_diff1;
+    AINFO << "max l warm up diff between osqp & ipoptqp: "
+        << l_max_diff2;
+    AINFO << "max l warm up diff between ipopt & ipoptqp: "
+        << l_max_diff3;
+
+    double n_max_diff1 = 0.0;
+    double n_max_diff2 = 0.0;
+    double n_max_diff3 = 0.0;
+    for (int c = 0; c < n_warm_up->cols(); ++c) {
+      for (int r = 0; r < n_warm_up->rows(); ++r) {
+        n_max_diff1 = std::max(n_max_diff1,
+            std::abs(n_warm_up->coeff(r, c) -
+                n_warm_up_ipopt(r, c)));
+        n_max_diff2 = std::max(n_max_diff2,
+            std::abs(n_warm_up->coeff(r, c) -
+                n_warm_up_ipoptqp(r, c)));
+        n_max_diff3 = std::max(n_max_diff3,
+            std::abs(n_warm_up_ipoptqp(r, c) -
+                n_warm_up_ipopt(r, c)));
+      }
+    }
+    AINFO << "max n warm up diff between osqp & ipopt: "
+        << n_max_diff1;
+    AINFO << "max n warm up diff between osqp & ipoptqp: "
+        << n_max_diff2;
+    AINFO << "max n warm up diff between ipopt & ipoptqp: "
+        << n_max_diff3;
+
+    return true;
   }
 }
 }  // namespace planning

modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_problem.h

@@ -19,7 +19,7 @@
  */
 
 #pragma once
-
+#include <algorithm>
 #include "Eigen/Dense"
 
 #include "modules/planning/open_space/trajectory_smoother/dual_variable_warm_start_ipopt_interface.h"

modules/planning/proto/planner_open_space_config.proto

@@ -6,6 +6,7 @@ enum DualWarmUpMode {
     IPOPT = 0;
     IPOPTQP = 1;
     OSQP = 2;
+    DEBUG = 3;
 }
 
 message PlannerOpenSpaceConfig {
@@ -56,6 +57,7 @@ message DualVariableWarmStartConfig {
   optional double weight_d = 1 [default = 1.0];
   optional IpoptConfig ipopt_config = 2;
   optional DualWarmUpMode qp_format = 3;
+  optional double min_safety_distance = 4 [default = 0.0];
 }
 
 message DistanceApproachConfig {