Merge pull request #86 from robotology/unconstrained

Add the possibility to solve unconstrained QP problem without setting the constraint matrix
robotology · Mar 26, 2021 · 41b371d · 41b371d
2 parents 9b00910 + af7aa6d
commit 41b371d
Show file tree

Hide file tree

Showing 4 changed files with 78 additions and 21 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -5,7 +5,7 @@
 cmake_minimum_required(VERSION 3.5)
 
 project(OsqpEigen
-  VERSION 0.6.1)
+  VERSION 0.6.3)
 
 # ouptut paths
 set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_BINDIR}")

diff --git a/src/Data.cpp b/src/Data.cpp
@@ -87,13 +87,16 @@ OSQPData* const & OsqpEigen::Data::getData() const
 
 bool OsqpEigen::Data::isSet() const
 {
+    const bool areConstraintsOk = (m_data->m == 0) ||
+        m_isLinearConstraintsMatrixSet &&
+        m_isLowerBoundSet &&
+        m_isUpperBoundSet;
+
     return m_isNumberOfVariablesSet &&
         m_isNumberOfConstraintsSet &&
         m_isHessianMatrixSet &&
         m_isGradientSet &&
-        m_isLinearConstraintsMatrixSet &&
-        m_isLowerBoundSet &&
-        m_isUpperBoundSet;
+        areConstraintsOk;
 }
 
 bool OsqpEigen::Data::setGradient(Eigen::Ref<Eigen::Matrix<c_float, Eigen::Dynamic, 1>> gradient)

diff --git a/src/Solver.cpp b/src/Solver.cpp
@@ -79,6 +79,25 @@ bool OsqpEigen::Solver::initSolver()
         return false;
     }
 
+    // if the number of constraints is equal to zero the user may not
+    // call setLinearConstraintsMatrix()
+    if(m_data->getData()->m == 0)
+    {
+        if(m_data->getData()->A == nullptr)
+        {
+            // let's create the matrix manually. This is required by osqp. Please check
+            // https://github.com/oxfordcontrol/osqp/issues/295
+            Eigen::SparseMatrix<c_float> A(m_data->getData()->m, m_data->getData()->n);
+            if(!m_data->setLinearConstraintsMatrix(A))
+            {
+                debugStream() << "[OsqpEigen::Solver::initSolver] Unable to set the empty linear constraint "
+                              << "matrix in case of unconstrained optimization problem"
+                              << std::endl;
+                return false;
+            }
+        }
+    }
+
     OSQPWorkspace* workspace;
     if(osqp_setup(&workspace, m_data->getData(),
                   m_settings->getSettings()) != 0 ){

diff --git a/tests/QPTest.cpp b/tests/QPTest.cpp
@@ -10,20 +10,55 @@
 
 #include <OsqpEigen/OsqpEigen.h>
 
+TEST_CASE("QPProblem - Unconstrained")
+{
+    constexpr double tolerance = 1e-4;
+
+    Eigen::SparseMatrix<double> H_s(2,2);
+    H_s.insert(0,0) = 3;
+    H_s.insert(0,1) = 2;
+    H_s.insert(1,0) = 2;
+    H_s.insert(1,1) = 4;
+
+    Eigen::Vector2d gradient;
+    gradient << 3, 1;
+
+    OsqpEigen::Solver solver;
+    solver.settings()->setVerbosity(true);
+    solver.settings()->setAlpha(1.0);
+
+    solver.data()->setNumberOfVariables(2);
+    solver.data()->setNumberOfConstraints(0);
+
+    REQUIRE(solver.data()->setHessianMatrix(H_s));
+    REQUIRE(solver.data()->setGradient(gradient));
+
+    REQUIRE(solver.initSolver());
+    REQUIRE(solver.solve());
+
+    // expected solution
+    Eigen::Vector2d expectedSolution;
+    expectedSolution << -1.2500,  0.3750;
+
+    REQUIRE(solver.getSolution().isApprox(expectedSolution, tolerance));
+}
+
+
 TEST_CASE("QPProblem")
 {
-    Eigen::Matrix2d H;
-    H << 4, 1,
-        1, 2;
-    Eigen::SparseMatrix<double> H_s;
-    H_s = H.sparseView();
-
-    Eigen::Matrix<double,3,2> A;
-    A << 1, 1,
-        1, 0,
-        0, 1;
-    Eigen::SparseMatrix<double> A_s;
-    A_s = A.sparseView();
+    constexpr double tolerance = 1e-4;
+
+    Eigen::SparseMatrix<double> H_s(2,2);
+    H_s.insert(0,0) = 4;
+    H_s.insert(0,1) = 1;
+    H_s.insert(1,0) = 1;
+    H_s.insert(1,1) = 2;
+
+    Eigen::SparseMatrix<double> A_s(3,2);
+    A_s.insert(0,0) = 1;
+    A_s.insert(0,1) = 1;
+    A_s.insert(1,0) = 1;
+    A_s.insert(2,1) = 1;
 
     Eigen::Vector2d gradient;
     gradient << 1, 1;
@@ -35,7 +70,8 @@ TEST_CASE("QPProblem")
     upperBound << 1, 0.7, 0.7;
 
     OsqpEigen::Solver solver;
-    solver.settings()->setVerbosity(false);
+    solver.settings()->setVerbosity(true);
+    solver.settings()->setAlpha(1.0);
 
     REQUIRE_FALSE(solver.data()->setHessianMatrix(H_s));
     solver.data()->setNumberOfVariables(2);
@@ -47,12 +83,11 @@ TEST_CASE("QPProblem")
     REQUIRE(solver.data()->setLowerBound(lowerBound));
     REQUIRE(solver.data()->setUpperBound(upperBound));
 
-
     REQUIRE(solver.initSolver());
 
     REQUIRE(solver.solve());
+    Eigen::Vector2d expectedSolution;
+    expectedSolution << 0.3,  0.7;
 
-    std::cerr << "The solution of the QP problem is" << std::endl;
-    std::cerr << "[ " << solver.getSolution() << " ]"
-              << std::endl;
+    REQUIRE(solver.getSolution().isApprox(expectedSolution, tolerance));
 }