diff --git a/CMakeLists.txt b/CMakeLists.txt index 890c0170..1e5cb79c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -439,7 +439,169 @@ if (${BUILD_MATLAB_INTERFACE}) COPY ${MATLAB_INTERFACE_DIR}/${MATLAB_INTERFACE_NAME}.m DESTINATION ${MATLAB_INTERFACE_DESTINATION} ) - + + file( + COPY ${MATLAB_INTERFACE_DIR}/${MATLAB_INTERFACE_NAME}_options.m + DESTINATION ${MATLAB_INTERFACE_DESTINATION} + ) + + # HERE STARTS A HACK + # Copy over new interface .m file + file( + COPY ${MATLAB_INTERFACE_DIR}/@LCQProblem/LCQProblem.m + DESTINATION ${MATLAB_INTERFACE_DESTINATION}/@LCQProblem + ) + # Include new matlab interface stuff + # constructProblem + matlab_add_mex( + NAME constructProblem + SRC ${MATLAB_INTERFACE_DIR}/@LCQProblem/constructProblem.cpp + OUTPUT_NAME ${MATLAB_INTERFACE_DESTINATION}/@LCQProblem/constructProblem + LINK_TO "-llcqpow -lqpOASES -losqp -lmwblas -lmwlapack -lmwma57" + ) + + target_link_directories( + constructProblem + PUBLIC ${CMAKE_BINARY_DIR}/lib + ) + + target_compile_options( + constructProblem + PUBLIC -D${DEF_SOLVER} + PUBLIC -D__USE_LONG_FINTS__ + PUBLIC -D__MATLAB__ + PUBLIC -D__NO_COPYRIGHT__ + PUBLIC -D__cpluplus + PUBLIC -O + PUBLIC -largeArrayDims + PUBLIC -lmwblas + PUBLIC -lmwlapack + PUBLIC -lmwma57 + PUBLIC -llcqpow + PUBLIC -lqpOASES + PUBLIC -losqp + ) + + target_include_directories( + constructProblem + PRIVATE include + SYSTEM ${osqp_include} + SYSTEM ${qpoases_include} + ) + + # destructProblem + matlab_add_mex( + NAME destructProblem + SRC ${MATLAB_INTERFACE_DIR}/@LCQProblem/destructProblem.cpp + OUTPUT_NAME ${MATLAB_INTERFACE_DESTINATION}/@LCQProblem/destructProblem + LINK_TO "-llcqpow -lqpOASES -losqp -lmwblas -lmwlapack -lmwma57" + ) + + target_link_directories( + destructProblem + PUBLIC ${CMAKE_BINARY_DIR}/lib + ) + + target_compile_options( + destructProblem + PUBLIC -D${DEF_SOLVER} + PUBLIC -D__USE_LONG_FINTS__ + PUBLIC -D__MATLAB__ + PUBLIC -D__NO_COPYRIGHT__ + PUBLIC -D__cpluplus + PUBLIC -O + PUBLIC -largeArrayDims + PUBLIC -lmwblas + PUBLIC -lmwlapack + PUBLIC -lmwma57 + PUBLIC -llcqpow + PUBLIC -lqpOASES + PUBLIC -losqp + ) + + target_include_directories( + destructProblem + PRIVATE include + SYSTEM ${osqp_include} + SYSTEM ${qpoases_include} + ) + + # loadLCQP + matlab_add_mex( + NAME loadLCQP + SRC ${MATLAB_INTERFACE_DIR}/@LCQProblem/loadLCQP.cpp + OUTPUT_NAME ${MATLAB_INTERFACE_DESTINATION}/@LCQProblem/loadLCQP + LINK_TO "-llcqpow -lqpOASES -losqp -lmwblas -lmwlapack -lmwma57" + ) + + target_link_directories( + loadLCQP + PUBLIC ${CMAKE_BINARY_DIR}/lib + ) + + target_compile_options( + loadLCQP + PUBLIC -D${DEF_SOLVER} + PUBLIC -D__USE_LONG_FINTS__ + PUBLIC -D__MATLAB__ + PUBLIC -D__NO_COPYRIGHT__ + PUBLIC -D__cpluplus + PUBLIC -O + PUBLIC -largeArrayDims + PUBLIC -lmwblas + PUBLIC -lmwlapack + PUBLIC -lmwma57 + PUBLIC -llcqpow + PUBLIC -lqpOASES + PUBLIC -losqp + PUBLIC -g + ) + + target_include_directories( + loadLCQP + PRIVATE include + SYSTEM ${osqp_include} + SYSTEM ${qpoases_include} + ) + + # runProblem + matlab_add_mex( + NAME runProblem + SRC ${MATLAB_INTERFACE_DIR}/@LCQProblem/runProblem.cpp + OUTPUT_NAME ${MATLAB_INTERFACE_DESTINATION}/@LCQProblem/runProblem + LINK_TO "-llcqpow -lqpOASES -losqp -lmwblas -lmwlapack -lmwma57" + ) + + target_link_directories( + runProblem + PUBLIC ${CMAKE_BINARY_DIR}/lib + ) + + target_compile_options( + runProblem + PUBLIC -D${DEF_SOLVER} + PUBLIC -D__USE_LONG_FINTS__ + PUBLIC -D__MATLAB__ + PUBLIC -D__NO_COPYRIGHT__ + PUBLIC -D__cpluplus + PUBLIC -O + PUBLIC -largeArrayDims + PUBLIC -lmwblas + PUBLIC -lmwlapack + PUBLIC -lmwma57 + PUBLIC -llcqpow + PUBLIC -lqpOASES + PUBLIC -losqp + PUBLIC -g + ) + + target_include_directories( + runProblem + PRIVATE include + SYSTEM ${osqp_include} + SYSTEM ${qpoases_include} + ) + endif() endif() diff --git a/include/LCQProblem.hpp b/include/LCQProblem.hpp index 8d264478..1c9926ac 100644 --- a/include/LCQProblem.hpp +++ b/include/LCQProblem.hpp @@ -241,6 +241,30 @@ namespace LCQPow { */ inline void setOptions( const Options& _options ); + /** Get nV + * + * @return Returns nV. + */ + int getNV() const; + + /** Get nV + * + * @return Returns nC. + */ + int getNC() const; + + /** Get nV + * + * @return Returns nComp. + */ + int getNComp() const; + + /** Get (a copy of) Options object + * + * @return Returns deep copy of current Options object. + */ + Options getOptions() const; + /** * PROTECTED METHODS diff --git a/include/Utilities.hpp b/include/Utilities.hpp index 6a4a34e3..8bd82193 100644 --- a/include/Utilities.hpp +++ b/include/Utilities.hpp @@ -206,6 +206,7 @@ namespace LCQPow { /** Clear sparse matrix **/ static void ClearSparseMat(csc* M); + static void ClearSparseMatCPP(csc* M); /** Clear sparse matrix **/ @@ -368,4 +369,4 @@ namespace LCQPow { }; } -#endif // LCQPOW_UTILITIES_HPP \ No newline at end of file +#endif // LCQPOW_UTILITIES_HPP diff --git a/interfaces/matlab/@LCQProblem/LCQProblem.m b/interfaces/matlab/@LCQProblem/LCQProblem.m new file mode 100644 index 00000000..be91e5d1 --- /dev/null +++ b/interfaces/matlab/@LCQProblem/LCQProblem.m @@ -0,0 +1,29 @@ +classdef LCQProblem < handle + properties%(Access=private) + self % ptr to C++ LCQProblem object. On a 64 bit x86 + end + + methods + % Constuctor + function obj = LCQProblem(nV, nC, nComp) + obj.constructProblem(nV, nC, nComp); + end + + + % + loadLCQP(obj, Q, g, L, R, lbL, ubL, lbR, ubR, A, lbA, ubA, lb, ub, opts); + + % + [varargout] = runSolver(obj); + + function delete(obj) + % obj is always scalar, if it isn't we panic :) + obj.destructProblem(); + end + end + + methods(Access=private) + constructProblem(obj, nV, nC, nComp); + destructProblem(obj); + end +end diff --git a/interfaces/matlab/@LCQProblem/constructProblem.cpp b/interfaces/matlab/@LCQProblem/constructProblem.cpp new file mode 100644 index 00000000..f7ba14e4 --- /dev/null +++ b/interfaces/matlab/@LCQProblem/constructProblem.cpp @@ -0,0 +1,85 @@ +/* + * This file is part of LCQPow. + * + * LCQPow -- A Solver for Quadratic Programs with Commplementarity Constraints. + * Copyright (C) 2020 - 2022 by Jonas Hall et al. + * + * LCQPow is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * LCQPow is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with LCQPow; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + */ + +#include "mex.hpp" +#include "mexAdapter.hpp" +#include "LCQProblem.hpp" +#include + +using namespace matlab::data; +using matlab::mex::ArgumentList; + +class MexFunction : public matlab::mex::Function { + public: + void operator()(matlab::mex::ArgumentList outputs, matlab::mex::ArgumentList inputs) { + std::shared_ptr matlab = getEngine(); + matlab::data::ArrayFactory factory; + + // Check inputs are valid + checkArguments(outputs, inputs); + + // Assume the inputs are integral, this is generally not true but, shrug, it is fine. + int nV = (int) inputs[1][0]; + int nC = (int) inputs[2][0]; + int nComp = (int) inputs[3][0]; + + // Use raw pointer because we will have to handle this as an implicit unique pointer stored in matlab. + LCQPow::LCQProblem* problem = new LCQPow::LCQProblem(nV,nC,nComp); + + // Reinterpret the pointer to an unsigned integer + std::uintptr_t self = reinterpret_cast(problem); + + // Set the self property the current object + matlab->setProperty(inputs[0], u"self", factory.createScalar(self)); + } + + void checkArguments(matlab::mex::ArgumentList outputs, matlab::mex::ArgumentList inputs) { + std::shared_ptr matlab = getEngine(); + matlab::data::ArrayFactory factory; + if(outputs.size() > 0) + { + matlab->feval(u"error", 0, + std::vector({ factory.createScalar("no outputs returned") })); + + } + if(inputs.size() != 4) + { + matlab->feval(u"error", 0, + std::vector({ factory.createScalar("Incorrect number of inputs") })); + + } + if (inputs[1].getType() != matlab::data::ArrayType::DOUBLE || inputs[1].getNumberOfElements() != 1) + { + matlab->feval(u"error", 0, + std::vector({ factory.createScalar("nV must be a scalar double") })); + } + if (inputs[2].getType() != matlab::data::ArrayType::DOUBLE || inputs[2].getNumberOfElements() != 1) + { + matlab->feval(u"error", 0, + std::vector({ factory.createScalar("nC must be a scalar double") })); + } + if (inputs[3].getType() != matlab::data::ArrayType::DOUBLE || inputs[3].getNumberOfElements() != 1) + { + matlab->feval(u"error", 0, + std::vector({ factory.createScalar("nComp must be a scalar double") })); + } + } +}; diff --git a/interfaces/matlab/@LCQProblem/destructProblem.cpp b/interfaces/matlab/@LCQProblem/destructProblem.cpp new file mode 100644 index 00000000..e800cc45 --- /dev/null +++ b/interfaces/matlab/@LCQProblem/destructProblem.cpp @@ -0,0 +1,78 @@ +/* + * This file is part of LCQPow. + * + * LCQPow -- A Solver for Quadratic Programs with Commplementarity Constraints. + * Copyright (C) 2020 - 2022 by Jonas Hall et al. + * + * LCQPow is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * LCQPow is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with LCQPow; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + */ + +#include "mex.hpp" +#include "mexAdapter.hpp" +#include "LCQProblem.hpp" +#include + +using namespace matlab::data; +using matlab::mex::ArgumentList; + +class MexFunction : public matlab::mex::Function { + public: + void operator()(matlab::mex::ArgumentList outputs, matlab::mex::ArgumentList inputs) { + std::shared_ptr matlab = getEngine(); + matlab::data::ArrayFactory factory; + + // Check inputs are valid + checkArguments(outputs, inputs); + + // Get self value + matlab::data::TypedArray self_array(std::move(matlab->getProperty(inputs[0], u"self"))); + + if (self_array.isEmpty()) + { + return; + } + + std::uintptr_t self_ptr = reinterpret_cast((std::uintptr_t) (self_array[0])); + + // Use raw pointer because we will have to handle this as an implicit unique pointer stored in matlab. + LCQPow::LCQProblem* problem = reinterpret_cast(self_ptr); + + if(problem != nullptr) + { + delete problem; + } + + // Set the self property the current object + //matlab->setProperty(inputs[0], u"self", factory.createEmptyArray()); + matlab->setProperty(inputs[0], u"self", factory.createScalar((std::uintptr_t)0)); + } + + void checkArguments(matlab::mex::ArgumentList outputs, matlab::mex::ArgumentList inputs) { + std::shared_ptr matlab = getEngine(); + matlab::data::ArrayFactory factory; + if(outputs.size() > 0) + { + matlab->feval(u"error", 0, + std::vector({ factory.createScalar("no outputs returned") })); + + } + if(inputs.size() != 1) + { + matlab->feval(u"error", 0, + std::vector({ factory.createScalar("Incorrect number of inputs") })); + + } + } +}; diff --git a/interfaces/matlab/@LCQProblem/loadLCQP.cpp b/interfaces/matlab/@LCQProblem/loadLCQP.cpp new file mode 100644 index 00000000..9caa12ad --- /dev/null +++ b/interfaces/matlab/@LCQProblem/loadLCQP.cpp @@ -0,0 +1,930 @@ +/* + * This file is part of LCQPow. + * + * LCQPow -- A Solver for Quadratic Programs with Commplementarity Constraints. + * Copyright (C) 2020 - 2022 by Jonas Hall et al. + * + * LCQPow is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * LCQPow is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with LCQPow; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + */ + +#include "mex.hpp" +#include "mexAdapter.hpp" +#include "LCQProblem.hpp" +#include +#include +#include +#include +#include + +/* + * This is a macro which processes a matlab::data::Array into a double* pointer + */ +#define UNWRAP_ARRAY(NAME, IDX, SIZE) if(!checkDimensionAndTypeDouble(inputs[IDX], SIZE, 1, #NAME, true)) return 1; \ + bool NAME##_present = !inputs[IDX].isEmpty(); \ + matlab::data::TypedArray NAME##_arr(std::move(inputs[IDX])); \ + matlab::data::buffer_ptr_t NAME##_arr_ptr = NAME##_arr.release(); \ + auto del_##NAME = NAME##_arr_ptr.get_deleter(); \ + if(NAME##_present) \ + { \ + NAME = NAME##_arr_ptr.release(); \ + } + +#define MAYBE_CLEANUP_VECTOR(NAME) if(NAME##_present) \ + { \ + del_##NAME(NAME); \ + } + +#define GET_SCALAR_DOUBLE_OPTION(NAME, SETTER) if(name == #NAME) \ + { \ + if(!checkDimensionAndTypeDouble(field_arr, 1, 1, "params." #NAME)) return 1; \ + matlab::data::TypedArray field(std::move(field_arr)); \ + options.SETTER(field[0]); \ + continue; \ + } + +#define GET_SCALAR_INT_OPTION(NAME, SETTER) if(name == #NAME) \ + { \ + if(!checkDimensionAndTypeDouble(field_arr, 1, 1, "params." #NAME)) return 1; \ + matlab::data::TypedArray field(std::move(field_arr)); \ + options.SETTER((int) (field[0])); \ + continue; \ + } + +#define GET_SCALAR_BOOL_OPTION(NAME, SETTER) if(name == #NAME) \ + { \ + if(!checkDimensionAndTypeBool(field_arr, 1, 1, "params." #NAME)) return 1; \ + matlab::data::TypedArray field(std::move(field_arr)); \ + options.SETTER(field[0]); \ + continue; \ + } + +using namespace matlab::data; +using matlab::mex::ArgumentList; + +class MexFunction : public matlab::mex::Function { + public: + void operator()(matlab::mex::ArgumentList outputs, matlab::mex::ArgumentList inputs) + { + std::shared_ptr matlab = getEngine(); + matlab::data::ArrayFactory factory; + + // Check inputs are valid + checkArguments(outputs, inputs); + + // Get the uintptr_t array from self member property + matlab::data::TypedArray self_array(std::move(matlab->getProperty(inputs[0], u"self"))); + + // Check if we have no value, which _should_ never happen but fail out anyway. + if(self_array.isEmpty()) + { + // TODO(@anton) probably need to throw an exception here. + return; + } + + std::uintptr_t self_ptr = reinterpret_cast((std::uintptr_t) (self_array[0])); + + LCQPow::LCQProblem* problem = reinterpret_cast(self_ptr); + // Check if self ptr is null and fail out immediately. + if(problem == nullptr) + { + // TODO(@anton) probably need to throw an exception here. + return; + } + + // Initialize values needed for options handling + LCQPow::Options options; + double* x0 = nullptr; + double* y0 = nullptr; + + if(!inputs[14].isEmpty()) + { + if(!checkTypeStruct(inputs[14], "opts")) return; + matlab::data::StructArray opts_struct_arr(std::move(inputs[14])); + + handleOptions(problem, opts_struct_arr[0], options, &x0, &y0); + } + + // Check if we are using sparse matrices + bool dense = inputs[1].getType() == matlab::data::ArrayType::DOUBLE; + // Build problem: + if(dense) + { + buildDense(problem, outputs, inputs, x0, y0); + } + else + { + buildSparse(problem, outputs, inputs, x0, y0); + } + // call set options + // Set options and print them + problem->setOptions(options); + } + + int handleOptions(LCQPow::LCQProblem* problem, matlab::data::Struct opts_struct, LCQPow::Options& options, double** x0, double** y0) + { + const std::string params_fieldnames[] = { + "x0", + "y0", + "stationarityTolerance", + "complementarityTolerance", + "initialPenaltyParameter", + "penaltyUpdateFactor", + "solveZeroPenaltyFirst", + "maxIterations", + "maxPenaltyParameter", + "nDynamicPenalty", + "etaDynamicPenalty", + "printLevel", + "storeSteps", + "qpSolver", + "perturbStep", + "qpOASES_options", + "OSQP_options" + }; + + int nV = problem->getNV(); + int nC = problem->getNC(); + int nComp = problem->getNComp(); + bool dual_guess_passed = false; + matlab::data::Array dual_guess_field_arr; + for(auto name : params_fieldnames) + { + matlab::data::Array field_arr; + try + { + field_arr = std::move(opts_struct[name]); // don't copy just move. + } + catch(matlab::data::InvalidFieldNameException err) // I hate there isn't a way to check without exception handling. + { + continue; // missing, do nothing. + } + + GET_SCALAR_DOUBLE_OPTION(stationarityTolerance, setStationarityTolerance); + GET_SCALAR_DOUBLE_OPTION(complementarityTolerance, setComplementarityTolerance); + GET_SCALAR_DOUBLE_OPTION(initialPenaltyParameter, setInitialPenaltyParameter); + GET_SCALAR_DOUBLE_OPTION(penaltyUpdateFactor, setPenaltyUpdateFactor); + GET_SCALAR_DOUBLE_OPTION(etaDynamicPenalty, setEtaDynamicPenalty); + GET_SCALAR_DOUBLE_OPTION(maxPenaltyParameter, setMaxPenaltyParameter); + GET_SCALAR_BOOL_OPTION(solveZeroPenaltyFirst, setSolveZeroPenaltyFirst); + GET_SCALAR_BOOL_OPTION(storeSteps, setStoreSteps); + GET_SCALAR_INT_OPTION(maxIterations, setMaxIterations); + GET_SCALAR_INT_OPTION(nDynamicPenatly, setNDynamicPenalty); + GET_SCALAR_INT_OPTION(printLevel, setPrintLevel); + GET_SCALAR_INT_OPTION(printLevel, setPrintLevel); + GET_SCALAR_INT_OPTION(qpSolver, setQPSolver); + + if(name == "x0") { + if(!checkDimensionAndTypeDouble(field_arr, nV, 1, "params.x0")) return 1; + + matlab::data::TypedArray field(std::move(field_arr)); + *x0 = new double[nV]; // copy because otherwise we need to carry the deleter around :( + std::copy(field.begin(), field.end(), *x0); + } + + if(name == "y0") { + dual_guess_passed = true; + dual_guess_field_arr = std::move(field_arr); + } + + if(name == "qpOASES_options") { + if(!checkTypeStruct(field_arr, "params.qpOASES_options")) return 1; + + matlab::data::StructArray field(std::move(field_arr)); + qpOASES::Options opts; + setupqpOASESOptions(&opts, field); + options.setqpOASESOptions(opts); + } + + if(name == "OSQP_options") { + if(!checkTypeStruct(field_arr, "params.OSQP_options")) return 1; + + matlab::data::StructArray field(std::move(field_arr)); + OSQPSettings* opts = (OSQPSettings *)c_malloc(sizeof(OSQPSettings)); + osqp_set_default_settings(opts); + setupOSQPOptions(opts, field); + options.setOSQPOptions(opts); + c_free(opts); + } + } + + if(dual_guess_passed) { + LCQPow::QPSolver sol = options.getQPSolver(); + int nDualsIn = 0; + if(sol < LCQPow::QPSolver::OSQP_SPARSE) { + nDualsIn = nV + nC + 2*nComp; + if(!checkDimensionAndTypeDouble(dual_guess_field_arr, nDualsIn, 1, "params.y0")) return 1; + } else { + nDualsIn = nC + 2*nComp; + if(!checkDimensionAndTypeDouble(dual_guess_field_arr, nDualsIn, 1, "params.y0")) return 1; + } + matlab::data::TypedArray field(std::move(dual_guess_field_arr)); + *y0 = new double[nDualsIn]; // copy because otherwise we need to carry the deleter around :( + std::copy(field.begin(), field.end(), *y0); + } + + return 0; + } + + void checkArguments(matlab::mex::ArgumentList outputs, matlab::mex::ArgumentList inputs) + { + std::shared_ptr matlab = getEngine(); + matlab::data::ArrayFactory factory; + if(outputs.size() > 0) + { + matlab->feval(u"error", 0, + std::vector({ factory.createScalar("no outputs returned") })); + + } + if(inputs.size() != 15) + { + matlab->feval(u"error", 0, + std::vector({ factory.createScalar("Incorrect number of inputs") })); + + } + } + + int buildDense(LCQPow::LCQProblem* problem, matlab::mex::ArgumentList outputs, matlab::mex::ArgumentList inputs, const double* const x0, const double* const y0) + { + double* Q = nullptr; + double* g = nullptr; + double* L = nullptr; + double* R = nullptr; + double* lbL = nullptr; + double* ubL = nullptr; + double* lbR = nullptr; + double* ubR = nullptr; + double* lb = nullptr; + double* ub = nullptr; + double* A = nullptr; + double* lbA = nullptr; + double* ubA = nullptr; + + // Temporary matrices to keep the column major formats + double* L_col = nullptr; + double* R_col = nullptr; + double* A_col = nullptr; + + int nV = problem->getNV(); + int nC = problem->getNC(); + int nComp = problem->getNComp(); + + // PARSE INPUTS + // Get Q. Q is assumed symmetric PSD so we don't care that matlab uses column major order. + if(!checkDimensionAndTypeDouble(inputs[1], nV, nV, "Q")) return 1; + matlab::data::TypedArray Q_arr(std::move(inputs[1])); + matlab::data::buffer_ptr_t Q_arr_ptr = Q_arr.release(); + auto del_Q = Q_arr_ptr.get_deleter(); + Q = Q_arr_ptr.release(); // NOTE: WE ARE NOW RESPONSIBLE FOR FREEING THIS POINTER + + // Get L + if(!checkDimensionAndTypeDouble(inputs[3], nComp, nV, "L")) return 1; + matlab::data::TypedArray L_arr(std::move(inputs[3])); + matlab::data::buffer_ptr_t L_col_ptr = L_arr.release(); + auto del_L = L_col_ptr.get_deleter(); + L_col = L_arr.release().release(); // NOTE: WE ARE NOW RESPONSIBLE FOR FREEING THIS POINTER + if(L_col != nullptr) + { + L = new double[nComp*nV]; + colMajorToRowMajor(L_col, L, nComp, nV); + del_L(L_col); + } + + // Get R + if(!checkDimensionAndTypeDouble(inputs[4], nComp, nV, "R")) return 1; + matlab::data::TypedArray R_arr(std::move(inputs[4])); + matlab::data::buffer_ptr_t R_col_ptr = R_arr.release(); + auto del_R = R_col_ptr.get_deleter(); + R_col = R_col_ptr.release(); // NOTE: WE ARE NOW RESPONSIBLE FOR FREEING THIS POINTER + if(R_col != nullptr) + { + R = new double[nComp*nV]; + colMajorToRowMajor(R_col, R, nComp, nV); + del_R(R_col); + } + + // Get A + if(nC>0 && !checkDimensionAndTypeDouble(inputs[9], nC, nV, "A")) return 1; + matlab::data::TypedArray A_arr(std::move(inputs[9])); + if(nC>0 && !A_arr.isEmpty()) + { + matlab::data::buffer_ptr_t A_col_ptr = A_arr.release(); + auto del_A = A_col_ptr.get_deleter(); + A_col = A_col_ptr.release(); // NOTE: WE ARE NOW RESPONSIBLE FOR FREEING THIS POINTER + A = new double[nC*nV]; + colMajorToRowMajor(A_col, A, nC, nV); + del_A(A_col); + } + + // Get vectors + if(!checkDimensionAndTypeDouble(inputs[2], nV, 1, "g")) return 1; + matlab::data::TypedArray g_arr(std::move(inputs[2])); + matlab::data::buffer_ptr_t g_arr_ptr = g_arr.release(); + auto del_g = g_arr_ptr.get_deleter(); + g = g_arr_ptr.release(); + + // Get all bounding (optional) vectors + UNWRAP_ARRAY(lbL,5,nComp); + UNWRAP_ARRAY(ubL,6,nComp); + UNWRAP_ARRAY(lbR,7,nComp); + UNWRAP_ARRAY(ubR,8,nComp); + UNWRAP_ARRAY(lbA,10,nC); + UNWRAP_ARRAY(ubA,11,nC); + UNWRAP_ARRAY(lb,12,nV); + UNWRAP_ARRAY(ub,13,nV); + + LCQPow::ReturnValue ret = problem->loadLCQP(Q, g, L, R, lbL, ubL, lbR, ubR, A, lbA, ubA, lb, ub, x0, y0); + + // Cleanup arrays + if(A != nullptr) + { + delete[] A; + } + if(L != nullptr) + { + delete[] L; + } + if(R != nullptr) + { + delete[] R; + } + if(Q != nullptr) + { + del_Q(Q); + } + // Cleanup vectors + del_g(g); + MAYBE_CLEANUP_VECTOR(lbL); + MAYBE_CLEANUP_VECTOR(ubL); + MAYBE_CLEANUP_VECTOR(lbR); + MAYBE_CLEANUP_VECTOR(ubR); + MAYBE_CLEANUP_VECTOR(lbA); + MAYBE_CLEANUP_VECTOR(ubA); + MAYBE_CLEANUP_VECTOR(lb); + MAYBE_CLEANUP_VECTOR(ub); + + return ret; + } + + int buildSparse(LCQPow::LCQProblem* problem, matlab::mex::ArgumentList outputs, matlab::mex::ArgumentList inputs, const double* const x0, const double* const y0) + { + std::shared_ptr matlab = getEngine(); + matlab::data::ArrayFactory factory; + int nV = problem->getNV(); + int nC = problem->getNC(); + int nComp = problem->getNComp(); + if ( !(inputs[1].getType() == matlab::data::ArrayType::SPARSE_DOUBLE) || + !(inputs[3].getType() == matlab::data::ArrayType::SPARSE_DOUBLE) || + !(inputs[4].getType() == matlab::data::ArrayType::SPARSE_DOUBLE) || + (nC > 0 && !(inputs[9].getType() == matlab::data::ArrayType::SPARSE_DOUBLE))) + { + std::ostringstream msg; + msg << "If using the sparse mode, please make sure to provide all matrices in sparse format!\n"; + matlab->feval(u"error", 0, std::vector({factory.createScalar(msg.str())})); + return LCQPow::ReturnValue::DENSE_SPARSE_MISSMATCH; + } + matlab::data::SparseArray Q_arr(std::move(inputs[1])); + matlab::data::SparseArray L_arr(std::move(inputs[3])); + matlab::data::SparseArray R_arr(std::move(inputs[4])); + //matlab::data::SparseArray Q_arr(inputs[1]); + //matlab::data::SparseArray L_arr(inputs[3]); + //matlab::data::SparseArray R_arr(inputs[4]); + // Read sparse matrices + csc* Q = readSparseMatrix(Q_arr, nV, nV); + csc* L = readSparseMatrix(L_arr, nComp, nV); + csc* R = readSparseMatrix(R_arr, nComp, nV); + csc* A = nullptr; + if (nC > 0) { + matlab::data::SparseArray A_arr(std::move(inputs[9])); + //matlab::data::SparseArray A_arr(inputs[9]); + A = readSparseMatrix(A_arr, nC, nV); + } + + // Read vectors + double* g; + double* lbL = nullptr; + double* ubL = nullptr; + double* lbR = nullptr; + double* ubR = nullptr; + double* lbA = nullptr; + double* ubA = nullptr; + double* lb = nullptr; + double* ub = nullptr; + + // Get vectors + if(!checkDimensionAndTypeDouble(inputs[2], nV, 1, "g")) return 1; + matlab::data::TypedArray g_arr(std::move(inputs[2])); + matlab::data::buffer_ptr_t g_arr_ptr = g_arr.release(); + auto del_g = g_arr_ptr.get_deleter(); + g = g_arr_ptr.release(); + + // Get all bounding (optional) vectors + UNWRAP_ARRAY(lbL,5,nComp); + UNWRAP_ARRAY(ubL,6,nComp); + UNWRAP_ARRAY(lbR,7,nComp); + UNWRAP_ARRAY(ubR,8,nComp); + UNWRAP_ARRAY(lbA,10,nC); + UNWRAP_ARRAY(ubA,11,nC); + UNWRAP_ARRAY(lb,12,nV); + UNWRAP_ARRAY(ub,13,nV); + + LCQPow::ReturnValue ret = problem->loadLCQP(Q, g, L, R, lbL, ubL, lbR, ubR, A, lbA, ubA, lb, ub, x0, y0); + + // Cleanup vectors + del_g(g); + MAYBE_CLEANUP_VECTOR(lbL); + MAYBE_CLEANUP_VECTOR(ubL); + MAYBE_CLEANUP_VECTOR(lbR); + MAYBE_CLEANUP_VECTOR(ubR); + MAYBE_CLEANUP_VECTOR(lbA); + MAYBE_CLEANUP_VECTOR(ubA); + MAYBE_CLEANUP_VECTOR(lb); + MAYBE_CLEANUP_VECTOR(ub); + + // Clear sparse specific memory + LCQPow::Utilities::ClearSparseMat(Q); + LCQPow::Utilities::ClearSparseMat(L); + LCQPow::Utilities::ClearSparseMat(R); + if (A != 0) LCQPow::Utilities::ClearSparseMat(A); + return ret; + } + + csc* readSparseMatrix(matlab::data::SparseArray& mat, int nRow, int nCol) + { + double* x = (double*) malloc(mat.getNumberOfNonZeroElements()*sizeof(double)); + int* i = (int*) malloc(mat.getNumberOfNonZeroElements()*sizeof(int)); + int n = nCol; + int m = nRow; + int* p = (int*) malloc((nCol+1)*sizeof(int)); + + // Get necessary values + auto mat_it = mat.begin(); + auto mat_it_end = mat.end(); + double* x_it = x; + int* p_it = p; + int curr_col = 0; + p[0] = 0; + int nc = 0; + int idx = 0; + // Iterate over all nonzeros; + while(mat_it != mat_it_end) + { + // get index for current nonzero + auto mat_idx = mat.getIndex(mat_it); + int row = mat_idx.first; + int col = mat_idx.second; + // If we have moved on to the next column: populate pointers in p + if(col > curr_col) + { + for(int ii = curr_col+1; ii < col; ii++) + { + p[ii] = p[curr_col]; + } + p[col] = idx; + curr_col = col; + } + // update row data + i[idx] = row; + // copy nonzero into buffer + *x_it = *mat_it; + idx++;x_it++;mat_it++; // move pointers + } + // Populate remaining pointers in p + for(int ii = curr_col+1; ii < nCol; ii++) + { + p[ii] = p[curr_col]; + } + p[nCol] = idx; + + csc* M = (csc *)malloc(sizeof(csc)); + + if(M==nullptr) return nullptr; + + M->m = m; + M->n = n; + M->p = p; + M->i = i; + M->x = x; + M->nz = -1; + M->nzmax = mat.getNumberOfNonZeroElements(); + return M; + } + + void colMajorToRowMajor(double* col_maj, double* row_maj, int m, int n) + { + for (int i = 0; i < m; i++) + for (int j = 0; j < n; j++) + row_maj[i*n + j] = col_maj[j*m + i]; + } + + bool checkDimensionAndTypeDouble(const matlab::data::Array& arr, int m, int n, std::string name, bool allowEmpty = false) + { + std::shared_ptr matlab = getEngine(); + matlab::data::ArrayFactory factory; + if (allowEmpty && arr.isEmpty()) + { + return true; + } + + if (arr.getType() != matlab::data::ArrayType::DOUBLE) + { + std::ostringstream msg; + msg << "Invalid type: " << name << " must be of type double.\n"; + matlab->feval(u"error", 0, std::vector({factory.createScalar(msg.str())})); + return false; + } + + matlab::data::ArrayDimensions dims = arr.getDimensions(); + if (dims.size() != 2 || dims[0] != m || dims[1] != n) { + std::ostringstream msg; + msg << "Invalid dimension: " << name << " must be " << m << " x " << n << "\n"; + matlab->feval(u"error", 0, std::vector({factory.createScalar(msg.str())})); + return false; + } + + return true; + } + + bool checkDimensionAndTypeBool(const matlab::data::Array& arr, int m, int n, std::string name, bool allowEmpty = false) + { + std::shared_ptr matlab = getEngine(); + matlab::data::ArrayFactory factory; + if (allowEmpty && arr.isEmpty()) + { + return true; + } + + if (arr.getType() != matlab::data::ArrayType::LOGICAL) + { + std::ostringstream msg; + msg << "Invalid type: " << name << " must be of type logical.\n"; + matlab->feval(u"error", 0, std::vector({factory.createScalar(msg.str())})); + return false; + } + + matlab::data::ArrayDimensions dims = arr.getDimensions(); + if (dims.size() != 2 || dims[0] != m || dims[1] != n) { + std::ostringstream msg; + msg << "Invalid dimension: " << name << " must be " << m << " x " << n << "\n"; + matlab->feval(u"error", 0, std::vector({factory.createScalar(msg.str())})); + return false; + } + + return true; + } + + bool checkTypeStruct(const matlab::data::Array& arr, std::string name) + { + std::shared_ptr matlab = getEngine(); + matlab::data::ArrayFactory factory; + if(arr.isEmpty()) + { + return true; + } + + if(arr.getType() != matlab::data::ArrayType::STRUCT) + { + std::ostringstream msg; + msg << "Invalid type: " << name << " must be of type struct.\n"; + matlab->feval(u"error", 0, std::vector({factory.createScalar(msg.str())})); + return false; + } + + matlab::data::ArrayDimensions dims = arr.getDimensions(); + if(dims.size() != 2 || dims[0] != 1 || dims[1] != 1) + { + std::ostringstream msg; + msg << "Invalid dimension: " << name << " must be a scalar struct\n"; + matlab->feval(u"error", 0, std::vector({factory.createScalar(msg.str())})); + return false; + } + + return true; + } + + /******************************/ + /** QPOASES OPTIONS HANDLING **/ + /******************************/ + + /* + * has options value + */ + bool hasOptionsValue(const matlab::data::Struct& options, const std::string& name, double* optionValue) + { + std::shared_ptr matlab = getEngine(); + matlab::data::ArrayFactory factory; + matlab::data::Array option; + try + { + option = std::move(options[name]); + } + catch(matlab::data::InvalidFieldNameException err) // I hate there isn't a way to check without exception handling. + { + std::ostringstream msg; + msg << "Option struct does not contain entry " << name << ", using default value instead!\n"; + matlab->feval(u"warning", 0, std::vector({factory.createScalar(msg.str())})); + return qpOASES::BT_FALSE; + } + + if(option.getType()== matlab::data::ArrayType::DOUBLE && !option.isEmpty() && option.getNumberOfElements() == 1) + { + matlab::data::TypedArray val_arr(std::move(option)); + *optionValue = val_arr[0]; + return qpOASES::BT_TRUE; + } + else + { + std::ostringstream msg; + msg << "Option " << name << "is not scalar, using default value instead!\n"; + matlab->feval(u"warning", 0, std::vector({factory.createScalar(msg.str())})); + return qpOASES::BT_FALSE; + } + } + + /* + * TODO(@anton): This is currently broken. + */ + bool hasOptionsValue(const matlab::data::Struct& options, const std::string& name, char** optionValue) + { + std::shared_ptr matlab = getEngine(); + matlab::data::ArrayFactory factory; + matlab::data::Array option; + try + { + option = std::move(options[name]); + } + catch(matlab::data::InvalidFieldNameException err) // I hate there isn't a way to check without exception handling. + { + std::ostringstream msg; + msg << "Option struct does not contain entry " << name << ", using default value instead!\n"; + matlab->feval(u"warning", 0, std::vector({factory.createScalar(msg.str())})); + return qpOASES::BT_FALSE; + } + + if(option.getType()== matlab::data::ArrayType::MATLAB_STRING && !option.isEmpty() && option.getNumberOfElements() == 1) + { + matlab::data::TypedArray val_arr(std::move(option)); + matlab::data::MATLABString str = val_arr[0]; + // *optionValue = val_arr[0]; + // fix this + return qpOASES::BT_TRUE; + } + else + { + std::ostringstream msg; + msg << "Option " << name << "is not scalar, using default value instead!\n"; + matlab->feval(u"warning", 0, std::vector({factory.createScalar(msg.str())})); + return qpOASES::BT_FALSE; + } + } + +/* + * setup qpOASES options + */ + void setupqpOASESOptions(qpOASES::Options* options, const matlab::data::StructArray& optionsStructArr) + { + std::shared_ptr matlab = getEngine(); + matlab::data::ArrayFactory factory; + double optionValue; + qpOASES::int_t optionValueInt; + matlab::data::Struct optionsStruct(std::move(optionsStructArr[0])); + + /* Check for correct number of option entries; + * may occur, e.g., if user types options. = ; */ + if(optionsStructArr.getNumberOfFields() != 31) + { + std::ostringstream msg; + msg << "qpOASES options might be set incorrectly as struct has wrong number of entries!" << std::endl; + matlab->feval(u"warning", 0, std::vector({factory.createScalar(msg.str())})); + } + + if(hasOptionsValue(optionsStruct, "printLevel", &optionValue)) + { +#ifdef __SUPPRESSANYOUTPUT__ + options->printLevel = qpOASES::PL_NONE; +#else + optionValueInt = (qpOASES::int_t)optionValue; + options->printLevel = (REFER_NAMESPACE_QPOASES PrintLevel)optionValueInt; + if(options->printLevel < qpOASES::PL_DEBUG_ITER) + options->printLevel = qpOASES::PL_DEBUG_ITER; + if(options->printLevel > qpOASES::PL_HIGH) + options->printLevel = qpOASES::PL_HIGH; +#endif + } + + if(hasOptionsValue(optionsStruct, "enableRamping", &optionValue)) + { + optionValueInt = (qpOASES::int_t)optionValue; + options->enableRamping = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt; + } + + if(hasOptionsValue(optionsStruct, "enableFarBounds", &optionValue)) + { + optionValueInt = (qpOASES::int_t)optionValue; + options->enableFarBounds = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt; + } + + if(hasOptionsValue(optionsStruct, "enableFlippingBounds", &optionValue)) + { + optionValueInt = (qpOASES::int_t)optionValue; + options->enableFlippingBounds = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt; + } + + if(hasOptionsValue(optionsStruct, "enableRegularisation", &optionValue)) + { + optionValueInt = (qpOASES::int_t)optionValue; + options->enableRegularisation = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt; + } + + if(hasOptionsValue(optionsStruct, "enableFullLITests", &optionValue)) + { + optionValueInt = (qpOASES::int_t)optionValue; + options->enableFullLITests = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt; + } + + if(hasOptionsValue(optionsStruct, "enableNZCTests", &optionValue)) + { + optionValueInt = (qpOASES::int_t)optionValue; + options->enableNZCTests = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt; + } + + if(hasOptionsValue(optionsStruct, "enableDriftCorrection", &optionValue)) + options->enableDriftCorrection = (qpOASES::int_t)optionValue; + + if(hasOptionsValue(optionsStruct, "enableCholeskyRefactorisation", &optionValue)) + options->enableCholeskyRefactorisation = (qpOASES::int_t)optionValue; + + if(hasOptionsValue(optionsStruct, "enableEqualities", &optionValue)) + { + optionValueInt = (qpOASES::int_t)optionValue; + options->enableEqualities = (REFER_NAMESPACE_QPOASES BooleanType)optionValueInt; + } + + if(hasOptionsValue(optionsStruct, "terminationTolerance", &optionValue)) + options->terminationTolerance = optionValue; + + if(hasOptionsValue(optionsStruct, "boundTolerance", &optionValue)) + options->boundTolerance = optionValue; + + if(hasOptionsValue(optionsStruct, "boundRelaxation", &optionValue)) + options->boundRelaxation = optionValue; + + if(hasOptionsValue(optionsStruct, "epsNum", &optionValue)) + options->epsNum = optionValue; + + if(hasOptionsValue(optionsStruct, "epsDen", &optionValue)) + options->epsDen = optionValue; + + if(hasOptionsValue(optionsStruct, "maxPrimalJump", &optionValue)) + options->maxPrimalJump = optionValue; + + if(hasOptionsValue(optionsStruct, "maxDualJump", &optionValue)) + options->maxDualJump = optionValue; + + if(hasOptionsValue(optionsStruct, "initialRamping", &optionValue)) + options->initialRamping = optionValue; + + if(hasOptionsValue(optionsStruct, "finalRamping", &optionValue)) + options->finalRamping = optionValue; + + if(hasOptionsValue(optionsStruct, "initialFarBounds", &optionValue)) + options->initialFarBounds = optionValue; + + if(hasOptionsValue(optionsStruct, "growFarBounds", &optionValue)) + options->growFarBounds = optionValue; + + if(hasOptionsValue(optionsStruct, "initialStatusBounds", &optionValue)) + { + optionValueInt = (qpOASES::int_t)optionValue; + if(optionValueInt < -1) + optionValueInt = -1; + if(optionValueInt > 1) + optionValueInt = 1; + options->initialStatusBounds = (REFER_NAMESPACE_QPOASES SubjectToStatus)optionValueInt; + } + + if(hasOptionsValue(optionsStruct, "epsFlipping", &optionValue)) + options->epsFlipping = optionValue; + + if(hasOptionsValue(optionsStruct, "numRegularisationSteps", &optionValue)) + options->numRegularisationSteps = (qpOASES::int_t)optionValue; + + if(hasOptionsValue(optionsStruct, "epsRegularisation", &optionValue)) + options->epsRegularisation = optionValue; + + if(hasOptionsValue(optionsStruct, "numRefinementSteps", &optionValue)) + options->numRefinementSteps = (qpOASES::int_t)optionValue; + + if(hasOptionsValue(optionsStruct, "epsIterRef", &optionValue)) + options->epsIterRef = optionValue; + + if(hasOptionsValue(optionsStruct, "epsLITests", &optionValue)) + options->epsLITests = optionValue; + + if(hasOptionsValue(optionsStruct, "epsNZCTests", &optionValue)) + options->epsNZCTests = optionValue; + } + + +/* + * setup OSQP options + */ + void setupOSQPOptions(OSQPSettings* settings, const matlab::data::StructArray& optionsStructArr) + { + std::shared_ptr matlab = getEngine(); + matlab::data::ArrayFactory factory; + matlab::data::Struct optionsStruct(std::move(optionsStructArr[0])); + /* Check for correct number of option entries; + * may occur, e.g., if user types options. = ; */ + if(optionsStructArr.getNumberOfFields() != 22) + { + std::ostringstream msg; + msg << "OSQP options might be set incorrectly as struct has wrong number of entries!" << std::endl; + matlab->feval(u"warning", 0, std::vector({factory.createScalar(msg.str())})); + } + + double optionValue; + + if(hasOptionsValue(optionsStruct, "rho", &optionValue)) + settings->rho = (c_float) optionValue; + + if(hasOptionsValue(optionsStruct, "sigma", &optionValue)) + settings->sigma = (c_float) optionValue; + + if(hasOptionsValue(optionsStruct, "scaling", &optionValue)) + settings->scaling = (c_int) optionValue; + + if(hasOptionsValue(optionsStruct, "adaptive_rho", &optionValue)) + settings->adaptive_rho = (c_int) optionValue; + + if(hasOptionsValue(optionsStruct, "adaptive_rho_interval", &optionValue)) + settings->adaptive_rho_interval = (c_int) optionValue; + + if(hasOptionsValue(optionsStruct, "adaptive_rho_tolerance", &optionValue)) + settings->adaptive_rho_tolerance = (c_float) optionValue; + + if(hasOptionsValue(optionsStruct, "adaptive_rho_fraction", &optionValue)) + settings->adaptive_rho_fraction = (c_float) optionValue; + + if(hasOptionsValue(optionsStruct, "max_iter", &optionValue)) + settings->max_iter = (c_int) optionValue; + + if(hasOptionsValue(optionsStruct, "eps_abs", &optionValue)) + settings->eps_abs = (c_float) optionValue; + + if(hasOptionsValue(optionsStruct, "eps_rel", &optionValue)) + settings->eps_rel = (c_float) optionValue; + + if(hasOptionsValue(optionsStruct, "eps_prim_inf", &optionValue)) + settings->eps_prim_inf = (c_float) optionValue; + + if(hasOptionsValue(optionsStruct, "eps_dual_inf", &optionValue)) + settings->eps_dual_inf = (c_float) optionValue; + + if(hasOptionsValue(optionsStruct, "alpha", &optionValue)) + settings->alpha = (c_float) optionValue; + + char* optionStr; + if(hasOptionsValue(optionsStruct, "linsys_solver", &optionStr)) + { + std::ostringstream msg; + msg << "Setting OSQP solver through matlab interface currently not supported (will use qdldl)." << std::endl; + matlab->feval(u"warning", 0, std::vector({factory.createScalar(msg.str())})); + } + + if(hasOptionsValue(optionsStruct, "delta", &optionValue)) + settings->delta = (c_float) optionValue; + + if(hasOptionsValue(optionsStruct, "polish", &optionValue)) + settings->polish = (c_int) optionValue; + + if(hasOptionsValue(optionsStruct, "polish_refine_iter", &optionValue)) + settings->polish_refine_iter = (c_int) optionValue; + + if(hasOptionsValue(optionsStruct, "verbose", &optionValue)) + settings->verbose = (c_int) optionValue; + + if(hasOptionsValue(optionsStruct, "scaled_termination", &optionValue)) + settings->scaled_termination = (c_int) optionValue; + + if(hasOptionsValue(optionsStruct, "check_termination", &optionValue)) + settings->check_termination = (c_int) optionValue; + + if(hasOptionsValue(optionsStruct, "warm_start", &optionValue)) + settings->warm_start = (c_int) optionValue; + + if(hasOptionsValue(optionsStruct, "time_limit", &optionValue)) + settings->time_limit = (c_float) optionValue; + } +}; diff --git a/interfaces/matlab/@LCQProblem/runProblem.cpp b/interfaces/matlab/@LCQProblem/runProblem.cpp new file mode 100644 index 00000000..80a27a65 --- /dev/null +++ b/interfaces/matlab/@LCQProblem/runProblem.cpp @@ -0,0 +1,176 @@ +/* + * This file is part of LCQPow. + * + * LCQPow -- A Solver for Quadratic Programs with Commplementarity Constraints. + * Copyright (C) 2020 - 2022 by Jonas Hall et al. + * + * LCQPow is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * LCQPow is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with LCQPow; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + */ + +#include "mex.hpp" +#include "mexAdapter.hpp" +#include "LCQProblem.hpp" +#include + +using namespace matlab::data; +using matlab::mex::ArgumentList; + +class MexFunction : public matlab::mex::Function { + public: + void operator()(matlab::mex::ArgumentList outputs, matlab::mex::ArgumentList inputs) { + std::shared_ptr matlab = getEngine(); + matlab::data::ArrayFactory factory; + + // Check inputs are valid + checkArguments(outputs, inputs); + // Get self value + matlab::data::TypedArray self_array(std::move(matlab->getProperty(inputs[0], u"self"))); + + if (self_array.isEmpty()) + { + return; + } + + std::uintptr_t self_ptr = reinterpret_cast((std::uintptr_t) (self_array[0])); + + // Use raw pointer because we will have to handle this as an implicit unique pointer stored in matlab. + LCQPow::LCQProblem* problem = reinterpret_cast(self_ptr); + + // Run the LCQPow algorithm on setup object. + std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now(); + LCQPow::ReturnValue ret = problem->runSolver(); + std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now(); + double elapsed_secs = (double)(end - begin).count()/1000.0/1000.0/1000.0; + + // Get dimensions and options for future use + LCQPow::Options opts = problem->getOptions(); + int nV = problem->getNV(); + int nC = problem->getNC(); + int nComp = problem->getNComp(); + int nDuals = problem->getNumberOfDuals(); + + // Setup output arrays + double* x_opt = new double[nV]; + double* y_opt = new double[nDuals]; + LCQPow::OutputStatistics stats; + + // Get output data + problem->getPrimalSolution(x_opt); + problem->getDualSolution(y_opt); + problem->getOutputStatistics(stats); + + // Generate output array. This does copy because mathworks changed how createArrayFromBuffer + // works in 2024b but didn't document this change. Therefore we can't use it + matlab::data::TypedArray x_out = factory.createArray({nV, 1}, x_opt, x_opt + nV); + + // Move (not copy) the primal output to the first output + outputs[0] = std::move(x_out); + + // Repeat this for y + if(outputs.size() >= 2) + { + matlab::data::TypedArray y_out = factory.createArray({nDuals, 1}, y_opt, y_opt + nV); + outputs[1] = std::move(y_out); + } + + // Process output statistics if necessary. + if(outputs.size() >= 3) + { + matlab::data::Array stats_struct; + if(opts.getStoreSteps()) + { + stats_struct = std::move(factory.createStructArray({1,1}, {"iters_total", "iters_outer", "iters_subproblem", "rho_opt", + "elapsed_time", "exit_flag", "solution_type", "qp_exit_flag", + "innerIters", "xSteps", "accumulatedSubproblemIters", "stepLength", "stepSize", + "statVals", "objVals", "phiVals", "meritVals", "subproblemIters"})); + } + else + { + stats_struct = std::move(factory.createStructArray({1,1}, {"iters_total", "iters_outer", "iters_subproblem", "rho_opt", + "elapsed_time", "exit_flag", "solution_type", "qp_exit_flag"})); + } + + // Add always present entries + int iters_total = stats.getIterTotal(); + stats_struct[0]["iters_total"] = factory.createScalar(iters_total); + stats_struct[0]["iters_outer"] = factory.createScalar(stats.getIterOuter()); + stats_struct[0]["iters_subproblem"] = factory.createScalar(stats.getSubproblemIter()); + stats_struct[0]["rho_opt"] = factory.createScalar(stats.getRhoOpt()); + stats_struct[0]["elapsed_time"] = factory.createScalar(elapsed_secs); + stats_struct[0]["exit_flag"] = factory.createScalar((int) ret); + stats_struct[0]["solution_type"] = factory.createScalar((int) stats.getSolutionStatus()); + stats_struct[0]["qp_exit_flag"] = factory.createScalar(stats.getQPSolverExitFlag()); + + // Add iter values: + if(opts.getStoreSteps() && stats.getIterTotal() > 0) + { + // xSteps + // TODO(@anton) perhaps this is one too many copies? but annoyingly nothing much can be done I think. + std::vector> xSteps_vec = stats.getxStepsStdVec(); + std::vector xSteps; + xSteps.reserve(iters_total*nV); + for(auto step : xSteps_vec) + { + xSteps.insert(xSteps.end(), step.begin(), step.end()); + } + stats_struct[0]["xSteps"] = factory.createArray({iters_total, nV}, xSteps.begin(), xSteps.end()); + // inner iters + std::vector innerIters = stats.getInnerItersStdVec(); + stats_struct[0]["innerIters"] = factory.createArray({iters_total, 1}, innerIters.begin(), innerIters.end()); + // accuSubproblemItters + std::vector accuSubproblemIters = stats.getInnerItersStdVec(); + stats_struct[0]["accumulatedSubproblemIters"] = factory.createArray({iters_total, 1}, accuSubproblemIters.begin(), accuSubproblemIters.end()); + // stepLength + std::vector stepLength = stats.getStepLengthStdVec(); + stats_struct[0]["stepLength"] = factory.createArray({iters_total, 1}, stepLength.begin(), stepLength.end()); + // stepSize + std::vector stepSize = stats.getStepSizeStdVec(); + stats_struct[0]["stepSize"] = factory.createArray({iters_total, 1}, stepSize.begin(), stepSize.end()); + // statVals + std::vector statVals = stats.getStatValsStdVec(); + stats_struct[0]["statVals"] = factory.createArray({iters_total, 1}, statVals.begin(), statVals.end()); + // objVals + std::vector objVals = stats.getObjValsStdVec(); + stats_struct[0]["objVals"] = factory.createArray({iters_total, 1}, objVals.begin(), objVals.end()); + // phiVals + std::vector phiVals = stats.getPhiValsStdVec(); + stats_struct[0]["phiVals"] = factory.createArray({iters_total, 1}, phiVals.begin(), phiVals.end()); + // meritVals + std::vector meritVals = stats.getMeritValsStdVec(); + stats_struct[0]["meritVals"] = factory.createArray({iters_total, 1}, meritVals.begin(), meritVals.end()); + } + outputs[2] = std::move(stats_struct); + } + } + + void checkArguments(matlab::mex::ArgumentList outputs, matlab::mex::ArgumentList inputs) { + std::shared_ptr matlab = getEngine(); + matlab::data::ArrayFactory factory; + if(outputs.size() > 3) + { + matlab->feval(u"error", 0, + std::vector({ factory.createScalar("A maximum of 3 outputs are provided") })); + + } + if(inputs.size() > 1) + { + matlab->feval(u"error", 0, + std::vector({ factory.createScalar("Incorrect number of inputs") })); + + } + + + } +}; diff --git a/interfaces/matlab/LCQPow.cpp b/interfaces/matlab/LCQPow.cpp index 8f381bb1..34202c5c 100644 --- a/interfaces/matlab/LCQPow.cpp +++ b/interfaces/matlab/LCQPow.cpp @@ -107,7 +107,8 @@ void colMajorToRowMajor(double* col_maj, double* row_maj, int m, int n) row_maj[i*n + j] = col_maj[j*m + i]; } -void printOptions( Options options ) { +void printOptions(Options options) +{ mexPrintf(" \n Using LCQPow Options: \n"); mexPrintf(" rho0: %g \n", options.getInitialPenaltyParameter()); mexPrintf(" beta: %g \n", options.getPenaltyUpdateFactor()); @@ -140,10 +141,9 @@ csc* readSparseMatrix(const mxArray* mat, int nRow, int nCol) return LCQPow::Utilities::createCSC(nRow, nCol, M_p[nCol], M_data, M_i, M_p); } -void readVectors( - const mxArray** prhs, int nrhs, int nC, double** g, - double** lbL, double** ubL, double** lbR, double** ubR, - double** lbA, double** ubA, double** lb, double** ub) +void readVectors(const mxArray** prhs, int nrhs, int nC, double** g, + double** lbL, double** ubL, double** lbR, double** ubR, + double** lbA, double** ubA, double** lb, double** ub) { *g = (double*) mxGetPr( prhs[1] ); *lbL = (double*) mxGetPr( prhs[4] ); @@ -165,8 +165,8 @@ void readVectors( } } -int LCQPSparse(LCQProblem& lcqp, int nV, int nComp, int nC, int nrhs, const mxArray* prhs[], double* x0, double* y0) { - +int LCQPSparse(LCQProblem& lcqp, int nV, int nComp, int nC, int nrhs, const mxArray* prhs[], double* x0, double* y0) +{ if ( !mxIsSparse(prhs[0]) || !mxIsSparse(prhs[2]) || !mxIsSparse(prhs[3]) || (nC > 0 && !mxIsSparse(prhs[8])) ) { mexPrintf("If using the sparse mode, please make sure to provide all matrices in sparse format!\n"); @@ -286,7 +286,7 @@ int LCQPDense(LCQProblem& lcqp, int nV, int nComp, int nC, int nrhs, const mxArr /* * has options value */ -bool hasOptionsValue( const mxArray* optionsPtr, const char* const optionString, double** optionValue ) +bool hasOptionsValue(const mxArray* optionsPtr, const char* const optionString, double** optionValue) { mxArray* optionName = mxGetField( optionsPtr,0,optionString ); @@ -315,7 +315,7 @@ bool hasOptionsValue( const mxArray* optionsPtr, const char* const optionString, /* * has options value */ -bool hasOptionsValue( const mxArray* optionsPtr, const char* const optionString, char** optionValue ) +bool hasOptionsValue(const mxArray* optionsPtr, const char* const optionString, char** optionValue) { mxArray* optionName = mxGetField( optionsPtr,0,optionString ); @@ -344,7 +344,7 @@ bool hasOptionsValue( const mxArray* optionsPtr, const char* const optionString, /* * setup qpOASES options */ -void setupqpOASESOptions( qpOASES::Options* options, const mxArray* optionsPtr ) +void setupqpOASESOptions(qpOASES::Options* options, const mxArray* optionsPtr) { double* optionValue; qpOASES::int_t optionValueInt; @@ -567,7 +567,7 @@ void setupOSQPOptions(OSQPSettings* settings, const mxArray* optionsPtr) /* * The main mex function */ -void mexFunction( int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[] ) +void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) { // Validate number of output arguments int nlhs_min = 1; int nlhs_max = 3; diff --git a/interfaces/matlab/LCQPow_options.m b/interfaces/matlab/LCQPow_options.m index 00b23bb6..53af4ae3 100644 --- a/interfaces/matlab/LCQPow_options.m +++ b/interfaces/matlab/LCQPow_options.m @@ -1,19 +1,14 @@ - -function [ options ] = LCQPow_default_options( ) - +function [ options ] = LCQPow_options( ) % setup options struct with default values - options = struct( 'stationarityTolerance', 1.0e3*eps, ... - 'complementarityTolerance', 1.0e6*eps, ... - 'initialPenaltyParameter', 0.01, ... - 'penaltyUpdateFactor', 2.0 ... - 'solveZeroPenaltyFirst', 1, ... - 'perturbStep', 1, ... - 'maxIterations', 1000, ... - 'maxPenaltyParameter', 1.0e8, ... - 'printLevel', 1, ... - 'nDynamicPenalty', 3, ... - 'etaDynamicPenalty', 0.9, ... - ); - + options = struct( 'stationarityTolerance', 1.0e3*eps, ... + 'complementarityTolerance', 1.0e6*eps, ... + 'initialPenaltyParameter', 0.01, ... + 'penaltyUpdateFactor', 2.0, ... + 'solveZeroPenaltyFirst', true, ... + 'perturbStep', true, ... + 'maxIterations', 1000, ... + 'maxPenaltyParameter', 1.0e8, ... + 'printLevel', 1, ... + 'nDynamicPenalty', 3, ... + 'etaDynamicPenalty', 0.9); end - diff --git a/src/LCQProblem.cpp b/src/LCQProblem.cpp index 18e4d18c..b5a1640a 100644 --- a/src/LCQProblem.cpp +++ b/src/LCQProblem.cpp @@ -32,15 +32,15 @@ #include #include - + using qpOASES::QProblem; namespace LCQPow { - LCQProblem::LCQProblem( ) { } + LCQProblem::LCQProblem() {} - LCQProblem::LCQProblem( int _nV, int _nC, int _nComp ) + LCQProblem::LCQProblem(int _nV, int _nC, int _nComp) { /* consistency checks */ if ( _nV <= 0 ) @@ -79,19 +79,19 @@ namespace LCQPow { } - LCQProblem::~LCQProblem( ) { + LCQProblem::~LCQProblem() + { clear(); } - ReturnValue LCQProblem::loadLCQP( const double* const _Q, const double* const _g, - const double* const _L, const double* const _R, - const double* const _lbL, const double* const _ubL, - const double* const _lbR, const double* const _ubR, - const double* const _A, const double* const _lbA, const double* const _ubA, - const double* const _lb, const double* const _ub, - const double* const _x0, const double* const _y0 - ) + ReturnValue LCQProblem::loadLCQP(const double* const _Q, const double* const _g, + const double* const _L, const double* const _R, + const double* const _lbL, const double* const _ubL, + const double* const _lbR, const double* const _ubR, + const double* const _A, const double* const _lbA, const double* const _ubA, + const double* const _lb, const double* const _ub, + const double* const _x0, const double* const _y0) { ReturnValue ret; @@ -144,14 +144,13 @@ namespace LCQPow { } - ReturnValue LCQProblem::loadLCQP( const char* const Q_file, const char* const g_file, - const char* const L_file, const char* const R_file, - const char* const lbL_file, const char* const ubL_file, - const char* const lbR_file, const char* const ubR_file, - const char* const A_file, const char* const lbA_file, const char* const ubA_file, - const char* const lb_file, const char* const ub_file, - const char* const x0_file, const char* const y0_file - ) + ReturnValue LCQProblem::loadLCQP(const char* const Q_file, const char* const g_file, + const char* const L_file, const char* const R_file, + const char* const lbL_file, const char* const ubL_file, + const char* const lbR_file, const char* const ubR_file, + const char* const A_file, const char* const lbA_file, const char* const ubA_file, + const char* const lb_file, const char* const ub_file, + const char* const x0_file, const char* const y0_file) { ReturnValue ret; @@ -387,14 +386,13 @@ namespace LCQPow { } - ReturnValue LCQProblem::loadLCQP( const csc* const _Q, const double* const _g, - const csc* const _L, const csc* const _R, - const double* const _lbL, const double* const _ubL, - const double* const _lbR, const double* const _ubR, - const csc* const _A, const double* const _lbA, const double* const _ubA, - const double* const _lb, const double* const _ub, - const double* const _x0, const double* const _y0 - ) + ReturnValue LCQProblem::loadLCQP(const csc* const _Q, const double* const _g, + const csc* const _L, const csc* const _R, + const double* const _lbL, const double* const _ubL, + const double* const _lbR, const double* const _ubR, + const csc* const _A, const double* const _lbA, const double* const _ubA, + const double* const _lb, const double* const _ub, + const double* const _x0, const double* const _y0) { ReturnValue ret; @@ -441,7 +439,7 @@ namespace LCQPow { } - ReturnValue LCQProblem::runSolver( ) + ReturnValue LCQProblem::runSolver() { // Initialize variables ReturnValue ret = initializeSolver(); @@ -560,8 +558,8 @@ namespace LCQPow { } - ReturnValue LCQProblem::setConstraints( const double* const L_new, const double* const R_new, - const double* const A_new, const double* const lbA_new, const double* const ubA_new ) + ReturnValue LCQProblem::setConstraints(const double* const L_new, const double* const R_new, + const double* const A_new, const double* const lbA_new, const double* const ubA_new) { if ( nV == 0 || nComp == 0 ) return LCQPOBJECT_NOT_SETUP; @@ -626,16 +624,15 @@ namespace LCQPow { } - ReturnValue LCQProblem::setConstraints( const csc* const L_new, const csc* const R_new, - const csc* const A_new, const double* const lbA_new, const double* const ubA_new - ) + ReturnValue LCQProblem::setConstraints(const csc* const L_new, const csc* const R_new, + const csc* const A_new, const double* const lbA_new, const double* const ubA_new) { // Create sparse matrices L_sparse = Utilities::copyCSC(L_new); R_sparse = Utilities::copyCSC(R_new); // Get number of elements - int tmpA_nnx = L_sparse->p[nV] + R_sparse->p[nV]; + int tmpA_nnx = L_sparse->p[nV] + R_sparse->p[nV]; if (Utilities::isNotNullPtr(A_new)) { tmpA_nnx += Utilities::isNotNullPtr(A_new->p) ? A_new->p[nV] : 0; @@ -723,8 +720,8 @@ namespace LCQPow { } - ReturnValue LCQProblem::setComplementarityBounds(const double* const lbL_new, const double* const ubL_new, const double* const lbR_new, const double* const ubR_new) { - + ReturnValue LCQProblem::setComplementarityBounds(const double* const lbL_new, const double* const ubL_new, const double* const lbR_new, const double* const ubR_new) + { if (Utilities::isNotNullPtr(lbL_new )) { lbL = new double[nComp]; } @@ -785,7 +782,7 @@ namespace LCQPow { } - ReturnValue LCQProblem::setQ( const csc* const Q_new ) + ReturnValue LCQProblem::setQ(const csc* const Q_new) { if (nV <= 0) return LCQPOBJECT_NOT_SETUP; @@ -796,7 +793,7 @@ namespace LCQPow { } - void LCQProblem::setQk( ) + void LCQProblem::setQk() { if (sparseSolver) { std::vector Qk_data; @@ -882,7 +879,7 @@ namespace LCQPow { } - ReturnValue LCQProblem::initializeSolver( ) + ReturnValue LCQProblem::initializeSolver() { ReturnValue ret = SUCCESSFUL_RETURN; if (options.getQPSolver() == QPSolver::QPOASES_DENSE) { @@ -1034,7 +1031,7 @@ namespace LCQPow { } - ReturnValue LCQProblem::switchToSparseMode( ) + ReturnValue LCQProblem::switchToSparseMode() { // Only perform this action if required @@ -1068,7 +1065,7 @@ namespace LCQPow { } - ReturnValue LCQProblem::switchToDenseMode( ) + ReturnValue LCQProblem::switchToDenseMode() { // Only perform this action if required @@ -1148,17 +1145,20 @@ namespace LCQPow { } - bool LCQProblem::stationarityCheck( ) { + bool LCQProblem::stationarityCheck() + { return Utilities::MaxAbs(statk, nV) < options.getStationarityTolerance(); } - bool LCQProblem::complementarityCheck( ) { + bool LCQProblem::complementarityCheck() + { return getPhi() < options.getComplementarityTolerance(); } - double LCQProblem::getObj( ) { + double LCQProblem::getObj() + { double lin = Utilities::DotProduct(g, xk, nV); if (sparseSolver) { @@ -1169,7 +1169,8 @@ namespace LCQPow { } - double LCQProblem::getPhi( ) { + double LCQProblem::getPhi() + { double phi_lin = 0; // Linear term @@ -1185,7 +1186,8 @@ namespace LCQPow { } - double LCQProblem::getMerit( ) { + double LCQProblem::getMerit() + { double lin = Utilities::DotProduct(g, xk, nV); if (sparseSolver) { @@ -1196,7 +1198,8 @@ namespace LCQPow { } - void LCQProblem::updatePenalty( ) { + void LCQProblem::updatePenalty() + { // Clear Leyffer history if (options.getNDynamicPenalty() > 0) complHistory.clear(); @@ -1213,8 +1216,8 @@ namespace LCQPow { } } - - void LCQProblem::getOptimalStepLength( ) { + void LCQProblem::getOptimalStepLength() + { double qk; @@ -1237,13 +1240,15 @@ namespace LCQPow { } - void LCQProblem::updateStep( ) { + void LCQProblem::updateStep() + { // xk = xk + alphak*pk Utilities::WeightedVectorAdd(1, xk, alphak, pk, xk, nV); } - void LCQProblem::updateStationarity( ) { + void LCQProblem::updateStationarity() + { // stat = Qk*xk + g - A'*yk_A - yk_x // 1) Objective contribution: Qk*xk + g if (sparseSolver) { @@ -1272,7 +1277,8 @@ namespace LCQPow { } - bool LCQProblem::leyfferCheckPositive( ) { + bool LCQProblem::leyfferCheckPositive() + { size_t n = (size_t)options.getNDynamicPenalty(); @@ -1313,7 +1319,8 @@ namespace LCQPow { } - void LCQProblem::updateQk( ) { + void LCQProblem::updateQk() + { // Smart update in sparse case if (sparseSolver) { double factor = rho*(1 - 1.0/options.getPenaltyUpdateFactor()); @@ -1326,19 +1333,22 @@ namespace LCQPow { } - void LCQProblem::updateOuterIter( ) { + void LCQProblem::updateOuterIter() + { outerIter++; stats.updateIterOuter(1); } - void LCQProblem::updateTotalIter( ) { + void LCQProblem::updateTotalIter() + { totalIter++; stats.updateIterTotal(1); } - void LCQProblem::perturbGradient( ) { + void LCQProblem::perturbGradient() + { int randNum; for (int i = 0; i < nV; i++) { @@ -1350,7 +1360,8 @@ namespace LCQPow { } - void LCQProblem::perturbStep( ) { + void LCQProblem::perturbStep() + { int randNum; for (int i = 0; i < nV; i++) { @@ -1362,7 +1373,8 @@ namespace LCQPow { } - void LCQProblem::storeSteps( ) { + void LCQProblem::storeSteps() + { stats.updateTrackingVectors( xk, innerIter, @@ -1378,7 +1390,8 @@ namespace LCQPow { } - void LCQProblem::transformDuals( ) { + void LCQProblem::transformDuals() + { double* tmp = new double[nComp]; @@ -1409,7 +1422,8 @@ namespace LCQPow { } - void LCQProblem::determineStationarityType( ) { + void LCQProblem::determineStationarityType() + { std::vector weakComp = getWeakComplementarities( ); @@ -1453,7 +1467,7 @@ namespace LCQPow { } - std::vector LCQProblem::getWeakComplementarities( ) + std::vector LCQProblem::getWeakComplementarities() { double* Lx = new double[nComp]; double* Rx = new double[nComp]; @@ -1482,7 +1496,7 @@ namespace LCQPow { } - AlgorithmStatus LCQProblem::getPrimalSolution( double* const xOpt ) const + AlgorithmStatus LCQProblem::getPrimalSolution(double* const xOpt) const { if (Utilities::isNotNullPtr(xOpt) && Utilities::isNotNullPtr(xk)) { for (int i = 0; i < nV; i++) @@ -1493,7 +1507,7 @@ namespace LCQPow { } - AlgorithmStatus LCQProblem::getDualSolution( double* const yOpt ) const + AlgorithmStatus LCQProblem::getDualSolution(double* const yOpt) const { if (Utilities::isNotNullPtr(yOpt) && Utilities::isNotNullPtr(yk)) { for (int i = 0; i < nDuals; i++) @@ -1504,19 +1518,38 @@ namespace LCQPow { } - int LCQProblem::getNumberOfPrimals( ) const + int LCQProblem::getNumberOfPrimals() const { return nV; } - int LCQProblem::getNumberOfDuals( ) const + int LCQProblem::getNumberOfDuals() const { return nDuals; } +int LCQProblem::getNV() const +{ + return nV; +} - void LCQProblem::getOutputStatistics( OutputStatistics& _stats) const +int LCQProblem::getNC() const +{ + return nC; +} +int LCQProblem::getNComp() const +{ + return nComp; +} + +Options LCQProblem::getOptions() const +{ + return options; +} + + + void LCQProblem::getOutputStatistics(OutputStatistics& _stats) const { _stats = stats; } @@ -1525,7 +1558,7 @@ namespace LCQPow { /* * p r i n t I t e r a t i o n */ - void LCQProblem::printIteration( ) + void LCQProblem::printIteration() { if (options.getPrintLevel() == PrintLevel::NONE) return; @@ -1638,7 +1671,7 @@ namespace LCQPow { /// Clear allocated memory - void LCQProblem::clear( ) + void LCQProblem::clear() { if (Utilities::isNotNullPtr(Q)) { delete[] Q; @@ -1800,4 +1833,4 @@ namespace LCQPow { /* * end of file - */ \ No newline at end of file + */ diff --git a/src/SubsolverQPOASES.cpp b/src/SubsolverQPOASES.cpp index e2f1505e..4fe0e320 100644 --- a/src/SubsolverQPOASES.cpp +++ b/src/SubsolverQPOASES.cpp @@ -11,7 +11,7 @@ * * LCQPow is distributed in the hope that it will be useful, * but WITQOUT ANY WARRANTY; without even the implied warranty of - * MERCQANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. * See the GNU Lesser General Public License for more details. * * You should have received a copy of the GNU Lesser General Public @@ -282,4 +282,4 @@ namespace LCQPow { A_sparse = 0; } } -} \ No newline at end of file +} diff --git a/src/Utilities.cpp b/src/Utilities.cpp index aadf0b7a..a3a3846b 100644 --- a/src/Utilities.cpp +++ b/src/Utilities.cpp @@ -281,11 +281,32 @@ namespace LCQPow { M->x = NULL; } - free (M); + free(M); M = NULL; } } +void Utilities::ClearSparseMatCPP(csc* M) +{ + if (M != nullptr) { + if (M->p != nullptr) { + delete M->p; + M->p = nullptr; + } + if (M->i != nullptr) { + delete M->i; + M->i = nullptr; + } + if (M->x != nullptr) { + delete M->x; + M->x = nullptr; + } + + delete M; + M = nullptr; + } +} + void Utilities::ClearSparseMat(csc** M) {