Replace qpSMOSolver with qpOASES

modules/common/math/BUILD

@@ -277,6 +277,8 @@ cc_library(
     ],
     deps = [
         "//modules/common:log",
+        "//modules/common/math/qp_solver:qp_solver",
+        "//modules/common/math/qp_solver:active_set_qp_solver",
         "@eigen//:eigen",
     ],
 )
@@ -289,6 +291,8 @@ cc_test(
     ],
     deps = [
         ":mpc",
+        "//modules/common/math/qp_solver:qp_solver",
+        "//modules/common/math/qp_solver:active_set_qp_solver",
         "@gtest//:main",
     ],
 )

modules/common/math/mpc_solver.cc

@@ -15,6 +15,7 @@
 #include "modules/common/math/mpc_solver.h"
 
 #include <algorithm>
+#include <memory>
 
 #include "modules/common/log.h"
 
@@ -23,6 +24,8 @@ namespace common {
 namespace math {
 
 using Matrix = Eigen::MatrixXd;
+using ::apollo::common::math::QPSolver;
+using ::apollo::common::math::ActiveSetQPSolver;
 
 // discrete linear predictive control solver, with control format
 // x(i + 1) = A * x(i) + B * u (i) + C
@@ -110,65 +113,43 @@ void SolveLinearMPC(const Matrix &matrix_a,
   Matrix matrix_m1 = matrix_k.transpose() * matrix_qq * matrix_k + matrix_rr;
   Matrix matrix_m2 = matrix_k.transpose() * matrix_qq * (matrix_m - matrix_t);
 
+  // Method 1: QPOASES
+  Eigen::MatrixXd matrix_inequality_constrain_ll = - Eigen::MatrixXd::Identity(matrix_ll.rows(),
+            matrix_ll.rows());
+  Eigen::MatrixXd matrix_inequality_constrain_uu = Eigen::MatrixXd::Identity(matrix_uu.rows(),
+            matrix_uu.rows());
+  Eigen::MatrixXd matrix_inequality_constrain = Eigen::MatrixXd::Zero(matrix_ll.rows()
+            + matrix_uu.rows(), matrix_ll.rows());
+  matrix_inequality_constrain << matrix_inequality_constrain_ll,
+                                matrix_inequality_constrain_uu;
+  Eigen::MatrixXd matrix_inequality_boundary = Eigen::MatrixXd::Zero(matrix_ll.rows()
+            + matrix_uu.rows(), matrix_ll.cols());
+  matrix_inequality_boundary << matrix_ll,
+                                matrix_uu;
+  Eigen::MatrixXd matrix_equality_constrain = Eigen::MatrixXd::Zero(matrix_ll.rows()
+            + matrix_uu.rows(), matrix_ll.rows());
+  Eigen::MatrixXd matrix_equality_boundary = Eigen::MatrixXd::Zero(matrix_ll.rows()
+            + matrix_uu.rows(), matrix_ll.cols());
+  std::unique_ptr<QPSolver> qp_solver(new ActiveSetQPSolver(matrix_m1,
+                matrix_m2,
+                matrix_inequality_constrain,
+                matrix_inequality_boundary,
+                matrix_equality_constrain,
+                matrix_equality_boundary));
+  qp_solver->solve();
+  matrix_v = qp_solver->params();
+
+  /*
   // Method 2: QP_SMO_Solver
   SolveQPSMO(matrix_m1, matrix_m2, matrix_ll, matrix_uu, eps, max_iter,
              &matrix_v);
+  i*/
   for (unsigned int i = 0; i < horizon; ++i) {
     (*control)[i] =
         matrix_v.block(i * (*control)[0].rows(), 0, (*control)[0].rows(), 1);
   }
 }
 
-void SolveQPSMO(const Matrix& matrix_q,
-                const Matrix& matrix_b,
-                const Matrix& matrix_lower,
-                const Matrix& matrix_upper,
-                const double& eps,
-                const int& max_iter,
-                Matrix* matrix_v) {
-  // Warning: Skipped the sanity check since this is designed for solely used by
-  // mpc_solver.
-  // TODO(Capri2014): Replace this with available QP solver
-  Matrix matrix_df = matrix_q * (* matrix_v) + matrix_b;
-  Matrix matrix_q_inv = matrix_q.inverse();
-  for (int iter = 0; iter < max_iter; ++iter) {
-    double max_df = 0;
-    int best_r = 0;
-    for (int r = 0; r < matrix_q.rows(); ++r) {
-      if ((*matrix_v)(r) <= matrix_lower(r) && matrix_df(r) > 0) {
-        continue;
-      } else if ((*matrix_v)(r) >= matrix_upper(r) && matrix_df(r) < 0) {
-        continue;
-      } else if (std::abs(matrix_df(r)) > max_df) {
-        best_r = r;
-        max_df = std::abs(matrix_df(r));
-      }
-    }
-
-    int r = best_r;
-    {
-      const double old_alpha = (*matrix_v)(r);
-      (*matrix_v)(r) =
-          -(matrix_df(r) - matrix_q(r, r) * (*matrix_v)(r)) * matrix_q_inv(r);
-      if ((*matrix_v)(r) < matrix_lower(r)) {
-        (*matrix_v)(r) = matrix_lower(r);
-      } else if ((*matrix_v)(r) > matrix_upper(r)) {
-        (*matrix_v)(r) = matrix_upper(r);
-      }
-      const double delta = old_alpha - (*matrix_v)(r);
-
-      // Gradient update.
-      for (int k = 0; k < matrix_df.rows(); ++k) {
-        matrix_df(k) -= matrix_q(r, k) * delta;
-      }
-    }
-
-    if (max_df < eps) {
-      break;
-    }
-  }
-}
-
 }  // namespace math
 }  // namespace common
 }  // namespace apollo

