add feasibility checks

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);

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:

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<Parameter, Eigen::Dynamic, Eigen::Dynamic>;

src/wrappers/qpWrapperBase.cpp

@@ -240,4 +240,13 @@ namespace cvx::internal
         }
     }
 
+    bool QPWrapperBase::isFeasible(double tolerance) const
+    {
+        Eigen::SparseMatrix<double> A = eval(A_params);
+        Eigen::VectorXd l = eval(l_params);
+        Eigen::VectorXd u = eval(u_params);
+        Eigen::VectorXd Ax = A * Eigen::Map<Eigen::VectorXd>((*solution).data(), (*solution).size());
+        return ((Ax - l).array() > -tolerance).all() && ((Ax - u).array() < tolerance).all();
+    }
+
 } // namespace cvx::internal

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<Eigen::VectorXd>((*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

tests/run_tests.cpp

@@ -5,7 +5,6 @@
 
 #include <iostream>
 
-
 // Tests
 #include "test_constraint.hpp"
 

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));
 }

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));
     }
 }

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<double, n, 1> 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";
 }