diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..3c62bf6 --- /dev/null +++ b/.clang-format @@ -0,0 +1,105 @@ +--- +Language: Cpp +# BasedOnStyle: WebKit +AccessModifierOffset: -4 +AlignAfterOpenBracket: Align +AlignConsecutiveAssignments: false +AlignConsecutiveDeclarations: false +AlignEscapedNewlines: Left +AlignOperands: true +AlignTrailingComments: false +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: false +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: false +AlwaysBreakTemplateDeclarations: false +BinPackArguments: false +BinPackParameters: false +BraceWrapping: + AfterClass: true + AfterControlStatement: true + AfterEnum: true + AfterFunction: true + AfterNamespace: true + AfterObjCDeclaration: false + AfterStruct: true + AfterUnion: true + BeforeCatch: false + BeforeElse: false + IndentBraces: false + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakBeforeBinaryOperators: All +BreakBeforeBraces: Custom +BreakBeforeInheritanceComma: false +BreakBeforeTernaryOperators: true +BreakConstructorInitializersBeforeComma: true +BreakConstructorInitializers: BeforeComma +BreakAfterJavaFieldAnnotations: false +BreakStringLiterals: true +ColumnLimit: 100 +CommentPragmas: '^ IWYU pragma:' +CompactNamespaces: false +ConstructorInitializerAllOnOneLineOrOnePerLine: false +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DerivePointerAlignment: false +DisableFormat: false +ExperimentalAutoDetectBinPacking: false +FixNamespaceComments: true +ForEachMacros: + - foreach + - Q_FOREACH + - BOOST_FOREACH +IncludeCategories: + - Regex: '^"(llvm|llvm-c|clang|clang-c)/' + Priority: 2 + - Regex: '^(<|"(gtest|gmock|isl|json)/)' + Priority: 3 + - Regex: '.*' + Priority: 1 +IncludeIsMainRegex: '(Test)?$' +IndentCaseLabels: false +IndentWidth: 4 +IndentWrappedFunctionNames: false +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: true +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBlockIndentWidth: 4 +ObjCSpaceAfterProperty: true +ObjCSpaceBeforeProtocolList: true +PenaltyBreakAssignment: 10 +PenaltyBreakBeforeFirstCallParameter: 1000 +PenaltyBreakComment: 10 +PenaltyBreakString: 10 +PenaltyExcessCharacter: 100 +PenaltyReturnTypeOnItsOwnLine: 5 +PointerAlignment: Left +ReflowComments: true +SortIncludes: true +SortUsingDeclarations: true +SpaceAfterCStyleCast: false +SpaceAfterTemplateKeyword: true +SpaceBeforeAssignmentOperators: true +SpaceBeforeParens: ControlStatements +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 1 +SpacesInAngles: false +SpacesInContainerLiterals: true +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +Standard: Cpp11 +TabWidth: 4 +UseTab: Never diff --git a/example/src/MPCExample.cpp b/example/src/MPCExample.cpp index e85799a..6ee6b63 100644 --- a/example/src/MPCExample.cpp +++ b/example/src/MPCExample.cpp @@ -5,7 +5,6 @@ * @date 2018 */ - // osqp-eigen #include "OsqpEigen/OsqpEigen.h" @@ -14,185 +13,189 @@ #include -void setDynamicsMatrices(Eigen::Matrix &a, Eigen::Matrix &b) +void setDynamicsMatrices(Eigen::Matrix& a, Eigen::Matrix& b) { - a << 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0., 0. , - 0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0. , - 0., 0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0. , - 0.0488, 0., 0., 1., 0., 0., 0.0016, 0., 0., 0.0992, 0., 0. , - 0., -0.0488, 0., 0., 1., 0., 0., -0.0016, 0., 0., 0.0992, 0. , - 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.0992, - 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0. , - 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0. , - 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0. , - 0.9734, 0., 0., 0., 0., 0., 0.0488, 0., 0., 0.9846, 0., 0. , - 0., -0.9734, 0., 0., 0., 0., 0., -0.0488, 0., 0., 0.9846, 0. , - 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.9846; - - b << 0., -0.0726, 0., 0.0726, - -0.0726, 0., 0.0726, 0. , - -0.0152, 0.0152, -0.0152, 0.0152, - -0., -0.0006, -0., 0.0006, - 0.0006, 0., -0.0006, 0.0000, - 0.0106, 0.0106, 0.0106, 0.0106, - 0, -1.4512, 0., 1.4512, - -1.4512, 0., 1.4512, 0. , - -0.3049, 0.3049, -0.3049, 0.3049, - -0., -0.0236, 0., 0.0236, - 0.0236, 0., -0.0236, 0. , - 0.2107, 0.2107, 0.2107, 0.2107; + a << 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., + 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0.0488, 0., 0., 1., 0., 0., 0.0016, + 0., 0., 0.0992, 0., 0., 0., -0.0488, 0., 0., 1., 0., 0., -0.0016, 0., 0., 0.0992, 0., 0., + 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.0992, 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., + 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., + 0., 0., 0.9734, 0., 0., 0., 0., 0., 0.0488, 0., 0., 0.9846, 0., 0., 0., -0.9734, 0., 0., 0., + 0., 0., -0.0488, 0., 0., 0.9846, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.9846; + + b << 0., -0.0726, 0., 0.0726, -0.0726, 0., 0.0726, 0., -0.0152, 0.0152, -0.0152, 0.0152, -0., + -0.0006, -0., 0.0006, 0.0006, 0., -0.0006, 0.0000, 0.0106, 0.0106, 0.0106, 0.0106, 0, + -1.4512, 0., 1.4512, -1.4512, 0., 1.4512, 0., -0.3049, 0.3049, -0.3049, 0.3049, -0., + -0.0236, 0., 0.0236, 0.0236, 0., -0.0236, 0., 0.2107, 0.2107, 0.2107, 0.2107; } - -void setInequalityConstraints(Eigen::Matrix &xMax, Eigen::Matrix &xMin, - Eigen::Matrix &uMax, Eigen::Matrix &uMin) +void setInequalityConstraints(Eigen::Matrix& xMax, + Eigen::Matrix& xMin, + Eigen::Matrix& uMax, + Eigen::Matrix& uMin) { double u0 = 10.5916; // input inequality constraints - uMin << 9.6 - u0, - 9.6 - u0, - 9.6 - u0, - 9.6 - u0; + uMin << 9.6 - u0, 9.6 - u0, 9.6 - u0, 9.6 - u0; - uMax << 13 - u0, - 13 - u0, - 13 - u0, - 13 - u0; + uMax << 13 - u0, 13 - u0, 13 - u0, 13 - u0; // state inequality constraints - xMin << -M_PI/6,-M_PI/6,-OsqpEigen::INFTY,-OsqpEigen::INFTY,-OsqpEigen::INFTY,-1., - -OsqpEigen::INFTY, -OsqpEigen::INFTY,-OsqpEigen::INFTY,-OsqpEigen::INFTY, - -OsqpEigen::INFTY,-OsqpEigen::INFTY; + xMin << -M_PI / 6, -M_PI / 6, -OsqpEigen::INFTY, -OsqpEigen::INFTY, -OsqpEigen::INFTY, -1., + -OsqpEigen::INFTY, -OsqpEigen::INFTY, -OsqpEigen::INFTY, -OsqpEigen::INFTY, + -OsqpEigen::INFTY, -OsqpEigen::INFTY; - xMax << M_PI/6,M_PI/6, OsqpEigen::INFTY,OsqpEigen::INFTY,OsqpEigen::INFTY, - OsqpEigen::INFTY, OsqpEigen::INFTY,OsqpEigen::INFTY,OsqpEigen::INFTY, - OsqpEigen::INFTY,OsqpEigen::INFTY,OsqpEigen::INFTY; + xMax << M_PI / 6, M_PI / 6, OsqpEigen::INFTY, OsqpEigen::INFTY, OsqpEigen::INFTY, + OsqpEigen::INFTY, OsqpEigen::INFTY, OsqpEigen::INFTY, OsqpEigen::INFTY, OsqpEigen::INFTY, + OsqpEigen::INFTY, OsqpEigen::INFTY; } -void setWeightMatrices(Eigen::DiagonalMatrix &Q, Eigen::DiagonalMatrix &R) +void setWeightMatrices(Eigen::DiagonalMatrix& Q, Eigen::DiagonalMatrix& R) { Q.diagonal() << 0, 0, 10., 10., 10., 10., 0, 0, 0, 5., 5., 5.; R.diagonal() << 0.1, 0.1, 0.1, 0.1; } -void castMPCToQPHessian(const Eigen::DiagonalMatrix &Q, const Eigen::DiagonalMatrix &R, int mpcWindow, - Eigen::SparseMatrix &hessianMatrix) +void castMPCToQPHessian(const Eigen::DiagonalMatrix& Q, + const Eigen::DiagonalMatrix& R, + int mpcWindow, + Eigen::SparseMatrix& hessianMatrix) { - hessianMatrix.resize(12*(mpcWindow+1) + 4 * mpcWindow, 12*(mpcWindow+1) + 4 * mpcWindow); + hessianMatrix.resize(12 * (mpcWindow + 1) + 4 * mpcWindow, + 12 * (mpcWindow + 1) + 4 * mpcWindow); - //populate hessian matrix - for(int i = 0; i<12*(mpcWindow+1) + 4 * mpcWindow; i++){ - if(i < 12*(mpcWindow+1)){ - int posQ=i%12; + // populate hessian matrix + for (int i = 0; i < 12 * (mpcWindow + 1) + 4 * mpcWindow; i++) + { + if (i < 12 * (mpcWindow + 1)) + { + int posQ = i % 12; float value = Q.diagonal()[posQ]; - if(value != 0) - hessianMatrix.insert(i,i) = value; - } - else{ - int posR=i%4; + if (value != 0) + hessianMatrix.insert(i, i) = value; + } else + { + int posR = i % 4; float value = R.diagonal()[posR]; - if(value != 0) - hessianMatrix.insert(i,i) = value; + if (value != 0) + hessianMatrix.insert(i, i) = value; } } } -void castMPCToQPGradient(const Eigen::DiagonalMatrix &Q, const Eigen::Matrix &xRef, int mpcWindow, - Eigen::VectorXd &gradient) +void castMPCToQPGradient(const Eigen::DiagonalMatrix& Q, + const Eigen::Matrix& xRef, + int mpcWindow, + Eigen::VectorXd& gradient) { - Eigen::Matrix Qx_ref; + Eigen::Matrix Qx_ref; Qx_ref = Q * (-xRef); // populate the gradient vector - gradient = Eigen::VectorXd::Zero(12*(mpcWindow+1) + 4*mpcWindow, 1); - for(int i = 0; i<12*(mpcWindow+1); i++){ - int posQ=i%12; - float value = Qx_ref(posQ,0); - gradient(i,0) = value; + gradient = Eigen::VectorXd::Zero(12 * (mpcWindow + 1) + 4 * mpcWindow, 1); + for (int i = 0; i < 12 * (mpcWindow + 1); i++) + { + int posQ = i % 12; + float value = Qx_ref(posQ, 0); + gradient(i, 0) = value; } } -void castMPCToQPConstraintMatrix(const Eigen::Matrix &dynamicMatrix, const Eigen::Matrix &controlMatrix, - int mpcWindow, Eigen::SparseMatrix &constraintMatrix) +void castMPCToQPConstraintMatrix(const Eigen::Matrix& dynamicMatrix, + const Eigen::Matrix& controlMatrix, + int mpcWindow, + Eigen::SparseMatrix& constraintMatrix) { - constraintMatrix.resize(12*(mpcWindow+1) + 12*(mpcWindow+1) + 4 * mpcWindow, 12*(mpcWindow+1) + 4 * mpcWindow); + constraintMatrix.resize(12 * (mpcWindow + 1) + 12 * (mpcWindow + 1) + 4 * mpcWindow, + 12 * (mpcWindow + 1) + 4 * mpcWindow); // populate linear constraint matrix - for(int i = 0; i<12*(mpcWindow+1); i++){ - constraintMatrix.insert(i,i) = -1; + for (int i = 0; i < 12 * (mpcWindow + 1); i++) + { + constraintMatrix.insert(i, i) = -1; } - for(int i = 0; i < mpcWindow; i++) - for(int j = 0; j<12; j++) - for(int k = 0; k<12; k++){ - float value = dynamicMatrix(j,k); - if(value != 0){ - constraintMatrix.insert(12 * (i+1) + j, 12 * i + k) = value; + for (int i = 0; i < mpcWindow; i++) + for (int j = 0; j < 12; j++) + for (int k = 0; k < 12; k++) + { + float value = dynamicMatrix(j, k); + if (value != 0) + { + constraintMatrix.insert(12 * (i + 1) + j, 12 * i + k) = value; } } - for(int i = 0; i < mpcWindow; i++) - for(int j = 0; j < 12; j++) - for(int k = 0; k < 4; k++){ - float value = controlMatrix(j,k); - if(value != 0){ - constraintMatrix.insert(12*(i+1)+j, 4*i+k+12*(mpcWindow + 1)) = value; + for (int i = 0; i < mpcWindow; i++) + for (int j = 0; j < 12; j++) + for (int k = 0; k < 4; k++) + { + float value = controlMatrix(j, k); + if (value != 0) + { + constraintMatrix.insert(12 * (i + 1) + j, 4 * i + k + 12 * (mpcWindow + 1)) + = value; } } - for(int i = 0; i<12*(mpcWindow+1) + 4*mpcWindow; i++){ - constraintMatrix.insert(i+(mpcWindow+1)*12,i) = 1; + for (int i = 0; i < 12 * (mpcWindow + 1) + 4 * mpcWindow; i++) + { + constraintMatrix.insert(i + (mpcWindow + 1) * 12, i) = 1; } } -void castMPCToQPConstraintVectors(const Eigen::Matrix &xMax, const Eigen::Matrix &xMin, - const Eigen::Matrix &uMax, const Eigen::Matrix &uMin, - const Eigen::Matrix &x0, - int mpcWindow, Eigen::VectorXd &lowerBound, Eigen::VectorXd &upperBound) +void castMPCToQPConstraintVectors(const Eigen::Matrix& xMax, + const Eigen::Matrix& xMin, + const Eigen::Matrix& uMax, + const Eigen::Matrix& uMin, + const Eigen::Matrix& x0, + int mpcWindow, + Eigen::VectorXd& lowerBound, + Eigen::VectorXd& upperBound) { // evaluate the lower and the upper inequality vectors - Eigen::VectorXd lowerInequality = Eigen::MatrixXd::Zero(12*(mpcWindow+1) + 4 * mpcWindow, 1); - Eigen::VectorXd upperInequality = Eigen::MatrixXd::Zero(12*(mpcWindow+1) + 4 * mpcWindow, 1); - for(int i=0; i &x0, - Eigen::VectorXd &lowerBound, Eigen::VectorXd &upperBound) +void updateConstraintVectors(const Eigen::Matrix& x0, + Eigen::VectorXd& lowerBound, + Eigen::VectorXd& upperBound) { - lowerBound.block(0,0,12,1) = -x0; - upperBound.block(0,0,12,1) = -x0; + lowerBound.block(0, 0, 12, 1) = -x0; + upperBound.block(0, 0, 12, 1) = -x0; } - -double getErrorNorm(const Eigen::Matrix &x, - const Eigen::Matrix &xRef) +double getErrorNorm(const Eigen::Matrix& x, const Eigen::Matrix& xRef) { // evaluate the error Eigen::Matrix error = x - xRef; @@ -201,7 +204,6 @@ double getErrorNorm(const Eigen::Matrix &x, return error.norm(); } - int main() { // set the preview window @@ -233,8 +235,8 @@ int main() Eigen::VectorXd upperBound; // set the initial and the desired states - x0 << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ; - xRef << 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0; + x0 << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; + xRef << 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0; // set MPC problem quantities setDynamicsMatrices(a, b); @@ -251,20 +253,26 @@ int main() OsqpEigen::Solver solver; // settings - //solver.settings()->setVerbosity(false); + // solver.settings()->setVerbosity(false); solver.settings()->setWarmStart(true); // set the initial data of the QP solver solver.data()->setNumberOfVariables(12 * (mpcWindow + 1) + 4 * mpcWindow); solver.data()->setNumberOfConstraints(2 * 12 * (mpcWindow + 1) + 4 * mpcWindow); - if(!solver.data()->setHessianMatrix(hessian)) return 1; - if(!solver.data()->setGradient(gradient)) return 1; - if(!solver.data()->setLinearConstraintsMatrix(linearMatrix)) return 1; - if(!solver.data()->setLowerBound(lowerBound)) return 1; - if(!solver.data()->setUpperBound(upperBound)) return 1; + if (!solver.data()->setHessianMatrix(hessian)) + return 1; + if (!solver.data()->setGradient(gradient)) + return 1; + if (!solver.data()->setLinearConstraintsMatrix(linearMatrix)) + return 1; + if (!solver.data()->setLowerBound(lowerBound)) + return 1; + if (!solver.data()->setUpperBound(upperBound)) + return 1; // instantiate the solver - if(!solver.initSolver()) return 1; + if (!solver.initSolver()) + return 1; // controller input and QPSolution vector Eigen::Vector4d ctr; @@ -273,10 +281,12 @@ int main() // number of iteration steps int numberOfSteps = 50; - for (int i = 0; i < numberOfSteps; i++){ + for (int i = 0; i < numberOfSteps; i++) + { // solve the QP problem - if(solver.solveProblem() != OsqpEigen::ErrorExitFlag::NoError) return 1; + if (solver.solveProblem() != OsqpEigen::ErrorExitFlag::NoError) + return 1; // get the controller input QPSolution = solver.getSolution(); @@ -290,7 +300,8 @@ int main() // update the constraint bound updateConstraintVectors(x0, lowerBound, upperBound); - if(!solver.updateBounds(lowerBound, upperBound)) return 1; - } + if (!solver.updateBounds(lowerBound, upperBound)) + return 1; + } return 0; } diff --git a/include/OsqpEigen/Compat.hpp b/include/OsqpEigen/Compat.hpp index 99db26f..7556f79 100644 --- a/include/OsqpEigen/Compat.hpp +++ b/include/OsqpEigen/Compat.hpp @@ -19,33 +19,31 @@ using c_float = OSQPFloat; using csc = OSQPCscMatrix; // Those symbols are available in the library but hidden from the public API -extern "C" -{ - extern OSQPCscMatrix * csc_spalloc(c_int m, c_int n, c_int nzmax, c_int values, c_int triplet); - extern void csc_spfree(OSQPCscMatrix * A); +extern "C" { +extern OSQPCscMatrix* csc_spalloc(c_int m, c_int n, c_int nzmax, c_int values, c_int triplet); +extern void csc_spfree(OSQPCscMatrix* A); } -# ifndef OSQP_CUSTOM_MEMORY -# define c_malloc malloc -# define c_free free -# endif +#ifndef OSQP_CUSTOM_MEMORY +#define c_malloc malloc +#define c_free free +#endif namespace OsqpEigen { struct OSQPData { - OSQPInt n; ///< number of variables n - OSQPInt m; ///< number of constraints m - OSQPCscMatrix * P; ///< the upper triangular part of the quadratic objective matrix P (size n x n). - OSQPCscMatrix * A; ///< linear constraints matrix A (size m x n) - OSQPFloat * q; ///< dense array for linear part of objective function (size n) - OSQPFloat * l; ///< dense array for lower bound (size m) - OSQPFloat * u; ///< dense array for upper bound (size m) + OSQPInt n; ///< number of variables n + OSQPInt m; ///< number of constraints m + OSQPCscMatrix* P; ///< the upper triangular part of the quadratic objective matrix P (size n x n). + OSQPCscMatrix* A; ///< linear constraints matrix A (size m x n) + OSQPFloat* q; ///< dense array for linear part of objective function (size n) + OSQPFloat* l; ///< dense array for lower bound (size m) + OSQPFloat* u; ///< dense array for upper bound (size m) }; } // namespace OsqpEigen -#endif - -#endif +#endif // OSQP_EIGEN_OSQP_IS_V1 +#endif // OSQP_EIGEN_COMPAT_HPP diff --git a/include/OsqpEigen/Constants.hpp b/include/OsqpEigen/Constants.hpp index 577e790..6dde607 100644 --- a/include/OsqpEigen/Constants.hpp +++ b/include/OsqpEigen/Constants.hpp @@ -15,48 +15,47 @@ */ namespace OsqpEigen { - constexpr c_float INFTY = OSQP_INFTY; /**< Infinity constant. */ - - /** - * Status of the solver - */ - enum class Status : int - { - DualInfeasibleInaccurate = OSQP_DUAL_INFEASIBLE_INACCURATE, - PrimalInfeasibleInaccurate = OSQP_PRIMAL_INFEASIBLE_INACCURATE, - SolvedInaccurate = OSQP_SOLVED_INACCURATE, - Solved = OSQP_SOLVED, - MaxIterReached = OSQP_MAX_ITER_REACHED, - PrimalInfeasible = OSQP_PRIMAL_INFEASIBLE, - DualInfeasible = OSQP_DUAL_INFEASIBLE, - Sigint = OSQP_SIGINT, -# ifdef PROFILING - TimeLimitReached = OSQP_TIME_LIMIT_REACHED, -# endif // ifdef PROFILING - NonCvx = OSQP_NON_CVX, - Unsolved = OSQP_UNSOLVED - }; +constexpr c_float INFTY = OSQP_INFTY; /**< Infinity constant. */ +/** + * Status of the solver + */ +enum class Status : int +{ + DualInfeasibleInaccurate = OSQP_DUAL_INFEASIBLE_INACCURATE, + PrimalInfeasibleInaccurate = OSQP_PRIMAL_INFEASIBLE_INACCURATE, + SolvedInaccurate = OSQP_SOLVED_INACCURATE, + Solved = OSQP_SOLVED, + MaxIterReached = OSQP_MAX_ITER_REACHED, + PrimalInfeasible = OSQP_PRIMAL_INFEASIBLE, + DualInfeasible = OSQP_DUAL_INFEASIBLE, + Sigint = OSQP_SIGINT, +#ifdef PROFILING + TimeLimitReached = OSQP_TIME_LIMIT_REACHED, +#endif // ifdef PROFILING + NonCvx = OSQP_NON_CVX, + Unsolved = OSQP_UNSOLVED +}; - /** - * Error status of the Solver - */ - enum class ErrorExitFlag : int - { - NoError = 0, - DataValidationError = OSQP_DATA_VALIDATION_ERROR, - SettingsValidationError = OSQP_SETTINGS_VALIDATION_ERROR, +/** + * Error status of the Solver + */ +enum class ErrorExitFlag : int +{ + NoError = 0, + DataValidationError = OSQP_DATA_VALIDATION_ERROR, + SettingsValidationError = OSQP_SETTINGS_VALIDATION_ERROR, #ifdef OSQP_EIGEN_OSQP_IS_V1 - LinsysSolverLoadError = OSQP_ALGEBRA_LOAD_ERROR, + LinsysSolverLoadError = OSQP_ALGEBRA_LOAD_ERROR, #else - LinsysSolverLoadError = OSQP_LINSYS_SOLVER_LOAD_ERROR, + LinsysSolverLoadError = OSQP_LINSYS_SOLVER_LOAD_ERROR, #endif - LinsysSolverInitError = OSQP_LINSYS_SOLVER_INIT_ERROR, - NonCvxError = OSQP_NONCVX_ERROR, - MemAllocError = OSQP_MEM_ALLOC_ERROR, - WorkspaceNotInitError = OSQP_WORKSPACE_NOT_INIT_ERROR - }; + LinsysSolverInitError = OSQP_LINSYS_SOLVER_INIT_ERROR, + NonCvxError = OSQP_NONCVX_ERROR, + MemAllocError = OSQP_MEM_ALLOC_ERROR, + WorkspaceNotInitError = OSQP_WORKSPACE_NOT_INIT_ERROR +}; -} +} // namespace OsqpEigen #endif diff --git a/include/OsqpEigen/Data.hpp b/include/OsqpEigen/Data.hpp index f77b57c..58173ee 100644 --- a/include/OsqpEigen/Data.hpp +++ b/include/OsqpEigen/Data.hpp @@ -22,134 +22,135 @@ */ namespace OsqpEigen { +/** + * Data class is a wrapper of the OSQP OSQPData struct. + */ +class Data +{ + OSQPData* m_data; /**< OSQPData struct. */ + bool m_isNumberOfVariablesSet; /**< Boolean true if the number of variables is set. */ + bool m_isNumberOfConstraintsSet; /**< Boolean true if the number of constraints is set. */ + bool m_isHessianMatrixSet; /**< Boolean true if the hessian matrix is set. */ + bool m_isGradientSet; /**< Boolean true if the gradient vector is set. */ + bool m_isLinearConstraintsMatrixSet; /**< Boolean true if the linear constrain matrix is set. */ + bool m_isLowerBoundSet; /**< Boolean true if the lower bound vector is set. */ + bool m_isUpperBoundSet; /**< Boolean true if the upper bound vector is set. */ + +public: + /** + * Constructor. + */ + Data(); + + /** + * Constructor. + * @param n is the number of variables; + * @param m is the number of constraints. + */ + Data(int n, int m); + + /** + * Deconstructor. + */ + ~Data(); + + /** + * Clear the hessian matrix. + */ + void clearHessianMatrix(); + + /** + * Clear the linear constraints matrix. + */ + void clearLinearConstraintsMatrix(); + + /** + * Set the number of variables. + * @param n is the number of variables. + */ + void setNumberOfVariables(int n); + + /** + * Set the number of constraints. + * @param m is the number of constraints. + */ + void setNumberOfConstraints(int m); + + /** + * Set the quadratic part of the cost function (Hessian). + * It is assumed to be a simmetric matrix. + * @param hessianMatrix is the Hessian matrix. + * @return true/false in case of success/failure. + */ + template + bool setHessianMatrix(const Eigen::SparseCompressedBase& hessianMatrix); + + /** + * Set the linear part of the cost function (Gradient). + * @param gradientVector is the Gradient vector. + * @note the elements of the gradient are not copied inside the library. + * The user has to guarantee that the lifetime of the object passed is the same of the + * OsqpEigen object + * @return true/false in case of success/failure. + */ + bool setGradient(Eigen::Ref> gradientVector); + + Eigen::Matrix getGradient(); + + /** + * Set the linear constraint matrix A (size m x n) + * @param linearConstraintsMatrix is the linear constraints matrix A. + * @return true/false in case of success/failure. + */ + template + bool + setLinearConstraintsMatrix(const Eigen::SparseCompressedBase& linearConstraintsMatrix); + + /** + * Set the array for lower bound (size m). + * @param lowerBoundVector is the lower bound constraint. + * @note the elements of the lowerBoundVector are not copied inside the library. + * The user has to guarantee that the lifetime of the object passed is the same of the + * OsqpEigen object + * @return true/false in case of success/failure. + */ + bool setLowerBound(Eigen::Ref> lowerBoundVector); + + /** + * Set the array for upper bound (size m). + * @param upperBoundVector is the upper bound constraint. + * @note the elements of the upperBoundVector are not copied inside the library. + * The user has to guarantee that the lifetime of the object passed is the same of the + * OsqpEigen object. + * @return true/false in case of success/failure. + */ + bool setUpperBound(Eigen::Ref> upperBoundVector); + + /** + * Set the array for upper and lower bounds (size m). + * @param lowerBound is the lower bound constraint. + * @param upperBound is the upper bound constraint. + * @note the elements of the upperBound and lowerBound are not copied inside the library. + * The user has to guarantee that the lifetime of the object passed is the same of the + * OsqpEigen object. + * @return true/false in case of success/failure. + */ + bool setBounds(Eigen::Ref> lowerBound, + Eigen::Ref> upperBound); + + /** + * Get the OSQPData struct. + * @return a const point to the OSQPData struct. + */ + OSQPData* const& getData() const; + /** - * Data class is a wrapper of the OSQP OSQPData struct. + * Verify if all the matrix and vectors are already set. + * @return true if all the OSQPData struct are set. */ - class Data - { - OSQPData *m_data; /**< OSQPData struct. */ - bool m_isNumberOfVariablesSet; /**< Boolean true if the number of variables is set. */ - bool m_isNumberOfConstraintsSet; /**< Boolean true if the number of constraints is set. */ - bool m_isHessianMatrixSet; /**< Boolean true if the hessian matrix is set. */ - bool m_isGradientSet; /**< Boolean true if the gradient vector is set. */ - bool m_isLinearConstraintsMatrixSet; /**< Boolean true if the linear constrain matrix is set. */ - bool m_isLowerBoundSet; /**< Boolean true if the lower bound vector is set. */ - bool m_isUpperBoundSet; /**< Boolean true if the upper bound vector is set. */ - - public: - /** - * Constructor. - */ - Data(); - - /** - * Constructor. - * @param n is the number of variables; - * @param m is the number of constraints. - */ - Data(int n, int m); - - /** - * Deconstructor. - */ - ~Data(); - - /** - * Clear the hessian matrix. - */ - void clearHessianMatrix(); - - /** - * Clear the linear constraints matrix. - */ - void clearLinearConstraintsMatrix(); - - /** - * Set the number of variables. - * @param n is the number of variables. - */ - void setNumberOfVariables(int n); - - /** - * Set the number of constraints. - * @param m is the number of constraints. - */ - void setNumberOfConstraints(int m); - - /** - * Set the quadratic part of the cost function (Hessian). - * It is assumed to be a simmetric matrix. - * @param hessianMatrix is the Hessian matrix. - * @return true/false in case of success/failure. - */ - template - bool setHessianMatrix(const Eigen::SparseCompressedBase &hessianMatrix); - - /** - * Set the linear part of the cost function (Gradient). - * @param gradientVector is the Gradient vector. - * @note the elements of the gradient are not copied inside the library. - * The user has to guarantee that the lifetime of the object passed is the same of the - * OsqpEigen object - * @return true/false in case of success/failure. - */ - bool setGradient(Eigen::Ref> gradientVector); - - Eigen::Matrix getGradient(); - - /** - * Set the linear constraint matrix A (size m x n) - * @param linearConstraintsMatrix is the linear constraints matrix A. - * @return true/false in case of success/failure. - */ - template - bool setLinearConstraintsMatrix(const Eigen::SparseCompressedBase &linearConstraintsMatrix); - - /** - * Set the array for lower bound (size m). - * @param lowerBoundVector is the lower bound constraint. - * @note the elements of the lowerBoundVector are not copied inside the library. - * The user has to guarantee that the lifetime of the object passed is the same of the - * OsqpEigen object - * @return true/false in case of success/failure. - */ - bool setLowerBound(Eigen::Ref> lowerBoundVector); - - /** - * Set the array for upper bound (size m). - * @param upperBoundVector is the upper bound constraint. - * @note the elements of the upperBoundVector are not copied inside the library. - * The user has to guarantee that the lifetime of the object passed is the same of the - * OsqpEigen object. - * @return true/false in case of success/failure. - */ - bool setUpperBound(Eigen::Ref> upperBoundVector); - - /** - * Set the array for upper and lower bounds (size m). - * @param lowerBound is the lower bound constraint. - * @param upperBound is the upper bound constraint. - * @note the elements of the upperBound and lowerBound are not copied inside the library. - * The user has to guarantee that the lifetime of the object passed is the same of the - * OsqpEigen object. - * @return true/false in case of success/failure. - */ - bool setBounds(Eigen::Ref> lowerBound, - Eigen::Ref> upperBound); - - /** - * Get the OSQPData struct. - * @return a const point to the OSQPData struct. - */ - OSQPData *const & getData() const; - - /** - * Verify if all the matrix and vectors are already set. - * @return true if all the OSQPData struct are set. - */ - bool isSet() const; - }; -} + bool isSet() const; +}; +} // namespace OsqpEigen #include diff --git a/include/OsqpEigen/Data.tpp b/include/OsqpEigen/Data.tpp index 09c622e..cd0999e 100644 --- a/include/OsqpEigen/Data.tpp +++ b/include/OsqpEigen/Data.tpp @@ -9,75 +9,93 @@ #include -template -bool OsqpEigen::Data::setHessianMatrix(const Eigen::SparseCompressedBase &hessianMatrix) +template +bool OsqpEigen::Data::setHessianMatrix(const Eigen::SparseCompressedBase& hessianMatrix) { - if(m_isHessianMatrixSet){ + if (m_isHessianMatrixSet) + { debugStream() << "[OsqpEigen::Data::setHessianMatrix] The hessian matrix was already set. " - << "Please use clearHessianMatrix() method to deallocate memory." - << std::endl; + << "Please use clearHessianMatrix() method to deallocate memory." + << std::endl; return false; } - if(!m_isNumberOfVariablesSet){ - debugStream() << "[OsqpEigen::Data::setHessianMatrix] Please set the number of variables before " - << "add the hessian matrix." - << std::endl; + if (!m_isNumberOfVariablesSet) + { + debugStream() << "[OsqpEigen::Data::setHessianMatrix] Please set the number of variables " + "before add the hessian matrix." + << std::endl; return false; } // check if the number of row and columns are equal to the number of the optimization variables - if ((hessianMatrix.rows() != m_data->n) || (hessianMatrix.cols()!= m_data->n)){ - debugStream() << "[OsqpEigen::Data::setHessianMatrix] The Hessian matrix has to be a n x n size matrix." - << std::endl; + if ((hessianMatrix.rows() != m_data->n) || (hessianMatrix.cols() != m_data->n)) + { + debugStream() << "[OsqpEigen::Data::setHessianMatrix] The Hessian matrix has to be a n x n " + "size matrix." + << std::endl; return false; } - //set the hessian matrix - // osqp 0.6.0 required only the upper triangular part of the hessian matrix + // set the hessian matrix + // osqp 0.6.0 required only the upper triangular part of the hessian matrix Derived hessianMatrixUpperTriangular = hessianMatrix.template triangularView(); - if(!OsqpEigen::SparseMatrixHelper::createOsqpSparseMatrix(hessianMatrixUpperTriangular, m_data->P)){ - debugStream() << "[OsqpEigen::Data::setHessianMatrix] Unable to instantiate the osqp sparse matrix." - << std::endl; + if (!OsqpEigen::SparseMatrixHelper::createOsqpSparseMatrix(hessianMatrixUpperTriangular, + m_data->P)) + { + debugStream() << "[OsqpEigen::Data::setHessianMatrix] Unable to instantiate the osqp " + "sparse matrix." + << std::endl; return false; } m_isHessianMatrixSet = true; return true; } -template -bool OsqpEigen::Data::setLinearConstraintsMatrix(const Eigen::SparseCompressedBase &linearConstraintsMatrix) +template +bool OsqpEigen::Data::setLinearConstraintsMatrix( + const Eigen::SparseCompressedBase& linearConstraintsMatrix) { - if(m_isLinearConstraintsMatrixSet){ - debugStream() << "[OsqpEigen::Data::setLinearConstraintsMatrix] The linear constraint matrix was already set. " - << "Please use clearLinearConstraintsMatrix() method to deallocate memory." - << std::endl; + if (m_isLinearConstraintsMatrixSet) + { + debugStream() << "[OsqpEigen::Data::setLinearConstraintsMatrix] The linear constraint " + "matrix was already set. Please use clearLinearConstraintsMatrix() method " + "to deallocate memory." + << std::endl; return false; } - if(!m_isNumberOfConstraintsSet){ - debugStream() << "[OsqpEigen::Data::setLinearConstraintsMatrix] Please set the number of constraints before add the constraint matrix." - << std::endl; + if (!m_isNumberOfConstraintsSet) + { + debugStream() << "[OsqpEigen::Data::setLinearConstraintsMatrix] Please set the number of " + "constraints before add the constraint matrix." + << std::endl; return false; } - if(!m_isNumberOfVariablesSet){ - debugStream() << "[OsqpEigen::Data::setLinearConstraintsMatrix] Please set the number of variables before add the constraint matrix." - << std::endl; + if (!m_isNumberOfVariablesSet) + { + debugStream() << "[OsqpEigen::Data::setLinearConstraintsMatrix] Please set the number of " + "variables before add the constraint matrix." + << std::endl; return false; } - if ((linearConstraintsMatrix.rows() != m_data->m) || (linearConstraintsMatrix.cols()!= m_data->n)){ - debugStream() << "[OsqpEigen::Data::setLinearConstraintsMatrix] The Linear constraints matrix has to be a m x n size matrix." - << std::endl; + if ((linearConstraintsMatrix.rows() != m_data->m) + || (linearConstraintsMatrix.cols() != m_data->n)) + { + debugStream() << "[OsqpEigen::Data::setLinearConstraintsMatrix] The Linear constraints " + "matrix has to be a m x n size matrix." + << std::endl; return false; } // set the hessian matrix - if(!OsqpEigen::SparseMatrixHelper::createOsqpSparseMatrix(linearConstraintsMatrix, - m_data->A)){ - debugStream() << "[OsqpEigen::Data::setLinearConstraintsMatrix] osqp sparse matrix not created." - << std::endl; + if (!OsqpEigen::SparseMatrixHelper::createOsqpSparseMatrix(linearConstraintsMatrix, m_data->A)) + { + debugStream() << "[OsqpEigen::Data::setLinearConstraintsMatrix] osqp sparse matrix not " + "created." + << std::endl; return false; } diff --git a/include/OsqpEigen/Debug.hpp b/include/OsqpEigen/Debug.hpp index bdb5b53..e568e3d 100644 --- a/include/OsqpEigen/Debug.hpp +++ b/include/OsqpEigen/Debug.hpp @@ -3,8 +3,9 @@ #include -namespace OsqpEigen { - std::ostream &debugStream(); -} +namespace OsqpEigen +{ +std::ostream& debugStream(); +} // namespace OsqpEigen #endif /* OSQPEIGEN_DEBUG_HPP */ diff --git a/include/OsqpEigen/OsqpEigen.h b/include/OsqpEigen/OsqpEigen.h index 9cc54e1..82f4a15 100644 --- a/include/OsqpEigen/OsqpEigen.h +++ b/include/OsqpEigen/OsqpEigen.h @@ -13,4 +13,4 @@ #include #include -#endif //OSQPEIGEN_OSQPEIGEN_H +#endif // OSQPEIGEN_OSQPEIGEN_H diff --git a/include/OsqpEigen/Settings.hpp b/include/OsqpEigen/Settings.hpp index c4d0a21..938c0cb 100644 --- a/include/OsqpEigen/Settings.hpp +++ b/include/OsqpEigen/Settings.hpp @@ -16,191 +16,191 @@ */ namespace OsqpEigen { +/** + * settings class is a wrapper of the OSQP OSQPSettings struct. + * All the setter methods refer to this particular kind of optimizer. + * Here + * you can find further information. + */ +class Settings +{ + OSQPSettings* m_settings; /**< OSQPSettings struct. */ +public: + /** + * Constructor. + */ + Settings(); + + /** + * Deconstructor. + */ + ~Settings(); + + /** + * Reset the default settings for the optimization problem. + */ + void resetDefaultSettings(); + + /** + * Set the ADMM step rho. + * @param rho a ADMM step constant. + */ + void setRho(const double rho); + + /** + * Set the ADMM step sigma. + * @param sigma a ADMM step constant. + */ + void setSigma(const double sigma); + + /** + * Set the heuristic data scaling iterations. If 0, scaling disabled. + * @param scaling is the heuristic data scaling iteration. + */ + void setScaling(const int scaling); + + /** + * Set if the rho step size adaptive feature is active. + * @param isRhoStepSizeAdactive if True the feature is active. + */ + void setAdaptiveRho(const bool isRhoStepSizeAdactive); + + /** + * Set the number of iterations between rho adaptations rho. If 0, it is automatic. + * @param rhoInterval number of iterations. + */ + void setAdaptiveRhoInterval(const int rhoInterval); + + /** + * Set the tolerance for adapting rho. The new rho has to be X times larger or 1/X times + * smaller than the current one to trigger a new factorization. + * @param adaptiveRhoTolerance is the tolerance. + */ + void setAdaptiveRhoTolerance(const double adaptiveRhoTolerance); + + /** + * Set the interval for adapting rho (fraction of the setup time). + * @param adaptiveRhoFraction interval of the adapting rho. + */ + void setAdaptiveRhoFraction(const double adaptiveRhoFraction); + + /** + * Set the max number of iterations. + * @param maxIteration max number of iteration + */ + [[deprecated("Use setMaxIteration(int) instead.")]] void + setMaxIteraction(const int maxIteration); + + /** + * Set the max number of iterations. + * @param maxIteration max number of iteration + */ + void setMaxIteration(const int maxIteration); + + /** + * Set the absolute convergence tolerance. + * @param absoluteTolerance absoulte tolerance of the solver. + */ + void setAbsoluteTolerance(const double absoluteTolerance); + + /** + * Set the relative convergence tolerance. + * @param relativeTolerance relative tolerance of the solver. + */ + void setRelativeTolerance(const double relativeTolerance); + + /** + * Set the primal infeasibility tolerance. + * @param primalInfeasibilityTolerance tolerance of the primal variables. + */ + [[deprecated("Use setPrimalInfeasibilityTolerance() instead.")]] void + setPrimalInfeasibilityTollerance(const double primalInfeasibilityTolerance); + + /** + * Set the primal infeasibility tolerance. + * @param primalInfeasibilityTolerance tolerance of the primal variables. + */ + void setPrimalInfeasibilityTolerance(const double primalInfeasibilityTolerance); + + /** + * Set the dual infeasibility tolerance. + * @param dualInfeasibilityTolerance tolerance of the dual variables. + */ + [[deprecated("Use setDualInfeasibilityTolerance() instead.")]] void + setDualInfeasibilityTollerance(const double dualInfeasibilityTolerance); + + /** + * Set the dual infeasibility tolerance. + * @param dualInfeasibilityTolerance tolerance of the dual variables. + */ + void setDualInfeasibilityTolerance(const double dualInfeasibilityTolerance); + + /** + * Set the relaxation parameter. + * @param alpha is the relaxation parameter. + */ + void setAlpha(const double alpha); + + /** + * Set linear solver + * @param linsysSolver is the name of the solver + */ + void setLinearSystemSolver(const int linsysSolver); + + /** + * Set the relaxation parameter for polish. + * @param delta is the relaxation parameter. + */ + void setDelta(const double delta); + /** - * settings class is a wrapper of the OSQP OSQPSettings struct. - * All the setter methods refer to this particular kind of optimizer. - * Here - * you can find further information. - */ - class Settings - { - OSQPSettings *m_settings; /**< OSQPSettings struct. */ - public: - - /** - * Constructor. - */ - Settings(); - - /** - * Deconstructor. - */ - ~Settings(); - - /** - * Reset the default settings for the optimization problem. - */ - void resetDefaultSettings(); - - /** - * Set the ADMM step rho. - * @param rho a ADMM step constant. - */ - void setRho(const double rho); - - /** - * Set the ADMM step sigma. - * @param sigma a ADMM step constant. - */ - void setSigma(const double sigma); - - /** - * Set the heuristic data scaling iterations. If 0, scaling disabled. - * @param scaling is the heuristic data scaling iteration. - */ - void setScaling(const int scaling); - - /** - * Set if the rho step size adaptive feature is active. - * @param isRhoStepSizeAdactive if True the feature is active. - */ - void setAdaptiveRho(const bool isRhoStepSizeAdactive); - - /** - * Set the number of iterations between rho adaptations rho. If 0, it is automatic. - * @param rhoInterval number of iterations. - */ - void setAdaptiveRhoInterval(const int rhoInterval); - - /** - * Set the tolerance for adapting rho. The new rho has to be X times larger or 1/X times - * smaller than the current one to trigger a new factorization. - * @param adaptiveRhoTolerance is the tolerance. - */ - void setAdaptiveRhoTolerance(const double adaptiveRhoTolerance); - - /** - * Set the interval for adapting rho (fraction of the setup time). - * @param adaptiveRhoFraction interval of the adapting rho. - */ - void setAdaptiveRhoFraction(const double adaptiveRhoFraction); - - /** - * Set the max number of iterations. - * @param maxIteration max number of iteration - */ - [[deprecated("Use setMaxIteration(int) instead.")]] void setMaxIteraction(const int maxIteration); - - /** - * Set the max number of iterations. - * @param maxIteration max number of iteration - */ - void setMaxIteration(const int maxIteration); - - /** - * Set the absolute convergence tolerance. - * @param absoluteTolerance absoulte tolerance of the solver. - */ - void setAbsoluteTolerance(const double absoluteTolerance); - - /** - * Set the relative convergence tolerance. - * @param relativeTolerance relative tolerance of the solver. - */ - void setRelativeTolerance(const double relativeTolerance); - - /** - * Set the primal infeasibility tolerance. - * @param primalInfeasibilityTolerance tolerance of the primal variables. - */ - [[deprecated("Use setPrimalInfeasibilityTolerance() instead.")]] - void setPrimalInfeasibilityTollerance(const double primalInfeasibilityTolerance); - - /** - * Set the primal infeasibility tolerance. - * @param primalInfeasibilityTolerance tolerance of the primal variables. - */ - void setPrimalInfeasibilityTolerance(const double primalInfeasibilityTolerance); - - /** - * Set the dual infeasibility tolerance. - * @param dualInfeasibilityTolerance tolerance of the dual variables. - */ - [[deprecated("Use setDualInfeasibilityTolerance() instead.")]] - void setDualInfeasibilityTollerance(const double dualInfeasibilityTolerance); - - /** - * Set the dual infeasibility tolerance. - * @param dualInfeasibilityTolerance tolerance of the dual variables. - */ - void setDualInfeasibilityTolerance(const double dualInfeasibilityTolerance); - - /** - * Set the relaxation parameter. - * @param alpha is the relaxation parameter. - */ - void setAlpha(const double alpha); - - /** - * Set linear solver - * @param linsysSolver is the name of the solver - */ - void setLinearSystemSolver(const int linsysSolver); - - /** - * Set the relaxation parameter for polish. - * @param delta is the relaxation parameter. - */ - void setDelta(const double delta); - - /** - * Set if the polish feature is active. - * @param polish if True the feature is active. - */ - void setPolish(const bool polish); - - /** - * Set the iterative refinement steps in polish. - * @param polishRefineIter iterative refinement step. - */ - void setPolishRefineIter(const int polishRefineIter); - - /** - * Set the Verbose mode. - * @param isVerbose if true the verbose mode is activate. - */ - void setVerbosity(const bool isVerbose); - - /** - * Set the scaled termination criteria. - * @param scaledTermination if true the scaled termination criteria is used. - */ - void setScaledTerimination(const bool scaledTermination); - - /** - * Set check termination interval. If 0, termination checking is disabled. - * @param checkTermination if 0 the termination checking is disabled. - */ - void setCheckTermination(const int checkTermination); - - /** - * Set warm start. - * @param warmStart if true the warm start is set. - */ - void setWarmStart(const bool warmStart); - - /** - * Set the maximum number of seconds allowed to solve the problem. - * @param timeLimit is the time limit in seconds. If 0, then disabled. - */ - void setTimeLimit(const double timeLimit); - - /** - * Get a pointer to Settings struct. - * @return a const pointer to OSQPSettings struct. - */ - OSQPSettings* const & getSettings() const; - }; -} + * Set if the polish feature is active. + * @param polish if True the feature is active. + */ + void setPolish(const bool polish); + + /** + * Set the iterative refinement steps in polish. + * @param polishRefineIter iterative refinement step. + */ + void setPolishRefineIter(const int polishRefineIter); + + /** + * Set the Verbose mode. + * @param isVerbose if true the verbose mode is activate. + */ + void setVerbosity(const bool isVerbose); + + /** + * Set the scaled termination criteria. + * @param scaledTermination if true the scaled termination criteria is used. + */ + void setScaledTerimination(const bool scaledTermination); + + /** + * Set check termination interval. If 0, termination checking is disabled. + * @param checkTermination if 0 the termination checking is disabled. + */ + void setCheckTermination(const int checkTermination); + + /** + * Set warm start. + * @param warmStart if true the warm start is set. + */ + void setWarmStart(const bool warmStart); + + /** + * Set the maximum number of seconds allowed to solve the problem. + * @param timeLimit is the time limit in seconds. If 0, then disabled. + */ + void setTimeLimit(const double timeLimit); + + /** + * Get a pointer to Settings struct. + * @return a const pointer to OSQPSettings struct. + */ + OSQPSettings* const& getSettings() const; +}; +} // namespace OsqpEigen #endif diff --git a/include/OsqpEigen/Solver.hpp b/include/OsqpEigen/Solver.hpp index 2228e2e..d404398 100644 --- a/include/OsqpEigen/Solver.hpp +++ b/include/OsqpEigen/Solver.hpp @@ -18,289 +18,298 @@ // OsqpEigen #include +#include #include #include -#include /** * OsqpEigen namespace. */ namespace OsqpEigen { - /** - * Solver class is a wrapper of the OSQP OSQPWorkspace struct. - */ - class Solver - { - bool m_isSolverInitialized; /**< Boolean true if solver is initialized. */ +/** + * Solver class is a wrapper of the OSQP OSQPWorkspace struct. + */ +class Solver +{ + bool m_isSolverInitialized; /**< Boolean true if solver is initialized. */ #ifdef OSQP_EIGEN_OSQP_IS_V1 - std::unique_ptr> m_solver; /**< Pointer to OSQPSolver struct. */ + std::unique_ptr> m_solver; /**< Pointer to + OSQPSolver struct. */ #else - std::unique_ptr> m_workspace; /**< Pointer to OSQPWorkspace struct. */ + std::unique_ptr> m_workspace; /**< Pointer to + OSQPWorkspace + struct. */ #endif - std::unique_ptr m_settings; /**< Pointer to Settings class. */ - std::unique_ptr m_data; /**< Pointer to Data class. */ - Eigen::Matrix m_primalVariables; - Eigen::Matrix m_dualVariables; - Eigen::Matrix m_solution; - Eigen::Matrix m_dualSolution; - - std::vector m_hessianNewIndices; - std::vector m_hessianNewValues; - - std::vector m_constraintsNewIndices; - std::vector m_constraintsNewValues; - - std::vector> m_oldHessianTriplet, m_newHessianTriplet, m_newUpperTriangularHessianTriplets; - std::vector> m_oldLinearConstraintsTriplet, m_newLinearConstraintsTriplet; - - /** - * Evaluate the position and the values of the new elements of a sparse matrix. - * @param oldMatrixTriplet vector containing the triplets of the old sparse matrix; - * @param newMatrixTriplet vector containing the triplets of the mew sparse matrix; - * @param newIndices vector of the index mapping new elements - * to position in the sparse matrix; - * @param newValues vector of new elements in the sparse matrix. - * @return true if the sparsity patern is not changed false otherwise. - */ - template - bool evaluateNewValues(const std::vector> &oldMatrixTriplet, - const std::vector> &newMatrixTriplet, - std::vector &newIndices, - std::vector &newValues) const; - - /** - * Takes only the triplets which belongs to the upper triangular part of the matrix. - * @param fullMatrixTriplets vector containing the triplets of the sparse matrix; - * @param upperTriangularMatrixTriplets vector containing the triplets of the mew sparse matrix; - */ - template - void selectUpperTriangularTriplets(const std::vector> &fullMatrixTriplets, - std::vector> &upperTriangularMatrixTriplets) const; + std::unique_ptr m_settings; /**< Pointer to Settings class. */ + std::unique_ptr m_data; /**< Pointer to Data class. */ + Eigen::Matrix m_primalVariables; + Eigen::Matrix m_dualVariables; + Eigen::Matrix m_solution; + Eigen::Matrix m_dualSolution; + + std::vector m_hessianNewIndices; + std::vector m_hessianNewValues; + + std::vector m_constraintsNewIndices; + std::vector m_constraintsNewValues; + + std::vector> m_oldHessianTriplet, m_newHessianTriplet, + m_newUpperTriangularHessianTriplets; + std::vector> m_oldLinearConstraintsTriplet, + m_newLinearConstraintsTriplet; + + /** + * Evaluate the position and the values of the new elements of a sparse matrix. + * @param oldMatrixTriplet vector containing the triplets of the old sparse matrix; + * @param newMatrixTriplet vector containing the triplets of the mew sparse matrix; + * @param newIndices vector of the index mapping new elements + * to position in the sparse matrix; + * @param newValues vector of new elements in the sparse matrix. + * @return true if the sparsity patern is not changed false otherwise. + */ + template + bool evaluateNewValues(const std::vector>& oldMatrixTriplet, + const std::vector>& newMatrixTriplet, + std::vector& newIndices, + std::vector& newValues) const; + + /** + * Takes only the triplets which belongs to the upper triangular part of the matrix. + * @param fullMatrixTriplets vector containing the triplets of the sparse matrix; + * @param upperTriangularMatrixTriplets vector containing the triplets of the mew sparse matrix; + */ + template + void selectUpperTriangularTriplets( + const std::vector>& fullMatrixTriplets, + std::vector>& upperTriangularMatrixTriplets) const; #ifdef OSQP_EIGEN_OSQP_IS_V1 - /** - * Custom Deleter for the OSQPSolver. It is required to free the @ref m_workspace unique_ptr - * @param ptr raw pointer to the workspace - */ - static void OSQPSolverDeleter(OSQPSolver* ptr) noexcept; + /** + * Custom Deleter for the OSQPSolver. It is required to free the @ref m_workspace unique_ptr + * @param ptr raw pointer to the workspace + */ + static void OSQPSolverDeleter(OSQPSolver* ptr) noexcept; #else - /** - * Custom Deleter for the OSQPWorkspace. It is required to free the @ref m_workspace unique_ptr - * @param ptr raw pointer to the workspace - */ - static void OSQPWorkspaceDeleter(OSQPWorkspace* ptr) noexcept; + /** + * Custom Deleter for the OSQPWorkspace. It is required to free the @ref m_workspace unique_ptr + * @param ptr raw pointer to the workspace + */ + static void OSQPWorkspaceDeleter(OSQPWorkspace* ptr) noexcept; #endif - inline const OSQPData * getData() const noexcept { + inline const OSQPData* getData() const noexcept + { #ifdef OSQP_EIGEN_OSQP_IS_V1 - return m_data->getData(); + return m_data->getData(); #else - return m_workspace->data; + return m_workspace->data; #endif - } + } - inline const OSQPInfo * getInfo() const noexcept { + inline const OSQPInfo* getInfo() const noexcept + { #ifdef OSQP_EIGEN_OSQP_IS_V1 - return m_solver->info; + return m_solver->info; #else - return m_workspace->info; + return m_workspace->info; #endif - } + } - inline const OSQPSolution * getOSQPSolution() const noexcept { + inline const OSQPSolution* getOSQPSolution() const noexcept + { #ifdef OSQP_EIGEN_OSQP_IS_V1 - return m_solver->solution; + return m_solver->solution; #else - return m_workspace->solution; + return m_workspace->solution; #endif - } - - public: - - /** - * Constructor. - */ - Solver(); - - /** - * Initialize the solver with the actual initial data and settings. - * @return true/false in case of success/failure. - */ - bool initSolver(); - - /** - * Check if the solver is initialized. - * @return true if the solver is initialized. - */ - bool isInitialized(); - - /** - * Dealocate memory. - */ - void clearSolver(); - - /** - * Set to zero all the solver variables. - * @return true/false in case of success/failure. - */ - bool clearSolverVariables(); - - /** - * Solve the QP optimization problem. - * @return true/false in case of success/failure. - */ - [[deprecated("Use solveProblem() instead.")]] - bool solve(); - - /** - * Solve the QP optimization problem. - * @return the error exit flag - */ - OsqpEigen::ErrorExitFlag solveProblem(); - - /** - * Get the status of the solver - * @return The inner solver status - */ - OsqpEigen::Status getStatus() const; - - /** - * Get the primal objective value - * @return The primal objective value - */ - const c_float getObjValue() const; - - /** - * Get the optimization problem solution. - * @return an Eigen::Vector contating the optimization result. - */ - const Eigen::Matrix &getSolution(); - - /** - * Get the dual optimization problem solution. - * @return an Eigen::Vector contating the optimization result. - */ - const Eigen::Matrix &getDualSolution(); - - /** - * Update the linear part of the cost function (Gradient). - * @param gradient is the Gradient vector. - * @note the elements of the gradient are not copied inside the library. - * The user has to guarantee that the lifetime of the objects passed is the same of the - * OsqpEigen object. - * @return true/false in case of success/failure. - */ - bool updateGradient(const Eigen::Ref>& gradient); - - /** - * Update the lower bounds limit (size m). - * @param lowerBound is the lower bound constraint vector. - * @note the elements of the lowerBound are not copied inside the library. - * The user has to guarantee that the lifetime of the object passed is the same of the - * OsqpEigen object. - * @return true/false in case of success/failure. - */ - bool updateLowerBound(const Eigen::Ref>& lowerBound); - - /** - * Update the upper bounds limit (size m). - * @param upperBound is the upper bound constraint vector. - * @note the elements of the upperBound are not copied inside the library. - * The user has to guarantee that the lifetime of the object passed is the same of the - * OsqpEigen object. - * @return true/false in case of success/failure. - */ - bool updateUpperBound(const Eigen::Ref>& upperBound); - - /** - * Update both upper and lower bounds (size m). - * @param lowerBound is the lower bound constraint vector; - * @param upperBound is the upper bound constraint vector. - * @note the elements of the lowerBound and upperBound are not copied inside the library. - * The user has to guarantee that the lifetime of the objects passed is the same of the - * OsqpEigen object - * @return true/false in case of success/failure. - */ - bool updateBounds(const Eigen::Ref>& lowerBound, - const Eigen::Ref>& upperBound); - - /** - * Update the quadratic part of the cost function (Hessian). - * It is assumed to be a simmetric matrix. - * \note - * If the sparsity pattern is preserved the matrix is simply update - * otherwise the entire solver will be reinitialized. In this case - * the primal and dual variable are copied in the new workspace. - * - * @param hessian is the Hessian matrix. - * @return true/false in case of success/failure. - */ - template - bool updateHessianMatrix(const Eigen::SparseCompressedBase &hessianMatrix); - - /** - * Update the linear constraints matrix (A) - * \note - * If the sparsity pattern is preserved the matrix is simply update - * otherwise the entire solver will be reinitialized. In this case - * the primal and dual variable are copied in the new workspace. - * - * @param linearConstraintsMatrix is the linear constraint matrix A - * @return true/false in case of success/failure. - */ - template - bool updateLinearConstraintsMatrix(const Eigen::SparseCompressedBase &linearConstraintsMatrix); - - /** - * Set the entire - * @param linearConstraintsMatrix is the linear constraint matrix A - * @return true/false in case of success/failure. - */ - template - bool setWarmStart(const Eigen::Matrix &primalVariable, - const Eigen::Matrix &dualVariable); - - template - bool setPrimalVariable(const Eigen::Matrix &primalVariable); - - template - bool setDualVariable(const Eigen::Matrix &dualVariable); - - template - bool getPrimalVariable(Eigen::Matrix &primalVariable); - - template - bool getDualVariable(Eigen::Matrix &dualVariable); - - /** - * Get the solver settings pointer. - * @return the pointer to Settings object. - */ - const std::unique_ptr& settings() const; - - /** - * Get the pointer to the solver initial data. - * @return the pointer to Data object. - */ - const std::unique_ptr& data() const; + } + +public: + /** + * Constructor. + */ + Solver(); + + /** + * Initialize the solver with the actual initial data and settings. + * @return true/false in case of success/failure. + */ + bool initSolver(); + + /** + * Check if the solver is initialized. + * @return true if the solver is initialized. + */ + bool isInitialized(); + + /** + * Dealocate memory. + */ + void clearSolver(); + + /** + * Set to zero all the solver variables. + * @return true/false in case of success/failure. + */ + bool clearSolverVariables(); + + /** + * Solve the QP optimization problem. + * @return true/false in case of success/failure. + */ + [[deprecated("Use solveProblem() instead.")]] bool solve(); + + /** + * Solve the QP optimization problem. + * @return the error exit flag + */ + OsqpEigen::ErrorExitFlag solveProblem(); + + /** + * Get the status of the solver + * @return The inner solver status + */ + OsqpEigen::Status getStatus() const; + + /** + * Get the primal objective value + * @return The primal objective value + */ + const c_float getObjValue() const; + + /** + * Get the optimization problem solution. + * @return an Eigen::Vector contating the optimization result. + */ + const Eigen::Matrix& getSolution(); + + /** + * Get the dual optimization problem solution. + * @return an Eigen::Vector contating the optimization result. + */ + const Eigen::Matrix& getDualSolution(); + + /** + * Update the linear part of the cost function (Gradient). + * @param gradient is the Gradient vector. + * @note the elements of the gradient are not copied inside the library. + * The user has to guarantee that the lifetime of the objects passed is the same of the + * OsqpEigen object. + * @return true/false in case of success/failure. + */ + bool + updateGradient(const Eigen::Ref>& gradient); + + /** + * Update the lower bounds limit (size m). + * @param lowerBound is the lower bound constraint vector. + * @note the elements of the lowerBound are not copied inside the library. + * The user has to guarantee that the lifetime of the object passed is the same of the + * OsqpEigen object. + * @return true/false in case of success/failure. + */ + bool + updateLowerBound(const Eigen::Ref>& lowerBound); + + /** + * Update the upper bounds limit (size m). + * @param upperBound is the upper bound constraint vector. + * @note the elements of the upperBound are not copied inside the library. + * The user has to guarantee that the lifetime of the object passed is the same of the + * OsqpEigen object. + * @return true/false in case of success/failure. + */ + bool + updateUpperBound(const Eigen::Ref>& upperBound); + + /** + * Update both upper and lower bounds (size m). + * @param lowerBound is the lower bound constraint vector; + * @param upperBound is the upper bound constraint vector. + * @note the elements of the lowerBound and upperBound are not copied inside the library. + * The user has to guarantee that the lifetime of the objects passed is the same of the + * OsqpEigen object + * @return true/false in case of success/failure. + */ + bool + updateBounds(const Eigen::Ref>& lowerBound, + const Eigen::Ref>& upperBound); + + /** + * Update the quadratic part of the cost function (Hessian). + * It is assumed to be a simmetric matrix. + * \note + * If the sparsity pattern is preserved the matrix is simply update + * otherwise the entire solver will be reinitialized. In this case + * the primal and dual variable are copied in the new workspace. + * + * @param hessian is the Hessian matrix. + * @return true/false in case of success/failure. + */ + template + bool updateHessianMatrix(const Eigen::SparseCompressedBase& hessianMatrix); + + /** + * Update the linear constraints matrix (A) + * \note + * If the sparsity pattern is preserved the matrix is simply update + * otherwise the entire solver will be reinitialized. In this case + * the primal and dual variable are copied in the new workspace. + * + * @param linearConstraintsMatrix is the linear constraint matrix A + * @return true/false in case of success/failure. + */ + template + bool updateLinearConstraintsMatrix( + const Eigen::SparseCompressedBase& linearConstraintsMatrix); + + /** + * Set the entire + * @param linearConstraintsMatrix is the linear constraint matrix A + * @return true/false in case of success/failure. + */ + template + bool setWarmStart(const Eigen::Matrix& primalVariable, + const Eigen::Matrix& dualVariable); + + template + bool setPrimalVariable(const Eigen::Matrix& primalVariable); + + template bool setDualVariable(const Eigen::Matrix& dualVariable); + + template bool getPrimalVariable(Eigen::Matrix& primalVariable); + + template bool getDualVariable(Eigen::Matrix& dualVariable); + + /** + * Get the solver settings pointer. + * @return the pointer to Settings object. + */ + const std::unique_ptr& settings() const; + + /** + * Get the pointer to the solver initial data. + * @return the pointer to Data object. + */ + const std::unique_ptr& data() const; #ifdef OSQP_EIGEN_OSQP_IS_V1 - /** - * Get the pointer to the OSQP solver. - * @return the pointer to Solver object. - */ - const std::unique_ptr>& solver() const; + /** + * Get the pointer to the OSQP solver. + * @return the pointer to Solver object. + */ + const std::unique_ptr>& solver() const; #else - /** - * Get the pointer to the OSQP workspace. - * @return the pointer to Workspace object. - */ - const std::unique_ptr>& workspace() const; + /** + * Get the pointer to the OSQP workspace. + * @return the pointer to Workspace object. + */ + const std::unique_ptr>& workspace() const; #endif - }; +}; - #include -} +#include +} // namespace OsqpEigen #endif diff --git a/include/OsqpEigen/Solver.tpp b/include/OsqpEigen/Solver.tpp index 39d8e59..d17ec1c 100644 --- a/include/OsqpEigen/Solver.tpp +++ b/include/OsqpEigen/Solver.tpp @@ -6,41 +6,49 @@ */ #include -# ifndef OSQP_EIGEN_OSQP_IS_V1 +#ifndef OSQP_EIGEN_OSQP_IS_V1 #include #include -# endif +#endif #include "Debug.hpp" -template -bool OsqpEigen::Solver::updateHessianMatrix(const Eigen::SparseCompressedBase &hessianMatrix) +template +bool OsqpEigen::Solver::updateHessianMatrix( + const Eigen::SparseCompressedBase& hessianMatrix) { - if(!m_isSolverInitialized){ - debugStream() << "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been initialized." - << std::endl; + if (!m_isSolverInitialized) + { + debugStream() << "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been " + "initialized." + << std::endl; return false; } - if(((c_int)hessianMatrix.rows() != getData()->n)|| - ((c_int)hessianMatrix.cols() != getData()->n)){ - debugStream() << "[OsqpEigen::Solver::updateHessianMatrix] The hessian matrix has to be a nxn matrix" - << std::endl; + if (((c_int)hessianMatrix.rows() != getData()->n) + || ((c_int)hessianMatrix.cols() != getData()->n)) + { + debugStream() << "[OsqpEigen::Solver::updateHessianMatrix] The hessian matrix has to be a " + "nxn matrix" + << std::endl; return false; } - // evaluate the triplets from old and new hessian sparse matrices - if(!OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToTriplets(getData()->P, - m_oldHessianTriplet)){ - debugStream() << "[OsqpEigen::Solver::updateHessianMatrix] Unable to evaluate triplets from the old hessian matrix." - << std::endl; + if (!OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToTriplets(getData()->P, + m_oldHessianTriplet)) + { + debugStream() << "[OsqpEigen::Solver::updateHessianMatrix] Unable to evaluate triplets " + "from the old hessian matrix." + << std::endl; return false; } - if(!OsqpEigen::SparseMatrixHelper::eigenSparseMatrixToTriplets(hessianMatrix, - m_newHessianTriplet)){ - debugStream() << "[OsqpEigen::Solver::updateHessianMatrix] Unable to evaluate triplets from the old hessian matrix." - << std::endl; + if (!OsqpEigen::SparseMatrixHelper::eigenSparseMatrixToTriplets(hessianMatrix, + m_newHessianTriplet)) + { + debugStream() << "[OsqpEigen::Solver::updateHessianMatrix] Unable to evaluate triplets " + "from the old hessian matrix." + << std::endl; return false; } @@ -50,35 +58,57 @@ bool OsqpEigen::Solver::updateHessianMatrix(const Eigen::SparseCompressedBase 0) { + if (evaluateNewValues(m_oldHessianTriplet, + m_newUpperTriangularHessianTriplets, + m_hessianNewIndices, + m_hessianNewValues)) + { + if (m_hessianNewValues.size() > 0) + { #ifdef OSQP_EIGEN_OSQP_IS_V1 - if(osqp_update_data_mat(m_solver.get(), m_hessianNewValues.data(), m_hessianNewIndices.data(), m_hessianNewIndices.size(), nullptr, nullptr, 0) != 0){ + if (osqp_update_data_mat(m_solver.get(), + m_hessianNewValues.data(), + m_hessianNewIndices.data(), + m_hessianNewIndices.size(), + nullptr, + nullptr, + 0) + != 0) + { #else - if(osqp_update_P(m_workspace.get(), m_hessianNewValues.data(), m_hessianNewIndices.data(), m_hessianNewIndices.size()) != 0){ + if (osqp_update_P(m_workspace.get(), + m_hessianNewValues.data(), + m_hessianNewIndices.data(), + m_hessianNewIndices.size()) + != 0) + { #endif - debugStream() << "[OsqpEigen::Solver::updateHessianMatrix] Unable to update hessian matrix." - << std::endl; + debugStream() << "[OsqpEigen::Solver::updateHessianMatrix] Unable to update " + "hessian matrix." + << std::endl; return false; } } - } - else{ + } else + { // the sparsity pattern has changed // the solver has to be setup again // get the primal and the dual variables - if(!getPrimalVariable(m_primalVariables)){ - debugStream() << "[OsqpEigen::Solver::updateHessianMatrix] Unable to get the primal variable." - << std::endl; + if (!getPrimalVariable(m_primalVariables)) + { + debugStream() << "[OsqpEigen::Solver::updateHessianMatrix] Unable to get the primal " + "variable." + << std::endl; return false; } - if(!getDualVariable(m_dualVariables)){ - debugStream() << "[OsqpEigen::Solver::updateHessianMatrix] Unable to get the dual variable." - << std::endl; + if (!getDualVariable(m_dualVariables)) + { + debugStream() << "[OsqpEigen::Solver::updateHessianMatrix] Unable to get the dual " + "variable." + << std::endl; return false; } @@ -86,10 +116,11 @@ bool OsqpEigen::Solver::updateHessianMatrix(const Eigen::SparseCompressedBaseclearHessianMatrix(); // set new hessian matrix - if(!m_data->setHessianMatrix(hessianMatrix)){ - debugStream() << "[OsqpEigen::Solver::updateHessianMatrix] Unable to update the hessian matrix in " - << "OptimizaroData object." - << std::endl; + if (!m_data->setHessianMatrix(hessianMatrix)) + { + debugStream() << "[OsqpEigen::Solver::updateHessianMatrix] Unable to update the " + "hessian matrix in " + << "OptimizaroData object." << std::endl; return false; } @@ -97,56 +128,71 @@ bool OsqpEigen::Solver::updateHessianMatrix(const Eigen::SparseCompressedBase -bool OsqpEigen::Solver::updateLinearConstraintsMatrix(const Eigen::SparseCompressedBase &linearConstraintsMatrix) +template +bool OsqpEigen::Solver::updateLinearConstraintsMatrix( + const Eigen::SparseCompressedBase& linearConstraintsMatrix) { - if(!m_isSolverInitialized){ - debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] The solver has not been initialized." - << std::endl; + if (!m_isSolverInitialized) + { + debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] The solver has not " + "been initialized." + << std::endl; return false; } - if(((c_int)linearConstraintsMatrix.rows() != getData()->m)|| - ((c_int)linearConstraintsMatrix.cols() != getData()->n)){ - debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] The constraints matrix has to be a mxn matrix" - << std::endl; + if (((c_int)linearConstraintsMatrix.rows() != getData()->m) + || ((c_int)linearConstraintsMatrix.cols() != getData()->n)) + { + debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] The constraints " + "matrix has to be a mxn matrix" + << std::endl; return false; } // evaluate the triplets from old and new hessian sparse matrices - if(!OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToTriplets(getData()->A, - m_oldLinearConstraintsTriplet)){ - debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] Unable to evaluate triplets from the old hessian matrix." - << std::endl; + if (!OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToTriplets(getData()->A, + m_oldLinearConstraintsTriplet)) + { + debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] Unable to evaluate " + "triplets from the old hessian matrix." + << std::endl; return false; } - if(!OsqpEigen::SparseMatrixHelper::eigenSparseMatrixToTriplets(linearConstraintsMatrix, - m_newLinearConstraintsTriplet)){ - debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] Unable to evaluate triplets from the old hessian matrix." - << std::endl; + if (!OsqpEigen::SparseMatrixHelper::eigenSparseMatrixToTriplets(linearConstraintsMatrix, + m_newLinearConstraintsTriplet)) + { + debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] Unable to evaluate " + "triplets from the old hessian matrix." + << std::endl; return false; } @@ -154,35 +200,57 @@ bool OsqpEigen::Solver::updateLinearConstraintsMatrix(const Eigen::SparseCompres // according to the osqp library it can be done only if the sparsity pattern of the // matrix does not change. - if(evaluateNewValues(m_oldLinearConstraintsTriplet, m_newLinearConstraintsTriplet, - m_constraintsNewIndices, m_constraintsNewValues)){ - if (m_constraintsNewValues.size() > 0) { + if (evaluateNewValues(m_oldLinearConstraintsTriplet, + m_newLinearConstraintsTriplet, + m_constraintsNewIndices, + m_constraintsNewValues)) + { + if (m_constraintsNewValues.size() > 0) + { #ifdef OSQP_EIGEN_OSQP_IS_V1 - if(osqp_update_data_mat(m_solver.get(), nullptr, nullptr, 0, m_constraintsNewValues.data(), m_constraintsNewIndices.data(), m_constraintsNewIndices.size()) != 0){ + if (osqp_update_data_mat(m_solver.get(), + nullptr, + nullptr, + 0, + m_constraintsNewValues.data(), + m_constraintsNewIndices.data(), + m_constraintsNewIndices.size()) + != 0) + { #else - if(osqp_update_A(m_workspace.get(), m_constraintsNewValues.data(), m_constraintsNewIndices.data(), m_constraintsNewIndices.size()) != 0){ + if (osqp_update_A(m_workspace.get(), + m_constraintsNewValues.data(), + m_constraintsNewIndices.data(), + m_constraintsNewIndices.size()) + != 0) + { #endif - debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] Unable to update linear constraints matrix." - << std::endl; + debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] Unable to " + "update linear constraints matrix." + << std::endl; return false; } } - } - else{ + } else + { // the sparsity pattern has changed // the solver has to be setup again // get the primal and the dual variables - if(!getPrimalVariable(m_primalVariables)){ - debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] Unable to get the primal variable." - << std::endl; + if (!getPrimalVariable(m_primalVariables)) + { + debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] Unable to get the " + "primal variable." + << std::endl; return false; } - if(!getDualVariable(m_dualVariables)){ - debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] Unable to get the dual variable." - << std::endl; + if (!getDualVariable(m_dualVariables)) + { + debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] Unable to get the " + "dual variable." + << std::endl; return false; } @@ -190,90 +258,102 @@ bool OsqpEigen::Solver::updateLinearConstraintsMatrix(const Eigen::SparseCompres m_data->clearLinearConstraintsMatrix(); // set new linear constraints matrix - if(!m_data->setLinearConstraintsMatrix(linearConstraintsMatrix)){ - debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] Unable to update the hessian matrix in " - << "Data object." - << std::endl; + if (!m_data->setLinearConstraintsMatrix(linearConstraintsMatrix)) + { + debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] Unable to update " + "the hessian matrix in " + << "Data object." << std::endl; return false; } // clear the old solver clearSolver(); - if(!initSolver()){ - debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] Unable to Initialize the solver." - << std::endl; + if (!initSolver()) + { + debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] Unable to " + "Initialize the solver." + << std::endl; return false; } // set the old primal and dual variables - if(!setPrimalVariable(m_primalVariables)){ - debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] Unable to set the primal variable." - << std::endl; + if (!setPrimalVariable(m_primalVariables)) + { + debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] Unable to set the " + "primal variable." + << std::endl; return false; } - if(!setDualVariable(m_dualVariables)){ - debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] Unable to set the dual variable." - << std::endl; + if (!setDualVariable(m_dualVariables)) + { + debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] Unable to set the " + "dual variable." + << std::endl; return false; } } return true; } -template -bool OsqpEigen::Solver::setWarmStart(const Eigen::Matrix &primalVariable, - const Eigen::Matrix &dualVariable) +template +bool OsqpEigen::Solver::setWarmStart(const Eigen::Matrix& primalVariable, + const Eigen::Matrix& dualVariable) { - if(!m_isSolverInitialized){ + if (!m_isSolverInitialized) + { debugStream() << "[OsqpEigen::Solver::setWarmStart] The solver is not initialized" << std::endl; return false; } - if(primalVariable.rows() != getData()->n){ - debugStream() << "[OsqpEigen::Solver::setWarmStart] The size of the primal variable vector has to be equal to " - << " the number of variables." - << std::endl; + if (primalVariable.rows() != getData()->n) + { + debugStream() << "[OsqpEigen::Solver::setWarmStart] The size of the primal variable vector " + "has to be equal to " + << " the number of variables." << std::endl; return false; } - if(dualVariable.rows() != getData()->m){ - debugStream() << "[OsqpEigen::Solver::setWarmStart] The size of the dual variable vector has to be equal to " - << " the number of constraints." - << std::endl; + if (dualVariable.rows() != getData()->m) + { + debugStream() << "[OsqpEigen::Solver::setWarmStart] The size of the dual variable vector " + "has to be equal to " + << " the number of constraints." << std::endl; return false; } - m_primalVariables = primalVariable.template cast (); - m_dualVariables = dualVariable.template cast (); + m_primalVariables = primalVariable.template cast(); + m_dualVariables = dualVariable.template cast(); #ifdef OSQP_EIGEN_OSQP_IS_V1 return (osqp_warm_start(m_solver.get(), m_primalVariables.data(), m_dualVariables.data()) == 0); #else - return (osqp_warm_start(m_workspace.get(), m_primalVariables.data(), m_dualVariables.data()) == 0); + return (osqp_warm_start(m_workspace.get(), m_primalVariables.data(), m_dualVariables.data()) + == 0); #endif - } -template -bool OsqpEigen::Solver::setPrimalVariable(const Eigen::Matrix &primalVariable) +template +bool OsqpEigen::Solver::setPrimalVariable(const Eigen::Matrix& primalVariable) { - if(!m_isSolverInitialized){ + if (!m_isSolverInitialized) + { debugStream() << "[OsqpEigen::Solver::setPrimalVariable] The solver is not initialized" << std::endl; return false; } - if(primalVariable.rows() != getData()->n){ - debugStream() << "[OsqpEigen::Solver::setPrimalVariable] The size of the primal variable vector has to be equal to " - << " the number of variables." - << std::endl; + if (primalVariable.rows() != getData()->n) + { + debugStream() << "[OsqpEigen::Solver::setPrimalVariable] The size of the primal variable " + "vector has to be equal to " + << " the number of variables." << std::endl; return false; } - m_primalVariables = primalVariable.template cast (); + m_primalVariables = primalVariable.template cast(); #ifdef OSQP_EIGEN_OSQP_IS_V1 return (osqp_warm_start(m_solver.get(), m_primalVariables.data(), nullptr) == 0); @@ -282,18 +362,18 @@ bool OsqpEigen::Solver::setPrimalVariable(const Eigen::Matrix &primalVa #endif } - -template -bool OsqpEigen::Solver::setDualVariable(const Eigen::Matrix &dualVariable) +template +bool OsqpEigen::Solver::setDualVariable(const Eigen::Matrix& dualVariable) { - if(dualVariable.rows() != getData()->m){ - debugStream() << "[OsqpEigen::Solver::setDualVariable] The size of the dual variable vector has to be equal to " - << " the number of constraints." - << std::endl; + if (dualVariable.rows() != getData()->m) + { + debugStream() << "[OsqpEigen::Solver::setDualVariable] The size of the dual variable " + "vector has to be equal to " + << " the number of constraints." << std::endl; return false; } - m_dualVariables = dualVariable.template cast (); + m_dualVariables = dualVariable.template cast(); #ifdef OSQP_EIGEN_OSQP_IS_V1 return (osqp_warm_start(m_solver.get(), nullptr, m_dualVariables.data()) == 0); @@ -302,90 +382,108 @@ bool OsqpEigen::Solver::setDualVariable(const Eigen::Matrix &dualVariab #endif } -template -bool OsqpEigen::Solver::getPrimalVariable(Eigen::Matrix &primalVariable) +template +bool OsqpEigen::Solver::getPrimalVariable(Eigen::Matrix& primalVariable) { - if(!m_isSolverInitialized){ + if (!m_isSolverInitialized) + { debugStream() << "[OsqpEigen::Solver::getPrimalVariable] The solver is not initialized" << std::endl; return false; } - if(n == Eigen::Dynamic){ + if (n == Eigen::Dynamic) + { primalVariable.resize(getData()->n, 1); - } - else{ - if (n != getData()->n){ - debugStream() << "[OsqpEigen::Solver::getPrimalVariable] The size of the vector has to be equal to the number of " - << "variables. (You can use an eigen dynamic vector)" - << std::endl; + } else + { + if (n != getData()->n) + { + debugStream() << "[OsqpEigen::Solver::getPrimalVariable] The size of the vector has to " + "be equal to the number of variables. (You can use an eigen dynamic " + "vector)" + << std::endl; return false; } } #ifdef OSQP_EIGEN_OSQP_IS_V1 - primalVariable = Eigen::Map>(m_solver->solution->x, getData()->n).template cast (); + primalVariable = Eigen::Map>(m_solver->solution->x, getData()->n) + .template cast(); #else - primalVariable = Eigen::Map>(m_workspace->x, getData()->n).template cast (); + primalVariable + = Eigen::Map>(m_workspace->x, getData()->n).template cast(); #endif return true; } -template -bool OsqpEigen::Solver::getDualVariable(Eigen::Matrix &dualVariable) +template +bool OsqpEigen::Solver::getDualVariable(Eigen::Matrix& dualVariable) { - if(!m_isSolverInitialized){ + if (!m_isSolverInitialized) + { debugStream() << "[OsqpEigen::Solver::getDualVariable] The solver is not initialized" << std::endl; return false; } - - if(m == Eigen::Dynamic){ + if (m == Eigen::Dynamic) + { dualVariable.resize(getData()->m, 1); - } - else{ - if (m != getData()->m){ - debugStream() << "[OsqpEigen::Solver::getDualVariable] The size of the vector has to be equal to the number of " - << "constraints. (You can use an eigen dynamic vector)" - << std::endl; + } else + { + if (m != getData()->m) + { + debugStream() << "[OsqpEigen::Solver::getDualVariable] The size of the vector has to " + "be equal to the number of constraints. (You can use an eigen dynamic " + "vector)" + << std::endl; return false; } } #ifdef OSQP_EIGEN_OSQP_IS_V1 - dualVariable = Eigen::Map>(m_solver->solution->y, getData()->m).template cast (); + dualVariable = Eigen::Map>(m_solver->solution->y, getData()->m) + .template cast(); #else - dualVariable = Eigen::Map>(m_workspace->y, getData()->m).template cast (); + dualVariable + = Eigen::Map>(m_workspace->y, getData()->m).template cast(); #endif return true; } -template -bool OsqpEigen::Solver::evaluateNewValues(const std::vector> &oldMatrixTriplet, - const std::vector> &newMatrixTriplet, - std::vector &newIndices, - std::vector &newValues) const +template +bool OsqpEigen::Solver::evaluateNewValues(const std::vector>& oldMatrixTriplet, + const std::vector>& newMatrixTriplet, + std::vector& newIndices, + std::vector& newValues) const { - //When updating the matrices for osqp, we need to provide the indeces to modify of the value vector. The following can work since, when extracting triplets from osqp sparse matrices, the order of the triplets follows the same order of the value vector. - // check if the sparsity pattern is changed + // When updating the matrices for osqp, we need to provide the indeces to modify of the value + // vector. The following can work since, when extracting triplets from osqp sparse matrices, the + // order of the triplets follows the same order of the value vector. + // check if the sparsity pattern is changed size_t valuesAdded = 0; - if(newMatrixTriplet.size() == oldMatrixTriplet.size()){ - for(int i = 0; i < newMatrixTriplet.size(); i++){ + if (newMatrixTriplet.size() == oldMatrixTriplet.size()) + { + for (int i = 0; i < newMatrixTriplet.size(); i++) + { // check if the sparsity pattern is changed - if((newMatrixTriplet[i].row() != oldMatrixTriplet[i].row()) || - (newMatrixTriplet[i].col() != oldMatrixTriplet[i].col())) + if ((newMatrixTriplet[i].row() != oldMatrixTriplet[i].row()) + || (newMatrixTriplet[i].col() != oldMatrixTriplet[i].col())) return false; // check if an old value is changed - if(newMatrixTriplet[i].value() != oldMatrixTriplet[i].value()){ - if (valuesAdded >= newValues.size()) { - newValues.push_back((c_float) newMatrixTriplet[i].value()); - newIndices.push_back((c_int) i); + if (newMatrixTriplet[i].value() != oldMatrixTriplet[i].value()) + { + if (valuesAdded >= newValues.size()) + { + newValues.push_back((c_float)newMatrixTriplet[i].value()); + newIndices.push_back((c_int)i); valuesAdded++; - } else { + } else + { newValues[valuesAdded] = static_cast(newMatrixTriplet[i].value()); newIndices[valuesAdded] = static_cast(i); valuesAdded++; @@ -399,21 +497,29 @@ bool OsqpEigen::Solver::evaluateNewValues(const std::vector> & return false; } -template -void OsqpEigen::Solver::selectUpperTriangularTriplets(const std::vector> &fullMatrixTriplets, - std::vector> &upperTriangularMatrixTriplets) const { +template +void OsqpEigen::Solver::selectUpperTriangularTriplets( + const std::vector>& fullMatrixTriplets, + std::vector>& upperTriangularMatrixTriplets) const +{ int upperTriangularTriplets = 0; - for (int i = 0; i < fullMatrixTriplets.size(); ++i) { - if (fullMatrixTriplets[i].row() <= fullMatrixTriplets[i].col()) { - if (upperTriangularTriplets < upperTriangularMatrixTriplets.size()) { + for (int i = 0; i < fullMatrixTriplets.size(); ++i) + { + if (fullMatrixTriplets[i].row() <= fullMatrixTriplets[i].col()) + { + if (upperTriangularTriplets < upperTriangularMatrixTriplets.size()) + { upperTriangularMatrixTriplets[upperTriangularTriplets] = fullMatrixTriplets[i]; - } else { + } else + { upperTriangularMatrixTriplets.push_back(fullMatrixTriplets[i]); } upperTriangularTriplets++; } } - upperTriangularMatrixTriplets.erase(upperTriangularMatrixTriplets.begin() + upperTriangularTriplets, upperTriangularMatrixTriplets.end()); + upperTriangularMatrixTriplets.erase(upperTriangularMatrixTriplets.begin() + + upperTriangularTriplets, + upperTriangularMatrixTriplets.end()); } diff --git a/include/OsqpEigen/SparseMatrixHelper.hpp b/include/OsqpEigen/SparseMatrixHelper.hpp index 4104e82..20f403a 100644 --- a/include/OsqpEigen/SparseMatrixHelper.hpp +++ b/include/OsqpEigen/SparseMatrixHelper.hpp @@ -21,56 +21,56 @@ */ namespace OsqpEigen { - /** - * SparseMatrixHelper namespace is a namspace that contains helper function to handle osqp matrix. - * Use it to create ot update or manage an osqp sparse matrix. - * osqp sparse matrix in [compressed-column](https://people.sc.fsu.edu/~jburkardt/data/cc/cc.html) - * or triplet form. - */ - namespace SparseMatrixHelper - { +/** + * SparseMatrixHelper namespace is a namspace that contains helper function to handle osqp matrix. + * Use it to create ot update or manage an osqp sparse matrix. + * osqp sparse matrix in [compressed-column](https://people.sc.fsu.edu/~jburkardt/data/cc/cc.html) + * or triplet form. + */ +namespace SparseMatrixHelper +{ - /** - * Allocate an osqpSparseMatrix struct. - * NOTE: c_malloc function is used to allocate memory please call - * c_free to deallcate memory. - * @return a const point to the csc struct. - */ - template - bool createOsqpSparseMatrix(const Eigen::SparseCompressedBase& eigenSparseMatrix, - csc*& osqpSparseMatrix); +/** + * Allocate an osqpSparseMatrix struct. + * NOTE: c_malloc function is used to allocate memory please call + * c_free to deallcate memory. + * @return a const point to the csc struct. + */ +template +bool createOsqpSparseMatrix(const Eigen::SparseCompressedBase& eigenSparseMatrix, + csc*& osqpSparseMatrix); - /** - * Convert an osqp sparse matrix into an eigen sparse matrix. - * @param osqpSparseMatrix is a constant pointer to a constant csc struct; - * @param eigenSparseMatrix is the eigen sparse matrix object. - * @return a const point to the csc struct. - */ - template - bool osqpSparseMatrixToEigenSparseMatrix(const csc* const & osqpSparseMatrix, - Eigen::SparseMatrix &eigenSparseMatrix); +/** + * Convert an osqp sparse matrix into an eigen sparse matrix. + * @param osqpSparseMatrix is a constant pointer to a constant csc struct; + * @param eigenSparseMatrix is the eigen sparse matrix object. + * @return a const point to the csc struct. + */ +template +bool osqpSparseMatrixToEigenSparseMatrix(const csc* const& osqpSparseMatrix, + Eigen::SparseMatrix& eigenSparseMatrix); - /** - * Convert an osqp sparse matrix into a eigen triplet list. - * @param osqpSparseMatrix is reference to a constant pointer to a constant csc struct; - * @param tripletList is a std::vector containing the triplet. - * @return a const point to the csc struct. - */ - template - bool osqpSparseMatrixToTriplets(const csc* const & osqpSparseMatrix, - std::vector> &tripletList); +/** + * Convert an osqp sparse matrix into a eigen triplet list. + * @param osqpSparseMatrix is reference to a constant pointer to a constant csc struct; + * @param tripletList is a std::vector containing the triplet. + * @return a const point to the csc struct. + */ +template +bool osqpSparseMatrixToTriplets(const csc* const& osqpSparseMatrix, + std::vector>& tripletList); - /** - * Convert an eigen sparse matrix into a eigen triplet list. - * @param eigenSparseMatrix is the eigen sparse matrix object; - * @param tripletList is a std::vector containing the triplet. - * @return a const point to the csc struct. - */ - template - bool eigenSparseMatrixToTriplets(const Eigen::SparseCompressedBase &eigenSparseMatrix, - std::vector> &tripletList); - }; -} +/** + * Convert an eigen sparse matrix into a eigen triplet list. + * @param eigenSparseMatrix is the eigen sparse matrix object; + * @param tripletList is a std::vector containing the triplet. + * @return a const point to the csc struct. + */ +template +bool eigenSparseMatrixToTriplets(const Eigen::SparseCompressedBase& eigenSparseMatrix, + std::vector>& tripletList); +}; // namespace SparseMatrixHelper +} // namespace OsqpEigen #include diff --git a/include/OsqpEigen/SparseMatrixHelper.tpp b/include/OsqpEigen/SparseMatrixHelper.tpp index d5a18af..2d89eb5 100644 --- a/include/OsqpEigen/SparseMatrixHelper.tpp +++ b/include/OsqpEigen/SparseMatrixHelper.tpp @@ -5,16 +5,21 @@ * @date 2018 */ -#include #include +#include -template -bool OsqpEigen::SparseMatrixHelper::createOsqpSparseMatrix(const Eigen::SparseCompressedBase &eigenSparseMatrix, - csc*& osqpSparseMatrix) +template +bool OsqpEigen::SparseMatrixHelper::createOsqpSparseMatrix( + const Eigen::SparseCompressedBase& eigenSparseMatrix, csc*& osqpSparseMatrix) { - Eigen::SparseMatrix colMajorCopy; //Copying into a new sparse matrix to be sure to use a CSC matrix - colMajorCopy = eigenSparseMatrix; //This may perform merory allocation, but this is already the case for allocating the osqpSparseMatrix + // Copying into a new sparse matrix to be sure to use a CSC matrix + Eigen::SparseMatrix colMajorCopy; + + // This may perform memory allocation, but this is already the case for allocating the + // osqpSparseMatrix + colMajorCopy = eigenSparseMatrix; + // get number of row, columns and nonZeros from Eigen SparseMatrix c_int rows = colMajorCopy.rows(); c_int cols = colMajorCopy.cols(); @@ -26,26 +31,37 @@ bool OsqpEigen::SparseMatrixHelper::createOsqpSparseMatrix(const Eigen::SparseCo // instantiate csc matrix // MEMORY ALLOCATION!! - if(osqpSparseMatrix != nullptr){ - debugStream() << "[OsqpEigen::SparseMatrixHelper::createOsqpSparseMatrix] osqpSparseMatrix pointer is not a null pointer! " - << std::endl; + if (osqpSparseMatrix != nullptr) + { + debugStream() << "[OsqpEigen::SparseMatrixHelper::createOsqpSparseMatrix] osqpSparseMatrix " + "pointer is not a null pointer! " + << std::endl; return false; } osqpSparseMatrix = csc_spalloc(rows, cols, numberOfNonZeroCoeff, 1, 0); int innerOsqpPosition = 0; - for(int k = 0; k < cols; k++) { - if (colMajorCopy.isCompressed()) { + for (int k = 0; k < cols; k++) + { + if (colMajorCopy.isCompressed()) + { osqpSparseMatrix->p[k] = static_cast(outerIndexPtr[k]); - } else { - if (k == 0) { + } else + { + if (k == 0) + { osqpSparseMatrix->p[k] = 0; - } else { - osqpSparseMatrix->p[k] = osqpSparseMatrix->p[k-1] + innerNonZerosPtr[k-1]; + } else + { + osqpSparseMatrix->p[k] = osqpSparseMatrix->p[k - 1] + innerNonZerosPtr[k - 1]; } } - for (typename Eigen::SparseMatrix::InnerIterator it(colMajorCopy,k); it; ++it) { + for (typename Eigen::SparseMatrix::InnerIterator it(colMajorCopy, k); + it; + ++it) + { osqpSparseMatrix->i[innerOsqpPosition] = static_cast(it.row()); osqpSparseMatrix->x[innerOsqpPosition] = static_cast(it.value()); innerOsqpPosition++; @@ -58,14 +74,16 @@ bool OsqpEigen::SparseMatrixHelper::createOsqpSparseMatrix(const Eigen::SparseCo return true; } -template -bool OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToTriplets(const csc* const & osqpSparseMatrix, - std::vector> &tripletList) +template +bool OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToTriplets( + const csc* const& osqpSparseMatrix, std::vector>& tripletList) { // if the matrix is not instantiate the triplets vector is empty - if(osqpSparseMatrix == nullptr){ - debugStream() << "[OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToTriplets] the osqpSparseMatrix is not initialized." - << std::endl; + if (osqpSparseMatrix == nullptr) + { + debugStream() << "[OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToTriplets] the " + "osqpSparseMatrix is not initialized." + << std::endl; return false; } @@ -75,19 +93,20 @@ bool OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToTriplets(const csc* const // get values data c_float* valuePtr = osqpSparseMatrix->x; - c_int numberOfNonZeroCoeff = osqpSparseMatrix->p[osqpSparseMatrix->n]; + c_int numberOfNonZeroCoeff = osqpSparseMatrix->p[osqpSparseMatrix->n]; // populate the tripletes vector - int column=0; + int column = 0; int row; c_float value; tripletList.resize(numberOfNonZeroCoeff); - for(int i = 0; i= outerIndexPtr[column+1]) + while (i >= outerIndexPtr[column + 1]) column++; tripletList[i] = Eigen::Triplet(row, column, static_cast(value)); @@ -98,14 +117,16 @@ bool OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToTriplets(const csc* const return true; } -template -bool OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToEigenSparseMatrix(const csc* const & osqpSparseMatrix, - Eigen::SparseMatrix &eigenSparseMatrix) +template +bool OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToEigenSparseMatrix( + const csc* const& osqpSparseMatrix, Eigen::SparseMatrix& eigenSparseMatrix) { // if the matrix is not instantiate the eigen matrix is empty - if(osqpSparseMatrix == nullptr) { - debugStream() << "[OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToEigenSparseMatrix] the osqpSparseMatrix is not initialized." - << std::endl; + if (osqpSparseMatrix == nullptr) + { + debugStream() << "[OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToEigenSparseMatrix] the " + "osqpSparseMatrix is not initialized." + << std::endl; return false; } @@ -126,22 +147,30 @@ bool OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToEigenSparseMatrix(const cs return true; } -template -bool OsqpEigen::SparseMatrixHelper::eigenSparseMatrixToTriplets(const Eigen::SparseCompressedBase &eigenSparseMatrix, - std::vector> &tripletList) +template +bool OsqpEigen::SparseMatrixHelper::eigenSparseMatrixToTriplets( + const Eigen::SparseCompressedBase& eigenSparseMatrix, + std::vector>& tripletList) { - if(eigenSparseMatrix.nonZeros() == 0){ - debugStream() << "[OsqpEigen::SparseMatrixHelper::eigenSparseMatrixToTriplets] The eigenSparseMatrix is empty." - << std::endl; + if (eigenSparseMatrix.nonZeros() == 0) + { + debugStream() << "[OsqpEigen::SparseMatrixHelper::eigenSparseMatrixToTriplets] The " + "eigenSparseMatrix is empty." + << std::endl; return false; } tripletList.resize(eigenSparseMatrix.nonZeros()); // populate the triplet list int nonZero = 0; - for (int k=0; k < eigenSparseMatrix.outerSize(); ++k){ - for (typename Eigen::SparseCompressedBase::InnerIterator it(eigenSparseMatrix,k); it; ++it){ - tripletList[nonZero] = Eigen::Triplet(it.row(), it.col(), static_cast(it.value())); + for (int k = 0; k < eigenSparseMatrix.outerSize(); ++k) + { + for (typename Eigen::SparseCompressedBase::InnerIterator it(eigenSparseMatrix, k); + it; + ++it) + { + tripletList[nonZero] + = Eigen::Triplet(it.row(), it.col(), static_cast(it.value())); nonZero++; } } diff --git a/src/Data.cpp b/src/Data.cpp index ff044d9..d9952c9 100644 --- a/src/Data.cpp +++ b/src/Data.cpp @@ -45,7 +45,8 @@ OsqpEigen::Data::Data(int n, int m) void OsqpEigen::Data::clearHessianMatrix() { - if(m_isHessianMatrixSet){ + if (m_isHessianMatrixSet) + { m_isHessianMatrixSet = false; csc_spfree(m_data->P); m_data->P = nullptr; @@ -54,7 +55,8 @@ void OsqpEigen::Data::clearHessianMatrix() void OsqpEigen::Data::clearLinearConstraintsMatrix() { - if(m_isLinearConstraintsMatrixSet){ + if (m_isLinearConstraintsMatrixSet) + { m_isLinearConstraintsMatrixSet = false; csc_spfree(m_data->A); m_data->A = nullptr; @@ -80,7 +82,7 @@ void OsqpEigen::Data::setNumberOfConstraints(int m) m_data->m = m; } -auto OsqpEigen::Data::getData() const -> OSQPData*const & +auto OsqpEigen::Data::getData() const -> OSQPData *const & { return m_data; } @@ -88,22 +90,23 @@ auto OsqpEigen::Data::getData() const -> OSQPData*const & bool OsqpEigen::Data::isSet() const { const bool areConstraintsOk = (m_data->m == 0) || - (m_isLinearConstraintsMatrixSet && - m_isLowerBoundSet && - m_isUpperBoundSet); + (m_isLinearConstraintsMatrixSet && + m_isLowerBoundSet && + m_isUpperBoundSet); return m_isNumberOfVariablesSet && - m_isNumberOfConstraintsSet && - m_isHessianMatrixSet && - m_isGradientSet && - areConstraintsOk; + m_isNumberOfConstraintsSet && + m_isHessianMatrixSet && + m_isGradientSet && + areConstraintsOk; } bool OsqpEigen::Data::setGradient(Eigen::Ref> gradient) { - if(gradient.rows() != m_data->n){ + if (gradient.rows() != m_data->n) + { debugStream() << "[OsqpEigen::Data::setGradient] The size of the gradient must be equal to the number of the variables." - << std::endl; + << std::endl; return false; } m_isGradientSet = true; @@ -118,9 +121,10 @@ Eigen::Matrix OsqpEigen::Data::getGradient() bool OsqpEigen::Data::setLowerBound(Eigen::Ref> lowerBound) { - if(lowerBound.rows() != m_data->m){ + if (lowerBound.rows() != m_data->m) + { debugStream() << "[OsqpEigen::Data::setLowerBound] The size of the lower bound must be equal to the number of the variables." - << std::endl; + << std::endl; return false; } m_isLowerBoundSet = true; @@ -130,9 +134,10 @@ bool OsqpEigen::Data::setLowerBound(Eigen::Ref> upperBound) { - if(upperBound.rows() != m_data->m){ + if (upperBound.rows() != m_data->m) + { debugStream() << "[OsqpEigen::Data::setUpperBound] The size of the upper bound must be equal to the number of the variables." - << std::endl; + << std::endl; return false; } m_isUpperBoundSet = true; diff --git a/src/Debug.cpp b/src/Debug.cpp index 812ff10..cd6f702 100644 --- a/src/Debug.cpp +++ b/src/Debug.cpp @@ -1,27 +1,40 @@ - +/** + * @file Debug.cpp + * @copyright Released under the terms of the BSD 3-Clause License + * @date 2023 + */ #include -namespace OsqpEigen { - - // Taken from https://stackoverflow.com/a/8243866/2702753 - class NullStream : public std::ostream { - public: - NullStream() : std::ostream(nullptr) {} - NullStream(const NullStream &) : std::ostream(nullptr) {} - }; +namespace OsqpEigen +{ - template - const NullStream &operator<<(NullStream &&os, const T &value) { - return os; +// Taken from https://stackoverflow.com/a/8243866/2702753 +class NullStream : public std::ostream +{ +public: + NullStream() + : std::ostream(nullptr) + { + } + NullStream(const NullStream&) + : std::ostream(nullptr) + { } +}; - NullStream theStream; +template const NullStream& operator<<(NullStream&& os, const T& value) +{ + return os; +} + +NullStream theStream; - std::ostream &debugStream() { +std::ostream& debugStream() +{ #ifdef OSQP_EIGEN_DEBUG_OUTPUT - return std::cerr; + return std::cerr; #else - return theStream; + return theStream; #endif - } } +} // namespace OsqpEigen diff --git a/src/Settings.cpp b/src/Settings.cpp index d446e2a..e6fcdec 100644 --- a/src/Settings.cpp +++ b/src/Settings.cpp @@ -1,7 +1,7 @@ /** * @file Settings.cpp * @author Giulio Romualdi - * @copyright Released under the terms of the BSD 3-Clause License + * @copyright Released under the terms of the BSD 3-Clause License * @date 2018 */ @@ -10,7 +10,8 @@ #include #include -template inline void unused(Args&&...) {} +template +inline void unused(Args &&...) {} OsqpEigen::Settings::Settings() { @@ -45,47 +46,47 @@ void OsqpEigen::Settings::setScaling(const int scaling) void OsqpEigen::Settings::setAdaptiveRho(const bool isRhoStepSizeAdactive) { -# if EMBEDDED != 1 +#if EMBEDDED != 1 m_settings->adaptive_rho = (c_int)isRhoStepSizeAdactive; -# else - debugStream()<< "[OsqpEigen::Settings::setAdaptiveRho] OSPQ has been set to EMBEDDED, hence this setting is disabled." << std::endl; +#else + debugStream() << "[OsqpEigen::Settings::setAdaptiveRho] OSPQ has been set to EMBEDDED, hence this setting is disabled." << std::endl; unused(isRhoStepSizeAdactive); -# endif +#endif } void OsqpEigen::Settings::setAdaptiveRhoInterval(const int rhoInterval) { -# if EMBEDDED != 1 +#if EMBEDDED != 1 m_settings->adaptive_rho_interval = (c_int)rhoInterval; -# else - debugStream()<< "[OsqpEigen::Settings::setAdaptiveRhoInterval] OSPQ has been set to EMBEDDED, hence this setting is disabled." << std::endl; +#else + debugStream() << "[OsqpEigen::Settings::setAdaptiveRhoInterval] OSPQ has been set to EMBEDDED, hence this setting is disabled." << std::endl; unused(rhoInterval); -# endif +#endif } void OsqpEigen::Settings::setAdaptiveRhoTolerance(const double adaptiveRhoTolerance) { -# if EMBEDDED != 1 +#if EMBEDDED != 1 m_settings->adaptive_rho_tolerance = (c_float)adaptiveRhoTolerance; -# else - debugStream()<< "[OsqpEigen::Settings::setAdaptiveRhoTolerance] OSPQ has been set to EMBEDDED, hence this setting is disabled." << std::endl; +#else + debugStream() << "[OsqpEigen::Settings::setAdaptiveRhoTolerance] OSPQ has been set to EMBEDDED, hence this setting is disabled." << std::endl; unused(adaptiveRhoTolerance); -# endif +#endif } void OsqpEigen::Settings::setAdaptiveRhoFraction(const double adaptiveRhoFraction) { -# if EMBEDDED != 1 -# ifdef PROFILING +#if EMBEDDED != 1 +#ifdef PROFILING m_settings->adaptive_rho_fraction = (c_float)adaptiveRhoFraction; -# else +#else debugStream() << "[OsqpEigen::Settings::setAdaptiveRhoFraction] OSPQ has been set without PROFILING, hence this setting is disabled." << std::endl; unused(adaptiveRhoFraction); -# endif //ifdef PROFILING -# else //# if EMBEDDED != 1 - debugStream()<< "[OsqpEigen::Settings::setAdaptiveRhoFraction] OSPQ has been set to EMBEDDED, hence this setting is disabled." << std::endl; +#endif // ifdef PROFILING +#else // # if EMBEDDED != 1 + debugStream() << "[OsqpEigen::Settings::setAdaptiveRhoFraction] OSPQ has been set to EMBEDDED, hence this setting is disabled." << std::endl; unused(adaptiveRhoFraction); -# endif //# if EMBEDDED != 1 +#endif // # if EMBEDDED != 1 } void OsqpEigen::Settings::setMaxIteraction(const int maxIteration) @@ -144,36 +145,36 @@ void OsqpEigen::Settings::setLinearSystemSolver(const int linsysSolver) void OsqpEigen::Settings::setDelta(const double delta) { -# ifndef EMBEDDED +#ifndef EMBEDDED m_settings->delta = (c_float)delta; -# else - debugStream()<< "[OsqpEigen::Settings::setDelta] OSPQ has been set to EMBEDDED, hence this setting is disabled." << std::endl; +#else + debugStream() << "[OsqpEigen::Settings::setDelta] OSPQ has been set to EMBEDDED, hence this setting is disabled." << std::endl; unused(delta); -# endif +#endif } void OsqpEigen::Settings::setPolish(const bool polish) { #ifndef EMBEDDED -# ifdef OSQP_EIGEN_OSQP_IS_V1 +#ifdef OSQP_EIGEN_OSQP_IS_V1 m_settings->polishing = (c_int)polish; -# else +#else m_settings->polish = (c_int)polish; -# endif +#endif #else - debugStream() << "[OsqpEigen::Settings::setPolish] OSPQ has been set to EMBEDDED, hence this setting is disabled." << std::endl; + debugStream() << "[OsqpEigen::Settings::setPolish] OSPQ has been set to EMBEDDED, hence this setting is disabled." << std::endl; unused(polish); #endif } void OsqpEigen::Settings::setPolishRefineIter(const int polishRefineIter) { -# ifndef EMBEDDED +#ifndef EMBEDDED m_settings->polish_refine_iter = (c_int)polishRefineIter; -# else - debugStream()<< "[OsqpEigen::Settings::setPolishRefineIter] OSPQ has been set to EMBEDDED, hence this setting is disabled." << std::endl; +#else + debugStream() << "[OsqpEigen::Settings::setPolishRefineIter] OSPQ has been set to EMBEDDED, hence this setting is disabled." << std::endl; unused(polishRefineIter); -# endif +#endif } void OsqpEigen::Settings::setVerbosity(const bool isVerbose) @@ -181,7 +182,7 @@ void OsqpEigen::Settings::setVerbosity(const bool isVerbose) #ifndef EMBEDDED m_settings->verbose = (c_int)isVerbose; #else - debugStream()<< "[OsqpEigen::Settings::setVerbosity] OSPQ has been set to EMBEDDED, hence this setting is disabled." << std::endl; + debugStream() << "[OsqpEigen::Settings::setVerbosity] OSPQ has been set to EMBEDDED, hence this setting is disabled." << std::endl; unused(isVerbose); #endif } @@ -207,15 +208,15 @@ void OsqpEigen::Settings::setWarmStart(const bool warmStart) void OsqpEigen::Settings::setTimeLimit(const double timeLimit) { -# ifdef PROFILING +#ifdef PROFILING m_settings->time_limit = (c_float)timeLimit; -# else +#else debugStream() << "[OsqpEigen::Settings::setTimeLimit] OSPQ has been set without PROFILING, hence this setting is disabled." << std::endl; unused(timeLimit); -# endif +#endif } -OSQPSettings* const & OsqpEigen::Settings::getSettings() const +OSQPSettings *const &OsqpEigen::Settings::getSettings() const { return m_settings; } diff --git a/src/Solver.cpp b/src/Solver.cpp index 4d214f7..87715f8 100644 --- a/src/Solver.cpp +++ b/src/Solver.cpp @@ -7,21 +7,25 @@ // OsqpEigen #include +#include #include #include -#include #ifdef OSQP_EIGEN_OSQP_IS_V1 -void OsqpEigen::Solver::OSQPSolverDeleter(OSQPSolver * ptr) noexcept +void OsqpEigen::Solver::OSQPSolverDeleter(OSQPSolver* ptr) noexcept #else -void OsqpEigen::Solver::OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept +void OsqpEigen::Solver::OSQPWorkspaceDeleter(OSQPWorkspace* ptr) noexcept #endif { - if(ptr != nullptr) + if (ptr != nullptr) + { osqp_cleanup(ptr); + } } -OsqpEigen::Solver::Solver() : m_isSolverInitialized(false), +OsqpEigen::Solver::Solver() + : m_isSolverInitialized(false) + , #ifdef OSQP_EIGEN_OSQP_IS_V1 m_solver{nullptr, Solver::OSQPSolverDeleter} #else @@ -34,40 +38,44 @@ OsqpEigen::Solver::Solver() : m_isSolverInitialized(false), bool OsqpEigen::Solver::clearSolverVariables() { - if(!m_isSolverInitialized) + if (!m_isSolverInitialized) { - debugStream() << "[OsqpEigen::Solver::clearSolverVariables] Unable to clear the solver variables. " - << "Are you sure that the solver is initialized?" << std::endl; - return false; + debugStream() << "[OsqpEigen::Solver::clearSolverVariables] Unable to clear the solver " + "variables. " + << "Are you sure that the solver is initialized?" << std::endl; + return false; } #ifndef OSQP_EIGEN_OSQP_IS_V1 - for(int i = 0; i < getData()->n; i++) + for (int i = 0; i < getData()->n; i++) { - m_workspace->x[i] = 0; - m_workspace->x_prev[i] = 0; + m_workspace->x[i] = 0; + m_workspace->x_prev[i] = 0; - m_workspace->Px[i] = 0; - m_workspace->Aty[i] = 0; - m_workspace->Atdelta_y[i] = 0; + m_workspace->Px[i] = 0; + m_workspace->Aty[i] = 0; + m_workspace->Atdelta_y[i] = 0; - m_workspace->delta_x[i] = 0; - m_workspace->Pdelta_x[i] = 0; + m_workspace->delta_x[i] = 0; + m_workspace->Pdelta_x[i] = 0; } - for(int i = 0; i < getData()->m; i++) + for (int i = 0; i < getData()->m; i++) { - m_workspace->z[i] = 0; - m_workspace->z_prev[i] = 0; - m_workspace->y[i] = 0; + m_workspace->z[i] = 0; + m_workspace->z_prev[i] = 0; + m_workspace->y[i] = 0; - m_workspace->Ax[i] = 0; - m_workspace->delta_y[i] = 0; + m_workspace->Ax[i] = 0; + m_workspace->delta_y[i] = 0; - m_workspace->Adelta_x[i] = 0; + m_workspace->Adelta_x[i] = 0; } - for(int i = 0; i < getData()->n + getData()->m; i++) { m_workspace->xz_tilde[i] = 0; } + for (int i = 0; i < getData()->n + getData()->m; i++) + { + m_workspace->xz_tilde[i] = 0; + } #endif return true; @@ -75,31 +83,32 @@ bool OsqpEigen::Solver::clearSolverVariables() bool OsqpEigen::Solver::initSolver() { - if(m_isSolverInitialized){ + if (m_isSolverInitialized) + { debugStream() << "[OsqpEigen::Solver::initSolver] The solver has been already initialized. " - << "Please use clearSolver() method to deallocate memory." - << std::endl; + << "Please use clearSolver() method to deallocate memory." << std::endl; return false; } - if(!m_data->isSet()){ - debugStream() << "[OsqpEigen::Solver::initSolver] Some data are not set." - << std::endl; + if (!m_data->isSet()) + { + debugStream() << "[OsqpEigen::Solver::initSolver] Some data are not set." << std::endl; return false; } // if the number of constraints is equal to zero the user may not // call setLinearConstraintsMatrix() - if(m_data->getData()->m == 0) + if (m_data->getData()->m == 0) { - if(m_data->getData()->A == nullptr) + if (m_data->getData()->A == nullptr) { // let's create the matrix manually. This is required by osqp. Please check // https://github.com/oxfordcontrol/osqp/issues/295 Eigen::SparseMatrix A(m_data->getData()->m, m_data->getData()->n); - if(!m_data->setLinearConstraintsMatrix(A)) + if (!m_data->setLinearConstraintsMatrix(A)) { - debugStream() << "[OsqpEigen::Solver::initSolver] Unable to set the empty linear constraint " + debugStream() << "[OsqpEigen::Solver::initSolver] Unable to set the empty linear " + "constraint " << "matrix in case of unconstrained optimization problem" << std::endl; return false; @@ -108,22 +117,31 @@ bool OsqpEigen::Solver::initSolver() } #ifdef OSQP_EIGEN_OSQP_IS_V1 - OSQPSolver * solver; + OSQPSolver* solver; auto data = m_data->getData(); - if(osqp_setup(&solver, data->P, data->q, data->A, data->l, data->u, data->m, data->n, m_settings->getSettings()) != 0) + if (osqp_setup(&solver, + data->P, + data->q, + data->A, + data->l, + data->u, + data->m, + data->n, + m_settings->getSettings()) + != 0) { debugStream() << "[OsqpEigen::Solver::initSolver] Unable to setup the workspace." - << std::endl; + << std::endl; return false; } m_solver.reset(solver); #else OSQPWorkspace* workspace; - if(osqp_setup(&workspace, m_data->getData(), - m_settings->getSettings()) != 0 ){ + if (osqp_setup(&workspace, m_data->getData(), m_settings->getSettings()) != 0) + { debugStream() << "[OsqpEigen::Solver::initSolver] Unable to setup the workspace." - << std::endl; + << std::endl; return false; } @@ -141,7 +159,8 @@ bool OsqpEigen::Solver::isInitialized() void OsqpEigen::Solver::clearSolver() { - if(m_isSolverInitialized){ + if (m_isSolverInitialized) + { #ifdef OSQP_EIGEN_OSQP_IS_V1 m_solver.reset(); #else @@ -153,16 +172,16 @@ void OsqpEigen::Solver::clearSolver() bool OsqpEigen::Solver::solve() { - if (this->solveProblem() != OsqpEigen::ErrorExitFlag::NoError) { - debugStream() << "[OsqpEigen::Solver::solve] Unable to solve the problem." - << std::endl; + if (this->solveProblem() != OsqpEigen::ErrorExitFlag::NoError) + { + debugStream() << "[OsqpEigen::Solver::solve] Unable to solve the problem." << std::endl; return false; } // check if the solution is feasible - if(this->getStatus() != OsqpEigen::Status::Solved) { - debugStream() << "[OsqpEigen::Solver::solve] The solution is unfeasible." - << std::endl; + if (this->getStatus() != OsqpEigen::Status::Solved) + { + debugStream() << "[OsqpEigen::Solver::solve] The solution is unfeasible." << std::endl; return false; } @@ -181,10 +200,11 @@ const c_float OsqpEigen::Solver::getObjValue() const OsqpEigen::ErrorExitFlag OsqpEigen::Solver::solveProblem() { - if(!m_isSolverInitialized){ - debugStream() << "[OsqpEigen::Solver::solveProblem] The solve has not been initialized yet. " - << "Please call initSolver() method." - << std::endl; + if (!m_isSolverInitialized) + { + debugStream() << "[OsqpEigen::Solver::solveProblem] The solve has not been initialized " + "yet. " + << "Please call initSolver() method." << std::endl; return OsqpEigen::ErrorExitFlag::WorkspaceNotInitError; } @@ -195,7 +215,7 @@ OsqpEigen::ErrorExitFlag OsqpEigen::Solver::solveProblem() #endif } -const Eigen::Matrix &OsqpEigen::Solver::getSolution() +const Eigen::Matrix& OsqpEigen::Solver::getSolution() { // copy data from an array to Eigen vector c_float* solution = getOSQPSolution()->x; @@ -204,11 +224,11 @@ const Eigen::Matrix &OsqpEigen::Solver::getSolution() return m_solution; } -const Eigen::Matrix &OsqpEigen::Solver::getDualSolution() +const Eigen::Matrix& OsqpEigen::Solver::getDualSolution() { // copy data from an array to Eigen vector c_float* solution = getOSQPSolution()->y; - m_dualSolution = Eigen::Map< Eigen::Matrix>(solution, getData()->m, 1); + m_dualSolution = Eigen::Map>(solution, getData()->m, 1); return m_dualSolution; } @@ -224,133 +244,164 @@ const std::unique_ptr& OsqpEigen::Solver::data() const } #ifdef OSQP_EIGEN_OSQP_IS_V1 -const std::unique_ptr> & OsqpEigen::Solver::solver() const +const std::unique_ptr>& +OsqpEigen::Solver::solver() const { return m_solver; } #else -const std::unique_ptr>& OsqpEigen::Solver::workspace() const +const std::unique_ptr>& +OsqpEigen::Solver::workspace() const { return m_workspace; } #endif -bool OsqpEigen::Solver::updateGradient(const Eigen::Ref>& gradient) +bool OsqpEigen::Solver::updateGradient( + const Eigen::Ref>& gradient) { - if(!m_isSolverInitialized){ + if (!m_isSolverInitialized) + { debugStream() << "[OsqpEigen::Solver::updateGradient] The solver is not initialized" << std::endl; return false; } // check if the dimension of the gradient is correct - if(gradient.rows() != getData()->n){ - debugStream() << "[OsqpEigen::Solver::updateGradient] The size of the gradient must be equal to the number of the variables." - << std::endl; + if (gradient.rows() != getData()->n) + { + debugStream() << "[OsqpEigen::Solver::updateGradient] The size of the gradient must be " + "equal to the number of the variables." + << std::endl; return false; } // update the gradient vector #ifdef OSQP_EIGEN_OSQP_IS_V1 - if(osqp_update_data_vec(m_solver.get(), gradient.data(), nullptr, nullptr)){ + if (osqp_update_data_vec(m_solver.get(), gradient.data(), nullptr, nullptr)) + { #else - if(osqp_update_lin_cost(m_workspace.get(), gradient.data())){ + if (osqp_update_lin_cost(m_workspace.get(), gradient.data())) + { #endif - debugStream() << "[OsqpEigen::Solver::updateGradient] Error when the update gradient is called." - << std::endl; + debugStream() << "[OsqpEigen::Solver::updateGradient] Error when the update gradient is " + "called." + << std::endl; return false; } return true; } -bool OsqpEigen::Solver::updateLowerBound(const Eigen::Ref>& lowerBound) +bool OsqpEigen::Solver::updateLowerBound( + const Eigen::Ref>& lowerBound) { - if(!m_isSolverInitialized){ + if (!m_isSolverInitialized) + { debugStream() << "[OsqpEigen::Solver::updateLowerBound] The solver is not initialized" << std::endl; return false; } // check if the dimension of the lowerBound vector is correct - if(lowerBound.rows() != getData()->m){ - debugStream() << "[OsqpEigen::Solver::updateLowerBound] The size of the lower bound must be equal to the number of the variables." - << std::endl; + if (lowerBound.rows() != getData()->m) + { + debugStream() << "[OsqpEigen::Solver::updateLowerBound] The size of the lower bound must " + "be equal to the number of the variables." + << std::endl; return false; } // update the lower bound vector #ifdef OSQP_EIGEN_OSQP_IS_V1 - if(osqp_update_data_vec(m_solver.get(), nullptr, lowerBound.data(), nullptr)){ + if (osqp_update_data_vec(m_solver.get(), nullptr, lowerBound.data(), nullptr)) + { #else - if(osqp_update_lower_bound(m_workspace.get(), lowerBound.data())){ + if (osqp_update_lower_bound(m_workspace.get(), lowerBound.data())) + { #endif - debugStream() << "[OsqpEigen::Solver::updateLowerBound] Error when the update lower bound is called." - << std::endl; + debugStream() << "[OsqpEigen::Solver::updateLowerBound] Error when the update lower bound " + "is called." + << std::endl; return false; } return true; } -bool OsqpEigen::Solver::updateUpperBound(const Eigen::Ref>& upperBound) +bool OsqpEigen::Solver::updateUpperBound( + const Eigen::Ref>& upperBound) { - if(!m_isSolverInitialized){ + if (!m_isSolverInitialized) + { debugStream() << "[OsqpEigen::Solver::updateUpperBound] The solver is not initialized" << std::endl; return false; } // check if the dimension of the upperBound vector is correct - if(upperBound.rows() != getData()->m){ - debugStream() << "[OsqpEigen::Solver::updateUpperBound] The size of the upper bound must be equal to the number of the variables." - << std::endl; + if (upperBound.rows() != getData()->m) + { + debugStream() << "[OsqpEigen::Solver::updateUpperBound] The size of the upper bound must " + "be equal to the number of the variables." + << std::endl; return false; } // update the upper bound vector #ifdef OSQP_EIGEN_OSQP_IS_V1 - if(osqp_update_data_vec(m_solver.get(), nullptr, nullptr, upperBound.data())){ + if (osqp_update_data_vec(m_solver.get(), nullptr, nullptr, upperBound.data())) + { #else - if(osqp_update_upper_bound(m_workspace.get(), upperBound.data())){ + if (osqp_update_upper_bound(m_workspace.get(), upperBound.data())) + { #endif - debugStream() << "[OsqpEigen::Solver::updateUpperBound] Error when the update upper bound is called." - << std::endl; + debugStream() << "[OsqpEigen::Solver::updateUpperBound] Error when the update upper bound " + "is called." + << std::endl; return false; } return true; } -bool OsqpEigen::Solver::updateBounds(const Eigen::Ref>& lowerBound, - const Eigen::Ref>& upperBound) +bool OsqpEigen::Solver::updateBounds( + const Eigen::Ref>& lowerBound, + const Eigen::Ref>& upperBound) { - if(!m_isSolverInitialized){ + if (!m_isSolverInitialized) + { debugStream() << "[OsqpEigen::Solver::updateBounds] The solver is not initialized" << std::endl; return false; } // check if the dimension of the upperBound vector is correct - if(upperBound.rows() != getData()->m){ - debugStream() << "[OsqpEigen::Solver::updateBounds] The size of the upper bound must be equal to the number of the variables." - << std::endl; + if (upperBound.rows() != getData()->m) + { + debugStream() << "[OsqpEigen::Solver::updateBounds] The size of the upper bound must be " + "equal to the number of the variables." + << std::endl; return false; } // check if the dimension of the lowerBound vector is correct - if(lowerBound.rows() != getData()->m){ - debugStream() << "[OsqpEigen::Solver::updateBounds] The size of the lower bound must be equal to the number of the variables." - << std::endl; + if (lowerBound.rows() != getData()->m) + { + debugStream() << "[OsqpEigen::Solver::updateBounds] The size of the lower bound must be " + "equal to the number of the variables." + << std::endl; return false; } // update lower and upper constraints #ifdef OSQP_EIGEN_OSQP_IS_V1 - if(osqp_update_data_vec(m_solver.get(), nullptr, lowerBound.data(), upperBound.data())){ + if (osqp_update_data_vec(m_solver.get(), nullptr, lowerBound.data(), upperBound.data())) + { #else - if(osqp_update_bounds(m_workspace.get(), lowerBound.data(), upperBound.data())){ + if (osqp_update_bounds(m_workspace.get(), lowerBound.data(), upperBound.data())) + { #endif debugStream() << "[OsqpEigen::Solver::updateBounds] Error when the update bounds is called." - << std::endl; + << std::endl; return false; } return true; diff --git a/tests/MPCTest.cpp b/tests/MPCTest.cpp index 4c33d8f..059af6f 100644 --- a/tests/MPCTest.cpp +++ b/tests/MPCTest.cpp @@ -20,191 +20,197 @@ // colors #define ANSI_TXT_GRN "\033[0;32m" -#define ANSI_TXT_MGT "\033[0;35m" //Magenta -#define ANSI_TXT_DFT "\033[0;0m" //Console default +#define ANSI_TXT_MGT "\033[0;35m" // Magenta +#define ANSI_TXT_DFT "\033[0;0m" // Console default #define GTEST_BOX "[ cout ] " -#define COUT_GTEST ANSI_TXT_GRN << GTEST_BOX //You could add the Default +#define COUT_GTEST ANSI_TXT_GRN << GTEST_BOX // You could add the Default #define COUT_GTEST_MGT COUT_GTEST << ANSI_TXT_MGT -void setDynamicsMatrices(Eigen::Matrix &a, Eigen::Matrix &b) +void setDynamicsMatrices(Eigen::Matrix& a, Eigen::Matrix& b) { - a << 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0., 0. , - 0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0. , - 0., 0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0. , - 0.0488, 0., 0., 1., 0., 0., 0.0016, 0., 0., 0.0992, 0., 0. , - 0., -0.0488, 0., 0., 1., 0., 0., -0.0016, 0., 0., 0.0992, 0. , - 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.0992, - 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0. , - 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0. , - 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0. , - 0.9734, 0., 0., 0., 0., 0., 0.0488, 0., 0., 0.9846, 0., 0. , - 0., -0.9734, 0., 0., 0., 0., 0., -0.0488, 0., 0., 0.9846, 0. , - 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.9846; - - b << 0., -0.0726, 0., 0.0726, - -0.0726, 0., 0.0726, 0. , - -0.0152, 0.0152, -0.0152, 0.0152, - -0., -0.0006, -0., 0.0006, - 0.0006, 0., -0.0006, 0.0000, - 0.0106, 0.0106, 0.0106, 0.0106, - 0, -1.4512, 0., 1.4512, - -1.4512, 0., 1.4512, 0. , - -0.3049, 0.3049, -0.3049, 0.3049, - -0., -0.0236, 0., 0.0236, - 0.0236, 0., -0.0236, 0. , - 0.2107, 0.2107, 0.2107, 0.2107; + a << 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., + 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0.0488, 0., 0., 1., 0., 0., 0.0016, + 0., 0., 0.0992, 0., 0., 0., -0.0488, 0., 0., 1., 0., 0., -0.0016, 0., 0., 0.0992, 0., 0., + 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.0992, 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., + 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., + 0., 0., 0.9734, 0., 0., 0., 0., 0., 0.0488, 0., 0., 0.9846, 0., 0., 0., -0.9734, 0., 0., 0., + 0., 0., -0.0488, 0., 0., 0.9846, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.9846; + + b << 0., -0.0726, 0., 0.0726, -0.0726, 0., 0.0726, 0., -0.0152, 0.0152, -0.0152, 0.0152, -0., + -0.0006, -0., 0.0006, 0.0006, 0., -0.0006, 0.0000, 0.0106, 0.0106, 0.0106, 0.0106, 0, + -1.4512, 0., 1.4512, -1.4512, 0., 1.4512, 0., -0.3049, 0.3049, -0.3049, 0.3049, -0., + -0.0236, 0., 0.0236, 0.0236, 0., -0.0236, 0., 0.2107, 0.2107, 0.2107, 0.2107; } - -void setInequalityConstraints(Eigen::Matrix &xMax, Eigen::Matrix &xMin, - Eigen::Matrix &uMax, Eigen::Matrix &uMin) +void setInequalityConstraints(Eigen::Matrix& xMax, + Eigen::Matrix& xMin, + Eigen::Matrix& uMax, + Eigen::Matrix& uMin) { c_float u0 = 10.5916; // input inequality constraints - uMin << 9.6 - u0, - 9.6 - u0, - 9.6 - u0, - 9.6 - u0; + uMin << 9.6 - u0, 9.6 - u0, 9.6 - u0, 9.6 - u0; - uMax << 13 - u0, - 13 - u0, - 13 - u0, - 13 - u0; + uMax << 13 - u0, 13 - u0, 13 - u0, 13 - u0; // state inequality constraints - xMin << -M_PI/6,-M_PI/6,-OsqpEigen::INFTY,-OsqpEigen::INFTY,-OsqpEigen::INFTY,-1., - -OsqpEigen::INFTY, -OsqpEigen::INFTY,-OsqpEigen::INFTY,-OsqpEigen::INFTY, - -OsqpEigen::INFTY,-OsqpEigen::INFTY; + xMin << -M_PI / 6, -M_PI / 6, -OsqpEigen::INFTY, -OsqpEigen::INFTY, -OsqpEigen::INFTY, -1., + -OsqpEigen::INFTY, -OsqpEigen::INFTY, -OsqpEigen::INFTY, -OsqpEigen::INFTY, + -OsqpEigen::INFTY, -OsqpEigen::INFTY; - xMax << M_PI/6,M_PI/6, OsqpEigen::INFTY,OsqpEigen::INFTY,OsqpEigen::INFTY, - OsqpEigen::INFTY, OsqpEigen::INFTY,OsqpEigen::INFTY,OsqpEigen::INFTY, - OsqpEigen::INFTY,OsqpEigen::INFTY,OsqpEigen::INFTY; + xMax << M_PI / 6, M_PI / 6, OsqpEigen::INFTY, OsqpEigen::INFTY, OsqpEigen::INFTY, + OsqpEigen::INFTY, OsqpEigen::INFTY, OsqpEigen::INFTY, OsqpEigen::INFTY, OsqpEigen::INFTY, + OsqpEigen::INFTY, OsqpEigen::INFTY; } -void setWeightMatrices(Eigen::DiagonalMatrix &Q, Eigen::DiagonalMatrix &R) +void setWeightMatrices(Eigen::DiagonalMatrix& Q, Eigen::DiagonalMatrix& R) { Q.diagonal() << 0, 0, 10., 10., 10., 10., 0, 0, 0, 5., 5., 5.; R.diagonal() << 0.1, 0.1, 0.1, 0.1; } -void castMPCToQPHessian(const Eigen::DiagonalMatrix &Q, const Eigen::DiagonalMatrix &R, int mpcWindow, - Eigen::SparseMatrix &hessianMatrix) +void castMPCToQPHessian(const Eigen::DiagonalMatrix& Q, + const Eigen::DiagonalMatrix& R, + int mpcWindow, + Eigen::SparseMatrix& hessianMatrix) { - hessianMatrix.resize(12*(mpcWindow+1) + 4 * mpcWindow, 12*(mpcWindow+1) + 4 * mpcWindow); + hessianMatrix.resize(12 * (mpcWindow + 1) + 4 * mpcWindow, + 12 * (mpcWindow + 1) + 4 * mpcWindow); - //populate hessian matrix - for(int i = 0; i<12*(mpcWindow+1) + 4 * mpcWindow; i++){ - if(i < 12*(mpcWindow+1)){ - int posQ=i%12; + // populate hessian matrix + for (int i = 0; i < 12 * (mpcWindow + 1) + 4 * mpcWindow; i++) + { + if (i < 12 * (mpcWindow + 1)) + { + int posQ = i % 12; float value = Q.diagonal()[posQ]; - if(value != 0) - hessianMatrix.insert(i,i) = value; - } - else{ - int posR=i%4; + if (value != 0) + hessianMatrix.insert(i, i) = value; + } else + { + int posR = i % 4; float value = R.diagonal()[posR]; - if(value != 0) - hessianMatrix.insert(i,i) = value; + if (value != 0) + hessianMatrix.insert(i, i) = value; } } } -void castMPCToQPGradient(const Eigen::DiagonalMatrix &Q, const Eigen::Matrix &xRef, int mpcWindow, - Eigen::Matrix &gradient) +void castMPCToQPGradient(const Eigen::DiagonalMatrix& Q, + const Eigen::Matrix& xRef, + int mpcWindow, + Eigen::Matrix& gradient) { - Eigen::Matrix Qx_ref; + Eigen::Matrix Qx_ref; Qx_ref = Q * (-xRef); // populate the gradient vector - gradient = Eigen::Matrix::Zero(12*(mpcWindow+1) + 4*mpcWindow, 1); - for(int i = 0; i<12*(mpcWindow+1); i++){ - int posQ=i%12; - float value = Qx_ref(posQ,0); - gradient(i,0) = value; + gradient = Eigen::Matrix::Zero(12 * (mpcWindow + 1) + 4 * mpcWindow, 1); + for (int i = 0; i < 12 * (mpcWindow + 1); i++) + { + int posQ = i % 12; + float value = Qx_ref(posQ, 0); + gradient(i, 0) = value; } } -void castMPCToQPConstraintMatrix(const Eigen::Matrix &dynamicMatrix, const Eigen::Matrix &controlMatrix, - int mpcWindow, Eigen::SparseMatrix &constraintMatrix) +void castMPCToQPConstraintMatrix(const Eigen::Matrix& dynamicMatrix, + const Eigen::Matrix& controlMatrix, + int mpcWindow, + Eigen::SparseMatrix& constraintMatrix) { - constraintMatrix.resize(12*(mpcWindow+1) + 12*(mpcWindow+1) + 4 * mpcWindow, 12*(mpcWindow+1) + 4 * mpcWindow); + constraintMatrix.resize(12 * (mpcWindow + 1) + 12 * (mpcWindow + 1) + 4 * mpcWindow, + 12 * (mpcWindow + 1) + 4 * mpcWindow); // populate linear constraint matrix - for(int i = 0; i<12*(mpcWindow+1); i++){ - constraintMatrix.insert(i,i) = -1; + for (int i = 0; i < 12 * (mpcWindow + 1); i++) + { + constraintMatrix.insert(i, i) = -1; } - for(int i = 0; i < mpcWindow; i++) - for(int j = 0; j<12; j++) - for(int k = 0; k<12; k++){ - float value = dynamicMatrix(j,k); - if(value != 0){ - constraintMatrix.insert(12 * (i+1) + j, 12 * i + k) = value; + for (int i = 0; i < mpcWindow; i++) + for (int j = 0; j < 12; j++) + for (int k = 0; k < 12; k++) + { + float value = dynamicMatrix(j, k); + if (value != 0) + { + constraintMatrix.insert(12 * (i + 1) + j, 12 * i + k) = value; } } - for(int i = 0; i < mpcWindow; i++) - for(int j = 0; j < 12; j++) - for(int k = 0; k < 4; k++){ - float value = controlMatrix(j,k); - if(value != 0){ - constraintMatrix.insert(12*(i+1)+j, 4*i+k+12*(mpcWindow + 1)) = value; + for (int i = 0; i < mpcWindow; i++) + for (int j = 0; j < 12; j++) + for (int k = 0; k < 4; k++) + { + float value = controlMatrix(j, k); + if (value != 0) + { + constraintMatrix.insert(12 * (i + 1) + j, 4 * i + k + 12 * (mpcWindow + 1)) + = value; } } - for(int i = 0; i<12*(mpcWindow+1) + 4*mpcWindow; i++){ - constraintMatrix.insert(i+(mpcWindow+1)*12,i) = 1; + for (int i = 0; i < 12 * (mpcWindow + 1) + 4 * mpcWindow; i++) + { + constraintMatrix.insert(i + (mpcWindow + 1) * 12, i) = 1; } } -void castMPCToQPConstraintVectors(const Eigen::Matrix &xMax, const Eigen::Matrix &xMin, - const Eigen::Matrix &uMax, const Eigen::Matrix &uMin, - const Eigen::Matrix &x0, - int mpcWindow, Eigen::Matrix &lowerBound, Eigen::Matrix &upperBound) +void castMPCToQPConstraintVectors(const Eigen::Matrix& xMax, + const Eigen::Matrix& xMin, + const Eigen::Matrix& uMax, + const Eigen::Matrix& uMin, + const Eigen::Matrix& x0, + int mpcWindow, + Eigen::Matrix& lowerBound, + Eigen::Matrix& upperBound) { // evaluate the lower and the upper inequality vectors - Eigen::Matrix lowerInequality = Eigen::Matrix::Zero(12*(mpcWindow+1) + 4 * mpcWindow, 1); - Eigen::Matrix upperInequality = Eigen::Matrix::Zero(12*(mpcWindow+1) + 4 * mpcWindow, 1); - for(int i=0; i lowerInequality + = Eigen::Matrix::Zero(12 * (mpcWindow + 1) + 4 * mpcWindow, 1); + Eigen::Matrix upperInequality + = Eigen::Matrix::Zero(12 * (mpcWindow + 1) + 4 * mpcWindow, 1); + for (int i = 0; i < mpcWindow + 1; i++) + { + lowerInequality.block(12 * i, 0, 12, 1) = xMin; + upperInequality.block(12 * i, 0, 12, 1) = xMax; } - for(int i=0; i lowerEquality = Eigen::Matrix::Zero(12*(mpcWindow+1),1 ); + Eigen::Matrix lowerEquality + = Eigen::Matrix::Zero(12 * (mpcWindow + 1), 1); Eigen::Matrix upperEquality; - lowerEquality.block(0,0,12,1) = -x0; + lowerEquality.block(0, 0, 12, 1) = -x0; upperEquality = lowerEquality; lowerEquality = lowerEquality; // merge inequality and equality vectors - lowerBound = Eigen::Matrix::Zero(2*12*(mpcWindow+1) + 4*mpcWindow,1 ); - lowerBound << lowerEquality, - lowerInequality; + lowerBound = Eigen::Matrix::Zero(2 * 12 * (mpcWindow + 1) + 4 * mpcWindow, 1); + lowerBound << lowerEquality, lowerInequality; - upperBound = Eigen::Matrix::Zero(2*12*(mpcWindow+1) + 4*mpcWindow,1 ); - upperBound << upperEquality, - upperInequality; + upperBound = Eigen::Matrix::Zero(2 * 12 * (mpcWindow + 1) + 4 * mpcWindow, 1); + upperBound << upperEquality, upperInequality; } - -void updateConstraintVectors(const Eigen::Matrix &x0, - Eigen::Matrix &lowerBound, Eigen::Matrix &upperBound) +void updateConstraintVectors(const Eigen::Matrix& x0, + Eigen::Matrix& lowerBound, + Eigen::Matrix& upperBound) { - lowerBound.block(0,0,12,1) = -x0; - upperBound.block(0,0,12,1) = -x0; + lowerBound.block(0, 0, 12, 1) = -x0; + upperBound.block(0, 0, 12, 1) = -x0; } - -c_float getErrorNorm(const Eigen::Matrix &x, - const Eigen::Matrix &xRef) +c_float +getErrorNorm(const Eigen::Matrix& x, const Eigen::Matrix& xRef) { // evaluate the error Eigen::Matrix error = x - xRef; @@ -213,12 +219,11 @@ c_float getErrorNorm(const Eigen::Matrix &x, return error.norm(); } - TEST_CASE("MPCTest") { // open the ofstream std::ofstream dataStream; - dataStream.open ("output.txt"); + dataStream.open("output.txt"); // set the preview window int mpcWindow = 20; @@ -249,8 +254,8 @@ TEST_CASE("MPCTest") Eigen::Matrix upperBound; // set the initial and the desired states - x0 << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ; - xRef << 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0; + x0 << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; + xRef << 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0; // set MPC problem quantities setDynamicsMatrices(a, b); @@ -272,7 +277,7 @@ TEST_CASE("MPCTest") // set the initial data of the QP solver solver.data()->setNumberOfVariables(12 * (mpcWindow + 1) + 4 * mpcWindow); - solver.data()->setNumberOfConstraints(2 * 12 * (mpcWindow + 1) + 4 * mpcWindow); + solver.data()->setNumberOfConstraints(2 * 12 * (mpcWindow + 1) + 4 * mpcWindow); REQUIRE(solver.data()->setHessianMatrix(hessian)); REQUIRE(solver.data()->setGradient(gradient)); REQUIRE(solver.data()->setLinearConstraintsMatrix(linearMatrix)); @@ -293,7 +298,8 @@ TEST_CASE("MPCTest") clock_t startTime, endTime; c_float avarageTime = 0; - for (int i = 0; i < numberOfSteps; i++){ + for (int i = 0; i < numberOfSteps; i++) + { startTime = clock(); // solve the QP problem @@ -305,7 +311,7 @@ TEST_CASE("MPCTest") // save data into file auto x0Data = x0.data(); - for(int j = 0; j < 12; j++) + for (int j = 0; j < 12; j++) dataStream << x0Data[j] << " "; dataStream << std::endl; @@ -319,14 +325,14 @@ TEST_CASE("MPCTest") endTime = clock(); avarageTime += static_cast(endTime - startTime) / CLOCKS_PER_SEC; - } + } // close the stream dataStream.close(); - std::cout << COUT_GTEST_MGT << "Avarage time = " << avarageTime / numberOfSteps - << " seconds." << ANSI_TXT_DFT << std::endl; + std::cout << COUT_GTEST_MGT << "Avarage time = " << avarageTime / numberOfSteps << " seconds." + << ANSI_TXT_DFT << std::endl; constexpr double tolerance = 1e-2; - REQUIRE(getErrorNorm(x0, xRef) <= tolerance); + REQUIRE(getErrorNorm(x0, xRef) <= tolerance); } diff --git a/tests/MPCUpdateMatricesTest.cpp b/tests/MPCUpdateMatricesTest.cpp index ae090fc..7f7b2c2 100644 --- a/tests/MPCUpdateMatricesTest.cpp +++ b/tests/MPCUpdateMatricesTest.cpp @@ -15,21 +15,22 @@ #include #include -#include #include +#include // colors #define ANSI_TXT_GRN "\033[0;32m" -#define ANSI_TXT_MGT "\033[0;35m" //Magenta -#define ANSI_TXT_DFT "\033[0;0m" //Console default +#define ANSI_TXT_MGT "\033[0;35m" // Magenta +#define ANSI_TXT_DFT "\033[0;0m" // Console default #define GTEST_BOX "[ cout ] " -#define COUT_GTEST ANSI_TXT_GRN << GTEST_BOX //You could add the Default +#define COUT_GTEST ANSI_TXT_GRN << GTEST_BOX // You could add the Default #define COUT_GTEST_MGT COUT_GTEST << ANSI_TXT_MGT #define T 0.1 -void setDynamicsMatrices(Eigen::Matrix &a, Eigen::Matrix &b, - Eigen::Matrix &c, +void setDynamicsMatrices(Eigen::Matrix& a, + Eigen::Matrix& b, + Eigen::Matrix& c, double t) { @@ -37,158 +38,175 @@ void setDynamicsMatrices(Eigen::Matrix &a, Eigen::Matrix &Q, Eigen::DiagonalMatrix &R) +void setWeightMatrices(Eigen::DiagonalMatrix& Q, Eigen::DiagonalMatrix& R) { Q.diagonal() << 10; R.diagonal() << 1; } -void castMPCToQPHessian(const Eigen::DiagonalMatrix &Q, const Eigen::DiagonalMatrix &R, int mpcWindow, - int k, Eigen::SparseMatrix &hessianMatrix) +void castMPCToQPHessian(const Eigen::DiagonalMatrix& Q, + const Eigen::DiagonalMatrix& R, + int mpcWindow, + int k, + Eigen::SparseMatrix& hessianMatrix) { Eigen::Matrix a; Eigen::Matrix b; Eigen::Matrix c; - hessianMatrix.resize(2*(mpcWindow+1) + 1 * mpcWindow, 2*(mpcWindow+1) + 1 * mpcWindow); + hessianMatrix.resize(2 * (mpcWindow + 1) + 1 * mpcWindow, 2 * (mpcWindow + 1) + 1 * mpcWindow); - //populate hessian matrix - for(int i = 0; i < 2 * (mpcWindow+1) + 1 * mpcWindow; i++){ + // populate hessian matrix + for (int i = 0; i < 2 * (mpcWindow + 1) + 1 * mpcWindow; i++) + { double t = (k + i) * T; setDynamicsMatrices(a, b, c, t); - if(i < 2 * (mpcWindow + 1)){ + if (i < 2 * (mpcWindow + 1)) + { // here the structure of the matrix c is used! - int pos=i%2; + int pos = i % 2; float value = c(pos) * Q.diagonal()[0] * c(pos); - if(value != 0) - hessianMatrix.insert(i,i) = value; - } - else{ + if (value != 0) + hessianMatrix.insert(i, i) = value; + } else + { float value = R.diagonal()[0]; - if(value != 0) - hessianMatrix.insert(i,i) = value; + if (value != 0) + hessianMatrix.insert(i, i) = value; } } } -void castMPCToQPGradient(const Eigen::DiagonalMatrix &Q, const Eigen::Matrix &yRef, int mpcWindow, - int k, Eigen::Matrix &gradient) +void castMPCToQPGradient(const Eigen::DiagonalMatrix& Q, + const Eigen::Matrix& yRef, + int mpcWindow, + int k, + Eigen::Matrix& gradient) { Eigen::Matrix a; Eigen::Matrix b; Eigen::Matrix c; - Eigen::Matrix Qy_ref; + Eigen::Matrix Qy_ref; Qy_ref = Q * (-yRef); // populate the gradient vector - gradient = Eigen::Matrix::Zero(2*(mpcWindow+1) + 1 *mpcWindow, 1); - for(int i = 0; i<2*(mpcWindow+1); i++){ + gradient = Eigen::Matrix::Zero(2 * (mpcWindow + 1) + 1 * mpcWindow, 1); + for (int i = 0; i < 2 * (mpcWindow + 1); i++) + { double t = (k + i) * T; setDynamicsMatrices(a, b, c, t); - int pos=i%2; - float value = Qy_ref(0,0) * c(pos); - gradient(i,0) = value; + int pos = i % 2; + float value = Qy_ref(0, 0) * c(pos); + gradient(i, 0) = value; } } -void castMPCToQPConstraintMatrix(int mpcWindow, int k, Eigen::SparseMatrix &constraintMatrix) +void castMPCToQPConstraintMatrix(int mpcWindow, + int k, + Eigen::SparseMatrix& constraintMatrix) { - constraintMatrix.resize(2*(mpcWindow+1), 2*(mpcWindow+1) + 1 * mpcWindow); + constraintMatrix.resize(2 * (mpcWindow + 1), 2 * (mpcWindow + 1) + 1 * mpcWindow); // populate linear constraint matrix - for(int i = 0; i<2*(mpcWindow+1); i++){ - constraintMatrix.insert(i,i) = -1; + for (int i = 0; i < 2 * (mpcWindow + 1); i++) + { + constraintMatrix.insert(i, i) = -1; } Eigen::Matrix a; Eigen::Matrix b; Eigen::Matrix c; - - for(int i = 0; i < mpcWindow; i++){ + for (int i = 0; i < mpcWindow; i++) + { double t = (k + i) * T; setDynamicsMatrices(a, b, c, t); - for(int j = 0; j<2; j++) - for(int k = 0; k<2; k++){ - float value = a(j,k); - if(value != 0){ - constraintMatrix.insert(2 * (i+1) + j, 2 * i + k) = value; + for (int j = 0; j < 2; j++) + for (int k = 0; k < 2; k++) + { + float value = a(j, k); + if (value != 0) + { + constraintMatrix.insert(2 * (i + 1) + j, 2 * i + k) = value; } } } - for(int i = 0; i < mpcWindow; i++) - for(int j = 0; j < 2; j++) - for(int k = 0; k < 1; k++){ + for (int i = 0; i < mpcWindow; i++) + for (int j = 0; j < 2; j++) + for (int k = 0; k < 1; k++) + { // b is constant - float value = b(j,k); - if(value != 0){ - constraintMatrix.insert(2*(i+1)+j, 1*i+k+2*(mpcWindow + 1)) = value; + float value = b(j, k); + if (value != 0) + { + constraintMatrix.insert(2 * (i + 1) + j, 1 * i + k + 2 * (mpcWindow + 1)) + = value; } } } -void castMPCToQPConstraintVectors(const Eigen::Matrix &x0, +void castMPCToQPConstraintVectors(const Eigen::Matrix& x0, int mpcWindow, - Eigen::Matrix &lowerBound, Eigen::Matrix &upperBound) + Eigen::Matrix& lowerBound, + Eigen::Matrix& upperBound) { // evaluate the lower and the upper equality vectors - lowerBound = Eigen::Matrix::Zero(2*(mpcWindow+1),1 ); - lowerBound.block(0,0,2,1) = -x0; + lowerBound = Eigen::Matrix::Zero(2 * (mpcWindow + 1), 1); + lowerBound.block(0, 0, 2, 1) = -x0; upperBound = lowerBound; } -bool updateHessianMatrix(OsqpEigen::Solver &solver, - const Eigen::DiagonalMatrix &Q, const Eigen::DiagonalMatrix &R, - int mpcWindow, int k) +bool updateHessianMatrix(OsqpEigen::Solver& solver, + const Eigen::DiagonalMatrix& Q, + const Eigen::DiagonalMatrix& R, + int mpcWindow, + int k) { Eigen::SparseMatrix hessianMatrix; castMPCToQPHessian(Q, R, mpcWindow, k, hessianMatrix); - if(!solver.updateHessianMatrix(hessianMatrix)) + if (!solver.updateHessianMatrix(hessianMatrix)) return false; return true; } -bool updateLinearConstraintsMatrix(OsqpEigen::Solver &solver, - int mpcWindow, int k) +bool updateLinearConstraintsMatrix(OsqpEigen::Solver& solver, int mpcWindow, int k) { Eigen::SparseMatrix constraintMatrix; castMPCToQPConstraintMatrix(mpcWindow, k, constraintMatrix); - if(!solver.updateLinearConstraintsMatrix(constraintMatrix)) + if (!solver.updateLinearConstraintsMatrix(constraintMatrix)) return false; return true; } - -void updateConstraintVectors(const Eigen::Matrix &x0, - Eigen::Matrix &lowerBound, Eigen::Matrix &upperBound) +void updateConstraintVectors(const Eigen::Matrix& x0, + Eigen::Matrix& lowerBound, + Eigen::Matrix& upperBound) { - lowerBound.block(0,0,2,1) = -x0; - upperBound.block(0,0,2,1) = -x0; + lowerBound.block(0, 0, 2, 1) = -x0; + upperBound.block(0, 0, 2, 1) = -x0; } TEST_CASE("MPCTest Update matrices") { // open the ofstream std::ofstream dataStream; - dataStream.open ("output.txt"); + dataStream.open("output.txt"); // set the preview window int mpcWindow = 100; @@ -257,7 +275,8 @@ TEST_CASE("MPCTest Update matrices") clock_t startTime, endTime; double avarageTime = 0; - for (int i = 0; i < numberOfSteps; i++){ + for (int i = 0; i < numberOfSteps; i++) + { startTime = clock(); setDynamicsMatrices(a, b, c, i * T); @@ -281,7 +300,7 @@ TEST_CASE("MPCTest Update matrices") // save data into file auto x0Data = x0.data(); - for(int j = 0; j < 2; j++) + for (int j = 0; j < 2; j++) dataStream << x0Data[j] << " "; dataStream << std::endl; @@ -292,11 +311,11 @@ TEST_CASE("MPCTest Update matrices") endTime = clock(); avarageTime += static_cast(endTime - startTime) / CLOCKS_PER_SEC; - } + } // close the stream dataStream.close(); - std::cout << COUT_GTEST_MGT << "Avarage time = " << avarageTime / numberOfSteps - << " seconds." << ANSI_TXT_DFT << std::endl; + std::cout << COUT_GTEST_MGT << "Avarage time = " << avarageTime / numberOfSteps << " seconds." + << ANSI_TXT_DFT << std::endl; } diff --git a/tests/QPTest.cpp b/tests/QPTest.cpp index 8424947..1e9fd73 100644 --- a/tests/QPTest.cpp +++ b/tests/QPTest.cpp @@ -14,11 +14,11 @@ TEST_CASE("QPProblem - Unconstrained") { constexpr double tolerance = 1e-4; - Eigen::SparseMatrix H_s(2,2); - H_s.insert(0,0) = 3; - H_s.insert(0,1) = 2; - H_s.insert(1,0) = 2; - H_s.insert(1,1) = 4; + Eigen::SparseMatrix H_s(2, 2); + H_s.insert(0, 0) = 3; + H_s.insert(0, 1) = 2; + H_s.insert(1, 0) = 2; + H_s.insert(1, 1) = 4; Eigen::Matrix gradient; gradient << 3, 1; @@ -38,27 +38,26 @@ TEST_CASE("QPProblem - Unconstrained") // expected solution Eigen::Matrix expectedSolution; - expectedSolution << -1.2500, 0.3750; + expectedSolution << -1.2500, 0.3750; REQUIRE(solver.getSolution().isApprox(expectedSolution, tolerance)); } - TEST_CASE("QPProblem") { constexpr double tolerance = 1e-4; - Eigen::SparseMatrix H_s(2,2); - H_s.insert(0,0) = 4; - H_s.insert(0,1) = 1; - H_s.insert(1,0) = 1; - H_s.insert(1,1) = 2; + Eigen::SparseMatrix H_s(2, 2); + H_s.insert(0, 0) = 4; + H_s.insert(0, 1) = 1; + H_s.insert(1, 0) = 1; + H_s.insert(1, 1) = 2; - Eigen::SparseMatrix A_s(3,2); - A_s.insert(0,0) = 1; - A_s.insert(0,1) = 1; - A_s.insert(1,0) = 1; - A_s.insert(2,1) = 1; + Eigen::SparseMatrix A_s(3, 2); + A_s.insert(0, 0) = 1; + A_s.insert(0, 1) = 1; + A_s.insert(1, 0) = 1; + A_s.insert(2, 1) = 1; Eigen::Matrix gradient; gradient << 1, 1; @@ -87,7 +86,7 @@ TEST_CASE("QPProblem") REQUIRE(solver.solveProblem() == OsqpEigen::ErrorExitFlag::NoError); Eigen::Matrix expectedSolution; - expectedSolution << 0.3, 0.7; + expectedSolution << 0.3, 0.7; REQUIRE(solver.getSolution().isApprox(expectedSolution, tolerance)); } diff --git a/tests/SparseMatrixTest.cpp b/tests/SparseMatrixTest.cpp index 0195445..327d75f 100644 --- a/tests/SparseMatrixTest.cpp +++ b/tests/SparseMatrixTest.cpp @@ -11,45 +11,47 @@ #include #include -template -bool computeTest(const Eigen::Matrix &mEigen) +template bool computeTest(const Eigen::Matrix& mEigen) { Eigen::SparseMatrix matrix, newMatrix, newMatrixFromCSR; matrix = mEigen.sparseView(); csc* osqpSparseMatrix = nullptr; - //NOTE: Dynamic memory allocation - if(!OsqpEigen::SparseMatrixHelper::createOsqpSparseMatrix(matrix, osqpSparseMatrix)) + // NOTE: Dynamic memory allocation + if (!OsqpEigen::SparseMatrixHelper::createOsqpSparseMatrix(matrix, osqpSparseMatrix)) return false; Eigen::SparseMatrix csrMatrix; csrMatrix = matrix; csc* otherOsqpSparseMatrix = nullptr; - if(!OsqpEigen::SparseMatrixHelper::createOsqpSparseMatrix(csrMatrix, otherOsqpSparseMatrix)) + if (!OsqpEigen::SparseMatrixHelper::createOsqpSparseMatrix(csrMatrix, otherOsqpSparseMatrix)) return false; - if(!OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToEigenSparseMatrix(osqpSparseMatrix, newMatrix)) + if (!OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToEigenSparseMatrix(osqpSparseMatrix, + newMatrix)) return false; - if(!OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToEigenSparseMatrix(otherOsqpSparseMatrix, newMatrixFromCSR)) + if (!OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToEigenSparseMatrix(otherOsqpSparseMatrix, + newMatrixFromCSR)) return false; if (!newMatrixFromCSR.isApprox(newMatrix)) return false; std::vector> tripletListCsc; - if(!OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToTriplets(osqpSparseMatrix, tripletListCsc)) + if (!OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToTriplets(osqpSparseMatrix, + tripletListCsc)) return false; - for(const auto& a: tripletListCsc) - std::cout << a.row() << " " <> tripletListEigen; OsqpEigen::SparseMatrixHelper::eigenSparseMatrixToTriplets(matrix, tripletListEigen); std::cout << "***********************************************" << std::endl; - for(const auto& a: tripletListEigen) - std::cout << a.row() << " " < &mEigen) csc_spfree(osqpSparseMatrix); csc_spfree(otherOsqpSparseMatrix); - return outcome; } @@ -67,9 +68,7 @@ TEST_CASE("SparseMatrix") { Eigen::Matrix3d m; - m << 0, 1.002311, 0, - 0, 0, 0, - 0, 0.90835435,0; + m << 0, 1.002311, 0, 0, 0, 0, 0, 0.90835435, 0; REQUIRE(computeTest(m)); } @@ -77,9 +76,7 @@ TEST_CASE("SparseMatrix") SECTION("Data type - float") { Eigen::Matrix3f m; - m << 0, 1, 0, - 0, 0, 0, - 0, 1,0; + m << 0, 1, 0, 0, 0, 0, 0, 1, 0; REQUIRE(computeTest(m)); } @@ -88,18 +85,15 @@ TEST_CASE("SparseMatrix") { Eigen::Matrix3i m; - m << 0, 1, 0, - 0, 0, 0, - 0, 1,0; + m << 0, 1, 0, 0, 0, 0, 0, 1, 0; REQUIRE(computeTest(m)); } SECTION("Data type - double") { - Eigen::Matrix m; - m << 0, 0, 0, 4, - 0, 0, 0, 0; + Eigen::Matrix m; + m << 0, 0, 0, 4, 0, 0, 0, 0; REQUIRE(computeTest(m)); } diff --git a/tests/UpdateMatricesTest.cpp b/tests/UpdateMatricesTest.cpp index 102a09b..a55e279 100644 --- a/tests/UpdateMatricesTest.cpp +++ b/tests/UpdateMatricesTest.cpp @@ -13,34 +13,31 @@ // colors #define ANSI_TXT_GRN "\033[0;32m" -#define ANSI_TXT_MGT "\033[0;35m" //Magenta -#define ANSI_TXT_DFT "\033[0;0m" //Console default +#define ANSI_TXT_MGT "\033[0;35m" // Magenta +#define ANSI_TXT_DFT "\033[0;0m" // Console default #define GTEST_BOX "[ cout ] " -#define COUT_GTEST ANSI_TXT_GRN << GTEST_BOX //You could add the Default +#define COUT_GTEST ANSI_TXT_GRN << GTEST_BOX // You could add the Default #define COUT_GTEST_MGT COUT_GTEST << ANSI_TXT_MGT Eigen::Matrix H; Eigen::SparseMatrix H_s; -Eigen::Matrix A; +Eigen::Matrix A; Eigen::SparseMatrix A_s; -Eigen::Matrix gradient; -Eigen::Matrix lowerBound; -Eigen::Matrix upperBound; +Eigen::Matrix gradient; +Eigen::Matrix lowerBound; +Eigen::Matrix upperBound; OsqpEigen::Solver solver; TEST_CASE("QPProblem - FirstRun") { // hessian matrix - H << 4, 0, - 0, 2; + H << 4, 0, 0, 2; H_s = H.sparseView(); H_s.pruned(0.01); // linear constraint matrix - A << 1, 1, - 1, 0, - 0, 1; + A << 1, 1, 1, 0, 0, 1; A_s = A.sparseView(); gradient << 1, 1; @@ -62,20 +59,16 @@ TEST_CASE("QPProblem - FirstRun") REQUIRE(solver.solveProblem() == OsqpEigen::ErrorExitFlag::NoError); auto solution = solver.getSolution(); - std::cout << COUT_GTEST_MGT << "Solution [" << solution(0) << " " - << solution(1) << "]" + std::cout << COUT_GTEST_MGT << "Solution [" << solution(0) << " " << solution(1) << "]" << ANSI_TXT_DFT << std::endl; } TEST_CASE("QPProblem - SparsityConstant") { // update hessian matrix - H << 4, 0, - 0, 2; + H << 4, 0, 0, 2; H_s = H.sparseView(); - A << 2, 1, - 1, 0, - 0, 1; + A << 2, 1, 1, 0, 0, 1; A_s = A.sparseView(); REQUIRE(solver.updateHessianMatrix(H_s)); @@ -83,20 +76,16 @@ TEST_CASE("QPProblem - SparsityConstant") REQUIRE(solver.solveProblem() == OsqpEigen::ErrorExitFlag::NoError); auto solution = solver.getSolution(); - std::cout << COUT_GTEST_MGT << "Solution [" << solution(0) << " " - << solution(1) << "]" + std::cout << COUT_GTEST_MGT << "Solution [" << solution(0) << " " << solution(1) << "]" << ANSI_TXT_DFT << std::endl; }; TEST_CASE("QPProblem - SparsityChange") { // update hessian matrix - H << 1, 1, - 1, 2; + H << 1, 1, 1, 2; H_s = H.sparseView(); - A << 1, 1, - 1, 0.4, - 0, 1; + A << 1, 1, 1, 0.4, 0, 1; A_s = A.sparseView(); REQUIRE(solver.updateHessianMatrix(H_s)); @@ -104,7 +93,6 @@ TEST_CASE("QPProblem - SparsityChange") REQUIRE(solver.solveProblem() == OsqpEigen::ErrorExitFlag::NoError); auto solution = solver.getSolution(); - std::cout << COUT_GTEST_MGT << "Solution [" << solution(0) << " " - << solution(1) << "]" + std::cout << COUT_GTEST_MGT << "Solution [" << solution(0) << " " << solution(1) << "]" << ANSI_TXT_DFT << std::endl; };