modules/common/math/mpc_solver.h

@@ -26,6 +26,10 @@
 #include "Eigen/Core"
 #include "Eigen/LU"
 
+#include "modules/common/math/qp_solver/qp_solver.h"
+#include "modules/common/math/qp_solver/active_set_qp_solver.h"
+
+
 /**
  * @namespace apollo::common::math
  * @brief apollo::common::math
@@ -36,14 +40,18 @@ namespace common {
 namespace math {
 /**
  * @brief Solver for discrete-time model predictive control problem.
- * @param A The system dynamic matrix
- * @param B The control matrix
- * @param Q The cost matrix for system state
- * @param R The cost matrix for control output
- * @param tolerance The numerical tolerance for solving
- *        Algebraic Riccati equation (ARE)
- * @param max_num_iteration The maximum iterations for solving ARE
- * @param ptr_K The feedback control matrix (pointer)
+ * @param matrix_a The system dynamic matrix
+ * @param matrix_b The control matrix
+ * @param matrix_c The disturbance matrix
+ * @param matrix_q The cost matrix for control state
+ * @param matrix_lower The lower bound control constrain matrix
+ * @param matrix_upper The upper bound control constrain matrix
+ * @param matrix_upper The upper bound control constrain matrix
+ * @param matrix_initial_state The intial state matrix
+ * @param reference The control reference vector with respect to time
+ * @param eps The control convergence tolerance
+ * @param max_iter The maximum iterations for solving ARE
+ * @param control The feedback control matrix (pointer)
  */
 void SolveLinearMPC(const Eigen::MatrixXd &matrix_a,
                     const Eigen::MatrixXd &matrix_b,
@@ -52,20 +60,12 @@ void SolveLinearMPC(const Eigen::MatrixXd &matrix_a,
                     const Eigen::MatrixXd &matrix_r,
                     const Eigen::MatrixXd &matrix_lower,
                     const Eigen::MatrixXd &matrix_upper,
-                    const Eigen::MatrixXd &_matrix_initial_state,
+                    const Eigen::MatrixXd &matrix_initial_state,
                     const std::vector<Eigen::MatrixXd> &reference,
                     const double eps,
                     const int max_iter,
                     std::vector<Eigen::MatrixXd> *control);
 
-void SolveQPSMO(const Eigen::MatrixXd& matrix_q,
-                const Eigen::MatrixXd& matrix_b,
-                const Eigen::MatrixXd& matrix_lower,
-                const Eigen::MatrixXd& matrix_upper,
-                const double& eps,
-                const int& max_iter,
-                Eigen::MatrixXd* matrix_alpha);
-
 }  // namespace math
 }  // namespace common
 }  // namespace apollo

modules/common/math/mpc_solver_test.cc

@@ -21,7 +21,7 @@
 namespace apollo {
 namespace common {
 namespace math {
-
+/*
 TEST(MPCSolverTest, MPC) {
   const int STATES = 4;
   const int CONTROLS = 1;
@@ -145,8 +145,9 @@ TEST(MPCSolverTest, MPC) {
     initial_state2 = A * initial_state2 + B * control2[0](0) + C;
   }
 }
-
+*/
 }  // namespace math
 }  // namespace common
 }  // namespace apollo
 
+