implement update methods in OptimizatorWorkspace class

include/OptimizatorWorkspace.hpp

@@ -4,13 +4,16 @@
  * @copyright  Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
  * @date 2018
  */
-
 #ifndef OPTIMIZATOR_WORKSPACE_HPP
 #define OPTIMIZATOR_WORKSPACE_HPP
 
+// Eigen
+#include <Eigen/Dense>
+
 // OSQP
 #include "osqp.h"
 
+// OSQPWrapper
 #include "OptimizatorData.hpp"
 #include "OptimizatorSettings.hpp"
 
@@ -33,7 +36,7 @@ namespace OSQPWrapper
          * @param settings is an OSQPWrapper::OptimizatorSettings object;
          */
         OptimizatorWorkspace(const OSQPWrapper::OptimizatorData& data,
-                             const OSQPWrapper::OptimizatorSettings& settings);
+                             OSQPWrapper::OptimizatorSettings& settings);
 
         /**
          * Deconstructor.
@@ -51,7 +54,58 @@ namespace OSQPWrapper
          * @return an Eigen::Vector contating the optimization result.
          */
         Eigen::VectorXd getSolution();
+
+        /**
+         * Update the linear part of the cost function (Gradient).
+         * @param gradient is the Gradient vector.
+         * @return true/false in case of success/failure.
+         */
+        template<int n>
+        bool updateGradient(Eigen::Matrix<c_float, n, 1>& gradien);
+
+        /**
+         * Update the lower bound constraints (size m).
+         * @param lowerBound is the lower bound constraint vector.
+         * @return true/false in case of success/failure.
+         */
+        template<int m>
+        bool updateLowerBound(Eigen::Matrix<c_float, m, 1>& lowerBound);
+
+        /**
+         * Update the upper bound constraints (size m).
+         * @param upperBound is the upper bound constraint vector.
+         * @return true/false in case of success/failure.
+         */
+        template<int m>
+        bool updateUpperBound(Eigen::Matrix<c_float, m, 1>& upperBound);
+
+        /**
+         * Update both upper and lower bounds (size m).
+         * @param lowerBound is the lower bound constraint vector;
+         * @param upperBound is the upper bound constraint vector.
+         * @return true/false in case of success/failure.
+         */
+        template<int m>
+        bool updateBounds(Eigen::Matrix<c_float, m, 1>& lowerBound,
+                          Eigen::Matrix<c_float, m, 1>& upperBound);
+
+        /**
+         * Update the quadratic part of the cost function (Hessian).
+         * It is assumed to be a simmetric matrix.
+         * @param hessian is the Hessian matrix.
+         * @return true/false in case of success/failure.
+         */
+        bool updateHessianMatrix(const OSQPWrapper::SparseMatrix& hessian);
+
+        /**
+         * Update the linear constraints matrix A (size m x n)
+         * @param A is the linear constraint matrix.
+         * @return true/false in case of success/failure.
+         */
+        bool updateLinearConstraintsMatrix(const OSQPWrapper::SparseMatrix& A);
     };
+
+    #include "OptimizatorWorkspace.tpp"
 }
 
 #endif

include/OptimizatorWorkspace.tpp

+/**
+ * @file OptimizatorWorkspace.tpp
+ * @author Giulio Romualdi
+ * @copyright Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
+ * @date 2018
+ */
+
+#include <iostream>
+
+template<int n>
+bool OSQPWrapper::OptimizatorWorkspace::updateGradient(Eigen::Matrix<c_float, n, 1>& gradient)
+{
+    // check if the dimension of the gradient is correct
+    if(gradient.rows() != m_workspace->data->n){
+        std::cerr << "[Optimizator Workspace] The size of the gradient must be equal to the number of the variables."
+                  << std::endl;
+        return false;
+    }
+
+    // update the gradient vector
+    if(!osqp_update_lin_cost(m_workspace, gradient.data())){
+        std::cerr << "[Optimizator Workspace] Error when the update gradient is called."
+                  << std::endl;
+        return false;
+    }
+    return true;
+}
+
+template<int m>
+bool OSQPWrapper::OptimizatorWorkspace::updateLowerBound(Eigen::Matrix<c_float, m, 1>& lowerBound)
+{
+    // check if the dimension of the lowerBound vector is correct
+    if(lowerBound.rows() != m_workspace->data->m){
+        std::cerr << "[Optimizator Workspace] The size of the lower bound must be equal to the number of the variables."
+                  << std::endl;
+        return false;
+    }
+
+    // update the lower bound vector
+    if(osqp_update_lower_bound(m_workspace, lowerBound.data())){
+        std::cerr << "[Optimizator Workspace] Error when the update lower bound is called."
+                  << std::endl;
+        return false;
+    }
+
+    return true;
+}
+
+template<int m>
+bool OSQPWrapper::OptimizatorWorkspace::updateUpperBound(Eigen::Matrix<c_float, m, 1>& upperBound)
+{
+    // check if the dimension of the upperBound vector is correct
+    if(upperBound.rows() != m_workspace->data->m){
+        std::cerr << "[Optimizator Workspace] The size of the upper bound must be equal to the number of the variables."
+                  << std::endl;
+        return false;
+    }
+
+    // update the upper bound vector
+    if(osqp_update_upper_bound(m_workspace, upperBound.data())){
+        std::cerr << "[Optimizator Workspace] Error when the update upper bound is called."
+                  << std::endl;
+        return false;
+    }
+    return true;
+}
+
+
+template<int m>
+bool OSQPWrapper::OptimizatorWorkspace::updateBounds(Eigen::Matrix<c_float, m, 1>& lowerBound,
+                                                     Eigen::Matrix<c_float, m, 1>& upperBound)
+{
+    // check if the dimension of the upperBound vector is correct
+    if(upperBound.rows() != m_workspace->data->m){
+        std::cerr << "[Optimizator Workspace] The size of the upper bound must be equal to the number of the variables."
+                  << std::endl;
+        return false;
+    }
+
+    // check if the dimension of the lowerBound vector is correct
+    if(lowerBound.rows() != m_workspace->data->m){
+        std::cerr << "[Optimizator Workspace] The size of the lower bound must be equal to the number of the variables."
+                  << std::endl;
+        return false;
+    }
+
+    // update lower and upper constraints
+    if(osqp_update_bounds(m_workspace, lowerBound.data(), upperBound.data())){
+        std::cerr << "[Optimizator Workspace] Error when the update bounds is called."
+                  << std::endl;
+        return false;
+    }
+    return true;
+}

src/OptimizatorWorkspace.cpp

@@ -11,7 +11,7 @@
 #include "OptimizatorWorkspace.hpp"
 
 OSQPWrapper::OptimizatorWorkspace::OptimizatorWorkspace(const OSQPWrapper::OptimizatorData& data,
-                                                        const OSQPWrapper::OptimizatorSettings& settings)
+                                                        OSQPWrapper::OptimizatorSettings& settings)
 {
     m_workspace = osqp_setup(data.getOptimizatorData(), settings.getOptimizatorSettings());
 }
@@ -36,3 +36,13 @@ Eigen::VectorXd OSQPWrapper::OptimizatorWorkspace::getSolution()
 
     return vector;
 }
+
+bool OSQPWrapper::OptimizatorWorkspace::updateHessianMatrix(const OSQPWrapper::SparseMatrix& hessian)
+{
+    m_workspace->data->P = hessian.getSparseMatrix();
+}
+
+bool OSQPWrapper::OptimizatorWorkspace::updateLinearConstraintsMatrix(const OSQPWrapper::SparseMatrix& A)
+{
+    m_workspace->data->A = A.getSparseMatrix();
+}