Merge pull request #18 from Kotakku/feat/admm_qp

Feat/admm qp
Kotakku · Jan 20, 2024 · a7cbf8c · a7cbf8c
2 parents b0323e7 + f071f4c
commit a7cbf8c
Show file tree

Hide file tree

Showing 10 changed files with 743 additions and 351 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -57,9 +57,8 @@ set(example_files
   optimize/optimize_without_constraint
   optimize/optimize_rosenbrock
   optimize/optimize_with_constraint
-  optimize/active_set_method_example
-  optimize/sqp_example
   optimize/quadprog
+  optimize/sqp_example
 
   path_planning/dubins_path
 
@@ -148,11 +147,6 @@ endforeach()
 endif()
 
 #################### install ####################
-install(
-  DIRECTORY include
-  DESTINATION share/${PROJECT_NAME}/
-)
-
 add_library(${PROJECT_NAME} INTERFACE)
 
 target_compile_features(${PROJECT_NAME} INTERFACE cxx_std_17)

diff --git a/docs/tech_note/optimize/quadprog.md b/docs/tech_note/optimize/quadprog.md
@@ -3,13 +3,14 @@
 以下の最適化問題を考える
 
 $$
-\begin{array}
+\begin{array}{}
 \text{min} \space \dfrac{1}{2}x^TQx + c^T x\\
-\text{s.t.} \space Ax = b, A_{eq}x \leq b_{eq}, l_x \leq x \leq u_x \\
+\text{s.t.} \space Ax = b, A_{eq}x \leq b_{eq} \\
 x \in \mathbb{R}^n
 \end{array}
 $$
 
+# 内点法による最適化
 ## KKT条件
 与えられた問題に対するKKT条件は以下
 
@@ -68,5 +69,10 @@ $$
 
 この方程式を解いて各変数を更新していく
 
+# ActiveSet法による最適化
+
+# ADMMによる最適化
+
+
 ## 参考文献
 - [二次計画法のアルゴリズム](https://jp.mathworks.com/help/optim/ug/quadratic-programming-algorithms.html#bvjx020-1)
diff --git a/example/optimize/quadprog.cpp b/example/optimize/quadprog.cpp
@@ -3,133 +3,39 @@
 #include <Eigen/Dense>
 #include <cpp_robotics/optimize/quadprog.hpp>
 
-void example1()
+int main()
 {
     using namespace cpp_robotics;
 
-    QuadProg qp;
+    QuadProgProblem problem;
 
     // 変数のサイズ      : 2
-    // 不等式制約のサイズ : 2
+    // 不等式制約のサイズ : 0
     // 等式制約のサイズ   : 0
-    qp.param.tol_con = 1e-3;
-    qp.param.tol_step = 1e-3;
-
-    qp.set_problem_size(2, 4, 0); 
+    // box制約          : true
+    problem.set_problem_size(2, 0, 0, true); 
 
     // この問題における制約なしの時の大域的最適解
     // x = (3, 1)
-    qp.Q << 
+    problem.Q << 
         1,0,
         0,1;
 
-    qp.c << 
+    problem.c << 
         -3,-1;
 
     // s.t.
-    qp.A << 
-        1, 0,
-        0, 1,
-        -1, 0,
-        0, -1;
-    qp.b << 
-        10, 10, 0, 0;
-
-    Eigen::VectorXd x0(2);
-    x0 << 1, 0;
-
-    auto result = qp.solve(x0);
-
-    std::cout << std::fixed << std::setprecision(3);
-
-    // std::cout << "is_solved: " << result.is_solved<< std::endl;
-    std::cout << "is_solved: " << (result.is_solved ? "true" : "false") << std::endl;
-    std::cout << "optx     : " << result.x.transpose() << std::endl;
-    std::cout << "itr      : " << result.iter_cnt << std::endl;
-    std::cout << "satisfy  : " << qp.satisfy(result.x) << std::endl;
-
-    std::cout << "(Ax-b < 0)? :" << std::endl;
-    std::cout << qp.A*result.x - qp.b << std::endl;
-
-    std::cout << "(Aeqx-beq = 0)? :" << std::endl;
-    std::cout << qp.Aeq*result.x - qp.beq << std::endl;
-}
-
-void example2()
-{
-    using namespace cpp_robotics;
-
-    QuadProg qp;
-
-    // 変数のサイズ      : 2
-    // 不等式制約のサイズ : 2
-    // 等式制約のサイズ   : 0
-    qp.param.tol_con = 1e-3;
-    qp.param.tol_step = 1e-3;
-
-    qp.set_problem_size(2, 4, 1); 
-
-    // min 0.5*(x1^2 + x2^2) - 3*x1
-    // qp.Q << 1, 0, 
-    //         0, 1;
-    // qp.c << -3, 1;
-
-    qp.Q << 
-        1.55356e+14, 6.74972e+13,
-        6.74972e+13, 2.93254e+13;
-
-    qp.Q << 
-        1.55356e0, 6.74972e0,
-        6.74972e0, 2.93254e0;
-
-    qp.c <<
-        -0.928814,
-        1.89986;
-    // s.t. x1 <= 4
-    //      -x2 <= -1 (x2 >= 1)
-    qp.A << 
-        1, 0,
-        0, 1,
-        -1, 0,
-        0, -1;
-    qp.b << 
-        0.1, 0.1, 0.1, 0.1;
-
-    // qp.Aeq << 
-    //     1, 1;
-    // qp.beq << 
-    //     1;
-    qp.Aeq << 1, 1;
-    qp.beq << 0.05;
-    // A, bに変数の上下限をいれると解けないので解法をl<x<uで設定する
-
-    // この問題における最適解
-    // x = (3, 1)
+    problem.lb << 0,0;
+    problem.ub << 10, 10;
 
     Eigen::VectorXd x0(2);
     x0 << 1, 0;
 
-    // std::cout << qp.evaluate_merit(Eigen::Vector2d(0, 0), Eigen::Vector2d(0, 0), 0, 0) << std::endl;
-    // std::cout << qp.evaluate_merit(Eigen::Vector2d(3, 0), Eigen::Vector2d(0, 0), 0, 0) << std::endl;
-    auto result = qp.solve(x0);
+    auto result = quadprog(problem, x0);
 
     std::cout << std::fixed << std::setprecision(3);
 
-    // std::cout << "is_solved: " << result.is_solved<< std::endl;
     std::cout << "is_solved: " << (result.is_solved ? "true" : "false") << std::endl;
     std::cout << "optx     : " << result.x.transpose() << std::endl;
     std::cout << "itr      : " << result.iter_cnt << std::endl;
-    std::cout << "satisfy  : " << qp.satisfy(result.x) << std::endl;
-
-    std::cout << "(Ax-b < 0)? :" << std::endl;
-    std::cout << qp.A*result.x - qp.b << std::endl;
-
-    std::cout << "(Aeqx-beq = 0)? :" << std::endl;
-    std::cout << qp.Aeq*result.x - qp.beq << std::endl;
-}
-
-int main()
-{
-    example1();
-    // example2();
 }
diff --git a/include/cpp_robotics/controller/linear_mpc.hpp b/include/cpp_robotics/controller/linear_mpc.hpp
@@ -81,26 +81,24 @@ class LinearMPC
 
         H_ = Bd_mat_.transpose()*Q_mat_*Bd_mat_ + R_mat_;
         g_ = -Bd_mat_.transpose()*Q_mat_;
-        qp_solver_.Q = H_;
+
+        qp_prob_.set_problem_size(input_size_*N_, 0, 0, u_limit_.has_value());
+        qp_prob_.Q = H_;
 
         if(u_limit_)
         {
             // Todo infinityがあったら制限なしということでG_mat_, h_mat_にいれない操作をする
-            Eigen::MatrixXd G_mat_ = Eigen::MatrixXd::Zero(input_size_*N_*2, input_size_*N_);
-            Eigen::VectorXd h_mat_ = Eigen::VectorXd::Zero(input_size_*N_*2);
-
-            G_mat_.block(0,0,input_size_*N_, input_size_*N_) = Eigen::MatrixXd::Identity(input_size_*N_, input_size_*N_);
-            G_mat_.block(input_size_*N_,0,input_size_*N_, input_size_*N_) = -Eigen::MatrixXd::Identity(input_size_*N_, input_size_*N_);
+            Eigen::VectorXd ulb_vec = Eigen::VectorXd::Zero(input_size_*N_);
+            Eigen::VectorXd uub_vec = Eigen::VectorXd::Zero(input_size_*N_);
 
             auto &[umin, umax] = u_limit_.value();
             for(size_t i = 0; i < N_; i++)
             {
-                h_mat_.segment(input_size_*i, input_size_) = umax;
-                h_mat_.segment(input_size_*(N_+i), input_size_) = -umin;
+                ulb_vec.segment(input_size_*i, input_size_) = umin;
+                uub_vec.segment(input_size_*i, input_size_) = umax;
             }
-
-            qp_solver_.A = G_mat_;
-            qp_solver_.b = h_mat_;
+            qp_prob_.lb = ulb_vec;
+            qp_prob_.ub = uub_vec;
         }
 
         U_ = Eigen::VectorXd::Zero(input_size_*N_);
@@ -129,7 +127,8 @@ class LinearMPC
             eps_mat.block(i*state_size_, 0, state_size_, 1) += x_ref[i];
         }
 
-        qp_solver_.c = (g_*eps_mat).transpose();
+        qp_prob_.c = (g_*eps_mat).transpose();
+        qp_solver_.set_problem(qp_prob_);
         latest_qp_result_ = qp_solver_.solve(U_);
 
         if(latest_qp_result_.is_solved)
@@ -190,7 +189,8 @@ class LinearMPC
     Eigen::MatrixXd g_;
 
     // QPソルバ
-    QuadProg qp_solver_;
+    QuadProgProblem qp_prob_;
+    QuadProg qp_solver_ = {QuadProg::Method::InteriorPointMethod};
     Eigen::VectorXd U_;
     QuadProg::Result latest_qp_result_;
 };