Merge pull request #89 from robotology/fix/solver_not_initialized

Check if the solver is initialized in some methods of Solver class
robotology · Mar 26, 2021 · 0f3f8c7 · 0f3f8c7
2 parents 41b371d + 138b6db
commit 0f3f8c7
Show file tree

Hide file tree

Showing 2 changed files with 49 additions and 0 deletions.
diff --git a/include/OsqpEigen/Solver.tpp b/include/OsqpEigen/Solver.tpp
@@ -216,6 +216,12 @@ template<typename T, int n, int m>
 bool OsqpEigen::Solver::setWarmStart(const Eigen::Matrix<T, n, 1> &primalVariable,
                                                   const Eigen::Matrix<T, m, 1> &dualVariable)
 {
+    if(!m_isSolverInitialized){
+        debugStream() << "[OsqpEigen::Solver::setWarmStart] The solver is not initialized"
+                      << std::endl;
+        return false;
+    }
+
     if(primalVariable.rows() != m_workspace->data->n){
         debugStream() << "[OsqpEigen::Solver::setWarmStart] The size of the primal variable vector has to be equal to "
                   << " the number of variables."
@@ -240,6 +246,12 @@ bool OsqpEigen::Solver::setWarmStart(const Eigen::Matrix<T, n, 1> &primalVariabl
 template<typename T, int n>
 bool OsqpEigen::Solver::setPrimalVariable(const Eigen::Matrix<T, n, 1> &primalVariable)
 {
+    if(!m_isSolverInitialized){
+        debugStream() << "[OsqpEigen::Solver::setPrimalVariable] The solver is not initialized"
+                      << std::endl;
+        return false;
+    }
+
     if(primalVariable.rows() != m_workspace->data->n){
         debugStream() << "[OsqpEigen::Solver::setPrimalVariable] The size of the primal variable vector has to be equal to "
                   << " the number of variables."
@@ -271,6 +283,12 @@ bool OsqpEigen::Solver::setDualVariable(const Eigen::Matrix<T, m, 1> &dualVariab
 template<typename T, int n>
 bool OsqpEigen::Solver::getPrimalVariable(Eigen::Matrix<T, n, 1> &primalVariable)
 {
+    if(!m_isSolverInitialized){
+        debugStream() << "[OsqpEigen::Solver::getPrimalVariable] The solver is not initialized"
+                      << std::endl;
+        return false;
+    }
+
     if(n == Eigen::Dynamic){
         primalVariable.resize(m_workspace->data->n, 1);
     }
@@ -291,6 +309,13 @@ bool OsqpEigen::Solver::getPrimalVariable(Eigen::Matrix<T, n, 1> &primalVariable
 template<typename T, int m>
 bool OsqpEigen::Solver::getDualVariable(Eigen::Matrix<T, m, 1> &dualVariable)
 {
+    if(!m_isSolverInitialized){
+        debugStream() << "[OsqpEigen::Solver::getDualVariable] The solver is not initialized"
+                      << std::endl;
+        return false;
+    }
+
+
     if(m == Eigen::Dynamic){
         dualVariable.resize(m_workspace->data->m, 1);
     }

diff --git a/src/Solver.cpp b/src/Solver.cpp
@@ -186,6 +186,12 @@ const std::unique_ptr<OSQPWorkspace, std::function<void(OSQPWorkspace *)>>& Osqp
 
 bool OsqpEigen::Solver::updateGradient(const Eigen::Ref<const Eigen::Matrix<c_float, Eigen::Dynamic, 1>>& gradient)
 {
+    if(!m_isSolverInitialized){
+        debugStream() << "[OsqpEigen::Solver::updateGradient] The solver is not initialized"
+                      << std::endl;
+        return false;
+    }
+
     // check if the dimension of the gradient is correct
     if(gradient.rows() != m_workspace->data->n){
         debugStream() << "[OsqpEigen::Solver::updateGradient] The size of the gradient must be equal to the number of the variables."
@@ -204,6 +210,12 @@ bool OsqpEigen::Solver::updateGradient(const Eigen::Ref<const Eigen::Matrix<c_fl
 
 bool OsqpEigen::Solver::updateLowerBound(const Eigen::Ref<const Eigen::Matrix<c_float, Eigen::Dynamic, 1>>& lowerBound)
 {
+    if(!m_isSolverInitialized){
+        debugStream() << "[OsqpEigen::Solver::updateLowerBound] The solver is not initialized"
+                      << std::endl;
+        return false;
+    }
+
     // check if the dimension of the lowerBound vector is correct
     if(lowerBound.rows() != m_workspace->data->m){
         debugStream() << "[OsqpEigen::Solver::updateLowerBound] The size of the lower bound must be equal to the number of the variables."
@@ -223,6 +235,12 @@ bool OsqpEigen::Solver::updateLowerBound(const Eigen::Ref<const Eigen::Matrix<c_
 
 bool OsqpEigen::Solver::updateUpperBound(const Eigen::Ref<const Eigen::Matrix<c_float, Eigen::Dynamic, 1>>& upperBound)
 {
+    if(!m_isSolverInitialized){
+        debugStream() << "[OsqpEigen::Solver::updateUpperBound] The solver is not initialized"
+                      << std::endl;
+        return false;
+    }
+
     // check if the dimension of the upperBound vector is correct
     if(upperBound.rows() != m_workspace->data->m){
         debugStream() << "[OsqpEigen::Solver::updateUpperBound] The size of the upper bound must be equal to the number of the variables."
@@ -242,6 +260,12 @@ bool OsqpEigen::Solver::updateUpperBound(const Eigen::Ref<const Eigen::Matrix<c_
 bool OsqpEigen::Solver::updateBounds(const Eigen::Ref<const Eigen::Matrix<c_float, Eigen::Dynamic, 1>>& lowerBound,
                                      const Eigen::Ref<const Eigen::Matrix<c_float, Eigen::Dynamic, 1>>& upperBound)
 {
+    if(!m_isSolverInitialized){
+        debugStream() << "[OsqpEigen::Solver::updateBounds] The solver is not initialized"
+                      << std::endl;
+        return false;
+    }
+
     // check if the dimension of the upperBound vector is correct
     if(upperBound.rows() != m_workspace->data->m){
         debugStream() << "[OsqpEigen::Solver::updateBounds] The size of the upper bound must be equal to the number of the variables."