diff --git a/include/wrappers/qpWrapperBase.hpp b/include/wrappers/qpWrapperBase.hpp index 68f577d..ded9bd2 100755 --- a/include/wrappers/qpWrapperBase.hpp +++ b/include/wrappers/qpWrapperBase.hpp @@ -13,6 +13,8 @@ namespace cvx::internal bool isConvex() const; + bool isFeasible(double tolerance) const final override; + size_t getNumInequalityConstraints() const; friend std::ostream &operator<<(std::ostream &os, const QPWrapperBase &wrapper); diff --git a/include/wrappers/socpWrapperBase.hpp b/include/wrappers/socpWrapperBase.hpp index cf92cc7..6f9a8c1 100755 --- a/include/wrappers/socpWrapperBase.hpp +++ b/include/wrappers/socpWrapperBase.hpp @@ -16,6 +16,8 @@ namespace cvx::internal size_t getNumPositiveConstraints() const; size_t getNumCones() const; + bool isFeasible(double tolerance) const final override; + friend std::ostream &operator<<(std::ostream &os, const SOCPWrapperBase &wrapper); protected: diff --git a/include/wrappers/wrapperBase.hpp b/include/wrappers/wrapperBase.hpp index f296185..8cc1f43 100644 --- a/include/wrappers/wrapperBase.hpp +++ b/include/wrappers/wrapperBase.hpp @@ -14,6 +14,7 @@ namespace cvx::internal virtual bool solve(bool verbose = false) = 0; virtual std::string getResultString() const = 0; size_t getNumVariables() const; + virtual bool isFeasible(double tolerance) const = 0; protected: using MatrixXp = Eigen::Matrix; diff --git a/src/wrappers/qpWrapperBase.cpp b/src/wrappers/qpWrapperBase.cpp index 7726b2c..a566675 100644 --- a/src/wrappers/qpWrapperBase.cpp +++ b/src/wrappers/qpWrapperBase.cpp @@ -240,4 +240,13 @@ namespace cvx::internal } } + bool QPWrapperBase::isFeasible(double tolerance) const + { + Eigen::SparseMatrix A = eval(A_params); + Eigen::VectorXd l = eval(l_params); + Eigen::VectorXd u = eval(u_params); + Eigen::VectorXd Ax = A * Eigen::Map((*solution).data(), (*solution).size()); + return ((Ax - l).array() > -tolerance).all() && ((Ax - u).array() < tolerance).all(); + } + } // namespace cvx::internal diff --git a/src/wrappers/socpWrapperBase.cpp b/src/wrappers/socpWrapperBase.cpp index 7a0528c..5200197 100644 --- a/src/wrappers/socpWrapperBase.cpp +++ b/src/wrappers/socpWrapperBase.cpp @@ -209,4 +209,39 @@ namespace cvx::internal return os; } + bool SOCPWrapperBase::isFeasible(double tolerance) const + { + Eigen::MatrixXd G = eval(G_params); + Eigen::VectorXd h = eval(h_params); + Eigen::MatrixXd A = -eval(A_params); + Eigen::VectorXd b = eval(b_params); + Eigen::VectorXd x = Eigen::Map((*solution).data(), (*solution).size()); + + if ((A * x - b).cwiseAbs().maxCoeff() > tolerance) + { + return false; + } + + const size_t n_var = this->getNumVariables(); + const size_t n_pc = this->getNumPositiveConstraints(); + const size_t n_cones = this->getNumCones(); + + if (((G.topRows(n_pc) * x + h.head(n_pc)).array() < -tolerance).any()) + { + return false; + } + + size_t k = n_pc; + for (size_t i = 0; i < n_cones; i++) + { + if ((G.block(k + 1, 0, soc_dims[i] - 1, n_var) * x + h.segment(k + 1, soc_dims[i] - 1)).norm() - (G.row(k) * x + h(k)) > tolerance) + { + return false; + } + k += soc_dims[i]; + } + + return true; + } + } // namespace cvx diff --git a/tests/run_tests.cpp b/tests/run_tests.cpp index e840a57..77f9d3e 100644 --- a/tests/run_tests.cpp +++ b/tests/run_tests.cpp @@ -5,7 +5,6 @@ #include - // Tests #include "test_constraint.hpp" diff --git a/tests/test_mpc_qp.hpp b/tests/test_mpc_qp.hpp index 341e930..bbae9ad 100644 --- a/tests/test_mpc_qp.hpp +++ b/tests/test_mpc_qp.hpp @@ -65,4 +65,5 @@ TEST_CASE("MPC QP") REQUIRE(x_sol.minCoeff() >= Approx(-5.).margin(1e-3)); REQUIRE(u_sol.maxCoeff() <= Approx(2.).margin(1e-3)); REQUIRE(u_sol.minCoeff() >= Approx(-2.).margin(1e-3)); + REQUIRE(solver.isFeasible(1e-8)); } \ No newline at end of file diff --git a/tests/test_portfolio_socp.hpp b/tests/test_portfolio_socp.hpp index c3e790c..21c2b8d 100644 --- a/tests/test_portfolio_socp.hpp +++ b/tests/test_portfolio_socp.hpp @@ -77,5 +77,6 @@ TEST_CASE("Portfolio SOCP") REQUIRE((x_eval - x_sol).cwiseAbs().maxCoeff() == Approx(0.).margin(1e-4)); REQUIRE(x_eval.minCoeff() >= 0.); REQUIRE(x_eval.sum() == Approx(1.)); + REQUIRE(solver.isFeasible(1e-8)); } } \ No newline at end of file diff --git a/tests/test_simple_socp.hpp b/tests/test_simple_socp.hpp index bd02331..b3ecc24 100644 --- a/tests/test_simple_socp.hpp +++ b/tests/test_simple_socp.hpp @@ -74,6 +74,8 @@ TEST_CASE("Simple SOCP") // Solve the problem and show solver output. t0 = std::chrono::high_resolution_clock::now(); const bool success = solver.solve(false); + REQUIRE(solver.isFeasible(1e-8)); + if (not success) { // This should not happen in this example. @@ -85,7 +87,8 @@ TEST_CASE("Simple SOCP") // Get Solution. Eigen::Matrix x_eval = eval(x); // Print the first solution. - std::cout << "First solution:\n" << x_eval << "\n\n"; + std::cout << "First solution:\n" + << x_eval << "\n\n"; // Test the constraints for (size_t i = 0; i < m; i++) { @@ -96,6 +99,7 @@ TEST_CASE("Simple SOCP") // Change the problem parameters and solve again. f.setRandom(); solver.solve(false); + REQUIRE(solver.isFeasible(1e-8)); x_eval = eval(x); // Test the constraints again @@ -105,5 +109,6 @@ TEST_CASE("Simple SOCP") } REQUIRE((F * x_eval - g).cwiseAbs().maxCoeff() == Approx(0.).margin(1e-8)); - std::cout << "Solution after changing the cost function:\n" << x_eval << "\n\n"; + std::cout << "Solution after changing the cost function:\n" + << x_eval << "\n\n"; } \ No newline at end of file