perf(velocity_smoother): use ProxQP for faster optimization (autowarefoundation#8028)

* perf(velocity_smoother): use ProxQP for faster optimization

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* consider max_iteration

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* disable warm start

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix test

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

Author: takayuki5168

common/qp_interface/include/qp_interface/osqp_interface.hpp

@@ -33,7 +33,7 @@ class OSQPInterface : public QPInterface
 public:
   /// \brief Constructor without problem formulation
   OSQPInterface(
-    const bool enable_warm_start = false,
+    const bool enable_warm_start = false, const int max_iteration = 20000,
     const c_float eps_abs = std::numeric_limits<c_float>::epsilon(),
     const c_float eps_rel = std::numeric_limits<c_float>::epsilon(), const bool polish = true,
     const bool verbose = false);

common/qp_interface/include/qp_interface/proxqp_interface.hpp

@@ -31,8 +31,8 @@ class ProxQPInterface : public QPInterface
 {
 public:
   explicit ProxQPInterface(
-    const bool enable_warm_start, const double eps_abs, const double eps_rel,
-    const bool verbose = false);
+    const bool enable_warm_start, const int max_iteration, const double eps_abs,
+    const double eps_rel, const bool verbose = false);
 
   int getIterationNumber() const override;
   bool isSolved() const override;

common/qp_interface/src/osqp_interface.cpp

@@ -21,8 +21,8 @@
 namespace autoware::common
 {
 OSQPInterface::OSQPInterface(
-  const bool enable_warm_start, const c_float eps_abs, const c_float eps_rel, const bool polish,
-  const bool verbose)
+  const bool enable_warm_start, const int max_iteration, const c_float eps_abs,
+  const c_float eps_rel, const bool polish, const bool verbose)
 : QPInterface(enable_warm_start), work_{nullptr, OSQPWorkspaceDeleter}
 {
   settings_ = std::make_unique<OSQPSettings>();
@@ -35,8 +35,8 @@ OSQPInterface::OSQPInterface(
     settings_->eps_abs = eps_abs;
     settings_->eps_prim_inf = 1.0E-4;
     settings_->eps_dual_inf = 1.0E-4;
-    settings_->warm_start = true;
-    settings_->max_iter = 4000;
+    settings_->warm_start = enable_warm_start;
+    settings_->max_iter = max_iteration;
     settings_->verbose = verbose;
     settings_->polish = polish;
   }

common/qp_interface/src/proxqp_interface.cpp

@@ -19,9 +19,11 @@ namespace autoware::common
 using proxsuite::proxqp::QPSolverOutput;
 
 ProxQPInterface::ProxQPInterface(
-  const bool enable_warm_start, const double eps_abs, const double eps_rel, const bool verbose)
+  const bool enable_warm_start, const int max_iteration, const double eps_abs, const double eps_rel,
+  const bool verbose)
 : QPInterface(enable_warm_start)
 {
+  settings_.max_iter = max_iteration;
   settings_.eps_abs = eps_abs;
   settings_.eps_rel = eps_rel;
   settings_.verbose = verbose;

common/qp_interface/test/test_osqp_interface.cpp

@@ -63,7 +63,7 @@ TEST(TestOsqpInterface, BasicQp)
 
   {
     // Define problem during optimization
-    autoware::common::OSQPInterface osqp(false, 1e-6);
+    autoware::common::OSQPInterface osqp(false, 4000, 1e-6);
     const auto solution = osqp.QPInterface::optimize(P, A, q, l, u);
     const auto status = osqp.getStatus();
     const auto polish_status = osqp.getPolishStatus();
@@ -72,7 +72,7 @@ TEST(TestOsqpInterface, BasicQp)
 
   {
     // Define problem during initialization
-    autoware::common::OSQPInterface osqp(false, 1e-6);
+    autoware::common::OSQPInterface osqp(false, 4000, 1e-6);
     const auto solution = osqp.QPInterface::optimize(P, A, q, l, u);
     const auto status = osqp.getStatus();
     const auto polish_status = osqp.getPolishStatus();
@@ -87,15 +87,15 @@ TEST(TestOsqpInterface, BasicQp)
     std::vector<double> q_ini(2, 0.0);
     std::vector<double> l_ini(4, 0.0);
     std::vector<double> u_ini(4, 0.0);
-    autoware::common::OSQPInterface osqp(false, 1e-6);
+    autoware::common::OSQPInterface osqp(false, 4000, 1e-6);
     osqp.QPInterface::optimize(P_ini, A_ini, q_ini, l_ini, u_ini);
   }
 
   {
     // Define problem during initialization with csc matrix
     CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P);
     CSC_Matrix A_csc = calCSCMatrix(A);
-    autoware::common::OSQPInterface osqp(false, 1e-6);
+    autoware::common::OSQPInterface osqp(false, 4000, 1e-6);
 
     const auto solution = osqp.optimize(P_csc, A_csc, q, l, u);
     const auto status = osqp.getStatus();
@@ -111,7 +111,7 @@ TEST(TestOsqpInterface, BasicQp)
     std::vector<double> q_ini(2, 0.0);
     std::vector<double> l_ini(4, 0.0);
     std::vector<double> u_ini(4, 0.0);
-    autoware::common::OSQPInterface osqp(false, 1e-6);
+    autoware::common::OSQPInterface osqp(false, 4000, 1e-6);
     osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini);
 
     // Redefine problem before optimization
@@ -132,7 +132,7 @@ TEST(TestOsqpInterface, BasicQp)
     std::vector<double> q_ini(2, 0.0);
     std::vector<double> l_ini(4, 0.0);
     std::vector<double> u_ini(4, 0.0);
-    autoware::common::OSQPInterface osqp(true, 1e-6);  // enable warm start
+    autoware::common::OSQPInterface osqp(true, 4000, 1e-6);  // enable warm start
     osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini);
 
     // Redefine problem before optimization

common/qp_interface/test/test_proxqp_interface.cpp

@@ -57,15 +57,15 @@ TEST(TestProxqpInterface, BasicQp)
 
   {
     // Define problem during optimization
-    autoware::common::ProxQPInterface proxqp(false, 1e-9, 1e-9, false);
+    autoware::common::ProxQPInterface proxqp(false, 4000, 1e-9, 1e-9, false);
     const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u);
     const auto status = proxqp.getStatus();
     check_result(solution, status);
   }
 
   {
     // Define problem during optimization with warm start
-    autoware::common::ProxQPInterface proxqp(true, 1e-9, 1e-9, false);
+    autoware::common::ProxQPInterface proxqp(true, 4000, 1e-9, 1e-9, false);
     {
       const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u);
       const auto status = proxqp.getStatus();

planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp

@@ -19,7 +19,7 @@
 #include "autoware/universe_utils/geometry/geometry.hpp"
 #include "autoware/universe_utils/system/time_keeper.hpp"
 #include "autoware/velocity_smoother/smoother/smoother_base.hpp"
-#include "osqp_interface/osqp_interface.hpp"
+#include "qp_interface/qp_interface.hpp"
 
 #include "autoware_planning_msgs/msg/trajectory_point.hpp"
 
@@ -59,7 +59,7 @@ class JerkFilteredSmoother : public SmootherBase
 
 private:
   Param smoother_param_;
-  autoware::common::osqp::OSQPInterface qp_solver_;
+  std::shared_ptr<autoware::common::QPInterface> qp_interface_;
   rclcpp::Logger logger_{rclcpp::get_logger("smoother").get_child("jerk_filtered_smoother")};
 
   TrajectoryPoints forwardJerkFilter(

planning/autoware_velocity_smoother/package.xml

@@ -30,6 +30,7 @@
   <depend>interpolation</depend>
   <depend>nav_msgs</depend>
   <depend>osqp_interface</depend>
+  <depend>qp_interface</depend>
   <depend>rclcpp</depend>
   <depend>tf2</depend>
   <depend>tf2_ros</depend>

planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp

@@ -15,6 +15,7 @@
 #include "autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp"
 
 #include "autoware/velocity_smoother/trajectory_utils.hpp"
+#include "qp_interface/proxqp_interface.hpp"
 
 #include <Eigen/Core>
 
@@ -40,11 +41,8 @@ JerkFilteredSmoother::JerkFilteredSmoother(
   p.over_j_weight = node.declare_parameter<double>("over_j_weight");
   p.jerk_filter_ds = node.declare_parameter<double>("jerk_filter_ds");
 
-  qp_solver_.updateMaxIter(20000);
-  qp_solver_.updateRhoInterval(0);  // 0 means automatic
-  qp_solver_.updateEpsRel(1.0e-6);  // def: 1.0e-4
-  qp_solver_.updateEpsAbs(1.0e-8);  // def: 1.0e-4
-  qp_solver_.updateVerbose(false);
+  qp_interface_ =
+    std::make_shared<autoware::common::ProxQPInterface>(false, 20000, 1.0e-8, 1.0e-6, false);
 }
 
 void JerkFilteredSmoother::setParam(const Param & smoother_param)
@@ -301,14 +299,13 @@ bool JerkFilteredSmoother::apply(
 
   // execute optimization
   time_keeper_->start_track("optimize");
-  const auto result = qp_solver_.optimize(P, A, q, lower_bound, upper_bound);
+  const auto optval = qp_interface_->optimize(P, A, q, lower_bound, upper_bound);
   time_keeper_->end_track("optimize");
-  const std::vector<double> optval = std::get<0>(result);
-  const int status_val = std::get<3>(result);
-  if (status_val != 1) {
-    RCLCPP_WARN(logger_, "optimization failed : %s", qp_solver_.getStatusMessage().c_str());
+  if (!qp_interface_->isSolved()) {
+    RCLCPP_WARN(logger_, "optimization failed : %s", qp_interface_->getStatus().c_str());
     return false;
   }
+
   const auto has_nan =
     std::any_of(optval.begin(), optval.end(), [](const auto v) { return std::isnan(v); });
   if (has_nan) {
@@ -332,16 +329,6 @@ bool JerkFilteredSmoother::apply(
     output.at(i).acceleration_mps2 = a_stop_decel;
   }
 
-  qp_solver_.logUnsolvedStatus("[autoware_velocity_smoother]");
-
-  const int status_polish = std::get<2>(result);
-  if (status_polish != 1) {
-    const auto msg = status_polish == 0    ? "unperformed"
-                     : status_polish == -1 ? "unsuccessful"
-                                           : "unknown";
-    RCLCPP_DEBUG(logger_, "osqp polish process failed : %s. The result may be inaccurate", msg);
-  }
-
   if (VERBOSE_TRAJECTORY_VELOCITY) {
     const auto s_output = trajectory_utils::calcArclengthArray(output);