From 948893997e378153c095e8b25883ad7a299f1118 Mon Sep 17 00:00:00 2001 From: Anton Pozharskiy Date: Mon, 7 Apr 2025 20:48:03 +0200 Subject: [PATCH 01/12] remove namespace indent --- interfaces/matlab/@LCQProblem/LCQProblem.m | 16 ++++++ src/LCQProblem.cpp | 61 ++++++++++++++-------- 2 files changed, 55 insertions(+), 22 deletions(-) create mode 100644 interfaces/matlab/@LCQProblem/LCQProblem.m diff --git a/interfaces/matlab/@LCQProblem/LCQProblem.m b/interfaces/matlab/@LCQProblem/LCQProblem.m new file mode 100644 index 00000000..31207599 --- /dev/null +++ b/interfaces/matlab/@LCQProblem/LCQProblem.m @@ -0,0 +1,16 @@ +classdef LCQProblem < handle + properties(Access=private) + self % ptr to C++ LCQProblem object + end + + methods + % Constuctor + obj = LCQProblem(nV, nC, nComp); + + % + loadLCQP(obj, args); + + % + runSolver(); + end +end diff --git a/src/LCQProblem.cpp b/src/LCQProblem.cpp index 18e4d18c..6cd219dd 100644 --- a/src/LCQProblem.cpp +++ b/src/LCQProblem.cpp @@ -723,8 +723,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]; } @@ -1148,17 +1148,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 +1172,8 @@ namespace LCQPow { } - double LCQProblem::getPhi( ) { + double LCQProblem::getPhi( ) + { double phi_lin = 0; // Linear term @@ -1185,7 +1189,8 @@ namespace LCQPow { } - double LCQProblem::getMerit( ) { + double LCQProblem::getMerit( ) + { double lin = Utilities::DotProduct(g, xk, nV); if (sparseSolver) { @@ -1196,7 +1201,8 @@ namespace LCQPow { } - void LCQProblem::updatePenalty( ) { + void LCQProblem::updatePenalty( ) + { // Clear Leyffer history if (options.getNDynamicPenalty() > 0) complHistory.clear(); @@ -1213,8 +1219,8 @@ namespace LCQPow { } } - - void LCQProblem::getOptimalStepLength( ) { + void LCQProblem::getOptimalStepLength( ) + { double qk; @@ -1237,13 +1243,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 +1280,8 @@ namespace LCQPow { } - bool LCQProblem::leyfferCheckPositive( ) { + bool LCQProblem::leyfferCheckPositive( ) + { size_t n = (size_t)options.getNDynamicPenalty(); @@ -1313,7 +1322,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 +1336,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 +1363,8 @@ namespace LCQPow { } - void LCQProblem::perturbStep( ) { + void LCQProblem::perturbStep( ) + { int randNum; for (int i = 0; i < nV; i++) { @@ -1362,7 +1376,8 @@ namespace LCQPow { } - void LCQProblem::storeSteps( ) { + void LCQProblem::storeSteps( ) + { stats.updateTrackingVectors( xk, innerIter, @@ -1378,7 +1393,8 @@ namespace LCQPow { } - void LCQProblem::transformDuals( ) { + void LCQProblem::transformDuals( ) + { double* tmp = new double[nComp]; @@ -1409,7 +1425,8 @@ namespace LCQPow { } - void LCQProblem::determineStationarityType( ) { + void LCQProblem::determineStationarityType( ) + { std::vector weakComp = getWeakComplementarities( ); @@ -1800,4 +1817,4 @@ namespace LCQPow { /* * end of file - */ \ No newline at end of file + */ From 5e836d02389b7e503dc97f3a952b3021f591968b Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Tue, 8 Apr 2025 16:49:03 +0200 Subject: [PATCH 02/12] some indentation --- src/LCQProblem.cpp | 127 ++++++++++++++++++++++----------------------- 1 file changed, 62 insertions(+), 65 deletions(-) diff --git a/src/LCQProblem.cpp b/src/LCQProblem.cpp index 6cd219dd..43589ae8 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,9 +624,8 @@ 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); @@ -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,19 +1145,19 @@ 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); @@ -1172,7 +1169,7 @@ namespace LCQPow { } - double LCQProblem::getPhi( ) + double LCQProblem::getPhi() { double phi_lin = 0; @@ -1189,7 +1186,7 @@ namespace LCQPow { } - double LCQProblem::getMerit( ) + double LCQProblem::getMerit() { double lin = Utilities::DotProduct(g, xk, nV); @@ -1201,7 +1198,7 @@ namespace LCQPow { } - void LCQProblem::updatePenalty( ) + void LCQProblem::updatePenalty() { // Clear Leyffer history if (options.getNDynamicPenalty() > 0) @@ -1219,7 +1216,7 @@ namespace LCQPow { } } - void LCQProblem::getOptimalStepLength( ) + void LCQProblem::getOptimalStepLength() { double qk; @@ -1243,14 +1240,14 @@ 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 @@ -1280,7 +1277,7 @@ namespace LCQPow { } - bool LCQProblem::leyfferCheckPositive( ) + bool LCQProblem::leyfferCheckPositive() { size_t n = (size_t)options.getNDynamicPenalty(); @@ -1322,7 +1319,7 @@ namespace LCQPow { } - void LCQProblem::updateQk( ) + void LCQProblem::updateQk() { // Smart update in sparse case if (sparseSolver) { @@ -1336,21 +1333,21 @@ 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; @@ -1363,7 +1360,7 @@ namespace LCQPow { } - void LCQProblem::perturbStep( ) + void LCQProblem::perturbStep() { int randNum; @@ -1376,7 +1373,7 @@ namespace LCQPow { } - void LCQProblem::storeSteps( ) + void LCQProblem::storeSteps() { stats.updateTrackingVectors( xk, @@ -1393,7 +1390,7 @@ namespace LCQPow { } - void LCQProblem::transformDuals( ) + void LCQProblem::transformDuals() { double* tmp = new double[nComp]; @@ -1425,7 +1422,7 @@ namespace LCQPow { } - void LCQProblem::determineStationarityType( ) + void LCQProblem::determineStationarityType() { std::vector weakComp = getWeakComplementarities( ); @@ -1470,7 +1467,7 @@ namespace LCQPow { } - std::vector LCQProblem::getWeakComplementarities( ) + std::vector LCQProblem::getWeakComplementarities() { double* Lx = new double[nComp]; double* Rx = new double[nComp]; @@ -1499,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++) @@ -1510,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++) @@ -1521,19 +1518,19 @@ namespace LCQPow { } - int LCQProblem::getNumberOfPrimals( ) const + int LCQProblem::getNumberOfPrimals() const { return nV; } - int LCQProblem::getNumberOfDuals( ) const + int LCQProblem::getNumberOfDuals() const { return nDuals; } - void LCQProblem::getOutputStatistics( OutputStatistics& _stats) const + void LCQProblem::getOutputStatistics(OutputStatistics& _stats) const { _stats = stats; } @@ -1542,7 +1539,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; @@ -1655,7 +1652,7 @@ namespace LCQPow { /// Clear allocated memory - void LCQProblem::clear( ) + void LCQProblem::clear() { if (Utilities::isNotNullPtr(Q)) { delete[] Q; From 9ec4ef05cd3d4aba960a00e496c6a54ec4f5e689 Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Tue, 8 Apr 2025 16:49:42 +0200 Subject: [PATCH 03/12] Begin on new interface with object storage --- CMakeLists.txt | 45 +++++++++- interfaces/matlab/@LCQProblem/LCQProblem.m | 17 +++- .../matlab/@LCQProblem/constructProblem.cpp | 85 +++++++++++++++++++ 3 files changed, 144 insertions(+), 3 deletions(-) create mode 100644 interfaces/matlab/@LCQProblem/constructProblem.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 890c0170..6be45585 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -439,7 +439,50 @@ if (${BUILD_MATLAB_INTERFACE}) COPY ${MATLAB_INTERFACE_DIR}/${MATLAB_INTERFACE_NAME}.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, for now only 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} + ) + endif() endif() diff --git a/interfaces/matlab/@LCQProblem/LCQProblem.m b/interfaces/matlab/@LCQProblem/LCQProblem.m index 31207599..f4bc296b 100644 --- a/interfaces/matlab/@LCQProblem/LCQProblem.m +++ b/interfaces/matlab/@LCQProblem/LCQProblem.m @@ -1,16 +1,29 @@ classdef LCQProblem < handle properties(Access=private) - self % ptr to C++ LCQProblem object + self % ptr to C++ LCQProblem object. On a 64 bit x86 end methods % Constuctor - obj = LCQProblem(nV, nC, nComp); + function obj = LCQProblem(nV, nC, nComp) + obj.constructProblem(nV, nC, nComp); + end + % loadLCQP(obj, args); % runSolver(); + + 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..64818f0e --- /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(outputs.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") })); + } + } +}; From 151f82933d5fdbdbff2b9be1f29cdb9af9adf50a Mon Sep 17 00:00:00 2001 From: Anton Pozharskiy Date: Tue, 8 Apr 2025 22:49:38 +0200 Subject: [PATCH 04/12] working destructor and begin skeleton for loadLCQP --- CMakeLists.txt | 42 +++++++++- interfaces/matlab/@LCQProblem/LCQProblem.m | 2 +- .../matlab/@LCQProblem/constructProblem.cpp | 2 +- .../matlab/@LCQProblem/destructProblem.cpp | 78 +++++++++++++++++++ interfaces/matlab/@LCQProblem/loadLCQP.cpp | 78 +++++++++++++++++++ interfaces/matlab/LCQPow.cpp | 22 +++--- 6 files changed, 209 insertions(+), 15 deletions(-) create mode 100644 interfaces/matlab/@LCQProblem/destructProblem.cpp create mode 100644 interfaces/matlab/@LCQProblem/loadLCQP.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 6be45585..63b9d760 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -446,7 +446,8 @@ if (${BUILD_MATLAB_INTERFACE}) COPY ${MATLAB_INTERFACE_DIR}/@LCQProblem/LCQProblem.m DESTINATION ${MATLAB_INTERFACE_DESTINATION}/@LCQProblem ) - # Include new matlab interface stuff, for now only constructProblem + # Include new matlab interface stuff + # constructProblem matlab_add_mex( NAME constructProblem SRC ${MATLAB_INTERFACE_DIR}/@LCQProblem/constructProblem.cpp @@ -475,7 +476,7 @@ if (${BUILD_MATLAB_INTERFACE}) PUBLIC -lqpOASES PUBLIC -losqp ) - + target_include_directories( constructProblem PRIVATE include @@ -483,6 +484,43 @@ if (${BUILD_MATLAB_INTERFACE}) 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} + ) + endif() endif() diff --git a/interfaces/matlab/@LCQProblem/LCQProblem.m b/interfaces/matlab/@LCQProblem/LCQProblem.m index f4bc296b..0164451d 100644 --- a/interfaces/matlab/@LCQProblem/LCQProblem.m +++ b/interfaces/matlab/@LCQProblem/LCQProblem.m @@ -1,5 +1,5 @@ classdef LCQProblem < handle - properties(Access=private) + properties%(Access=private) self % ptr to C++ LCQProblem object. On a 64 bit x86 end diff --git a/interfaces/matlab/@LCQProblem/constructProblem.cpp b/interfaces/matlab/@LCQProblem/constructProblem.cpp index 64818f0e..f7ba14e4 100644 --- a/interfaces/matlab/@LCQProblem/constructProblem.cpp +++ b/interfaces/matlab/@LCQProblem/constructProblem.cpp @@ -60,7 +60,7 @@ class MexFunction : public matlab::mex::Function { std::vector({ factory.createScalar("no outputs returned") })); } - if(outputs.size() != 4) + if(inputs.size() != 4) { matlab->feval(u"error", 0, std::vector({ factory.createScalar("Incorrect number of inputs") })); diff --git a/interfaces/matlab/@LCQProblem/destructProblem.cpp b/interfaces/matlab/@LCQProblem/destructProblem.cpp new file mode 100644 index 00000000..eb568f85 --- /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); + + // Assume the inputs are integral, this is generally not true but, shrug, it is fine. + 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..b36a6183 --- /dev/null +++ b/interfaces/matlab/@LCQProblem/loadLCQP.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); + + // Assume the inputs are integral, this is generally not true but, shrug, it is fine. + 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); + + + // Parse inputs + + // Parse options + + // call create problem + + // call set options + } + + 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() < 8 || inputs.size() > 14) + { + 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; From 2eb8facad40c034b392900e9db6e55b6aea5dcb5 Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Wed, 9 Apr 2025 14:48:19 +0200 Subject: [PATCH 05/12] some more work on LCQP but lots of stuff still to do --- interfaces/matlab/@LCQProblem/loadLCQP.cpp | 53 ++++++++++++++++++++-- src/SubsolverQPOASES.cpp | 4 +- 2 files changed, 50 insertions(+), 7 deletions(-) diff --git a/interfaces/matlab/@LCQProblem/loadLCQP.cpp b/interfaces/matlab/@LCQProblem/loadLCQP.cpp index b36a6183..bb03fd50 100644 --- a/interfaces/matlab/@LCQProblem/loadLCQP.cpp +++ b/interfaces/matlab/@LCQProblem/loadLCQP.cpp @@ -36,21 +36,37 @@ class MexFunction : public matlab::mex::Function { // Check inputs are valid checkArguments(outputs, inputs); - // Assume the inputs are integral, this is generally not true but, shrug, it is fine. + // Get the uintptr_t array from self member property matlab::data::TypedArray self_array(std::move(matlab->getProperty(inputs[0], u"self"))); - if (self_array.isEmpty()) + // 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])); - // 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); + // Check if self ptr is null and fail out immediately. + if(problem == nullptr) + { + // TODO(@anton) probably need to throw an exception here. + return; + } - - // Parse inputs + // TODO(@anton) support also sparse + boolean dense = true; + // Build problem: + if dense + { + buildDense(problem, outputs, inputs); + } + else + { + buildSparse(problem, outputs, inputs); + } // Parse options @@ -75,4 +91,31 @@ class MexFunction : public matlab::mex::Function { } } + + void buildDense(LCQPow::LCQProblem* problem, matlab::mex::ArgumentList outputs, matlab::mex::ArgumentList inputs) + { + // PARSE INPUTS + // Get Q. Q is assumed symmetric PSD so we don't care that matlab uses column major order. + matlab::data::TypedArray Q_arr(std::move(inputs[1])); + double* Q = Q_arr.release().release(); // NOTE: WE ARE NOW RESPONSIBLE FOR FREEING THIS POINTER + + matlab::data::TypedArray L_arr(std::move(inputs[2])); + double* L = L_arr.release().release(); // NOTE: WE ARE NOW RESPONSIBLE FOR FREEING THIS POINTER + + matlab::data::TypedArray R_arr(std::move(inputs[3])); + double* R = R_arr.release().release(); // NOTE: WE ARE NOW RESPONSIBLE FOR FREEING THIS POINTER + + matlab::data::TypedArray A_arr(std::move(inputs[8])); + double* A = A_arr.release().release(); // NOTE: WE ARE NOW RESPONSIBLE FOR FREEING THIS POINTER + + + } + + void buildSparse(LCQPow::LCQProblem* problem, matlab::mex::ArgumentList outputs, matlab::mex::ArgumentList inputs) + { + // PARSE INPUTS + // Get Q: + + } + }; 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 +} From c35a14b362b8cb908c90905ff5cfd5eb2f4944e8 Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Fri, 11 Apr 2025 17:14:11 +0200 Subject: [PATCH 06/12] Working dense loadLCLP --- CMakeLists.txt | 38 +++++ include/LCQProblem.hpp | 17 ++ interfaces/matlab/@LCQProblem/LCQProblem.m | 2 +- interfaces/matlab/@LCQProblem/loadLCQP.cpp | 182 +++++++++++++++++++-- src/LCQProblem.cpp | 14 ++ 5 files changed, 237 insertions(+), 16 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 63b9d760..cc980119 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -521,6 +521,44 @@ if (${BUILD_MATLAB_INTERFACE}) SYSTEM ${qpoases_include} ) + # destructProblem + 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} + ) + endif() endif() diff --git a/include/LCQProblem.hpp b/include/LCQProblem.hpp index 8d264478..dcf3b3be 100644 --- a/include/LCQProblem.hpp +++ b/include/LCQProblem.hpp @@ -241,6 +241,23 @@ 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; /** * PROTECTED METHODS diff --git a/interfaces/matlab/@LCQProblem/LCQProblem.m b/interfaces/matlab/@LCQProblem/LCQProblem.m index 0164451d..64cce024 100644 --- a/interfaces/matlab/@LCQProblem/LCQProblem.m +++ b/interfaces/matlab/@LCQProblem/LCQProblem.m @@ -11,7 +11,7 @@ % - loadLCQP(obj, args); + loadLCQP(obj, Q, g, L, R, lbL, ubL, lbR, ubR, A, lbA, ubA, lb, ub, opts); % runSolver(); diff --git a/interfaces/matlab/@LCQProblem/loadLCQP.cpp b/interfaces/matlab/@LCQProblem/loadLCQP.cpp index bb03fd50..27f4b081 100644 --- a/interfaces/matlab/@LCQProblem/loadLCQP.cpp +++ b/interfaces/matlab/@LCQProblem/loadLCQP.cpp @@ -23,7 +23,9 @@ #include "mexAdapter.hpp" #include "LCQProblem.hpp" #include - +#include +#include +#include using namespace matlab::data; using matlab::mex::ArgumentList; @@ -57,15 +59,15 @@ class MexFunction : public matlab::mex::Function { } // TODO(@anton) support also sparse - boolean dense = true; + bool dense = true; // Build problem: - if dense + if(dense) { - buildDense(problem, outputs, inputs); + buildDense(problem, outputs, inputs, nullptr, nullptr); } else { - buildSparse(problem, outputs, inputs); + buildSparse(problem, outputs, inputs, nullptr, nullptr); } // Parse options @@ -84,7 +86,7 @@ class MexFunction : public matlab::mex::Function { std::vector({ factory.createScalar("no outputs returned") })); } - if(inputs.size() < 8 || inputs.size() > 14) + if(inputs.size() != 15) { matlab->feval(u"error", 0, std::vector({ factory.createScalar("Incorrect number of inputs") })); @@ -92,30 +94,180 @@ class MexFunction : public matlab::mex::Function { } } - void buildDense(LCQPow::LCQProblem* problem, matlab::mex::ArgumentList outputs, matlab::mex::ArgumentList inputs) + void 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; matlab::data::TypedArray Q_arr(std::move(inputs[1])); - double* Q = Q_arr.release().release(); // NOTE: WE ARE NOW RESPONSIBLE FOR FREEING THIS POINTER + Q = Q_arr.release().release(); // NOTE: WE ARE NOW RESPONSIBLE FOR FREEING THIS POINTER + + // Get L + if(!checkDimensionAndTypeDouble(inputs[3], nComp, nV, "L")) return; + 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; + 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(!checkDimensionAndTypeDouble(inputs[9], nC, nV, "A")) return; + matlab::data::TypedArray A_arr(std::move(inputs[9])); + if(!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; + matlab::data::TypedArray g_arr(std::move(inputs[2])); + g = g_arr.release().release(); + + if(!checkDimensionAndTypeDouble(inputs[5], nComp, 1, "lbL", true)) return; + if(!inputs[5].isEmpty()) + { + matlab::data::TypedArray lbL_arr(std::move(inputs[5])); + lbL = lbL_arr.release().release(); + } + + if(!checkDimensionAndTypeDouble(inputs[6], nComp, 1, "ubL", true)) return; + if(!inputs[6].isEmpty()) + { + matlab::data::TypedArray ubL_arr(std::move(inputs[6])); + ubL = ubL_arr.release().release(); + } + + if(!checkDimensionAndTypeDouble(inputs[7], nComp, 1, "lbR", true)) return; + if(!inputs[7].isEmpty()) + { + matlab::data::TypedArray lbR_arr(std::move(inputs[7])); + lbR = lbR_arr.release().release(); + } + + if(!checkDimensionAndTypeDouble(inputs[8], nComp, 1, "ubR", true)) return; + if(!inputs[8].isEmpty()) + { + matlab::data::TypedArray ubR_arr(std::move(inputs[8])); + ubR = ubR_arr.release().release(); + } - matlab::data::TypedArray L_arr(std::move(inputs[2])); - double* L = L_arr.release().release(); // NOTE: WE ARE NOW RESPONSIBLE FOR FREEING THIS POINTER + if(!checkDimensionAndTypeDouble(inputs[10], nC, 1, "lbA", true)) return; + if(!inputs[10].isEmpty()) + { + matlab::data::TypedArray lbA_arr(std::move(inputs[10])); + lbA = lbA_arr.release().release(); + } - matlab::data::TypedArray R_arr(std::move(inputs[3])); - double* R = R_arr.release().release(); // NOTE: WE ARE NOW RESPONSIBLE FOR FREEING THIS POINTER + if(!checkDimensionAndTypeDouble(inputs[11], nC, 1, "ubA", true)) return; + if(!inputs[11].isEmpty()) + { + matlab::data::TypedArray ubA_arr(std::move(inputs[11])); + ubA = ubA_arr.release().release(); + } - matlab::data::TypedArray A_arr(std::move(inputs[8])); - double* A = A_arr.release().release(); // NOTE: WE ARE NOW RESPONSIBLE FOR FREEING THIS POINTER + if(!checkDimensionAndTypeDouble(inputs[12], nV, 1, "lb", true)) return; + if(!inputs[12].isEmpty()) + { + matlab::data::TypedArray lb_arr(std::move(inputs[12])); + lb = lb_arr.release().release(); + } + if(!checkDimensionAndTypeDouble(inputs[13], nV, 1, "ub", true)) return; + if(!inputs[13].isEmpty()) + { + matlab::data::TypedArray ub_arr(std::move(inputs[13])); + ub = ub_arr.release().release(); + } + LCQPow::ReturnValue ret = problem->loadLCQP(Q, g, L, R, lbL, ubL, lbR, ubR, A, lbA, ubA, lb, ub, x0, y0); } - void buildSparse(LCQPow::LCQProblem* problem, matlab::mex::ArgumentList outputs, matlab::mex::ArgumentList inputs) + void buildSparse(LCQPow::LCQProblem* problem, matlab::mex::ArgumentList outputs, matlab::mex::ArgumentList inputs, const double* const x0, const double* const y0) { // PARSE INPUTS // Get Q: } + 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; + } + }; diff --git a/src/LCQProblem.cpp b/src/LCQProblem.cpp index 43589ae8..72c033c6 100644 --- a/src/LCQProblem.cpp +++ b/src/LCQProblem.cpp @@ -1529,6 +1529,20 @@ namespace LCQPow { return nDuals; } +int LCQProblem::getNV() const +{ + return nV; +} + +int LCQProblem::getNC() const +{ + return nC; +} +int LCQProblem::getNComp() const +{ + return nComp; +} + void LCQProblem::getOutputStatistics(OutputStatistics& _stats) const { From 772f6d347ae6ce95e42b73dd243c808d0f0a1f3e Mon Sep 17 00:00:00 2001 From: Anton Pozharskiy Date: Mon, 14 Apr 2025 11:02:29 +0200 Subject: [PATCH 07/12] better memory managment in dense load --- interfaces/matlab/@LCQProblem/loadLCQP.cpp | 113 ++++++++++++--------- 1 file changed, 63 insertions(+), 50 deletions(-) diff --git a/interfaces/matlab/@LCQProblem/loadLCQP.cpp b/interfaces/matlab/@LCQProblem/loadLCQP.cpp index 27f4b081..1846d6c9 100644 --- a/interfaces/matlab/@LCQProblem/loadLCQP.cpp +++ b/interfaces/matlab/@LCQProblem/loadLCQP.cpp @@ -26,6 +26,27 @@ #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; \ + 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(); \ + std::cout << "Getting " << #name << std::endl; \ + } + +#define MAYBE_CLEANUP_VECTOR(name) if(name##_present) \ + { \ + del_##name(name); \ + std::cout << "Deleteing " << #name << std::endl; \ + } + using namespace matlab::data; using matlab::mex::ArgumentList; @@ -123,7 +144,9 @@ class MexFunction : public matlab::mex::Function { // 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; matlab::data::TypedArray Q_arr(std::move(inputs[1])); - Q = Q_arr.release().release(); // NOTE: WE ARE NOW RESPONSIBLE FOR FREEING THIS POINTER + 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; @@ -167,65 +190,55 @@ class MexFunction : public matlab::mex::Function { // Get vectors if(!checkDimensionAndTypeDouble(inputs[2], nV, 1, "g")) return; matlab::data::TypedArray g_arr(std::move(inputs[2])); - g = g_arr.release().release(); - - if(!checkDimensionAndTypeDouble(inputs[5], nComp, 1, "lbL", true)) return; - if(!inputs[5].isEmpty()) - { - matlab::data::TypedArray lbL_arr(std::move(inputs[5])); - lbL = lbL_arr.release().release(); - } - - if(!checkDimensionAndTypeDouble(inputs[6], nComp, 1, "ubL", true)) return; - if(!inputs[6].isEmpty()) - { - matlab::data::TypedArray ubL_arr(std::move(inputs[6])); - ubL = ubL_arr.release().release(); - } + 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); - if(!checkDimensionAndTypeDouble(inputs[7], nComp, 1, "lbR", true)) return; - if(!inputs[7].isEmpty()) - { - matlab::data::TypedArray lbR_arr(std::move(inputs[7])); - lbR = lbR_arr.release().release(); - } + LCQPow::ReturnValue ret = problem->loadLCQP(Q, g, L, R, lbL, ubL, lbR, ubR, A, lbA, ubA, lb, ub, x0, y0); - if(!checkDimensionAndTypeDouble(inputs[8], nComp, 1, "ubR", true)) return; - if(!inputs[8].isEmpty()) + // Cleanup arrays + if(A != nullptr) { - matlab::data::TypedArray ubR_arr(std::move(inputs[8])); - ubR = ubR_arr.release().release(); + delete[] A; + std::cout << "Deleting A" << std::endl; } - - if(!checkDimensionAndTypeDouble(inputs[10], nC, 1, "lbA", true)) return; - if(!inputs[10].isEmpty()) + if(L != nullptr) { - matlab::data::TypedArray lbA_arr(std::move(inputs[10])); - lbA = lbA_arr.release().release(); + delete[] L; + std::cout << "Deleting L" << std::endl; } - - if(!checkDimensionAndTypeDouble(inputs[11], nC, 1, "ubA", true)) return; - if(!inputs[11].isEmpty()) + if(R != nullptr) { - matlab::data::TypedArray ubA_arr(std::move(inputs[11])); - ubA = ubA_arr.release().release(); + delete[] R; + std::cout << "Deleting R" << std::endl; } - - if(!checkDimensionAndTypeDouble(inputs[12], nV, 1, "lb", true)) return; - if(!inputs[12].isEmpty()) + if(Q != nullptr) { - matlab::data::TypedArray lb_arr(std::move(inputs[12])); - lb = lb_arr.release().release(); + del_Q(Q); + std::cout << "Deleting Q" << std::endl; } - - if(!checkDimensionAndTypeDouble(inputs[13], nV, 1, "ub", true)) return; - if(!inputs[13].isEmpty()) - { - matlab::data::TypedArray ub_arr(std::move(inputs[13])); - ub = ub_arr.release().release(); - } - - 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); + std::cout << "Deleting g" << std::endl; + 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); + } void buildSparse(LCQPow::LCQProblem* problem, matlab::mex::ArgumentList outputs, matlab::mex::ArgumentList inputs, const double* const x0, const double* const y0) From 60e8c69c0eeb894039e480a94736315c87e37377 Mon Sep 17 00:00:00 2001 From: Anton Pozharskiy Date: Mon, 14 Apr 2025 20:15:51 +0200 Subject: [PATCH 08/12] broken memory management in sparse --- include/Utilities.hpp | 3 +- interfaces/matlab/@LCQProblem/loadLCQP.cpp | 712 ++++++++++++++++++++- src/Utilities.cpp | 32 +- 3 files changed, 712 insertions(+), 35 deletions(-) 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/loadLCQP.cpp b/interfaces/matlab/@LCQProblem/loadLCQP.cpp index 1846d6c9..29a46b8a 100644 --- a/interfaces/matlab/@LCQProblem/loadLCQP.cpp +++ b/interfaces/matlab/@LCQProblem/loadLCQP.cpp @@ -22,6 +22,7 @@ #include "mex.hpp" #include "mexAdapter.hpp" #include "LCQProblem.hpp" +#include #include #include #include @@ -30,21 +31,43 @@ /* * 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; \ - 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) \ +#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(); \ - std::cout << "Getting " << #name << std::endl; \ + NAME = NAME##_arr_ptr.release(); \ } -#define MAYBE_CLEANUP_VECTOR(name) if(name##_present) \ +#define MAYBE_CLEANUP_VECTOR(NAME) if(NAME##_present) \ { \ - del_##name(name); \ - std::cout << "Deleteing " << #name << std::endl; \ + 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; @@ -79,23 +102,138 @@ class MexFunction : public matlab::mex::Function { return; } - // TODO(@anton) support also sparse - bool dense = true; + // 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, nullptr, nullptr); + buildDense(problem, outputs, inputs, x0, y0); } else { - buildSparse(problem, outputs, inputs, nullptr, nullptr); + 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" + }; - // Parse 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); + } + } - // call create problem + 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); + } - // call set options + return 0; } void checkArguments(matlab::mex::ArgumentList outputs, matlab::mex::ArgumentList inputs) { @@ -115,7 +253,7 @@ class MexFunction : public matlab::mex::Function { } } - void buildDense(LCQPow::LCQProblem* problem, matlab::mex::ArgumentList outputs, matlab::mex::ArgumentList inputs, const double* const x0, const double* const y0) + 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; @@ -142,14 +280,14 @@ class MexFunction : public matlab::mex::Function { // 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; + 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; + 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(); @@ -162,7 +300,7 @@ class MexFunction : public matlab::mex::Function { } // Get R - if(!checkDimensionAndTypeDouble(inputs[4], nComp, nV, "R")) return; + 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(); @@ -175,7 +313,7 @@ class MexFunction : public matlab::mex::Function { } // Get A - if(!checkDimensionAndTypeDouble(inputs[9], nC, nV, "A")) return; + if(!checkDimensionAndTypeDouble(inputs[9], nC, nV, "A")) return 1; matlab::data::TypedArray A_arr(std::move(inputs[9])); if(!A_arr.isEmpty()) { @@ -188,7 +326,7 @@ class MexFunction : public matlab::mex::Function { } // Get vectors - if(!checkDimensionAndTypeDouble(inputs[2], nV, 1, "g")) return; + 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(); @@ -210,26 +348,21 @@ class MexFunction : public matlab::mex::Function { if(A != nullptr) { delete[] A; - std::cout << "Deleting A" << std::endl; } if(L != nullptr) { delete[] L; - std::cout << "Deleting L" << std::endl; } if(R != nullptr) { delete[] R; - std::cout << "Deleting R" << std::endl; } if(Q != nullptr) { del_Q(Q); - std::cout << "Deleting Q" << std::endl; } // Cleanup vectors del_g(g); - std::cout << "Deleting g" << std::endl; MAYBE_CLEANUP_VECTOR(lbL); MAYBE_CLEANUP_VECTOR(ubL); MAYBE_CLEANUP_VECTOR(lbR); @@ -238,16 +371,158 @@ class MexFunction : public matlab::mex::Function { MAYBE_CLEANUP_VECTOR(ubA); MAYBE_CLEANUP_VECTOR(lb); MAYBE_CLEANUP_VECTOR(ub); - + + return ret; } - void buildSparse(LCQPow::LCQProblem* problem, matlab::mex::ArgumentList outputs, matlab::mex::ArgumentList inputs, const double* const x0, const double* const y0) + int buildSparse(LCQPow::LCQProblem* problem, matlab::mex::ArgumentList outputs, matlab::mex::ArgumentList inputs, const double* const x0, const double* const y0) { - // PARSE INPUTS - // Get Q: + 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); + std::cout << Q << std::endl; + LCQPow::ReturnValue ret = problem->loadLCQP(Q, g, L, R, lbL, ubL, lbR, ubR, A, lbA, ubA, lb, ub, x0, y0); + std::cout << Q << std::endl; + + // Cleanup vectors + std::cout << "cleaning up vectors" << std::endl; + 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); + + // TODO(@anton): why does this double free? + // TODO(@anton): this now leaks memory, fix it. + std::cout << "clearing Q" << std::endl; + // Clear sparse specific memory + LCQPow::Utilities::ClearSparseMat(Q); + std::cout << "clearing L" << std::endl; + LCQPow::Utilities::ClearSparseMat(L); + std::cout << "clearing R" << std::endl; + LCQPow::Utilities::ClearSparseMat(R); + std::cout << "clearing A" << std::endl; + if (A != 0) LCQPow::Utilities::ClearSparseMat(A); + std::cout << "returning buildSparse" << std::endl; + std::cout << ret << std::endl; + 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)); + std::cout << p << std::endl; + + // 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 = -1; + 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] = 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 + } + + 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++) @@ -283,4 +558,375 @@ class MexFunction : public matlab::mex::Function { 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/src/Utilities.cpp b/src/Utilities.cpp index aadf0b7a..289a2e08 100644 --- a/src/Utilities.cpp +++ b/src/Utilities.cpp @@ -267,28 +267,58 @@ namespace LCQPow { void Utilities::ClearSparseMat(csc* M) { + std::cout << "Clearing sparse mat" << std::endl; if (isNotNullPtr(M)) { + std::cout << "clearing M->p: " << M->p << std::endl; if (isNotNullPtr(M->p)) { free(M->p); M->p = NULL; } + std::cout << "clearing M->i: " << M->i << std::endl; if (isNotNullPtr(M->i)) { free(M->i); M->i = NULL; } + std::cout << "clearing M->x: " << M->x << std::endl; if (isNotNullPtr(M->x)) { free(M->x); M->x = NULL; } - free (M); + free(M); M = NULL; } } +void Utilities::ClearSparseMatCPP(csc* M) +{ + if (M != nullptr) { + std::cout << "clearing M->p: " << M->p << std::endl; + if (M->p != nullptr) { + delete M->p; + M->p = nullptr; + } + std::cout << "clearing M->i: " << M->i << std::endl; + if (M->i != nullptr) { + delete M->i; + M->i = nullptr; + } + std::cout << "clearing M->x: " << M->x << std::endl; + if (M->x != nullptr) { + delete M->x; + M->x = nullptr; + } + + std::cout << "clearing M" << std::endl; + delete M; + M = nullptr; + } +} + void Utilities::ClearSparseMat(csc** M) { + std::cout << "Clearing sparse mat"; if (isNotNullPtr(*M)) { if (isNotNullPtr((*M)->p)) { free((*M)->p); From 5c1378a833c164d382dfa77f92968a9c0b0a9ae3 Mon Sep 17 00:00:00 2001 From: Anton Pozharskiy Date: Mon, 14 Apr 2025 22:34:38 +0200 Subject: [PATCH 09/12] fixed csc generation, now working --- .../matlab/@LCQProblem/constructProblem.cpp | 3 ++ interfaces/matlab/@LCQProblem/loadLCQP.cpp | 40 +++++++++++-------- src/LCQProblem.cpp | 5 +++ 3 files changed, 32 insertions(+), 16 deletions(-) diff --git a/interfaces/matlab/@LCQProblem/constructProblem.cpp b/interfaces/matlab/@LCQProblem/constructProblem.cpp index f7ba14e4..bdcfee28 100644 --- a/interfaces/matlab/@LCQProblem/constructProblem.cpp +++ b/interfaces/matlab/@LCQProblem/constructProblem.cpp @@ -40,6 +40,9 @@ class MexFunction : public matlab::mex::Function { int nV = (int) inputs[1][0]; int nC = (int) inputs[2][0]; int nComp = (int) inputs[3][0]; + std::cout << nV << std::endl; + std::cout << nC << std::endl; + std::cout << nComp << std::endl; // 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); diff --git a/interfaces/matlab/@LCQProblem/loadLCQP.cpp b/interfaces/matlab/@LCQProblem/loadLCQP.cpp index 29a46b8a..60d2e25a 100644 --- a/interfaces/matlab/@LCQProblem/loadLCQP.cpp +++ b/interfaces/matlab/@LCQProblem/loadLCQP.cpp @@ -128,6 +128,7 @@ class MexFunction : public matlab::mex::Function { } // call set options // Set options and print them + std::cout << "AAAAAAAAAAAAAAAAhhhhhh" << std::endl; problem->setOptions(options); } @@ -313,9 +314,9 @@ class MexFunction : public matlab::mex::Function { } // Get A - if(!checkDimensionAndTypeDouble(inputs[9], nC, nV, "A")) return 1; + if(nC>0 && !checkDimensionAndTypeDouble(inputs[9], nC, nV, "A")) return 1; matlab::data::TypedArray A_arr(std::move(inputs[9])); - if(!A_arr.isEmpty()) + 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(); @@ -392,20 +393,20 @@ class MexFunction : public matlab::mex::Function { 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]); + 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]); + matlab::data::SparseArray A_arr(std::move(inputs[9])); + //matlab::data::SparseArray A_arr(inputs[9]); A = readSparseMatrix(A_arr, nC, nV); } @@ -437,7 +438,7 @@ class MexFunction : public matlab::mex::Function { UNWRAP_ARRAY(lb,12,nV); UNWRAP_ARRAY(ub,13,nV); - std::cout << Q << std::endl; + std::cout << "pre load Q: " << Q << std::endl; LCQPow::ReturnValue ret = problem->loadLCQP(Q, g, L, R, lbL, ubL, lbR, ubR, A, lbA, ubA, lb, ub, x0, y0); std::cout << Q << std::endl; @@ -469,7 +470,7 @@ class MexFunction : public matlab::mex::Function { return ret; } - csc* readSparseMatrix(matlab::data::SparseArray mat, int nRow, int nCol) + 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)); @@ -483,7 +484,9 @@ class MexFunction : public matlab::mex::Function { auto mat_it_end = mat.end(); double* x_it = x; int* p_it = p; - int curr_col = -1; + int curr_col = 0; + p[0] = 0; + int nc = 0; int idx = 0; // Iterate over all nonzeros; while(mat_it != mat_it_end) @@ -494,19 +497,24 @@ class MexFunction : public matlab::mex::Function { 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++) + { + for(int ii = curr_col+1; ii < col; ii++) { - p[ii] = idx; + p[ii] = p[curr_col]; + std::cout << "ii " << ii << " nCoL " << nCol+1 << std::endl; } + p[col] = idx; curr_col = col; } // update row data i[idx] = row; // copy nonzero into buffer *x_it = *mat_it; + std::cout << "idx " << idx << ", nnz " << mat.getNumberOfNonZeroElements() << std::endl; + std::cout << "x_it - x " << x_it - x << ", nnz " << mat.getNumberOfNonZeroElements() << std::endl; idx++;x_it++;mat_it++; // move pointers } + p[nCol] = idx; csc* M = (csc *)malloc(sizeof(csc)); diff --git a/src/LCQProblem.cpp b/src/LCQProblem.cpp index 72c033c6..718799c1 100644 --- a/src/LCQProblem.cpp +++ b/src/LCQProblem.cpp @@ -632,6 +632,9 @@ namespace LCQPow { R_sparse = Utilities::copyCSC(R_new); // Get number of elements + for(int ii=0; iip[ii]: " << L_sparse->p[ii] << std::endl; + std::cout << "L_sparse->p[nV]: " << L_sparse->p[nV] << std::endl; int tmpA_nnx = L_sparse->p[nV] + R_sparse->p[nV]; if (Utilities::isNotNullPtr(A_new)) { @@ -788,6 +791,8 @@ namespace LCQPow { return LCQPOBJECT_NOT_SETUP; Q_sparse = Utilities::copyCSC(Q_new); + for(int ii=0; iip[ii]: " << Q_sparse->p[ii] << std::endl; return ReturnValue::SUCCESSFUL_RETURN; } From 45f81fa32f6fd2efe2ca179a3af7fcb2ff9aef2c Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Tue, 15 Apr 2025 13:48:36 +0200 Subject: [PATCH 10/12] many changes with working dense runProblem --- CMakeLists.txt | 45 ++++- include/LCQProblem.hpp | 7 + interfaces/matlab/@LCQProblem/LCQProblem.m | 2 +- .../matlab/@LCQProblem/destructProblem.cpp | 2 +- interfaces/matlab/@LCQProblem/loadLCQP.cpp | 7 +- interfaces/matlab/@LCQProblem/runProblem.cpp | 176 ++++++++++++++++++ interfaces/matlab/LCQPow_options.m | 29 ++- src/LCQProblem.cpp | 5 + 8 files changed, 250 insertions(+), 23 deletions(-) create mode 100644 interfaces/matlab/@LCQProblem/runProblem.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index cc980119..1e5cb79c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -440,6 +440,11 @@ if (${BUILD_MATLAB_INTERFACE}) 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( @@ -521,7 +526,7 @@ if (${BUILD_MATLAB_INTERFACE}) SYSTEM ${qpoases_include} ) - # destructProblem + # loadLCQP matlab_add_mex( NAME loadLCQP SRC ${MATLAB_INTERFACE_DIR}/@LCQProblem/loadLCQP.cpp @@ -559,6 +564,44 @@ if (${BUILD_MATLAB_INTERFACE}) 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 dcf3b3be..1c9926ac 100644 --- a/include/LCQProblem.hpp +++ b/include/LCQProblem.hpp @@ -259,6 +259,13 @@ namespace LCQPow { */ 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/interfaces/matlab/@LCQProblem/LCQProblem.m b/interfaces/matlab/@LCQProblem/LCQProblem.m index 64cce024..be91e5d1 100644 --- a/interfaces/matlab/@LCQProblem/LCQProblem.m +++ b/interfaces/matlab/@LCQProblem/LCQProblem.m @@ -14,7 +14,7 @@ loadLCQP(obj, Q, g, L, R, lbL, ubL, lbR, ubR, A, lbA, ubA, lb, ub, opts); % - runSolver(); + [varargout] = runSolver(obj); function delete(obj) % obj is always scalar, if it isn't we panic :) diff --git a/interfaces/matlab/@LCQProblem/destructProblem.cpp b/interfaces/matlab/@LCQProblem/destructProblem.cpp index eb568f85..e800cc45 100644 --- a/interfaces/matlab/@LCQProblem/destructProblem.cpp +++ b/interfaces/matlab/@LCQProblem/destructProblem.cpp @@ -36,7 +36,7 @@ class MexFunction : public matlab::mex::Function { // Check inputs are valid checkArguments(outputs, inputs); - // Assume the inputs are integral, this is generally not true but, shrug, it is fine. + // Get self value matlab::data::TypedArray self_array(std::move(matlab->getProperty(inputs[0], u"self"))); if (self_array.isEmpty()) diff --git a/interfaces/matlab/@LCQProblem/loadLCQP.cpp b/interfaces/matlab/@LCQProblem/loadLCQP.cpp index 60d2e25a..dbb422e0 100644 --- a/interfaces/matlab/@LCQProblem/loadLCQP.cpp +++ b/interfaces/matlab/@LCQProblem/loadLCQP.cpp @@ -75,7 +75,8 @@ using matlab::mex::ArgumentList; class MexFunction : public matlab::mex::Function { public: - void operator()(matlab::mex::ArgumentList outputs, matlab::mex::ArgumentList inputs) { + void operator()(matlab::mex::ArgumentList outputs, matlab::mex::ArgumentList inputs) + { std::shared_ptr matlab = getEngine(); matlab::data::ArrayFactory factory; @@ -128,7 +129,6 @@ class MexFunction : public matlab::mex::Function { } // call set options // Set options and print them - std::cout << "AAAAAAAAAAAAAAAAhhhhhh" << std::endl; problem->setOptions(options); } @@ -237,7 +237,8 @@ class MexFunction : public matlab::mex::Function { return 0; } - void checkArguments(matlab::mex::ArgumentList outputs, matlab::mex::ArgumentList inputs) { + void checkArguments(matlab::mex::ArgumentList outputs, matlab::mex::ArgumentList inputs) + { std::shared_ptr matlab = getEngine(); matlab::data::ArrayFactory factory; if(outputs.size() > 0) 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_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 718799c1..b8d4c75f 100644 --- a/src/LCQProblem.cpp +++ b/src/LCQProblem.cpp @@ -1548,6 +1548,11 @@ int LCQProblem::getNComp() const return nComp; } +Options LCQProblem::getOptions() const +{ + return options; +} + void LCQProblem::getOutputStatistics(OutputStatistics& _stats) const { From 04f16e72bd4e7fb6d4bafe81fbbfbbb675f44189 Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Tue, 15 Apr 2025 14:16:08 +0200 Subject: [PATCH 11/12] fix bug in sparse matrix conversion --- interfaces/matlab/@LCQProblem/loadLCQP.cpp | 23 ++++++---------------- src/LCQProblem.cpp | 7 +------ src/Utilities.cpp | 9 --------- 3 files changed, 7 insertions(+), 32 deletions(-) diff --git a/interfaces/matlab/@LCQProblem/loadLCQP.cpp b/interfaces/matlab/@LCQProblem/loadLCQP.cpp index dbb422e0..9caa12ad 100644 --- a/interfaces/matlab/@LCQProblem/loadLCQP.cpp +++ b/interfaces/matlab/@LCQProblem/loadLCQP.cpp @@ -439,12 +439,9 @@ class MexFunction : public matlab::mex::Function { UNWRAP_ARRAY(lb,12,nV); UNWRAP_ARRAY(ub,13,nV); - std::cout << "pre load Q: " << Q << std::endl; LCQPow::ReturnValue ret = problem->loadLCQP(Q, g, L, R, lbL, ubL, lbR, ubR, A, lbA, ubA, lb, ub, x0, y0); - std::cout << Q << std::endl; - + // Cleanup vectors - std::cout << "cleaning up vectors" << std::endl; del_g(g); MAYBE_CLEANUP_VECTOR(lbL); MAYBE_CLEANUP_VECTOR(ubL); @@ -455,19 +452,11 @@ class MexFunction : public matlab::mex::Function { MAYBE_CLEANUP_VECTOR(lb); MAYBE_CLEANUP_VECTOR(ub); - // TODO(@anton): why does this double free? - // TODO(@anton): this now leaks memory, fix it. - std::cout << "clearing Q" << std::endl; // Clear sparse specific memory LCQPow::Utilities::ClearSparseMat(Q); - std::cout << "clearing L" << std::endl; LCQPow::Utilities::ClearSparseMat(L); - std::cout << "clearing R" << std::endl; LCQPow::Utilities::ClearSparseMat(R); - std::cout << "clearing A" << std::endl; if (A != 0) LCQPow::Utilities::ClearSparseMat(A); - std::cout << "returning buildSparse" << std::endl; - std::cout << ret << std::endl; return ret; } @@ -478,7 +467,6 @@ class MexFunction : public matlab::mex::Function { int n = nCol; int m = nRow; int* p = (int*) malloc((nCol+1)*sizeof(int)); - std::cout << p << std::endl; // Get necessary values auto mat_it = mat.begin(); @@ -502,7 +490,6 @@ class MexFunction : public matlab::mex::Function { for(int ii = curr_col+1; ii < col; ii++) { p[ii] = p[curr_col]; - std::cout << "ii " << ii << " nCoL " << nCol+1 << std::endl; } p[col] = idx; curr_col = col; @@ -511,10 +498,13 @@ class MexFunction : public matlab::mex::Function { i[idx] = row; // copy nonzero into buffer *x_it = *mat_it; - std::cout << "idx " << idx << ", nnz " << mat.getNumberOfNonZeroElements() << std::endl; - std::cout << "x_it - x " << x_it - x << ", nnz " << mat.getNumberOfNonZeroElements() << std::endl; 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)); @@ -528,7 +518,6 @@ class MexFunction : public matlab::mex::Function { M->x = x; M->nz = -1; M->nzmax = mat.getNumberOfNonZeroElements(); - return M; } diff --git a/src/LCQProblem.cpp b/src/LCQProblem.cpp index b8d4c75f..b5a1640a 100644 --- a/src/LCQProblem.cpp +++ b/src/LCQProblem.cpp @@ -632,10 +632,7 @@ namespace LCQPow { R_sparse = Utilities::copyCSC(R_new); // Get number of elements - for(int ii=0; iip[ii]: " << L_sparse->p[ii] << std::endl; - std::cout << "L_sparse->p[nV]: " << L_sparse->p[nV] << std::endl; - 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; @@ -791,8 +788,6 @@ namespace LCQPow { return LCQPOBJECT_NOT_SETUP; Q_sparse = Utilities::copyCSC(Q_new); - for(int ii=0; iip[ii]: " << Q_sparse->p[ii] << std::endl; return ReturnValue::SUCCESSFUL_RETURN; } diff --git a/src/Utilities.cpp b/src/Utilities.cpp index 289a2e08..a3a3846b 100644 --- a/src/Utilities.cpp +++ b/src/Utilities.cpp @@ -267,19 +267,15 @@ namespace LCQPow { void Utilities::ClearSparseMat(csc* M) { - std::cout << "Clearing sparse mat" << std::endl; if (isNotNullPtr(M)) { - std::cout << "clearing M->p: " << M->p << std::endl; if (isNotNullPtr(M->p)) { free(M->p); M->p = NULL; } - std::cout << "clearing M->i: " << M->i << std::endl; if (isNotNullPtr(M->i)) { free(M->i); M->i = NULL; } - std::cout << "clearing M->x: " << M->x << std::endl; if (isNotNullPtr(M->x)) { free(M->x); M->x = NULL; @@ -293,23 +289,19 @@ namespace LCQPow { void Utilities::ClearSparseMatCPP(csc* M) { if (M != nullptr) { - std::cout << "clearing M->p: " << M->p << std::endl; if (M->p != nullptr) { delete M->p; M->p = nullptr; } - std::cout << "clearing M->i: " << M->i << std::endl; if (M->i != nullptr) { delete M->i; M->i = nullptr; } - std::cout << "clearing M->x: " << M->x << std::endl; if (M->x != nullptr) { delete M->x; M->x = nullptr; } - std::cout << "clearing M" << std::endl; delete M; M = nullptr; } @@ -318,7 +310,6 @@ void Utilities::ClearSparseMatCPP(csc* M) void Utilities::ClearSparseMat(csc** M) { - std::cout << "Clearing sparse mat"; if (isNotNullPtr(*M)) { if (isNotNullPtr((*M)->p)) { free((*M)->p); From 9cce0ec10385fea55fc08c7fb14abb0ff60949de Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Tue, 15 Apr 2025 14:31:01 +0200 Subject: [PATCH 12/12] remove more debug prints --- interfaces/matlab/@LCQProblem/constructProblem.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/interfaces/matlab/@LCQProblem/constructProblem.cpp b/interfaces/matlab/@LCQProblem/constructProblem.cpp index bdcfee28..f7ba14e4 100644 --- a/interfaces/matlab/@LCQProblem/constructProblem.cpp +++ b/interfaces/matlab/@LCQProblem/constructProblem.cpp @@ -40,9 +40,6 @@ class MexFunction : public matlab::mex::Function { int nV = (int) inputs[1][0]; int nC = (int) inputs[2][0]; int nComp = (int) inputs[3][0]; - std::cout << nV << std::endl; - std::cout << nC << std::endl; - std::cout << nComp << std::endl; // 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);