Polish and fix MPC/QP solver

modules/common/math/mpc_solver.cc

@@ -120,16 +120,17 @@ void SolveLinearMPC(const Matrix &matrix_a,
             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;
+  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;
+                                 - 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,
@@ -139,11 +140,6 @@ void SolveLinearMPC(const Matrix &matrix_a,
   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);

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;
@@ -57,10 +57,10 @@ TEST(MPCSolverTest, MPC) {
   R << 1;
 
   Eigen::MatrixXd lower_bound(CONTROLS, 1);
-  lower_bound << -0.2;
+  lower_bound << -10;
 
   Eigen::MatrixXd upper_bound(CONTROLS, 1);
-  upper_bound << 0.6;
+  upper_bound << 10;
 
   Eigen::MatrixXd initial_state(STATES, 1);
   initial_state << 0,
@@ -80,7 +80,6 @@ TEST(MPCSolverTest, MPC) {
   control_matrix << 0;
 
   std::vector<Eigen::MatrixXd> control(HORIZON, control_matrix);
-
   for (unsigned int i = 0; i < control.size(); ++i) {
     for (unsigned int i = 1; i < control.size(); ++i) {
       control[i-1] = control[i];
@@ -88,18 +87,17 @@ TEST(MPCSolverTest, MPC) {
     SolveLinearMPC(A, B, C, Q, R, lower_bound, upper_bound, initial_state,
                    reference, EPS, MAX_ITER, &control);
     EXPECT_FLOAT_EQ(upper_bound(0), control[0](0));
-    initial_state = A * initial_state + B * control[0](0) + C;
   }
 
   Eigen::MatrixXd initial_state1(STATES, 1);
-  initial_state1 << 300,
-                    300,
+  initial_state1 << 30,
+                    30,
                     0,
                     0;
 
   Eigen::MatrixXd reference_state1(STATES, 1);
-  reference_state1 << 200,
-                      200,
+  reference_state1 << 0,
+                      0,
                       0,
                       0;
 
@@ -107,7 +105,6 @@ TEST(MPCSolverTest, MPC) {
 
   control_matrix << 0;
   std::vector<Eigen::MatrixXd> control1(HORIZON, control_matrix);
-
   for (unsigned int i = 0; i < control1.size(); ++i) {
     for (unsigned int i = 1; i < control.size(); ++i) {
       control1[i-1] = control1[i];
@@ -115,18 +112,17 @@ TEST(MPCSolverTest, MPC) {
     SolveLinearMPC(A, B, C, Q, R, lower_bound, upper_bound, initial_state1,
                    reference1, EPS, MAX_ITER, &control1);
     EXPECT_FLOAT_EQ(lower_bound(0), control1[0](0));
-    initial_state1 = A * initial_state1 + B * control1[0](0) + C;
   }
 
   Eigen::MatrixXd initial_state2(STATES, 1);
-  initial_state2 << 300,
-                    300,
+  initial_state2 << 30,
+                    30,
                     0,
                     0;
 
   Eigen::MatrixXd reference_state2(STATES, 1);
-  reference_state2 << 300,
-                      300,
+  reference_state2 << 30,
+                      30,
                       0,
                       0;
 
@@ -141,11 +137,9 @@ TEST(MPCSolverTest, MPC) {
     }
     SolveLinearMPC(A, B, C, Q, R, lower_bound, upper_bound, initial_state2,
                    reference2, EPS, MAX_ITER, &control2);
-    EXPECT_FLOAT_EQ(0.0, control2[0](0));
-    initial_state2 = A * initial_state2 + B * control2[0](0) + C;
-  }
+    EXPECT_NEAR(0.0, control2[0](0), 1e-7);
+    }
 }
-*/
 }  // namespace math
 }  // namespace common
 }  // namespace apollo

modules/common/math/qp_solver/BUILD

@@ -41,6 +41,7 @@ cc_library(
         "//modules/common/math/qp_solver:qp_solver_gflags",
         "@eigen//:eigen",
         "@qp_oases//:qp_oases",
+        "//modules/common:log",
     ],
 )
 

modules/common/math/qp_solver/qp_solver.h

@@ -19,8 +19,8 @@
  * @brief: quadratic programming base class
  *
  *        min_x  : q(x) = 0.5 * x^T * Q * x  + x^T c
- *        with respect to:  A * x = b (inequality constraint)
- *                          C * x >= d (equality constraint)
+ *        with respect to:  A * x = b (equality constraint)
+ *                          C * x >= d (inequality constraint)
  **/
 
 #ifndef MODULES_COMMON_MATH_QP_SOLVER_QP_SOLVER_H_