implement OptimizatorData class

CMakeLists.txt

@@ -46,11 +46,14 @@ set(LIBRARY_TARGET_NAME OSQPWrapper)
 
 # List of CPP (source) library files.
 set(${LIBRARY_TARGET_NAME}_SRC
-  src/SparseMatrix.cpp)
+  src/SparseMatrix.cpp
+  src/OptimizatorData.cpp)
 
 set(${LIBRARY_TARGET_NAME}_HDR
   include/SparseMatrix.hpp
-  include/SparseMatrix.tpp)
+  include/SparseMatrix.tpp
+  include/OptimizatorData.hpp
+  include/OptimizatorData.tpp)
 
 add_library(${LIBRARY_TARGET_NAME} SHARED ${${LIBRARY_TARGET_NAME}_SRC} ${${LIBRARY_TARGET_NAME}_HDR})
 target_include_directories(${LIBRARY_TARGET_NAME} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"

include/OptimizatorData.hpp

+/**
+ * @file OptimizatorData.hpp
+ * @author Giulio Romualdi
+ * @copyright  Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
+ * @date 2018
+ */
+
+#ifndef OPTIMIZATOR_DATA_HPP
+#define OPTIMIZATOR_DATA_HPP
+
+// OSQP
+#include "osqp.h"
+
+#include "SparseMatrix.hpp"
+
+#include <Eigen/Dense>
+
+/**
+ * OSQPWrapper namespace.
+ */
+namespace OSQPWrapper
+{
+    /**
+     * OptimizatorData class.
+     */
+    class OptimizatorData
+    {
+        OSQPData *m_data;
+
+    public:
+        /**
+         * Constructor.
+         */
+        OptimizatorData();
+
+        /**
+         * Constructor.
+         * @param n is the number of variables;
+         * @param m is the number of constraints.
+         */
+        OptimizatorData(int n, int m);
+
+        /**
+         * Deconstructor.
+         */
+        ~OptimizatorData();
+
+        /**
+         * Set the number of variables.
+         * @param n is the number of variables.
+         */
+        void setNumberOfVariables(int n);
+
+        /**
+         * Set the number of constraints.
+         * @param m is the number of constraints.
+         */
+        void setNumberOfConstraints(int m);
+
+        /**
+         * Set 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 setHessianMatrix(const OSQPWrapper::SparseMatrix& hessian);
+
+        /**
+         * Set 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 setGradient(const Eigen::Matrix<c_float, n, 1>& gradien);
+
+        /**
+         * Set 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 setLinearConstraintsMatrix(const OSQPWrapper::SparseMatrix& A);
+
+        /**
+         * Set the array for lower bound (size m).
+         * @param lowerBound is the lower bound constraint.
+         * @return true/false in case of success/failure.
+         */
+        template<int m>
+        bool setLowerBound(const Eigen::Matrix<c_float, m, 1>& lowerBound);
+
+        /**
+         * Set the array for upper bound (size m).
+         * @param upperBound is the upper bound constraint.
+         * @return true/false in case of success/failure.
+         */
+        template<int m>
+        bool setUpperBound(const Eigen::Matrix<c_float, m, 1>& upperBound);
+
+        /**
+         * Get the OSQPData struct.
+         * @return a const point to the OSQPData struct.
+         */
+        OSQPData *const & getOptimizatorData() const;
+    };
+
+}
+
+#include "OptimizatorData.tpp"
+
+#endif

include/OptimizatorData.tpp

+/**
+ * @file OptimizatorData.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::OptimizatorData::setGradient(const Eigen::Matrix<c_float, n, 1>& gradient)
+{
+    if(n != m_data->n){
+        std::cerr << "[Optimizator Data] The size of the gradient must be equal to the number of the variables."
+                  << std::endl;
+        return false;
+    }
+    m_data->q = gradient.data();
+    return true;
+}
+
+template<int m>
+bool OSQPWrapper::OptimizatorData::setLowerBound(const Eigen::Matrix<c_float, m, 1>& lowerBound)
+{
+    if(m != m_data->m){
+        std::cerr << "[Optimizator Data] The size of the lower bound must be equal to the number of the variables."
+                  << std::endl;
+        return false;
+    }
+    m_data->l = lowerBound.data();
+    return true;
+}
+
+template<int m>
+bool OSQPWrapper::OptimizatorData::setUpperBound(const Eigen::Matrix<c_float, m, 1>& upperBound)
+{
+    if(m != m_data->m){
+        std::cerr << "[Optimizator Data] The size of the upper bound must be equal to the number of the variables."
+                  << std::endl;
+        return false;
+    }
+    m_data->u = upperBound.data();
+    return true;
+}

src/OptimizatorData.cpp

+/**
+ * @file OptimizatorData.cpp
+ * @author Giulio Romualdi
+ * @copyright Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
+ * @date 2018
+ */
+
+// std
+#include <iostream>
+#include <utility>
+
+// OSQPWrapper
+#include "SparseMatrix.hpp"
+#include "OptimizatorData.hpp"
+
+OSQPWrapper::OptimizatorData::OptimizatorData()
+{
+    m_data = (OSQPData *)c_malloc(sizeof(OSQPData));
+}
+
+OSQPWrapper::OptimizatorData::OptimizatorData(int n, int m)
+{
+    m_data = (OSQPData *)c_malloc(sizeof(OSQPData));
+    setNumberOfVariables(n);
+    setNumberOfConstraints(m);
+}
+
+OSQPWrapper::OptimizatorData::~OptimizatorData()
+{
+    c_free(m_data);
+}
+
+void OSQPWrapper::OptimizatorData::setNumberOfVariables(int n)
+{
+    m_data->n = n;
+}
+
+void OSQPWrapper::OptimizatorData::setNumberOfConstraints(int m)
+{
+    m_data->m = m;
+}
+
+bool OSQPWrapper::OptimizatorData::setHessianMatrix(const OSQPWrapper::SparseMatrix& hessian)
+{
+    std::pair<c_int, c_int> matrixSize = hessian.size();
+    if ((std::get<0>(matrixSize) != m_data->n) || (std::get<1>(matrixSize) != m_data->n)){
+        std::cerr << "[Optimizator Data] The Hessian matrix must have n x n size."
+                  << std::endl;
+        return false;
+    }
+    m_data->P = hessian.getSparseMatrix();
+    return true;
+}
+
+bool OSQPWrapper::OptimizatorData::setLinearConstraintsMatrix(const OSQPWrapper::SparseMatrix& A)
+{
+    std::pair<c_int, c_int> matrixSize = A.size();
+    if ((std::get<0>(matrixSize) != m_data->m) || (std::get<1>(matrixSize) != m_data->n)){
+        std::cerr << "[Optimizator Data] The Linear constraints matrix matrix must have m x n size."
+                  << std::endl;
+        return false;
+    }
+    m_data->A = A.getSparseMatrix();
+    return true;
+}
+
+OSQPData* const & OSQPWrapper::OptimizatorData::getOptimizatorData() const
+{
+    return m_data;
+}