├── .bazelignore ├── .clang-format ├── .github └── workflows │ ├── ci.yml │ ├── gh-pages.yml │ ├── test-pixi.yaml │ └── update-pixi-lockfile.yaml ├── .gitignore ├── BUILD.bazel ├── CMakeLists.txt ├── LICENSE ├── MODULE.bazel ├── README.md ├── cmake ├── AddInstallRPATHSupport.cmake ├── AddOsqpEigenUnitTest.cmake ├── AddUninstallTarget.cmake ├── AddWarningsConfigurationToTargets.cmake ├── FindVALGRIND.cmake ├── InstallBasicPackageFiles.cmake ├── OsqpEigenDependencies.cmake ├── OsqpEigenFindOptionalDependencies.cmake ├── try-osqp-v1-final.cpp ├── try-osqp-v1.cpp └── valgrind-macos.supp ├── docs ├── Doxyfile ├── Doxyfile-mcss ├── conf.py ├── figures │ └── mpc_result.png └── pages │ └── mpc.md ├── example ├── CMakeLists.txt └── src │ └── MPCExample.cpp ├── include └── OsqpEigen │ ├── Compat.hpp │ ├── Constants.hpp │ ├── Data.hpp │ ├── Data.tpp │ ├── Debug.hpp │ ├── OsqpEigen.h │ ├── Settings.hpp │ ├── Solver.hpp │ ├── Solver.tpp │ ├── SparseMatrixHelper.hpp │ └── SparseMatrixHelper.tpp ├── package.xml ├── pixi.lock ├── pixi.toml ├── src ├── Data.cpp ├── Debug.cpp ├── Settings.cpp └── Solver.cpp └── tests ├── BUILD.bazel ├── CMakeLists.txt ├── MODULE.bazel ├── MPCTest.cpp ├── MPCUpdateMatricesTest.cpp ├── QPTest.cpp ├── SparseMatrixTest.cpp └── UpdateMatricesTest.cpp /.bazelignore: -------------------------------------------------------------------------------- 1 | # See https://github.com/prefix-dev/pixi/discussions/3581 2 | .pixi 3 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | # BasedOnStyle: WebKit 4 | AccessModifierOffset: -4 5 | AlignAfterOpenBracket: Align 6 | AlignConsecutiveAssignments: false 7 | AlignConsecutiveDeclarations: false 8 | AlignEscapedNewlines: Left 9 | AlignOperands: true 10 | AlignTrailingComments: false 11 | AllowAllParametersOfDeclarationOnNextLine: true 12 | AllowShortBlocksOnASingleLine: false 13 | AllowShortCaseLabelsOnASingleLine: false 14 | AllowShortFunctionsOnASingleLine: None 15 | AllowShortIfStatementsOnASingleLine: false 16 | AllowShortLoopsOnASingleLine: false 17 | AlwaysBreakAfterDefinitionReturnType: None 18 | AlwaysBreakAfterReturnType: None 19 | AlwaysBreakBeforeMultilineStrings: false 20 | AlwaysBreakTemplateDeclarations: false 21 | BinPackArguments: false 22 | BinPackParameters: false 23 | BraceWrapping: 24 | AfterClass: true 25 | AfterControlStatement: true 26 | AfterEnum: true 27 | AfterFunction: true 28 | AfterNamespace: true 29 | AfterObjCDeclaration: false 30 | AfterStruct: true 31 | AfterUnion: true 32 | BeforeCatch: false 33 | BeforeElse: false 34 | IndentBraces: false 35 | SplitEmptyFunction: true 36 | SplitEmptyRecord: true 37 | SplitEmptyNamespace: true 38 | BreakBeforeBinaryOperators: All 39 | BreakBeforeBraces: Custom 40 | BreakBeforeInheritanceComma: false 41 | BreakBeforeTernaryOperators: true 42 | BreakConstructorInitializersBeforeComma: true 43 | BreakConstructorInitializers: BeforeComma 44 | BreakAfterJavaFieldAnnotations: false 45 | BreakStringLiterals: true 46 | ColumnLimit: 100 47 | CommentPragmas: '^ IWYU pragma:' 48 | CompactNamespaces: false 49 | ConstructorInitializerAllOnOneLineOrOnePerLine: false 50 | ConstructorInitializerIndentWidth: 4 51 | ContinuationIndentWidth: 4 52 | Cpp11BracedListStyle: true 53 | DerivePointerAlignment: false 54 | DisableFormat: false 55 | ExperimentalAutoDetectBinPacking: false 56 | FixNamespaceComments: true 57 | ForEachMacros: 58 | - foreach 59 | - Q_FOREACH 60 | - BOOST_FOREACH 61 | IncludeCategories: 62 | - Regex: '^"(llvm|llvm-c|clang|clang-c)/' 63 | Priority: 2 64 | - Regex: '^(<|"(gtest|gmock|isl|json)/)' 65 | Priority: 3 66 | - Regex: '.*' 67 | Priority: 1 68 | IncludeIsMainRegex: '(Test)?$' 69 | IndentCaseLabels: false 70 | IndentWidth: 4 71 | IndentWrappedFunctionNames: false 72 | JavaScriptQuotes: Leave 73 | JavaScriptWrapImports: true 74 | KeepEmptyLinesAtTheStartOfBlocks: true 75 | MacroBlockBegin: '' 76 | MacroBlockEnd: '' 77 | MaxEmptyLinesToKeep: 1 78 | NamespaceIndentation: None 79 | ObjCBlockIndentWidth: 4 80 | ObjCSpaceAfterProperty: true 81 | ObjCSpaceBeforeProtocolList: true 82 | PenaltyBreakAssignment: 10 83 | PenaltyBreakBeforeFirstCallParameter: 1000 84 | PenaltyBreakComment: 10 85 | PenaltyBreakString: 10 86 | PenaltyExcessCharacter: 100 87 | PenaltyReturnTypeOnItsOwnLine: 5 88 | PointerAlignment: Left 89 | ReflowComments: true 90 | SortIncludes: true 91 | SortUsingDeclarations: true 92 | SpaceAfterCStyleCast: false 93 | SpaceAfterTemplateKeyword: true 94 | SpaceBeforeAssignmentOperators: true 95 | SpaceBeforeParens: ControlStatements 96 | SpaceInEmptyParentheses: false 97 | SpacesBeforeTrailingComments: 1 98 | SpacesInAngles: false 99 | SpacesInContainerLiterals: true 100 | SpacesInCStyleCastParentheses: false 101 | SpacesInParentheses: false 102 | SpacesInSquareBrackets: false 103 | Standard: Cpp11 104 | TabWidth: 4 105 | UseTab: Never 106 | -------------------------------------------------------------------------------- /.github/workflows/ci.yml: -------------------------------------------------------------------------------- 1 | name: C++ CI Workflow 2 | 3 | on: 4 | push: 5 | pull_request: 6 | schedule: 7 | # * is a special character in YAML so you have to quote this string 8 | # Execute a "nightly" build at 2 AM UTC 9 | - cron: '0 2 * * *' 10 | 11 | env: 12 | vcpkg_robotology_TAG: v0.0.3 13 | Catch2_TAG: v3.8.0 14 | # Overwrite the VCPKG_INSTALLATION_ROOT env variable defined by GitHub Actions to point to our vcpkg 15 | VCPKG_INSTALLATION_ROOT: C:\robotology\vcpkg 16 | 17 | # Test with different operating systems 18 | jobs: 19 | build: 20 | name: '[${{ matrix.os }}@${{ matrix.build_type }}@${{ matrix.osqp_TAG }}] [float:${{ matrix.float }}]' 21 | runs-on: ${{ matrix.os }} 22 | strategy: 23 | matrix: 24 | build_type: [Debug, Release] 25 | os: [ubuntu-latest, windows-latest] 26 | osqp_TAG: ["v0.6.3", "v1.0.0.beta1", "v1.0.0"] 27 | float: [ON, OFF] 28 | fail-fast: false 29 | 30 | # operating system dependences 31 | steps: 32 | - uses: actions/checkout@master 33 | 34 | # Print environment variables to simplify development and debugging 35 | - name: Environment Variables 36 | shell: bash 37 | run: env 38 | 39 | # ============ 40 | # DEPENDENCIES 41 | # ============ 42 | 43 | # Remove apt repos that are known to break from time to time 44 | # See https://github.com/actions/virtual-environments/issues/323 45 | - name: Remove broken apt repos [Ubuntu] 46 | if: matrix.os == 'ubuntu-latest' 47 | run: | 48 | for apt_file in `grep -lr microsoft /etc/apt/sources.list.d/`; do sudo rm $apt_file; done 49 | 50 | - name: Dependencies [Windows] 51 | if: matrix.os == 'windows-latest' 52 | run: | 53 | # To avoid spending a huge time compiling vcpkg dependencies, we download a root that comes precompiled with all the ports that we need 54 | choco install -y wget unzip 55 | # To avoid problems with non-relocatable packages, we unzip the archive exactly in the same C:/robotology/vcpkg 56 | # that has been used to create the pre-compiled archive 57 | cd C:/ 58 | md C:/robotology 59 | md C:/robotology/vcpkg 60 | wget https://github.com/robotology/robotology-superbuild-dependencies-vcpkg/releases/download/${env:vcpkg_robotology_TAG}/vcpkg-robotology.zip 61 | unzip vcpkg-robotology.zip -d C:/robotology/vcpkg 62 | 63 | # Install Catch2 64 | cd C:/robotology/vcpkg 65 | ./vcpkg.exe install --triplet x64-windows catch2 66 | 67 | - name: Dependencies [Ubuntu] 68 | if: matrix.os == 'ubuntu-latest' 69 | run: | 70 | sudo apt-get update 71 | sudo apt-get install git build-essential cmake libeigen3-dev valgrind 72 | 73 | - name: Cache Source-based Dependencies 74 | id: cache-source-deps 75 | uses: actions/cache@v4 76 | with: 77 | path: ${{ github.workspace }}/install/deps 78 | key: source-deps-${{ runner.os }}-${{ matrix.build_type }}-use-float-${{ matrix.float }}-vcpkg-robotology-${{ env.vcpkg_robotology_TAG }}-osqp-${{ matrix.osqp_TAG }}-catch2-${{ env.Catch2_TAG }} 79 | 80 | - name: Source-based Dependencies [Windows] 81 | if: steps.cache-source-deps.outputs.cache-hit != 'true' && matrix.os == 'windows-latest' 82 | shell: bash 83 | run: | 84 | # osqp 85 | cd ${GITHUB_WORKSPACE} 86 | git clone --recursive -b ${{ matrix.osqp_TAG }} https://github.com/oxfordcontrol/osqp 87 | cd osqp 88 | mkdir -p build 89 | cd build 90 | cmake -A x64 -DCMAKE_TOOLCHAIN_FILE=${VCPKG_INSTALLATION_ROOT}/scripts/buildsystems/vcpkg.cmake \ 91 | -DDFLOAT=${{ matrix.float }} \ 92 | -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps .. 93 | 94 | cmake --build . --config ${{ matrix.build_type }} --target INSTALL 95 | 96 | - name: Source-based Dependencies [Ubuntu] 97 | if: steps.cache-source-deps.outputs.cache-hit != 'true' && (matrix.os == 'ubuntu-latest') 98 | shell: bash 99 | run: | 100 | # osqp 101 | cd ${GITHUB_WORKSPACE} 102 | git clone --recursive -b ${{ matrix.osqp_TAG }} https://github.com/oxfordcontrol/osqp 103 | cd osqp 104 | mkdir -p build 105 | cd build 106 | cmake -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps \ 107 | -DDFLOAT=${{ matrix.float }} .. 108 | cmake --build . --config ${{ matrix.build_type }} --target install 109 | 110 | # catch 2 111 | git clone -b ${Catch2_TAG} https://github.com/catchorg/Catch2.git 112 | cd Catch2 113 | mkdir -p build 114 | cd build 115 | cmake -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install/deps \ 116 | -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps \ 117 | -DBUILD_TESTING=OFF .. 118 | cmake --build . --config ${{ matrix.build_type }} --target install 119 | 120 | # =================== 121 | # CMAKE-BASED PROJECT 122 | # =================== 123 | 124 | - name: Configure [Windows] 125 | if: matrix.os == 'windows-latest' 126 | shell: bash 127 | run: | 128 | mkdir -p build 129 | cd build 130 | cmake -A x64 -DCMAKE_TOOLCHAIN_FILE=${VCPKG_INSTALLATION_ROOT}/scripts/buildsystems/vcpkg.cmake \ 131 | -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install/deps \ 132 | -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install \ 133 | -DBUILD_TESTING:BOOL=ON .. 134 | 135 | - name: Configure [Ubuntu] 136 | if: matrix.os == 'ubuntu-latest' 137 | shell: bash 138 | run: | 139 | mkdir -p build 140 | cd build 141 | cmake -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install/deps \ 142 | -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install \ 143 | -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} \ 144 | -DBUILD_TESTING:BOOL=ON \ 145 | -DOSQPEIGEN_RUN_Valgrind_tests:BOOL=ON .. 146 | 147 | - name: Build 148 | shell: bash 149 | run: | 150 | cd build 151 | export PATH=$PATH:/d/a/osqp-eigen/osqp-eigen/install/bin:/d/a/osqp-eigen/osqp-eigen/install/deps/bin:/c/robotology/vcpkg/installed/x64-windows/bin:/c/robotology/vcpkg/installed/x64-windows/debug/bin 152 | cmake --build . --config ${{ matrix.build_type }} 153 | 154 | - name: Test 155 | shell: bash 156 | run: | 157 | cd build 158 | export PATH=$PATH:/d/a/osqp-eigen/osqp-eigen/install/bin:/d/a/osqp-eigen/osqp-eigen/install/deps/bin:/c/robotology/vcpkg/installed/x64-windows/bin:/c/robotology/vcpkg/installed/x64-windows/debug/bin 159 | ctest --output-on-failure -C ${{ matrix.build_type }} . 160 | -------------------------------------------------------------------------------- /.github/workflows/gh-pages.yml: -------------------------------------------------------------------------------- 1 | name: gh-pages 2 | on: 3 | push: 4 | branches: master 5 | 6 | env: 7 | mcss_TAG: 374ec55a6610c1856e7374aea7dc1535ed8b64f8 8 | 9 | jobs: 10 | build: 11 | runs-on: ubuntu-20.04 12 | steps: 13 | - uses: actions/checkout@master 14 | 15 | - name: Dependencies 16 | run: | 17 | sudo apt update 18 | sudo apt install -y xsltproc texlive ghostscript graphviz texlive-base texlive-latex-extra texlive-fonts-extra texlive-fonts-recommended flex bison 19 | git clone --depth 1 --branch Release_1_9_1 https://github.com/doxygen/doxygen.git 20 | cd doxygen && mkdir build && cd build 21 | cmake -G "Unix Makefiles" .. 22 | sudo make install 23 | 24 | - name: Fetch Python deps 25 | run: python3 -m pip install jinja2 Pygments docutils 26 | 27 | - name: Fetch m.css 28 | run: | 29 | cd ${GITHUB_WORKSPACE} 30 | git clone https://github.com/mosra/m.css.git 31 | cd m.css 32 | git checkout ${mcss_TAG} 33 | 34 | - name: Build docs 35 | run: | 36 | cd docs 37 | mkdir site 38 | python3 ${GITHUB_WORKSPACE}/m.css/documentation/doxygen.py conf.py 39 | 40 | - name: Archive artifacts 41 | uses: actions/upload-artifact@v4 42 | with: 43 | name: site 44 | path: docs/site 45 | 46 | deploy: 47 | runs-on: ubuntu-20.04 48 | needs: [build] 49 | steps: 50 | - name: Download artifacts 51 | uses: actions/download-artifact@v4.1.7 52 | with: 53 | name: site 54 | path: site 55 | - name: Deploy 56 | uses: JamesIves/github-pages-deploy-action@3.7.1 57 | with: 58 | GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} 59 | BRANCH: gh-pages 60 | FOLDER: site 61 | -------------------------------------------------------------------------------- /.github/workflows/test-pixi.yaml: -------------------------------------------------------------------------------- 1 | name: Run tests with pixi 2 | 3 | on: 4 | # on demand 5 | workflow_dispatch: 6 | inputs: 7 | delete_pixi_lock: 8 | description: 'If true, delete pixi.lock, to test against the latest version of dependencies.' 9 | required: true 10 | default: 'false' 11 | pull_request: 12 | schedule: 13 | # * is a special character in YAML so you have to quote this string 14 | # Execute a "nightly" build twice a week 2 AM UTC 15 | - cron: '0 2 * * 2,5' 16 | 17 | jobs: 18 | pixi-test: 19 | name: '[pixi:${{ matrix.pixi_task }}:${{ matrix.os }}]' 20 | runs-on: ${{ matrix.os }} 21 | strategy: 22 | fail-fast: false 23 | matrix: 24 | os: [ 25 | ubuntu-24.04, 26 | ubuntu-24.04-arm, 27 | macos-latest, 28 | windows-2019 29 | ] 30 | pixi_task: [ 31 | test, 32 | test-bazel, 33 | test-bazel-ext-deps 34 | ] 35 | exclude: 36 | # https://github.com/ami-iit/bazel-cmake-deps-override does not support Windows at the moment 37 | - os: windows-2019 38 | pixi_task: test-bazel-ext-deps 39 | 40 | steps: 41 | - uses: actions/checkout@v4 42 | 43 | # On periodic jobs and when delete_pixi_lock option is true, delete the pixi.lock to check that the project compiles with latest version of dependencies 44 | - name: Delete pixi.lock on scheduled jobs or if delete_pixi_lock is true 45 | if: github.event_name == 'schedule' || (github.event_name == 'workflow_dispatch' && github.event.inputs.delete_pixi_lock == 'true') 46 | shell: bash 47 | run: | 48 | rm pixi.lock 49 | 50 | # To use valgrind even with conda/pixi we still need to install libc6-dbg via apt on Debian-based distros 51 | # See https://github.com/robotology/osqp-eigen/pull/171#issuecomment-2458149581 52 | - name: Install libc6-dbg 53 | if: contains(matrix.os, 'ubuntu') 54 | run: | 55 | sudo apt-get install libc6-dbg 56 | 57 | - name: Set up pixi 58 | uses: prefix-dev/setup-pixi@v0.8.1 59 | 60 | - name: Print pixi info 61 | run: pixi info 62 | 63 | - name: Build and test the project 64 | run: pixi run ${{ matrix.pixi_task }} 65 | 66 | # On tasks that used bazel, stop bazel to avoid permission errors while deleting the workspace 67 | - name: Shutdown bazel 68 | if: contains(matrix.pixi_task, 'bazel') 69 | run: | 70 | pixi run -e bazel bazel clean --expunge 71 | -------------------------------------------------------------------------------- /.github/workflows/update-pixi-lockfile.yaml: -------------------------------------------------------------------------------- 1 | name: Update lockfiles 2 | 3 | permissions: 4 | contents: write 5 | pull-requests: write 6 | 7 | on: 8 | workflow_dispatch: 9 | schedule: 10 | - cron: 0 5 1 * * 11 | 12 | jobs: 13 | pixi-update: 14 | runs-on: ubuntu-22.04 15 | steps: 16 | - uses: actions/checkout@v4 17 | 18 | - name: Set up pixi 19 | uses: prefix-dev/setup-pixi@v0.8.1 20 | with: 21 | run-install: false 22 | 23 | - name: Install pixi-diff-to-markdown 24 | run: pixi global install pixi-diff-to-markdown 25 | 26 | - name: Update lockfiles 27 | run: | 28 | set -o pipefail 29 | pixi update --json | pixi exec pixi-diff-to-markdown >> diff.md 30 | 31 | - name: Create pull request 32 | uses: peter-evans/create-pull-request@v6 33 | with: 34 | token: ${{ secrets.GITHUB_TOKEN }} 35 | commit-message: Update pixi lockfile 36 | title: Update pixi lockfile 37 | body-path: diff.md 38 | branch: update-pixi 39 | base: main 40 | labels: pixi 41 | delete-branch: true 42 | add-paths: pixi.lock 43 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | -*- mode: gitignore; -*- 2 | *~ 3 | \#*\# 4 | /.emacs.desktop 5 | /.emacs.desktop.lock 6 | *.elc 7 | auto-save-list 8 | tramp 9 | .\#* 10 | 11 | #build 12 | /build/ 13 | *.user 14 | .build 15 | .pixi 16 | 17 | # Ignore all bazel-* symlinks. There is no full list since this can change 18 | # based on the name of the directory bazel is cloned into. 19 | /bazel-* 20 | 21 | # For now, we do not commit the MODULE.bazel.lock, as bazel is not the main build system 22 | # used by the developers of the library 23 | *.bazel.lock 24 | -------------------------------------------------------------------------------- /BUILD.bazel: -------------------------------------------------------------------------------- 1 | load("@rules_cc//cc:defs.bzl", "cc_library") 2 | 3 | cc_library( 4 | name = "osqp-eigen", 5 | srcs = glob(["src/**/*.cpp"]), 6 | hdrs = glob( 7 | [ 8 | "include/**/*.hpp", 9 | "include/**/*.h", 10 | "include/**/*.tpp", 11 | ], 12 | ), 13 | defines = [ 14 | "OSQP_EIGEN_OSQP_IS_V1", 15 | "OSQP_EIGEN_OSQP_IS_V1_FINAL", 16 | ], 17 | includes = ["include"], 18 | visibility = ["//visibility:public"], 19 | deps = [ 20 | "@eigen", 21 | "@osqp", 22 | ], 23 | ) 24 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Authors: Giulio Romualdi 2 | # CopyPolicy: Released under the terms of the BSD 3-clause license 3 | 4 | # Set cmake minimum version 5 | cmake_minimum_required(VERSION 3.8...3.30) 6 | 7 | # Extract version numbers from package.xml 8 | # Based on code by DART (BSD-2-license) 9 | # Cf. https://github.com/dartsim/dart/blob/f07186fbdb4e89e5340abb9718d02715bb315922/CMakeLists.txt#L62-L68 10 | file(READ package.xml PACKAGE_XML) 11 | string(REGEX MATCH "[0-9]+\\.[0-9]+\\.[0-9]+" DIRTY_VERSION_STRING ${PACKAGE_XML}) 12 | string(REGEX REPLACE "^([0-9]+)\\.([0-9]+)\\.([0-9]+)$" "\\1" OsqpEigen_MAJOR_VERSION "${DIRTY_VERSION_STRING}") 13 | string(REGEX REPLACE "^([0-9]+)\\.([0-9]+)\\.([0-9]+)$" "\\2" OsqpEigen_MINOR_VERSION "${DIRTY_VERSION_STRING}") 14 | string(REGEX REPLACE "^([0-9]+)\\.([0-9]+)\\.([0-9]+)$" "\\3" OsqpEigen_PATCH_VERSION "${DIRTY_VERSION_STRING}") 15 | set(OsqpEigen_VERSION "${OsqpEigen_MAJOR_VERSION}.${OsqpEigen_MINOR_VERSION}.${OsqpEigen_PATCH_VERSION}") 16 | 17 | # Set project version 18 | project(OsqpEigen VERSION ${OsqpEigen_VERSION}) 19 | 20 | # output paths 21 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_BINDIR}") 22 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}") 23 | set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}") 24 | 25 | # Build shared libs 26 | set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) 27 | 28 | if(MSVC) 29 | set(CMAKE_DEBUG_POSTFIX "d") 30 | endif() 31 | 32 | list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) 33 | 34 | option(BUILD_SHARED_LIBS "Build libraries as shared as opposed to static" ON) 35 | 36 | # Disable C and C++ compiler extensions. 37 | # C/CXX_EXTENSIONS are ON by default to allow the compilers to use extended 38 | # variants of the C/CXX language. 39 | # However, this could expose cross-platform bugs in user code or in the headers 40 | # of third-party dependencies and thus it is strongly suggested to turn 41 | # extensions off. 42 | set(CMAKE_C_EXTENSIONS OFF) 43 | set(CMAKE_CXX_EXTENSIONS OFF) 44 | 45 | # add GNU dirs 46 | include(GNUInstallDirs) 47 | 48 | # include macros for warnings 49 | include(AddWarningsConfigurationToTargets) 50 | 51 | include(CMakePackageConfigHelpers) 52 | 53 | option(ENABLE_RPATH "Enable RPATH for this library" ON) 54 | mark_as_advanced(ENABLE_RPATH) 55 | include(AddInstallRPATHSupport) 56 | add_install_rpath_support(BIN_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_BINDIR}" 57 | LIB_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}" 58 | DEPENDS ENABLE_RPATH 59 | USE_LINK_PATH) 60 | 61 | # Encourage user to specify a build type (e.g. Release, Debug, etc.), otherwise set it to Release. 62 | if(NOT CMAKE_CONFIGURATION_TYPES) 63 | if(NOT CMAKE_BUILD_TYPE) 64 | message(STATUS "Setting build type to 'Release' as none was specified.") 65 | set_property(CACHE CMAKE_BUILD_TYPE PROPERTY VALUE "Release") 66 | endif() 67 | endif() 68 | 69 | option(BUILD_TESTING "Create tests using CMake" OFF) 70 | include(CTest) 71 | 72 | option(OSQP_EIGEN_DEBUG_OUTPUT "Print debug error messages to cerr" ON) 73 | 74 | # Check OsqpEigen dependencies, find necessary libraries. 75 | include(OsqpEigenDependencies) 76 | 77 | # Set default build type to "Release" in single-config generators 78 | if(NOT CMAKE_CONFIGURATION_TYPES) 79 | if(NOT CMAKE_BUILD_TYPE) 80 | set(CMAKE_BUILD_TYPE "Release" CACHE STRING 81 | "Choose the type of build, recommended options are: Debug or Release" FORCE) 82 | endif() 83 | set(OSQPEIGEN_BUILD_TYPES "Debug" "Release" "MinSizeRel" "RelWithDebInfo") 84 | set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS ${OSQPEIGEN_BUILD_TYPES}) 85 | endif() 86 | 87 | set(LIBRARY_TARGET_NAME OsqpEigen) 88 | 89 | # List of CPP (source) library files. 90 | set(${LIBRARY_TARGET_NAME}_SRC 91 | src/Data.cpp 92 | src/Settings.cpp 93 | src/Solver.cpp 94 | src/Debug.cpp) 95 | 96 | set(${LIBRARY_TARGET_NAME}_HDR 97 | include/OsqpEigen/OsqpEigen.h 98 | include/OsqpEigen/Constants.hpp 99 | include/OsqpEigen/SparseMatrixHelper.hpp 100 | include/OsqpEigen/SparseMatrixHelper.tpp 101 | include/OsqpEigen/Data.hpp 102 | include/OsqpEigen/Data.tpp 103 | include/OsqpEigen/Settings.hpp 104 | include/OsqpEigen/Solver.hpp 105 | include/OsqpEigen/Solver.tpp 106 | include/OsqpEigen/Compat.hpp 107 | include/OsqpEigen/Debug.hpp) 108 | 109 | add_library(${LIBRARY_TARGET_NAME} ${${LIBRARY_TARGET_NAME}_SRC} ${${LIBRARY_TARGET_NAME}_HDR}) 110 | target_include_directories(${LIBRARY_TARGET_NAME} PUBLIC "$" 111 | "$") 112 | 113 | ADD_WARNINGS_CONFIGURATION_TO_TARGETS(PRIVATE TARGETS ${LIBRARY_TARGET_NAME}) 114 | 115 | target_link_libraries(${LIBRARY_TARGET_NAME} PUBLIC osqp::osqp Eigen3::Eigen) 116 | if(OSQP_IS_V1) 117 | target_compile_definitions(${LIBRARY_TARGET_NAME} PUBLIC OSQP_EIGEN_OSQP_IS_V1) 118 | endif() 119 | if(OSQP_IS_V1_FINAL) 120 | target_compile_definitions(${LIBRARY_TARGET_NAME} PUBLIC OSQP_EIGEN_OSQP_IS_V1_FINAL) 121 | endif() 122 | 123 | add_library(OsqpEigen::OsqpEigen ALIAS OsqpEigen) 124 | 125 | set_target_properties(${LIBRARY_TARGET_NAME} PROPERTIES 126 | VERSION ${PROJECT_VERSION} 127 | PUBLIC_HEADER "${${LIBRARY_TARGET_NAME}_HDR}") 128 | 129 | target_compile_features(${LIBRARY_TARGET_NAME} PUBLIC cxx_std_14) 130 | 131 | if(OSQP_EIGEN_DEBUG_OUTPUT) 132 | target_compile_definitions(${LIBRARY_TARGET_NAME} PRIVATE "OSQP_EIGEN_DEBUG_OUTPUT") 133 | endif() 134 | 135 | # List exported CMake package dependencies 136 | set(OSQP_EIGEN_EXPORTED_DEPENDENCIES "") 137 | list(APPEND OSQP_EIGEN_EXPORTED_DEPENDENCIES osqp "Eigen3 CONFIG") 138 | 139 | install(TARGETS ${LIBRARY_TARGET_NAME} 140 | EXPORT ${PROJECT_NAME} 141 | COMPONENT runtime 142 | LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT shlib 143 | ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT lib 144 | RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT bin 145 | PUBLIC_HEADER DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/OsqpEigen") 146 | 147 | include(InstallBasicPackageFiles) 148 | install_basic_package_files(${PROJECT_NAME} 149 | NAMESPACE OsqpEigen:: 150 | VERSION ${${PROJECT_NAME}_VERSION} 151 | COMPATIBILITY SameMajorVersion 152 | VARS_PREFIX ${PROJECT_NAME} 153 | NO_CHECK_REQUIRED_COMPONENTS_MACRO 154 | DEPENDENCIES ${OSQP_EIGEN_EXPORTED_DEPENDENCIES}) 155 | 156 | # Install package.xml to share 157 | install(FILES package.xml DESTINATION share/cmake/osqp-eigen) 158 | 159 | ## Testing 160 | include(AddOsqpEigenUnitTest) 161 | add_subdirectory(tests) 162 | 163 | include(AddUninstallTarget) 164 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2022, Fondazione Istituto Italiano di Tecnologia 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /MODULE.bazel: -------------------------------------------------------------------------------- 1 | module( 2 | name = "osqp-eigen", 3 | version = "0.10.0", 4 | bazel_compatibility = [">=7.2.1"], 5 | compatibility_level = 1, 6 | ) 7 | 8 | # This dependency can only be used from bazel-central-registry 9 | bazel_dep(name = "rules_cc", version = "0.1.1") 10 | 11 | # These depenencies can be overriden using conda ones 12 | bazel_dep(name = "eigen", version = "3.4.0.bcr.2") 13 | bazel_dep(name = "osqp", version = "1.0.0") 14 | bazel_dep(name = "catch2", version = "3.8.0") 15 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # osqp-eigen 2 | 3 | | General | [![c++14](https://img.shields.io/badge/standard-C++14-blue.svg?style=flat&logo=c%2B%2B)](https://isocpp.org) [![License](https://img.shields.io/badge/License-BSD_3--Clause-orange.svg)](https://github.com/robotology/osqp-eigen/blob/master/LICENSE) | 4 | | :-------: | :----------------------------------------------------------: | 5 | | **CI/CD** | [![Codacy Badge](https://app.codacy.com/project/badge/Grade/a18710c10f1c4df19bc2759fd50e9cf5)](https://www.codacy.com/gh/robotology/osqp-eigen/dashboard?utm_source=github.com&utm_medium=referral&utm_content=robotology/osqp-eigen&utm_campaign=Badge_Grade) [![CI](https://github.com/robotology/osqp-eigen/workflows/C++%20CI%20Workflow/badge.svg)](https://github.com/robotology/osqp-eigen/workflows/C++%20CI%20Workflow/badge.svg) [![Azure](https://dev.azure.com/conda-forge/feedstock-builds/_apis/build/status/osqp-eigen-feedstock?branchName=master)](https://dev.azure.com/conda-forge/feedstock-builds/_build/results?buildId=341091&view=results) | 6 | | **conda** | [![Conda Recipe](https://img.shields.io/badge/recipe-osqp--eigen-green.svg)](https://anaconda.org/conda-forge/osqp-eigen) [![Conda Downloads](https://img.shields.io/conda/dn/conda-forge/osqp-eigen.svg)](https://anaconda.org/conda-forge/osqp-eigen) [![Conda Version](https://img.shields.io/conda/vn/conda-forge/osqp-eigen.svg)](https://anaconda.org/conda-forge/osqp-eigen) [![Conda Platforms](https://img.shields.io/conda/pn/conda-forge/osqp-eigen.svg)](https://anaconda.org/conda-forge/osqp-eigen) | 7 | 8 | 9 | 10 | Simple C++ wrapper for [osqp](http://osqp.readthedocs.io/en/latest/index.html) library. 11 | 12 | ## 📚 Documentation 13 | The documentation is available online at the accompanying [website](https://robotology.github.io/osqp-eigen). 14 | 15 | 16 | ## 📄 Dependences 17 | The project depends only on [`osqp`](http://osqp.readthedocs.io/en/latest/index.html) and [Eigen3](http://eigen.tuxfamily.org/index.php?title=Main_Page). Please install [Catch2](https://github.com/catchorg/Catch2) if you want to run the tests only for testing. 18 | 19 | ## 🛠️ Usage 20 | 21 | ### 📦 Install with conda (recommended) 22 | You can easily the library with [`conda`](https://github.com/conda-forge/osqp-eigen-feedstock) using the following command 23 | ``` 24 | conda install -c conda-forge osqp-eigen 25 | ``` 26 | `conda` will automatically install [`osqp`](http://osqp.readthedocs.io/en/latest/index.html) and [Eigen3](http://eigen.tuxfamily.org/index.php?title=Main_Page). 27 | 28 | ### ⚙️ Build from source (advanced) 29 | 30 | 1. Clone the repository 31 | ``` 32 | git clone https://github.com/robotology/osqp-eigen.git 33 | ``` 34 | 2. Build it 35 | ``` 36 | cd osqp-eigen 37 | mkdir build 38 | cd build 39 | cmake -DCMAKE_INSTALL_PREFIX:PATH= ../ 40 | make 41 | make install 42 | ``` 43 | 3. Add the following environmental variable 44 | ``` 45 | OsqpEigen_DIR=/path/where/you/installed/ 46 | ``` 47 | 48 | ## 🖥️ How to use the library 49 | 50 | **osqp-eigen** provides native `CMake` support which allows the library to be easily used in `CMake` projects. 51 | **osqp-eigen** exports a CMake target called `OsqpEigen::OsqpEigen` which can be imported using the `find_package` CMake command and used by calling `target_link_libraries` as in the following example: 52 | ```cmake 53 | cmake_minimum_required(VERSION 3.0) 54 | project(myproject) 55 | find_package(OsqpEigen REQUIRED) 56 | add_executable(example example.cpp) 57 | target_link_libraries(example OsqpEigen::OsqpEigen) 58 | ``` 59 | 60 | 61 | If you prefer to use the [`bazel`](https://bazel.build/) build system, **osqp-eigen** is available in the Bazel Central Registry, so you can use it following the docs available at [`https://registry.bazel.build/modules/osqp-eigen`](https://registry.bazel.build/modules/osqp-eigen). 62 | 63 | ## 🐛 Bug reports and support 64 | All types of [issues](https://github.com/robotology/osqp-eigen/issues/new) are welcome. 65 | 66 | ## 📝 License 67 | Materials in this repository are distributed under the following license: 68 | 69 | > All software is licensed under the BSD 3-Clause License. See [LICENSE](https://github.com/robotology/osqp-eigen/blob/master/LICENSE) file for details. 70 | -------------------------------------------------------------------------------- /cmake/AddInstallRPATHSupport.cmake: -------------------------------------------------------------------------------- 1 | #.rst: 2 | # AddInstallRPATHSupport 3 | # ---------------------- 4 | # 5 | # Add support to RPATH during installation to your project:: 6 | # 7 | # add_install_rpath_support([BIN_DIRS dir [dir]] 8 | # [LIB_DIRS dir [dir]] 9 | # [INSTALL_NAME_DIR [dir]] 10 | # [DEPENDS condition [condition]] 11 | # [USE_LINK_PATH]) 12 | # 13 | # Normally (depending on the platform) when you install a shared 14 | # library you can either specify its absolute path as the install name, 15 | # or leave just the library name itself. In the former case the library 16 | # will be correctly linked during run time by all executables and other 17 | # shared libraries, but it must not change its install location. This 18 | # is often the case for libraries installed in the system default 19 | # library directory (e.g. ``/usr/lib``). 20 | # In the latter case, instead, the library can be moved anywhere in the 21 | # file system but at run time the dynamic linker must be able to find 22 | # it. This is often accomplished by setting environmental variables 23 | # (i.e. ``LD_LIBRARY_PATH`` on Linux). 24 | # This procedure is usually not desirable for two main reasons: 25 | # 26 | # - by setting the variable you are changing the default behaviour 27 | # of the dynamic linker thus potentially breaking executables (not as 28 | # destructive as ``LD_PRELOAD``) 29 | # - the variable will be used only by applications spawned by the shell 30 | # and not by other processes. 31 | # 32 | # RPATH is aimed to solve the issues introduced by the second 33 | # installation method. Using run-path dependent libraries you can 34 | # create a directory structure containing executables and dependent 35 | # libraries that users can relocate without breaking it. 36 | # A run-path dependent library is a dependent library whose complete 37 | # install name is not known when the library is created. 38 | # Instead, the library specifies that the dynamic loader must resolve 39 | # the library’s install name when it loads the executable that depends 40 | # on the library. The executable or the other shared library will 41 | # hardcode in the binary itself the additional search directories 42 | # to be passed to the dynamic linker. This works great in conjunction 43 | # with relative paths. 44 | # This command will enable support to RPATH to your project. 45 | # It will enable the following things: 46 | # 47 | # - If the project builds shared libraries it will generate a run-path 48 | # enabled shared library, i.e. its install name will be resolved 49 | # only at run time. 50 | # - In all cases (building executables and/or shared libraries) 51 | # dependent shared libraries with RPATH support will be properly 52 | # 53 | # The command has the following parameters: 54 | # 55 | # Options: 56 | # - ``USE_LINK_PATH``: if passed the command will automatically adds to 57 | # the RPATH the path to all the dependent libraries. 58 | # 59 | # Arguments: 60 | # - ``BIN_DIRS`` list of directories when the targets (executable and 61 | # plugins) will be installed. 62 | # - ``LIB_DIRS`` list of directories to be added to the RPATH. These 63 | # directories will be added "relative" w.r.t. the ``BIN_DIRS`` and 64 | # ``LIB_DIRS``. 65 | # - ``INSTALL_NAME_DIR`` directory where the libraries will be installed. 66 | # This variable will be used only if ``CMAKE_SKIP_RPATH`` or 67 | # ``CMAKE_SKIP_INSTALL_RPATH`` is set to ``TRUE`` as it will set the 68 | # ``INSTALL_NAME_DIR`` on all targets 69 | # - ``DEPENDS`` list of conditions that should be ``TRUE`` to enable 70 | # RPATH, for example ``FOO; NOT BAR``. 71 | # 72 | # Note: see https://gitlab.kitware.com/cmake/cmake/issues/16589 for further 73 | # details. 74 | 75 | #======================================================================= 76 | # Copyright 2014 Istituto Italiano di Tecnologia (IIT) 77 | # @author Francesco Romano 78 | # 79 | # Distributed under the OSI-approved BSD License (the "License"); 80 | # see accompanying file Copyright.txt for details. 81 | # 82 | # This software is distributed WITHOUT ANY WARRANTY; without even the 83 | # implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 84 | # See the License for more information. 85 | #======================================================================= 86 | # (To distribute this file outside of CMake, substitute the full 87 | # License text for the above reference.) 88 | 89 | 90 | include(CMakeParseArguments) 91 | 92 | 93 | function(ADD_INSTALL_RPATH_SUPPORT) 94 | 95 | set(_options USE_LINK_PATH) 96 | set(_oneValueArgs INSTALL_NAME_DIR) 97 | set(_multiValueArgs BIN_DIRS 98 | LIB_DIRS 99 | DEPENDS) 100 | 101 | cmake_parse_arguments(_ARS "${_options}" 102 | "${_oneValueArgs}" 103 | "${_multiValueArgs}" 104 | "${ARGN}") 105 | 106 | # if either RPATH or INSTALL_RPATH is disabled 107 | # and the INSTALL_NAME_DIR variable is set, then hardcode the install name 108 | if(CMAKE_SKIP_RPATH OR CMAKE_SKIP_INSTALL_RPATH) 109 | if(DEFINED _ARS_INSTALL_NAME_DIR) 110 | set(CMAKE_INSTALL_NAME_DIR ${_ARS_INSTALL_NAME_DIR} PARENT_SCOPE) 111 | endif() 112 | endif() 113 | 114 | if (CMAKE_SKIP_RPATH OR (CMAKE_SKIP_INSTALL_RPATH AND CMAKE_SKIP_BUILD_RPATH)) 115 | return() 116 | endif() 117 | 118 | 119 | set(_rpath_available 1) 120 | if(DEFINED _ARS_DEPENDS) 121 | foreach(_dep ${_ARS_DEPENDS}) 122 | string(REGEX REPLACE " +" ";" _dep "${_dep}") 123 | if(NOT (${_dep})) 124 | set(_rpath_available 0) 125 | endif() 126 | endforeach() 127 | endif() 128 | 129 | if(_rpath_available) 130 | 131 | # Enable RPATH on OSX. 132 | set(CMAKE_MACOSX_RPATH TRUE PARENT_SCOPE) 133 | 134 | # Find system implicit lib directories 135 | set(_system_lib_dirs ${CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES}) 136 | if(EXISTS "/etc/debian_version") # is this a debian system ? 137 | if(CMAKE_LIBRARY_ARCHITECTURE) 138 | list(APPEND _system_lib_dirs "/lib/${CMAKE_LIBRARY_ARCHITECTURE}" 139 | "/usr/lib/${CMAKE_LIBRARY_ARCHITECTURE}") 140 | endif() 141 | endif() 142 | # This is relative RPATH for libraries built in the same project 143 | foreach(lib_dir ${_ARS_LIB_DIRS}) 144 | list(FIND _system_lib_dirs "${lib_dir}" isSystemDir) 145 | if("${isSystemDir}" STREQUAL "-1") 146 | foreach(bin_dir ${_ARS_LIB_DIRS} ${_ARS_BIN_DIRS}) 147 | file(RELATIVE_PATH _rel_path ${bin_dir} ${lib_dir}) 148 | if (${CMAKE_SYSTEM_NAME} MATCHES "Darwin") 149 | list(APPEND CMAKE_INSTALL_RPATH "@loader_path/${_rel_path}") 150 | else() 151 | list(APPEND CMAKE_INSTALL_RPATH "\$ORIGIN/${_rel_path}") 152 | endif() 153 | endforeach() 154 | endif() 155 | endforeach() 156 | if(NOT "${CMAKE_INSTALL_RPATH}" STREQUAL "") 157 | list(REMOVE_DUPLICATES CMAKE_INSTALL_RPATH) 158 | endif() 159 | set(CMAKE_INSTALL_RPATH ${CMAKE_INSTALL_RPATH} PARENT_SCOPE) 160 | 161 | # add the automatically determined parts of the RPATH 162 | # which point to directories outside the build tree to the install RPATH 163 | set(CMAKE_INSTALL_RPATH_USE_LINK_PATH ${_ARS_USE_LINK_PATH} PARENT_SCOPE) 164 | 165 | endif() 166 | 167 | endfunction() 168 | -------------------------------------------------------------------------------- /cmake/AddOsqpEigenUnitTest.cmake: -------------------------------------------------------------------------------- 1 | # Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved. 2 | # This software may be modified and distributed under the terms of the 3 | # GNU Lesser General Public License v2.1 or any later version. 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # BSD-3-Clause license. See the accompanying LICENSE file for details. 7 | 8 | osqpeigen_dependent_option(OSQPEIGEN_COMPILE_tests 9 | "Compile tests?" ON 10 | "OSQPEIGEN_HAS_Catch2;BUILD_TESTING" OFF) 11 | 12 | osqpeigen_dependent_option(OSQPEIGEN_RUN_Valgrind_tests 13 | "Run Valgrind tests?" OFF 14 | "OSQPEIGEN_COMPILE_tests;VALGRIND_FOUND" OFF) 15 | 16 | if (OSQPEIGEN_RUN_Valgrind_tests) 17 | set(CTEST_MEMORYCHECK_COMMAND ${VALGRIND_PROGRAM}) 18 | set(MEMORYCHECK_COMMAND ${VALGRIND_PROGRAM}) 19 | if (APPLE) 20 | set(MEMORYCHECK_SUPPRESSIONS "--suppressions=${PROJECT_SOURCE_DIR}/cmake/valgrind-macos.supp") 21 | else () 22 | set(MEMORYCHECK_SUPPRESSIONS "") 23 | endif () 24 | set(MEMORYCHECK_COMMAND_OPTIONS "--leak-check=full --error-exitcode=1 ${MEMORYCHECK_SUPPRESSIONS}" CACHE STRING "Options to pass to the memory checker") 25 | mark_as_advanced(MEMORYCHECK_COMMAND_OPTIONS) 26 | set(MEMCHECK_COMMAND_COMPLETE "${MEMORYCHECK_COMMAND} ${MEMORYCHECK_COMMAND_OPTIONS}") 27 | separate_arguments(MEMCHECK_COMMAND_COMPLETE) 28 | endif() 29 | 30 | function(add_osqpeigen_test) 31 | 32 | if(OSQPEIGEN_COMPILE_tests) 33 | 34 | set(options) 35 | set(oneValueArgs NAME) 36 | set(multiValueArgs SOURCES LINKS COMPILE_DEFINITIONS) 37 | 38 | set(prefix "osqp_eigen") 39 | 40 | cmake_parse_arguments(${prefix} 41 | "${options}" 42 | "${oneValueArgs}" 43 | "${multiValueArgs}" 44 | ${ARGN}) 45 | 46 | set(name ${${prefix}_NAME}) 47 | set(unit_test_files ${${prefix}_SOURCES}) 48 | 49 | set(targetname ${name}UnitTests) 50 | add_executable(${targetname} 51 | "${unit_test_files}") 52 | 53 | target_link_libraries(${targetname} PRIVATE Catch2::Catch2WithMain ${${prefix}_LINKS}) 54 | target_compile_definitions(${targetname} PRIVATE CATCH_CONFIG_FAST_COMPILE CATCH_CONFIG_DISABLE_MATCHERS) 55 | target_compile_features(${targetname} PUBLIC cxx_std_14) 56 | 57 | add_test(NAME ${targetname} COMMAND ${targetname}) 58 | target_compile_definitions(${targetname} PRIVATE ${${prefix}_COMPILE_DEFINITIONS}) 59 | 60 | if(OSQPEIGEN_RUN_Valgrind_tests) 61 | add_test(NAME memcheck_${targetname} COMMAND ${MEMCHECK_COMMAND_COMPLETE} $) 62 | endif() 63 | 64 | endif() 65 | 66 | endfunction() 67 | -------------------------------------------------------------------------------- /cmake/AddUninstallTarget.cmake: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: 2012-2021 Istituto Italiano di Tecnologia (IIT) 2 | # SPDX-FileCopyrightText: 2008-2013 Kitware Inc. 3 | # SPDX-License-Identifier: BSD-3-Clause 4 | 5 | #[=======================================================================[.rst: 6 | AddUninstallTarget 7 | ------------------ 8 | 9 | Add the "uninstall" target for your project:: 10 | 11 | include(AddUninstallTarget) 12 | 13 | 14 | will create a file ``cmake_uninstall.cmake`` in the build directory and add a 15 | custom target ``uninstall`` (or ``UNINSTALL`` on Visual Studio and Xcode) that 16 | will remove the files installed by your package (using 17 | ``install_manifest.txt``). 18 | See also 19 | https://gitlab.kitware.com/cmake/community/wikis/FAQ#can-i-do-make-uninstall-with-cmake 20 | 21 | The :module:`AddUninstallTarget` module must be included in your main 22 | ``CMakeLists.txt``. If included in a subdirectory it does nothing. 23 | This allows you to use it safely in your main ``CMakeLists.txt`` and include 24 | your project using ``add_subdirectory`` (for example when using it with 25 | :cmake:module:`FetchContent`). 26 | 27 | If the ``uninstall`` target already exists, the module does nothing. 28 | #]=======================================================================] 29 | 30 | 31 | # AddUninstallTarget works only when included in the main CMakeLists.txt 32 | if(NOT "${CMAKE_CURRENT_BINARY_DIR}" STREQUAL "${CMAKE_BINARY_DIR}") 33 | return() 34 | endif() 35 | 36 | # The name of the target is uppercase in MSVC and Xcode (for coherence with the 37 | # other standard targets) 38 | if("${CMAKE_GENERATOR}" MATCHES "^(Visual Studio|Xcode)") 39 | set(_uninstall "UNINSTALL") 40 | else() 41 | set(_uninstall "uninstall") 42 | endif() 43 | 44 | # If target is already defined don't do anything 45 | if(TARGET ${_uninstall}) 46 | return() 47 | endif() 48 | 49 | 50 | set(_filename cmake_uninstall.cmake) 51 | 52 | file(WRITE "${CMAKE_CURRENT_BINARY_DIR}/${_filename}" 53 | "if(NOT EXISTS \"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\") 54 | message(WARNING \"Cannot find install manifest: \\\"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\\\"\") 55 | return() 56 | endif() 57 | 58 | file(READ \"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\" files) 59 | string(STRIP \"\${files}\" files) 60 | string(REGEX REPLACE \"\\n\" \";\" files \"\${files}\") 61 | list(REVERSE files) 62 | foreach(file \${files}) 63 | if(IS_SYMLINK \"\$ENV{DESTDIR}\${file}\" OR EXISTS \"\$ENV{DESTDIR}\${file}\") 64 | message(STATUS \"Uninstalling: \$ENV{DESTDIR}\${file}\") 65 | execute_process( 66 | COMMAND \${CMAKE_COMMAND} -E remove \"\$ENV{DESTDIR}\${file}\" 67 | OUTPUT_VARIABLE rm_out 68 | RESULT_VARIABLE rm_retval) 69 | if(NOT \"\${rm_retval}\" EQUAL 0) 70 | message(FATAL_ERROR \"Problem when removing \\\"\$ENV{DESTDIR}\${file}\\\"\") 71 | endif() 72 | else() 73 | message(STATUS \"Not-found: \$ENV{DESTDIR}\${file}\") 74 | endif() 75 | endforeach(file) 76 | ") 77 | 78 | set(_desc "Uninstall the project...") 79 | if(CMAKE_GENERATOR STREQUAL "Unix Makefiles") 80 | set(_comment COMMAND \$\(CMAKE_COMMAND\) -E cmake_echo_color --switch=$\(COLOR\) --cyan "${_desc}") 81 | else() 82 | set(_comment COMMENT "${_desc}") 83 | endif() 84 | add_custom_target(${_uninstall} 85 | ${_comment} 86 | COMMAND ${CMAKE_COMMAND} -P ${_filename} 87 | USES_TERMINAL 88 | BYPRODUCTS uninstall_byproduct 89 | WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}") 90 | set_property(SOURCE uninstall_byproduct PROPERTY SYMBOLIC 1) 91 | 92 | set_property(TARGET ${_uninstall} PROPERTY FOLDER "CMakePredefinedTargets") 93 | -------------------------------------------------------------------------------- /cmake/AddWarningsConfigurationToTargets.cmake: -------------------------------------------------------------------------------- 1 | 2 | 3 | include(CMakeParseArguments) 4 | 5 | function(ADD_WARNINGS_CONFIGURATION_TO_TARGETS) 6 | 7 | set(_options PRIVATE 8 | PUBLIC 9 | INTERFACE) 10 | set(_oneValueArgs ) 11 | set(_multiValueArgs TARGETS) 12 | 13 | cmake_parse_arguments(_AWC2T "${_options}" 14 | "${_oneValueArgs}" 15 | "${_multiValueArgs}" 16 | "${ARGN}") 17 | 18 | # Check the kind of visibility requested 19 | set(VISIBILITIES) 20 | if (${_AWC2T_PRIVATE}) 21 | list(APPEND VISIBILITIES PRIVATE) 22 | endif() 23 | if (${_AWC2T_PUBLIC}) 24 | list(APPEND VISIBILITIES PUBLIC) 25 | endif() 26 | if (${_AWC2T_INTERFACE}) 27 | list(APPEND VISIBILITIES INTERFACE) 28 | endif() 29 | 30 | 31 | foreach(TARGET ${_AWC2T_TARGETS}) 32 | foreach(VISIBILITY ${VISIBILITIES}) 33 | #setting debug options 34 | set(COMPILE_OPTIONS "") 35 | if(MSVC) 36 | ### 37 | list(APPEND COMPILE_OPTIONS "/W3") 38 | else() 39 | ##Other systems 40 | if(${CMAKE_CXX_COMPILER_ID} MATCHES "Clang") 41 | list(APPEND COMPILE_OPTIONS "-Weverything") 42 | list(APPEND COMPILE_OPTIONS "-pedantic") 43 | list(APPEND COMPILE_OPTIONS "-Wnon-virtual-dtor") 44 | list(APPEND COMPILE_OPTIONS "-Woverloaded-virtual") 45 | list(APPEND COMPILE_OPTIONS "-Wc++11-extensions") 46 | #Remove some other warnings on paddings 47 | list(APPEND COMPILE_OPTIONS "-Wno-padded") 48 | list(APPEND COMPILE_OPTIONS "-Wno-cast-align") 49 | list(APPEND COMPILE_OPTIONS "-Wno-c++98-compat") 50 | 51 | elseif(${CMAKE_COMPILER_IS_GNUCC}) 52 | list(APPEND COMPILE_OPTIONS "-Wall") 53 | list(APPEND COMPILE_OPTIONS "-pedantic") 54 | list(APPEND COMPILE_OPTIONS "-Wextra") 55 | list(APPEND COMPILE_OPTIONS "-Woverloaded-virtual") 56 | list(APPEND COMPILE_OPTIONS "-Wconversion") 57 | endif() 58 | endif() 59 | 60 | target_compile_options(${TARGET} ${VISIBILITY} "$<$:${COMPILE_OPTIONS}>") 61 | 62 | endforeach() 63 | endforeach() 64 | 65 | endfunction() 66 | 67 | -------------------------------------------------------------------------------- /cmake/FindVALGRIND.cmake: -------------------------------------------------------------------------------- 1 | # Find Valgrind. 2 | # 3 | # This module defines: 4 | # VALGRIND_INCLUDE_DIR, where to find valgrind/memcheck.h, etc. 5 | # VALGRIND_PROGRAM, the valgrind executable. 6 | # VALGRIND_FOUND, If false, do not try to use valgrind. 7 | # 8 | # If you have valgrind installed in a non-standard place, you can define 9 | # VALGRIND_ROOT to tell cmake where it is. 10 | if (VALGRIND_FOUND) 11 | return() 12 | endif() 13 | 14 | find_path(VALGRIND_INCLUDE_DIR valgrind/memcheck.h 15 | /usr/include /usr/local/include ${VALGRIND_ROOT}/include) 16 | 17 | # if VALGRIND_ROOT is empty, we explicitly add /bin to the search 18 | # path, but this does not hurt... 19 | find_program(VALGRIND_PROGRAM NAMES valgrind PATH ${VALGRIND_ROOT}/bin) 20 | 21 | include(FindPackageHandleStandardArgs) 22 | find_package_handle_standard_args(VALGRIND DEFAULT_MSG 23 | VALGRIND_INCLUDE_DIR 24 | VALGRIND_PROGRAM) 25 | 26 | mark_as_advanced(VALGRIND_ROOT VALGRIND_INCLUDE_DIR VALGRIND_PROGRAM) 27 | -------------------------------------------------------------------------------- /cmake/OsqpEigenDependencies.cmake: -------------------------------------------------------------------------------- 1 | # Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. 2 | # This software may be modified and distributed under the terms of the 3 | # GNU Lesser General Public License v2.1 or any later version. 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # BSD-3-Clause license. See the accompanying LICENSE file for details. 7 | 8 | include(OsqpEigenFindOptionalDependencies) 9 | 10 | #--------------------------------------------- 11 | ## Required Dependencies 12 | find_package(Eigen3 3.2.92 REQUIRED) 13 | find_package(osqp REQUIRED) 14 | 15 | # OSQP_IS_V1 (and OSQP_EIGEN_OSQP_IS_V1) is defined for v1.0.0.beta1 and v1.0.0 (and later) 16 | if(NOT DEFINED OSQP_IS_V1) 17 | try_compile(OSQP_IS_V1 ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_LIST_DIR}/try-osqp-v1.cpp LINK_LIBRARIES osqp::osqp) 18 | endif() 19 | 20 | # OSQP_IS_V1 (and OSQP_EIGEN_OSQP_IS_V1) is defined only for v1.0.0 (and later) 21 | if(NOT DEFINED OSQP_IS_V1_FINAL) 22 | try_compile(OSQP_IS_V1_FINAL ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_LIST_DIR}/try-osqp-v1-final.cpp LINK_LIBRARIES osqp::osqp) 23 | endif() 24 | 25 | 26 | #--------------------------------------------- 27 | ## Optional Dependencies 28 | find_package(Catch2 3.0.1 QUIET) 29 | checkandset_optional_dependency(Catch2) 30 | 31 | find_package(VALGRIND QUIET) 32 | checkandset_optional_dependency(VALGRIND) 33 | -------------------------------------------------------------------------------- /cmake/OsqpEigenFindOptionalDependencies.cmake: -------------------------------------------------------------------------------- 1 | # Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. 2 | # This software may be modified and distributed under the terms of the 3 | # GNU Lesser General Public License v2.1 or any later version. 4 | # 5 | # This software may be modified and distributed under the terms of the 6 | # BSD-3-Clause license. See the accompanying LICENSE file for details. 7 | 8 | # This module checks if all the dependencies are installed and if the 9 | # dependencies to build some parts are satisfied. 10 | # For every dependency, it creates the following variables: 11 | # 12 | # OSQPEIGEN_USE_${Package}: Can be disabled by the user if he doesn't want to use that 13 | # dependency. 14 | # OSQPEIGEN_HAS_${Package}: Internal flag. It should be used to check if a part of 15 | # OSQPEIGEN should be built. It is on if OSQPEIGEN_USE_${Package} is 16 | # on and either the package was found or will be built. 17 | # OSQPEIGEN_BUILD_${Package}: Internal flag. Used to check if OSQPEIGEN has to build an 18 | # external package. 19 | # OSQPEIGEN_BUILD_DEPS_${Package}: Internal flag. Used to check if dependencies 20 | # required to build the package are available. 21 | # OSQPEIGEN_HAS_SYSTEM_${Package}: Internal flag. Used to check if the package is 22 | # available on the system. 23 | # OSQPEIGEN_USE_SYSTEM_${Package}: This flag is shown only for packages in the 24 | # extern folder that were also found on the system 25 | # (TRUE by default). If this flag is enabled, the 26 | # system installed library will be used instead of 27 | # the version shipped within the framework. 28 | 29 | 30 | include(CMakeDependentOption) 31 | 32 | # Check if a package is installed and set some cmake variables 33 | macro(checkandset_optional_dependency package) 34 | 35 | set(PREFIX "OSQPEIGEN") 36 | 37 | string(TOUPPER ${package} PKG) 38 | 39 | # OSQPEIGEN_HAS_SYSTEM_${package} 40 | if(${package}_FOUND OR ${PKG}_FOUND) 41 | set(${PREFIX}_HAS_SYSTEM_${package} TRUE) 42 | else() 43 | set(${PREFIX}_HAS_SYSTEM_${package} FALSE) 44 | endif() 45 | 46 | # OSQPEIGEN_USE_${package} 47 | cmake_dependent_option(${PREFIX}_USE_${package} "Use package ${package}" TRUE 48 | ${PREFIX}_HAS_SYSTEM_${package} FALSE) 49 | mark_as_advanced(${PREFIX}_USE_${package}) 50 | 51 | # OSQPEIGEN_USE_SYSTEM_${package} 52 | set(${PREFIX}_USE_SYSTEM_${package} ${${PREFIX}_USE_${package}} CACHE INTERNAL "Use system-installed ${package}, rather than a private copy (recommended)" FORCE) 53 | if(NOT "${package}" STREQUAL "${PKG}") 54 | unset(${PREFIX}_USE_SYSTEM_${PKG} CACHE) 55 | endif() 56 | 57 | # OSQPEIGEN_HAS_${package} 58 | if(${${PREFIX}_HAS_SYSTEM_${package}}) 59 | set(${PREFIX}_HAS_${package} ${${PREFIX}_USE_${package}}) 60 | else() 61 | set(${PREFIX}_HAS_${package} FALSE) 62 | endif() 63 | 64 | endmacro() 65 | 66 | macro(OSQPEIGEN_DEPENDENT_OPTION _option _doc _default _deps _force) 67 | 68 | if(DEFINED ${_option}) 69 | get_property(_option_strings_set CACHE ${_option} PROPERTY STRINGS SET) 70 | if(_option_strings_set) 71 | # If the user thinks he is smarter than the machine, he deserves an error 72 | get_property(_option_strings CACHE ${_option} PROPERTY STRINGS) 73 | list(GET _option_strings 0 _option_strings_first) 74 | string(REGEX REPLACE ".+\"(.+)\".+" "\\1" _option_strings_first "${_option_strings_first}") 75 | list(LENGTH _option_strings _option_strings_length) 76 | math(EXPR _option_strings_last_index "${_option_strings_length} - 1") 77 | list(GET _option_strings ${_option_strings_last_index} _option_strings_last) 78 | if("${${_option}}" STREQUAL "${_option_strings_last}") 79 | message(SEND_ERROR "That was a trick, you cannot outsmart me! I will never let you win! ${_option} stays OFF until I say so! \"${_option_strings_first}\" is needed to enable ${_option}. Now stop bothering me, and install your dependencies, if you really want to enable this option.") 80 | endif() 81 | unset(${_option} CACHE) 82 | endif() 83 | endif() 84 | 85 | cmake_dependent_option(${_option} "${_doc}" ${_default} "${_deps}" ${_force}) 86 | 87 | unset(_missing_deps) 88 | foreach(_dep ${_deps}) 89 | string(REGEX REPLACE " +" ";" _depx "${_dep}") 90 | if(NOT (${_depx})) 91 | list(APPEND _missing_deps "${_dep}") 92 | endif() 93 | endforeach() 94 | 95 | if(DEFINED _missing_deps) 96 | set(${_option}_disable_reason " (dependencies unsatisfied: \"${_missing_deps}\")") 97 | # Set a value that can be visualized on ccmake and on cmake-gui, but 98 | # still evaluates to false 99 | set(${_option} "OFF - Dependencies unsatisfied: '${_missing_deps}' - ${_option}-NOTFOUND" CACHE STRING "${_option_doc}" FORCE) 100 | string(REPLACE ";" "\;" _missing_deps "${_missing_deps}") 101 | set_property(CACHE ${_option} 102 | PROPERTY STRINGS "OFF - Dependencies unsatisfied: '${_missing_deps}' - ${_option}-NOTFOUND" 103 | "OFF - You can try as much as you want, but '${_missing_deps}' is needed to enable ${_option} - ${_option}-NOTFOUND" 104 | "OFF - Are you crazy or what? '${_missing_deps}' is needed to enable ${_option} - ${_option}-NOTFOUND" 105 | "OFF - Didn't I already tell you that '${_missing_deps}' is needed to enable ${_option}? - ${_option}-NOTFOUND" 106 | "OFF - Stop it! - ${_option}-NOTFOUND" 107 | "OFF - This is insane! Leave me alone! - ${_option}-NOTFOUND" 108 | "ON - All right, you win. The option is enabled. Are you happy now? You just broke the build.") 109 | # Set non-cache variable that will override the value in current scope 110 | # For parent scopes, the "-NOTFOUND ensures that the variable still 111 | # evaluates to false 112 | set(${_option} ${_force}) 113 | endif() 114 | 115 | endmacro() 116 | -------------------------------------------------------------------------------- /cmake/try-osqp-v1-final.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main() 4 | { 5 | // This function is only available in OSQP >= 1.0.0 6 | OSQPCscMatrix_set_data(nullptr, 0, 0, 0, nullptr, 0, 0); 7 | return 0; 8 | } 9 | -------------------------------------------------------------------------------- /cmake/try-osqp-v1.cpp: -------------------------------------------------------------------------------- 1 | // This file is only available on OSQP >= 1.0.0.beta1 2 | #include 3 | 4 | int main() 5 | { 6 | return 0; 7 | } 8 | -------------------------------------------------------------------------------- /cmake/valgrind-macos.supp: -------------------------------------------------------------------------------- 1 | { 2 | macOS-Sierra-10.12.5-Leak.1 3 | Memcheck:Leak 4 | match-leak-kinds: reachable 5 | fun:malloc_zone_malloc 6 | fun:NXCreateMapTableFromZone 7 | fun:NXCreateMapTableFromZone 8 | fun:_ZL18__sel_registerNamePKcii 9 | fun:sel_init 10 | fun:map_images_nolock 11 | fun:_ZN11objc_object21sidetable_retainCountEv 12 | fun:_ZN4dyldL18notifyBatchPartialE17dyld_image_statesbPFPKcS0_jPK15dyld_image_infoEbb 13 | fun:_ZN4dyld21registerObjCNotifiersEPFvjPKPKcPKPK11mach_headerEPFvS1_S6_ESC_ 14 | fun:_dyld_objc_notify_register 15 | fun:_objc_init 16 | fun:_os_object_init 17 | } 18 | { 19 | macOS-Sierra-10.12.5-Leak.2 20 | Memcheck:Leak 21 | match-leak-kinds: reachable 22 | fun:malloc_zone_malloc 23 | fun:NXCreateHashTableFromZone 24 | fun:NXCreateHashTable 25 | fun:NXCreateMapTableFromZone 26 | fun:NXCreateMapTableFromZone 27 | fun:_ZL18__sel_registerNamePKcii 28 | fun:sel_init 29 | fun:map_images_nolock 30 | fun:_ZN11objc_object21sidetable_retainCountEv 31 | fun:_ZN4dyldL18notifyBatchPartialE17dyld_image_statesbPFPKcS0_jPK15dyld_image_infoEbb 32 | fun:_ZN4dyld21registerObjCNotifiersEPFvjPKPKcPKPK11mach_headerEPFvS1_S6_ESC_ 33 | fun:_dyld_objc_notify_register 34 | } 35 | { 36 | macOS-Sierra-10.12.5-Leak.3 37 | Memcheck:Leak 38 | match-leak-kinds: reachable 39 | fun:malloc_zone_malloc 40 | fun:NXCreateHashTableFromZone 41 | fun:NXCreateHashTable 42 | fun:NXCreateMapTableFromZone 43 | fun:NXCreateMapTableFromZone 44 | fun:_ZL18__sel_registerNamePKcii 45 | fun:sel_init 46 | fun:map_images_nolock 47 | fun:_ZN11objc_object21sidetable_retainCountEv 48 | fun:_ZN4dyldL18notifyBatchPartialE17dyld_image_statesbPFPKcS0_jPK15dyld_image_infoEbb 49 | fun:_ZN4dyld21registerObjCNotifiersEPFvjPKPKcPKPK11mach_headerEPFvS1_S6_ESC_ 50 | fun:_dyld_objc_notify_register 51 | } 52 | { 53 | macOS-Sierra-10.12.5-Leak.4 54 | Memcheck:Leak 55 | match-leak-kinds: reachable 56 | fun:malloc 57 | fun:NXCreateHashTableFromZone 58 | fun:NXCreateHashTable 59 | fun:NXCreateMapTableFromZone 60 | fun:NXCreateMapTableFromZone 61 | fun:_ZL18__sel_registerNamePKcii 62 | fun:sel_init 63 | fun:map_images_nolock 64 | fun:_ZN11objc_object21sidetable_retainCountEv 65 | fun:_ZN4dyldL18notifyBatchPartialE17dyld_image_statesbPFPKcS0_jPK15dyld_image_infoEbb 66 | fun:_ZN4dyld21registerObjCNotifiersEPFvjPKPKcPKPK11mach_headerEPFvS1_S6_ESC_ 67 | fun:_dyld_objc_notify_register 68 | } 69 | { 70 | macOS-Sierra-10.12.5-Leak.5 71 | Memcheck:Leak 72 | match-leak-kinds: reachable 73 | fun:malloc 74 | fun:NXCreateMapTableFromZone 75 | fun:NXCreateMapTableFromZone 76 | fun:_ZL18__sel_registerNamePKcii 77 | fun:sel_init 78 | fun:map_images_nolock 79 | fun:_ZN11objc_object21sidetable_retainCountEv 80 | fun:_ZN4dyldL18notifyBatchPartialE17dyld_image_statesbPFPKcS0_jPK15dyld_image_infoEbb 81 | fun:_ZN4dyld21registerObjCNotifiersEPFvjPKPKcPKPK11mach_headerEPFvS1_S6_ESC_ 82 | fun:_dyld_objc_notify_register 83 | fun:_objc_init 84 | fun:_os_object_init 85 | } 86 | { 87 | macOS-Sierra-10.12.5-Leak.6 88 | Memcheck:Leak 89 | match-leak-kinds: reachable 90 | fun:malloc_zone_calloc 91 | fun:_NXHashRehashToCapacity 92 | fun:NXHashInsert 93 | fun:NXCreateHashTableFromZone 94 | fun:NXCreateHashTable 95 | fun:NXCreateMapTableFromZone 96 | fun:NXCreateMapTableFromZone 97 | fun:_ZL18__sel_registerNamePKcii 98 | fun:sel_init 99 | fun:map_images_nolock 100 | fun:_ZN11objc_object21sidetable_retainCountEv 101 | fun:_ZN4dyldL18notifyBatchPartialE17dyld_image_statesbPFPKcS0_jPK15dyld_image_infoEbb 102 | } 103 | { 104 | macOS-Sierra-10.12.5-Leak.7 105 | Memcheck:Leak 106 | match-leak-kinds: possible 107 | fun:calloc 108 | fun:map_images_nolock 109 | fun:_ZN11objc_object21sidetable_retainCountEv 110 | fun:_ZN4dyldL18notifyBatchPartialE17dyld_image_statesbPFPKcS0_jPK15dyld_image_infoEbb 111 | fun:_ZN4dyld21registerObjCNotifiersEPFvjPKPKcPKPK11mach_headerEPFvS1_S6_ESC_ 112 | fun:_dyld_objc_notify_register 113 | fun:_objc_init 114 | fun:_os_object_init 115 | fun:libdispatch_init 116 | fun:libSystem_initializer 117 | fun:_ZN16ImageLoaderMachO18doModInitFunctionsERKN11ImageLoader11LinkContextE 118 | fun:_ZN16ImageLoaderMachO16doInitializationERKN11ImageLoader11LinkContextE 119 | } 120 | 121 | { 122 | macOS-Sierra-10.12.5-reachable.1 123 | Memcheck:Leak 124 | match-leak-kinds: reachable 125 | fun:malloc_zone_calloc 126 | fun:_NXHashRehashToCapacity 127 | fun:NXHashInsert 128 | fun:NXCreateHashTableFromZone 129 | fun:NXCreateHashTable 130 | fun:NXCreateMapTableFromZone 131 | fun:NXCreateMapTableFromZone 132 | fun:_ZL18__sel_registerNamePKcii 133 | fun:sel_init 134 | fun:map_images_nolock 135 | fun:_ZN11objc_object21sidetable_retainCountEv 136 | fun:_ZN4dyldL18notifyBatchPartialE17dyld_image_statesbPFPKcS0_jPK15dyld_image_infoEbb 137 | } 138 | 139 | { 140 | macOS-Sierra-10.12.5-possible.1 141 | Memcheck:Leak 142 | match-leak-kinds: possible 143 | fun:calloc 144 | fun:map_images_nolock 145 | fun:_ZN11objc_object21sidetable_retainCountEv 146 | fun:_ZN4dyldL18notifyBatchPartialE17dyld_image_statesbPFPKcS0_jPK15dyld_image_infoEbb 147 | fun:_ZN4dyld21registerObjCNotifiersEPFvjPKPKcPKPK11mach_headerEPFvS1_S6_ESC_ 148 | fun:_dyld_objc_notify_register 149 | fun:_objc_init 150 | fun:_os_object_init 151 | fun:libdispatch_init 152 | fun:libSystem_initializer 153 | fun:_ZN16ImageLoaderMachO18doModInitFunctionsERKN11ImageLoader11LinkContextE 154 | fun:_ZN16ImageLoaderMachO16doInitializationERKN11ImageLoader11LinkContextE 155 | } 156 | 157 | { 158 | macOS-Sierra-10.12.5-check.1 159 | Memcheck:Param 160 | msg->desc.port.name 161 | fun:mach_msg_trap 162 | fun:mach_msg 163 | fun:task_set_special_port 164 | fun:_os_trace_create_debug_control_port 165 | fun:_libtrace_init 166 | fun:libSystem_initializer 167 | fun:_ZN16ImageLoaderMachO18doModInitFunctionsERKN11ImageLoader11LinkContextE 168 | fun:_ZN16ImageLoaderMachO16doInitializationERKN11ImageLoader11LinkContextE 169 | fun:_ZN11ImageLoader23recursiveInitializationERKNS_11LinkContextEjPKcRNS_21InitializerTimingListERNS_15UninitedUpwardsE 170 | fun:_ZN11ImageLoader23recursiveInitializationERKNS_11LinkContextEjPKcRNS_21InitializerTimingListERNS_15UninitedUpwardsE 171 | fun:_ZN11ImageLoader19processInitializersERKNS_11LinkContextEjRNS_21InitializerTimingListERNS_15UninitedUpwardsE 172 | fun:_ZN11ImageLoader15runInitializersERKNS_11LinkContextERNS_21InitializerTimingListE 173 | } 174 | -------------------------------------------------------------------------------- /docs/Doxyfile-mcss: -------------------------------------------------------------------------------- 1 | @INCLUDE = Doxyfile 2 | 3 | PROJECT_BRIEF = "" 4 | 5 | USE_MDFILE_AS_MAINPAGE = "../README.md" 6 | 7 | 8 | GENERATE_HTML = NO 9 | GENERATE_XML = YES 10 | XML_PROGRAMLISTING = NO 11 | 12 | XML_OUTPUT = "site/xml" 13 | HTML_OUTPUT = "site" -------------------------------------------------------------------------------- /docs/conf.py: -------------------------------------------------------------------------------- 1 | DOXYFILE = 'Doxyfile-mcss' 2 | 3 | MAIN_PROJECT_URL = './index.html' 4 | SHOW_UNDOCUMENTED = True 5 | 6 | LINKS_NAVBAR1 = [ 7 | ('Pages', 'pages', []), 8 | ('Classes', 'annotated', []), 9 | ('Files', 'files', []) 10 | ] 11 | LINKS_NAVBAR2 = [ 12 | ('GitHub', []) 13 | ] 14 | -------------------------------------------------------------------------------- /docs/figures/mpc_result.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotology/osqp-eigen/71f1f33cc913070cea652fec6193c14d9f99d056/docs/figures/mpc_result.png -------------------------------------------------------------------------------- /docs/pages/mpc.md: -------------------------------------------------------------------------------- 1 | # Using osqp-eigen in MPC fashion 2 | 3 | 4 | The problem is to develop a controller that allows a linear system to track a constant reference state \f$x_r\f$. This kind of problem can be solved using a lot of different controller architectures, however in order to write a tutorial for osqp-eigen library the [**MPC**](https://en.wikipedia.org/wiki/Model_predictive_control) approach will be chosen. 5 | Thus we have to find a controller low \f$u_0^*\f$ such that: 6 | \f[ 7 | \begin{split}\begin{array}{ll} 8 | u_0 ^* = \mbox{arg min}_{x_k, u_k} & (x_N-x_r)^T Q_N (x_N-x_r) + \sum_{k=0}^{N-1} (x_k-x_r)^T Q (x_k-x_r) + u_k^T R u_k \\ 9 | \mbox{subject to} & x_{k+1} = A x_k + B u_k \\ 10 | & x_{\rm min} \le x_k \le x_{\rm max} \\ 11 | & u_{\rm min} \le u_k \le u_{\rm max} \\ 12 | & x_0 = \bar{x} 13 | \end{array}\end{split} 14 | \f] 15 | where \f$Q\f$, \f$Q_N\f$ and \f$R\f$ are symmetric positive definite matrices; 16 | the states \f$x_k\f$ and the inputs \f$u_k\f$ have to be constrained between some lower and upper bounds and the reference state \f$x_r\f$ is 17 | \f[ 18 | x_r = \begin{bmatrix} 0 & 0 & 1 & 0 & \cdots & & 0 \end{bmatrix} ^\top 19 | \f] 20 | 21 | ## Convert MPC into a QP 22 | First of all the MPC problem has to be casted to a standard QP problem. 23 | \f[ 24 | \begin{split}\begin{array}{ll} 25 | \mbox{minimize} & \frac{1}{2} x^T P x + q^T x \\ 26 | \mbox{subject to} & l \leq A_c x \leq u 27 | \end{array}\end{split} 28 | \f] 29 | where the hessian matrix \f$P\f$ is equal to 30 | \f[ 31 | P = \text{diag}(Q, Q, ..., Q_N, R, ..., R) 32 | \f] 33 | while the gradient vector is 34 | \f[ 35 | q = \begin{bmatrix} 36 | -Q x_r \\ 37 | -Q x_r \\ 38 | \vdots \\ 39 | -Q_N x_r \\ 40 | 0\\ 41 | \vdots\\ 42 | 0 43 | \end{bmatrix} 44 | \f] 45 | 46 | The linear constraint matrix \f$A_c\f$ is 47 | \f[ 48 | A_c = 49 | \left[ 50 | \begin{array}{ccccc|cccc} 51 | -I & 0 & 0 & \cdots & 0 & 0 & 0 & \cdots & 0\\ 52 | A & -I & 0 & \cdots & 0 & B & 0 & \cdots & 0 \\ 53 | 0 & A & -I & \cdots & 0 & 0 & B & \cdots & 0\\ 54 | \vdots & \vdots & \vdots & \ddots & \vdots & \vdots & \vdots & \ddots & \vdots \\ 55 | 0 & 0 & 0 & \cdots & -I & 0 & 0 & \cdots & B\\ 56 | \hline 57 | I & 0 & 0 & \cdots & 0 & 0 & 0 & \cdots & 0\\ 58 | 0 & I & 0 & \cdots & 0 & 0 & 0 & \cdots & 0\\ 59 | 0 & 0 & I & \cdots & 0 & 0 & 0 & \cdots & 0\\ 60 | \vdots & \vdots & \vdots & \ddots & \vdots & \vdots & \vdots & \ddots & \vdots \\ 61 | 0 & 0 & 0 & \cdots & I & 0 & 0 & \cdots & 0\\ 62 | 0 & 0 & 0 & \cdots & 0 & I & 0 & \cdots & 0\\ 63 | 0 & 0 & 0 & \cdots & 0 & 0 & I & \cdots & 0\\ 64 | \vdots & \vdots & \vdots & \ddots & \vdots & \vdots & \vdots & \ddots & \vdots \\ 65 | 0 & 0 & 0 & \cdots & 0 & 0 & 0 & \cdots & I 66 | \end{array} 67 | \right] 68 | \f] 69 | while the upper and the lower bound are 70 | \f[ 71 | l = \begin{bmatrix} 72 | -x_0 \\ 73 | 0 \\ 74 | \vdots \\ 75 | 0 \\ 76 | x_{min}\\ 77 | \vdots\\ 78 | x_{min}\\ 79 | u_{min}\\ 80 | \vdots\\ 81 | u_{min}\\ 82 | \end{bmatrix} \quad 83 | u = \begin{bmatrix} 84 | -x_0 \\ 85 | 0 \\ 86 | \vdots \\ 87 | 0 \\ 88 | x_{max}\\ 89 | \vdots\\ 90 | x_{max}\\ 91 | u_{max}\\ 92 | \vdots\\ 93 | u_{max}\\ 94 | \end{bmatrix} 95 | \f] 96 | 97 | Since the osqp-eigen handles only QP problem this operation shall be done by the user. 98 | You can find the implementation of the following functions [**here**](https://github.com/GiulioRomualdi/osqp-eigen/blob/master/example/src/MPCExample.cpp#L71-L182). 99 | \code{.cpp} 100 | castMPCToQPHessian(Q, R, mpcWindow, hessian); 101 | castMPCToQPGradient(Q, xRef, mpcWindow, gradient); 102 | castMPCToQPConstraintMatrix(a, b, mpcWindow, linearMatrix); 103 | castMPCToQPConstraintVectors(xMax, xMin, uMax, uMin, x0, mpcWindow, lowerBound, upperBound); 104 | \endcode 105 | 106 | \subsection OSQP_init Solver initialization 107 | Now you are able to use the OSQP solver. We first create an instance of the solver 108 | \code{.cpp} 109 | // instantiate the solver 110 | OsqpEigen::Solver solver; 111 | \endcode 112 | when the solver is instantiated the [**default settings**](http://osqp.readthedocs.io/en/latest/interfaces/solver_settings.html) are automatically loaded, however you can change each setting using 113 | the following function 114 | \code{.cpp} 115 | solver.settings()->set() 116 | \endcode 117 | where `set()` is a setter function. You can find the list of all the setter 118 | functions in the `OsqpEigen::Settings` class. 119 | For example you can use the warm start variables in the optimization problem by calling 120 | \code{.cpp} 121 | solver.settings()->setWarmStart(true); 122 | \endcode 123 | 124 | Now you can set the data of the optimization problem (number of variables, number of constraints 125 | and so on) 126 | \code{.cpp} 127 | solver.data()->setNumberOfVariables(numberOfVariable); 128 | solver.data()->setNumberOfConstraints(numberOfConstraints); 129 | if(!solver.data()->setHessianMatrix(hessian)) return 1; 130 | if(!solver.data()->setGradient(gradient)) return 1; 131 | if(!solver.data()->setLinearConstraintMatrix(linearMatrix)) return 1; 132 | if(!solver.data()->setLowerBound(lowerBound)) return 1; 133 | if(!solver.data()->setUpperBound(upperBound)) return 1; 134 | \endcode 135 | The setter functions return `True` in case of success and `False` otherwise. 136 | 137 | Now you are able to initialize the solver. All data and settings will be stored inside the 138 | osqp struct and the optimization problem will be initialized. 139 | \code{.cpp} 140 | if(!solver.initSolver()) return 1; 141 | \endcode 142 | 143 | The optimization problem can be solved calling the following method 144 | \code{.cpp} 145 | if(solver.solveProblem() != OsqpEigen::ErrorExitFlag::NoError) return 1; 146 | \endcode 147 | and the solution can be easily got by calling the following method 148 | \code{.cpp} 149 | Eigen::VectorXd QPSolution = solver.getSolution(); 150 | \endcode 151 | 152 | If you need to update the bounds constraints and the gradient vector you 153 | can use the following methods: 154 | - `OsqpEigen::Solver::updateBounds` to update both upper and lower bounds; 155 | - `OsqpEigen::Solver::updateLowerBound` to update the lower bound; 156 | - `OsqpEigen::Solver::updateUpperBound` to update the upper bound; 157 | - `OsqpEigen::Solver::updateGradient` to update the gradient vector. 158 | 159 | \subsection results Example 160 | In the following the example of MPC controller is shown. 161 | \include MPCExample.cpp 162 | 163 | The example presented generates the following results 164 | \image html mpc_result.png 165 | -------------------------------------------------------------------------------- /example/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Authors: Giulio Romualdi 2 | # CopyPolicy: Released under the terms of the LGPLv2.1 or later 3 | 4 | cmake_minimum_required(VERSION 3.1) 5 | 6 | set (CMAKE_CXX_STANDARD 11) 7 | 8 | project(OsqpEigen-Example) 9 | 10 | find_package(OsqpEigen) 11 | find_package(Eigen3) 12 | 13 | include_directories(SYSTEM ${EIGEN3_INCLUDE_DIR}) 14 | 15 | #MPCExample 16 | add_executable(MPCExample src/MPCExample.cpp) 17 | target_link_libraries(MPCExample OsqpEigen::OsqpEigen) 18 | -------------------------------------------------------------------------------- /example/src/MPCExample.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file MPCExample.cpp 3 | * @author Giulio Romualdi 4 | * @copyright Released under the terms of the BSD 3-Clause License 5 | * @date 2018 6 | */ 7 | 8 | // osqp-eigen 9 | #include "OsqpEigen/OsqpEigen.h" 10 | 11 | // eigen 12 | #include 13 | 14 | #include 15 | 16 | void setDynamicsMatrices(Eigen::Matrix& a, Eigen::Matrix& b) 17 | { 18 | 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., 19 | 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0.0488, 0., 0., 1., 0., 0., 0.0016, 20 | 0., 0., 0.0992, 0., 0., 0., -0.0488, 0., 0., 1., 0., 0., -0.0016, 0., 0., 0.0992, 0., 0., 21 | 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.0992, 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 22 | 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 23 | 0., 0., 0.9734, 0., 0., 0., 0., 0., 0.0488, 0., 0., 0.9846, 0., 0., 0., -0.9734, 0., 0., 0., 24 | 0., 0., -0.0488, 0., 0., 0.9846, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.9846; 25 | 26 | b << 0., -0.0726, 0., 0.0726, -0.0726, 0., 0.0726, 0., -0.0152, 0.0152, -0.0152, 0.0152, -0., 27 | -0.0006, -0., 0.0006, 0.0006, 0., -0.0006, 0.0000, 0.0106, 0.0106, 0.0106, 0.0106, 0, 28 | -1.4512, 0., 1.4512, -1.4512, 0., 1.4512, 0., -0.3049, 0.3049, -0.3049, 0.3049, -0., 29 | -0.0236, 0., 0.0236, 0.0236, 0., -0.0236, 0., 0.2107, 0.2107, 0.2107, 0.2107; 30 | } 31 | 32 | void setInequalityConstraints(Eigen::Matrix& xMax, 33 | Eigen::Matrix& xMin, 34 | Eigen::Matrix& uMax, 35 | Eigen::Matrix& uMin) 36 | { 37 | double u0 = 10.5916; 38 | 39 | // input inequality constraints 40 | uMin << 9.6 - u0, 9.6 - u0, 9.6 - u0, 9.6 - u0; 41 | 42 | uMax << 13 - u0, 13 - u0, 13 - u0, 13 - u0; 43 | 44 | // state inequality constraints 45 | xMin << -M_PI / 6, -M_PI / 6, -OsqpEigen::INFTY, -OsqpEigen::INFTY, -OsqpEigen::INFTY, -1., 46 | -OsqpEigen::INFTY, -OsqpEigen::INFTY, -OsqpEigen::INFTY, -OsqpEigen::INFTY, 47 | -OsqpEigen::INFTY, -OsqpEigen::INFTY; 48 | 49 | xMax << M_PI / 6, M_PI / 6, OsqpEigen::INFTY, OsqpEigen::INFTY, OsqpEigen::INFTY, 50 | OsqpEigen::INFTY, OsqpEigen::INFTY, OsqpEigen::INFTY, OsqpEigen::INFTY, OsqpEigen::INFTY, 51 | OsqpEigen::INFTY, OsqpEigen::INFTY; 52 | } 53 | 54 | void setWeightMatrices(Eigen::DiagonalMatrix& Q, Eigen::DiagonalMatrix& R) 55 | { 56 | Q.diagonal() << 0, 0, 10., 10., 10., 10., 0, 0, 0, 5., 5., 5.; 57 | R.diagonal() << 0.1, 0.1, 0.1, 0.1; 58 | } 59 | 60 | void castMPCToQPHessian(const Eigen::DiagonalMatrix& Q, 61 | const Eigen::DiagonalMatrix& R, 62 | int mpcWindow, 63 | Eigen::SparseMatrix& hessianMatrix) 64 | { 65 | 66 | hessianMatrix.resize(12 * (mpcWindow + 1) + 4 * mpcWindow, 67 | 12 * (mpcWindow + 1) + 4 * mpcWindow); 68 | 69 | // populate hessian matrix 70 | for (int i = 0; i < 12 * (mpcWindow + 1) + 4 * mpcWindow; i++) 71 | { 72 | if (i < 12 * (mpcWindow + 1)) 73 | { 74 | int posQ = i % 12; 75 | float value = Q.diagonal()[posQ]; 76 | if (value != 0) 77 | hessianMatrix.insert(i, i) = value; 78 | } else 79 | { 80 | int posR = i % 4; 81 | float value = R.diagonal()[posR]; 82 | if (value != 0) 83 | hessianMatrix.insert(i, i) = value; 84 | } 85 | } 86 | } 87 | 88 | void castMPCToQPGradient(const Eigen::DiagonalMatrix& Q, 89 | const Eigen::Matrix& xRef, 90 | int mpcWindow, 91 | Eigen::VectorXd& gradient) 92 | { 93 | 94 | Eigen::Matrix Qx_ref; 95 | Qx_ref = Q * (-xRef); 96 | 97 | // populate the gradient vector 98 | gradient = Eigen::VectorXd::Zero(12 * (mpcWindow + 1) + 4 * mpcWindow, 1); 99 | for (int i = 0; i < 12 * (mpcWindow + 1); i++) 100 | { 101 | int posQ = i % 12; 102 | float value = Qx_ref(posQ, 0); 103 | gradient(i, 0) = value; 104 | } 105 | } 106 | 107 | void castMPCToQPConstraintMatrix(const Eigen::Matrix& dynamicMatrix, 108 | const Eigen::Matrix& controlMatrix, 109 | int mpcWindow, 110 | Eigen::SparseMatrix& constraintMatrix) 111 | { 112 | constraintMatrix.resize(12 * (mpcWindow + 1) + 12 * (mpcWindow + 1) + 4 * mpcWindow, 113 | 12 * (mpcWindow + 1) + 4 * mpcWindow); 114 | 115 | // populate linear constraint matrix 116 | for (int i = 0; i < 12 * (mpcWindow + 1); i++) 117 | { 118 | constraintMatrix.insert(i, i) = -1; 119 | } 120 | 121 | for (int i = 0; i < mpcWindow; i++) 122 | for (int j = 0; j < 12; j++) 123 | for (int k = 0; k < 12; k++) 124 | { 125 | float value = dynamicMatrix(j, k); 126 | if (value != 0) 127 | { 128 | constraintMatrix.insert(12 * (i + 1) + j, 12 * i + k) = value; 129 | } 130 | } 131 | 132 | for (int i = 0; i < mpcWindow; i++) 133 | for (int j = 0; j < 12; j++) 134 | for (int k = 0; k < 4; k++) 135 | { 136 | float value = controlMatrix(j, k); 137 | if (value != 0) 138 | { 139 | constraintMatrix.insert(12 * (i + 1) + j, 4 * i + k + 12 * (mpcWindow + 1)) 140 | = value; 141 | } 142 | } 143 | 144 | for (int i = 0; i < 12 * (mpcWindow + 1) + 4 * mpcWindow; i++) 145 | { 146 | constraintMatrix.insert(i + (mpcWindow + 1) * 12, i) = 1; 147 | } 148 | } 149 | 150 | void castMPCToQPConstraintVectors(const Eigen::Matrix& xMax, 151 | const Eigen::Matrix& xMin, 152 | const Eigen::Matrix& uMax, 153 | const Eigen::Matrix& uMin, 154 | const Eigen::Matrix& x0, 155 | int mpcWindow, 156 | Eigen::VectorXd& lowerBound, 157 | Eigen::VectorXd& upperBound) 158 | { 159 | // evaluate the lower and the upper inequality vectors 160 | Eigen::VectorXd lowerInequality 161 | = Eigen::MatrixXd::Zero(12 * (mpcWindow + 1) + 4 * mpcWindow, 1); 162 | Eigen::VectorXd upperInequality 163 | = Eigen::MatrixXd::Zero(12 * (mpcWindow + 1) + 4 * mpcWindow, 1); 164 | for (int i = 0; i < mpcWindow + 1; i++) 165 | { 166 | lowerInequality.block(12 * i, 0, 12, 1) = xMin; 167 | upperInequality.block(12 * i, 0, 12, 1) = xMax; 168 | } 169 | for (int i = 0; i < mpcWindow; i++) 170 | { 171 | lowerInequality.block(4 * i + 12 * (mpcWindow + 1), 0, 4, 1) = uMin; 172 | upperInequality.block(4 * i + 12 * (mpcWindow + 1), 0, 4, 1) = uMax; 173 | } 174 | 175 | // evaluate the lower and the upper equality vectors 176 | Eigen::VectorXd lowerEquality = Eigen::MatrixXd::Zero(12 * (mpcWindow + 1), 1); 177 | Eigen::VectorXd upperEquality; 178 | lowerEquality.block(0, 0, 12, 1) = -x0; 179 | upperEquality = lowerEquality; 180 | lowerEquality = lowerEquality; 181 | 182 | // merge inequality and equality vectors 183 | lowerBound = Eigen::MatrixXd::Zero(2 * 12 * (mpcWindow + 1) + 4 * mpcWindow, 1); 184 | lowerBound << lowerEquality, lowerInequality; 185 | 186 | upperBound = Eigen::MatrixXd::Zero(2 * 12 * (mpcWindow + 1) + 4 * mpcWindow, 1); 187 | upperBound << upperEquality, upperInequality; 188 | } 189 | 190 | void updateConstraintVectors(const Eigen::Matrix& x0, 191 | Eigen::VectorXd& lowerBound, 192 | Eigen::VectorXd& upperBound) 193 | { 194 | lowerBound.block(0, 0, 12, 1) = -x0; 195 | upperBound.block(0, 0, 12, 1) = -x0; 196 | } 197 | 198 | double getErrorNorm(const Eigen::Matrix& x, const Eigen::Matrix& xRef) 199 | { 200 | // evaluate the error 201 | Eigen::Matrix error = x - xRef; 202 | 203 | // return the norm 204 | return error.norm(); 205 | } 206 | 207 | int main() 208 | { 209 | // set the preview window 210 | int mpcWindow = 20; 211 | 212 | // allocate the dynamics matrices 213 | Eigen::Matrix a; 214 | Eigen::Matrix b; 215 | 216 | // allocate the constraints vector 217 | Eigen::Matrix xMax; 218 | Eigen::Matrix xMin; 219 | Eigen::Matrix uMax; 220 | Eigen::Matrix uMin; 221 | 222 | // allocate the weight matrices 223 | Eigen::DiagonalMatrix Q; 224 | Eigen::DiagonalMatrix R; 225 | 226 | // allocate the initial and the reference state space 227 | Eigen::Matrix x0; 228 | Eigen::Matrix xRef; 229 | 230 | // allocate QP problem matrices and vectors 231 | Eigen::SparseMatrix hessian; 232 | Eigen::VectorXd gradient; 233 | Eigen::SparseMatrix linearMatrix; 234 | Eigen::VectorXd lowerBound; 235 | Eigen::VectorXd upperBound; 236 | 237 | // set the initial and the desired states 238 | x0 << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 239 | xRef << 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0; 240 | 241 | // set MPC problem quantities 242 | setDynamicsMatrices(a, b); 243 | setInequalityConstraints(xMax, xMin, uMax, uMin); 244 | setWeightMatrices(Q, R); 245 | 246 | // cast the MPC problem as QP problem 247 | castMPCToQPHessian(Q, R, mpcWindow, hessian); 248 | castMPCToQPGradient(Q, xRef, mpcWindow, gradient); 249 | castMPCToQPConstraintMatrix(a, b, mpcWindow, linearMatrix); 250 | castMPCToQPConstraintVectors(xMax, xMin, uMax, uMin, x0, mpcWindow, lowerBound, upperBound); 251 | 252 | // instantiate the solver 253 | OsqpEigen::Solver solver; 254 | 255 | // settings 256 | // solver.settings()->setVerbosity(false); 257 | solver.settings()->setWarmStart(true); 258 | 259 | // set the initial data of the QP solver 260 | solver.data()->setNumberOfVariables(12 * (mpcWindow + 1) + 4 * mpcWindow); 261 | solver.data()->setNumberOfConstraints(2 * 12 * (mpcWindow + 1) + 4 * mpcWindow); 262 | if (!solver.data()->setHessianMatrix(hessian)) 263 | return 1; 264 | if (!solver.data()->setGradient(gradient)) 265 | return 1; 266 | if (!solver.data()->setLinearConstraintsMatrix(linearMatrix)) 267 | return 1; 268 | if (!solver.data()->setLowerBound(lowerBound)) 269 | return 1; 270 | if (!solver.data()->setUpperBound(upperBound)) 271 | return 1; 272 | 273 | // instantiate the solver 274 | if (!solver.initSolver()) 275 | return 1; 276 | 277 | // controller input and QPSolution vector 278 | Eigen::Vector4d ctr; 279 | Eigen::VectorXd QPSolution; 280 | 281 | // number of iteration steps 282 | int numberOfSteps = 50; 283 | 284 | for (int i = 0; i < numberOfSteps; i++) 285 | { 286 | 287 | // solve the QP problem 288 | if (solver.solveProblem() != OsqpEigen::ErrorExitFlag::NoError) 289 | return 1; 290 | 291 | // get the controller input 292 | QPSolution = solver.getSolution(); 293 | ctr = QPSolution.block(12 * (mpcWindow + 1), 0, 4, 1); 294 | 295 | // save data into file 296 | auto x0Data = x0.data(); 297 | 298 | // propagate the model 299 | x0 = a * x0 + b * ctr; 300 | 301 | // update the constraint bound 302 | updateConstraintVectors(x0, lowerBound, upperBound); 303 | if (!solver.updateBounds(lowerBound, upperBound)) 304 | return 1; 305 | } 306 | return 0; 307 | } 308 | -------------------------------------------------------------------------------- /include/OsqpEigen/Compat.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file SparseMatrixHelper.hpp 3 | * @author Pierre Gergondet 4 | * @copyright Released under the terms of the BSD 3-Clause License 5 | * @date 2023 6 | */ 7 | 8 | #ifndef OSQP_EIGEN_COMPAT_HPP 9 | #define OSQP_EIGEN_COMPAT_HPP 10 | 11 | // OSQP 12 | #include 13 | 14 | #include 15 | 16 | #ifdef OSQP_EIGEN_OSQP_IS_V1 17 | 18 | // Re-use the same name in the global namespace as the old versions of OSQP 19 | using c_int = OSQPInt; 20 | using c_float = OSQPFloat; 21 | using csc = OSQPCscMatrix; 22 | 23 | // Those symbols are available in the library but hidden from the public API 24 | extern "C" { 25 | extern OSQPCscMatrix* csc_spalloc(c_int m, c_int n, c_int nzmax, c_int values, c_int triplet); 26 | extern void csc_spfree(OSQPCscMatrix* A); 27 | } 28 | 29 | #ifndef OSQP_CUSTOM_MEMORY 30 | #define c_malloc malloc 31 | #define c_free free 32 | #endif 33 | 34 | namespace OsqpEigen 35 | { 36 | 37 | struct OSQPData 38 | { 39 | OSQPInt n; ///< number of variables n 40 | OSQPInt m; ///< number of constraints m 41 | OSQPCscMatrix* P; ///< the upper triangular part of the quadratic objective matrix P (size n x n). 42 | OSQPCscMatrix* A; ///< linear constraints matrix A (size m x n) 43 | OSQPFloat* q; ///< dense array for linear part of objective function (size n) 44 | OSQPFloat* l; ///< dense array for lower bound (size m) 45 | OSQPFloat* u; ///< dense array for upper bound (size m) 46 | }; 47 | 48 | inline OSQPCscMatrix* spalloc(OSQPInt m, 49 | OSQPInt n, 50 | OSQPInt nzmax) 51 | { 52 | OSQPCscMatrix* M = static_cast(calloc(1, sizeof(OSQPCscMatrix))); /* allocate the OSQPCscMatrix struct */ 53 | if (!M) 54 | { 55 | return static_cast(OSQP_NULL); 56 | } 57 | 58 | OSQPInt* M_p = static_cast(calloc(n + 1, sizeof(OSQPInt))); 59 | if (!M_p) 60 | { 61 | free(M); 62 | return static_cast(OSQP_NULL); 63 | } 64 | 65 | OSQPInt* M_i = static_cast(calloc(nzmax, sizeof(OSQPInt))); 66 | if (!M_i) 67 | { 68 | free(M); 69 | free(M_p); 70 | return static_cast(OSQP_NULL); 71 | } 72 | 73 | OSQPFloat* M_x = static_cast(calloc(nzmax, sizeof(OSQPFloat))); 74 | if (!M_x) 75 | { 76 | free(M); 77 | free(M_p); 78 | free(M_i); 79 | return static_cast(OSQP_NULL); 80 | } 81 | 82 | OSQPInt M_nnz = 0; 83 | 84 | if (nzmax >= 0) 85 | { 86 | M_nnz = nzmax; 87 | } 88 | 89 | #ifdef OSQP_EIGEN_OSQP_IS_V1_FINAL 90 | #define OSQPEigen_OSQPCscMatrix_set_data OSQPCscMatrix_set_data 91 | #else 92 | #define OSQPEigen_OSQPCscMatrix_set_data csc_set_data 93 | #endif 94 | 95 | OSQPEigen_OSQPCscMatrix_set_data(M, m, n, M_nnz, M_x, M_i, M_p); 96 | 97 | return M; 98 | } 99 | 100 | inline void spfree(OSQPCscMatrix* M) 101 | { 102 | if (M){ 103 | if (M->p) free(M->p); 104 | if (M->i) free(M->i); 105 | if (M->x) free(M->x); 106 | free(M); 107 | } 108 | } 109 | 110 | } // namespace OsqpEigen 111 | 112 | #else 113 | 114 | namespace OsqpEigen 115 | { 116 | 117 | inline csc* spalloc(c_int m, c_int n, c_int nzmax) 118 | { 119 | return csc_spalloc(m, n, nzmax, 1, 0); 120 | } 121 | 122 | inline void spfree(csc* M) 123 | { 124 | return csc_spfree(M); 125 | } 126 | 127 | } // namespace OsqpEigen 128 | 129 | 130 | #endif // OSQP_EIGEN_OSQP_IS_V1 131 | #endif // OSQP_EIGEN_COMPAT_HPP 132 | -------------------------------------------------------------------------------- /include/OsqpEigen/Constants.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Constants.hpp 3 | * @author Giulio Romualdi 4 | * @copyright Released under the terms of the BSD 3-Clause License 5 | * @date 2018 6 | */ 7 | 8 | #ifndef OSQPEIGEN_CONSTANTS_HPP 9 | #define OSQPEIGEN_CONSTANTS_HPP 10 | 11 | #include 12 | 13 | /** 14 | * OsqpEigen namespace. 15 | */ 16 | namespace OsqpEigen 17 | { 18 | constexpr c_float INFTY = OSQP_INFTY; /**< Infinity constant. */ 19 | 20 | /** 21 | * Status of the solver 22 | */ 23 | enum class Status : int 24 | { 25 | DualInfeasibleInaccurate = OSQP_DUAL_INFEASIBLE_INACCURATE, 26 | PrimalInfeasibleInaccurate = OSQP_PRIMAL_INFEASIBLE_INACCURATE, 27 | SolvedInaccurate = OSQP_SOLVED_INACCURATE, 28 | Solved = OSQP_SOLVED, 29 | MaxIterReached = OSQP_MAX_ITER_REACHED, 30 | PrimalInfeasible = OSQP_PRIMAL_INFEASIBLE, 31 | DualInfeasible = OSQP_DUAL_INFEASIBLE, 32 | Sigint = OSQP_SIGINT, 33 | #if defined(PROFILING) || defined(OSQP_EIGEN_OSQP_IS_V1) 34 | TimeLimitReached = OSQP_TIME_LIMIT_REACHED, 35 | #endif // ifdef PROFILING 36 | NonCvx = OSQP_NON_CVX, 37 | Unsolved = OSQP_UNSOLVED 38 | }; 39 | 40 | /** 41 | * Error status of the Solver 42 | */ 43 | enum class ErrorExitFlag : int 44 | { 45 | NoError = 0, 46 | DataValidationError = OSQP_DATA_VALIDATION_ERROR, 47 | SettingsValidationError = OSQP_SETTINGS_VALIDATION_ERROR, 48 | #ifdef OSQP_EIGEN_OSQP_IS_V1 49 | LinsysSolverLoadError = OSQP_ALGEBRA_LOAD_ERROR, 50 | #else 51 | LinsysSolverLoadError = OSQP_LINSYS_SOLVER_LOAD_ERROR, 52 | #endif 53 | LinsysSolverInitError = OSQP_LINSYS_SOLVER_INIT_ERROR, 54 | NonCvxError = OSQP_NONCVX_ERROR, 55 | MemAllocError = OSQP_MEM_ALLOC_ERROR, 56 | WorkspaceNotInitError = OSQP_WORKSPACE_NOT_INIT_ERROR 57 | }; 58 | 59 | } // namespace OsqpEigen 60 | 61 | #endif 62 | -------------------------------------------------------------------------------- /include/OsqpEigen/Data.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Data.hpp 3 | * @author Giulio Romualdi 4 | * @copyright Released under the terms of the BSD 3-Clause License 5 | * @date 2018 6 | */ 7 | 8 | #ifndef OSQPEIGEN_DATA_HPP 9 | #define OSQPEIGEN_DATA_HPP 10 | 11 | // Eigen 12 | #include 13 | 14 | // OSQP 15 | #include 16 | 17 | // OsqpEigen 18 | #include 19 | 20 | /** 21 | * OsqpEigen namespace. 22 | */ 23 | namespace OsqpEigen 24 | { 25 | /** 26 | * Data class is a wrapper of the OSQP OSQPData struct. 27 | */ 28 | class Data 29 | { 30 | OSQPData* m_data; /**< OSQPData struct. */ 31 | bool m_isNumberOfVariablesSet; /**< Boolean true if the number of variables is set. */ 32 | bool m_isNumberOfConstraintsSet; /**< Boolean true if the number of constraints is set. */ 33 | bool m_isHessianMatrixSet; /**< Boolean true if the hessian matrix is set. */ 34 | bool m_isGradientSet; /**< Boolean true if the gradient vector is set. */ 35 | bool m_isLinearConstraintsMatrixSet; /**< Boolean true if the linear constrain matrix is set. */ 36 | bool m_isLowerBoundSet; /**< Boolean true if the lower bound vector is set. */ 37 | bool m_isUpperBoundSet; /**< Boolean true if the upper bound vector is set. */ 38 | 39 | public: 40 | /** 41 | * Constructor. 42 | */ 43 | Data(); 44 | 45 | /** 46 | * Constructor. 47 | * @param n is the number of variables; 48 | * @param m is the number of constraints. 49 | */ 50 | Data(int n, int m); 51 | 52 | /** 53 | * Deconstructor. 54 | */ 55 | ~Data(); 56 | 57 | /** 58 | * Clear the hessian matrix. 59 | */ 60 | void clearHessianMatrix(); 61 | 62 | /** 63 | * Clear the linear constraints matrix. 64 | */ 65 | void clearLinearConstraintsMatrix(); 66 | 67 | /** 68 | * Set the number of variables. 69 | * @param n is the number of variables. 70 | */ 71 | void setNumberOfVariables(int n); 72 | 73 | /** 74 | * Set the number of constraints. 75 | * @param m is the number of constraints. 76 | */ 77 | void setNumberOfConstraints(int m); 78 | 79 | /** 80 | * Set the quadratic part of the cost function (Hessian). 81 | * It is assumed to be a symmetric matrix. 82 | * @param hessianMatrix is the Hessian matrix. 83 | * @return true/false in case of success/failure. 84 | */ 85 | template 86 | bool setHessianMatrix(const Eigen::SparseCompressedBase& hessianMatrix); 87 | 88 | /** 89 | * Set the linear part of the cost function (Gradient). 90 | * @param gradientVector is the Gradient vector. 91 | * @note the elements of the gradient are not copied inside the library. 92 | * The user has to guarantee that the lifetime of the object passed is the same of the 93 | * OsqpEigen object 94 | * @return true/false in case of success/failure. 95 | */ 96 | bool setGradient(Eigen::Ref> gradientVector); 97 | 98 | Eigen::Matrix getGradient(); 99 | 100 | /** 101 | * Set the linear constraint matrix A (size m x n) 102 | * @param linearConstraintsMatrix is the linear constraints matrix A. 103 | * @return true/false in case of success/failure. 104 | */ 105 | template 106 | bool 107 | setLinearConstraintsMatrix(const Eigen::SparseCompressedBase& linearConstraintsMatrix); 108 | 109 | /** 110 | * Set the array for lower bound (size m). 111 | * @param lowerBoundVector is the lower bound constraint. 112 | * @note the elements of the lowerBoundVector are not copied inside the library. 113 | * The user has to guarantee that the lifetime of the object passed is the same of the 114 | * OsqpEigen object 115 | * @return true/false in case of success/failure. 116 | */ 117 | bool setLowerBound(Eigen::Ref> lowerBoundVector); 118 | 119 | /** 120 | * Set the array for upper bound (size m). 121 | * @param upperBoundVector is the upper bound constraint. 122 | * @note the elements of the upperBoundVector are not copied inside the library. 123 | * The user has to guarantee that the lifetime of the object passed is the same of the 124 | * OsqpEigen object. 125 | * @return true/false in case of success/failure. 126 | */ 127 | bool setUpperBound(Eigen::Ref> upperBoundVector); 128 | 129 | /** 130 | * Set the array for upper and lower bounds (size m). 131 | * @param lowerBound is the lower bound constraint. 132 | * @param upperBound is the upper bound constraint. 133 | * @note the elements of the upperBound and lowerBound are not copied inside the library. 134 | * The user has to guarantee that the lifetime of the object passed is the same of the 135 | * OsqpEigen object. 136 | * @return true/false in case of success/failure. 137 | */ 138 | bool setBounds(Eigen::Ref> lowerBound, 139 | Eigen::Ref> upperBound); 140 | 141 | /** 142 | * Get the OSQPData struct. 143 | * @return a const point to the OSQPData struct. 144 | */ 145 | OSQPData* const& getData() const; 146 | 147 | /** 148 | * Verify if all the matrix and vectors are already set. 149 | * @return true if all the OSQPData struct are set. 150 | */ 151 | bool isSet() const; 152 | }; 153 | } // namespace OsqpEigen 154 | 155 | #include 156 | 157 | #endif 158 | -------------------------------------------------------------------------------- /include/OsqpEigen/Data.tpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Data.tpp 3 | * @author Giulio Romualdi 4 | * @copyright Released under the terms of the BSD 3-Clause License 5 | * @date 2018 6 | */ 7 | 8 | #include 9 | 10 | #include 11 | 12 | template 13 | bool OsqpEigen::Data::setHessianMatrix(const Eigen::SparseCompressedBase& hessianMatrix) 14 | { 15 | if (m_isHessianMatrixSet) 16 | { 17 | debugStream() << "[OsqpEigen::Data::setHessianMatrix] The hessian matrix was already set. " 18 | << "Please use clearHessianMatrix() method to deallocate memory." 19 | << std::endl; 20 | return false; 21 | } 22 | 23 | if (!m_isNumberOfVariablesSet) 24 | { 25 | debugStream() << "[OsqpEigen::Data::setHessianMatrix] Please set the number of variables " 26 | "before add the hessian matrix." 27 | << std::endl; 28 | return false; 29 | } 30 | 31 | // check if the number of row and columns are equal to the number of the optimization variables 32 | if ((hessianMatrix.rows() != m_data->n) || (hessianMatrix.cols() != m_data->n)) 33 | { 34 | debugStream() << "[OsqpEigen::Data::setHessianMatrix] The Hessian matrix has to be a n x n " 35 | "size matrix." 36 | << std::endl; 37 | return false; 38 | } 39 | 40 | // set the hessian matrix 41 | // osqp 0.6.0 required only the upper triangular part of the hessian matrix 42 | Derived hessianMatrixUpperTriangular = hessianMatrix.template triangularView(); 43 | if (!OsqpEigen::SparseMatrixHelper::createOsqpSparseMatrix(hessianMatrixUpperTriangular, 44 | m_data->P)) 45 | { 46 | debugStream() << "[OsqpEigen::Data::setHessianMatrix] Unable to instantiate the osqp " 47 | "sparse matrix." 48 | << std::endl; 49 | return false; 50 | } 51 | m_isHessianMatrixSet = true; 52 | return true; 53 | } 54 | 55 | template 56 | bool OsqpEigen::Data::setLinearConstraintsMatrix( 57 | const Eigen::SparseCompressedBase& linearConstraintsMatrix) 58 | { 59 | if (m_isLinearConstraintsMatrixSet) 60 | { 61 | debugStream() << "[OsqpEigen::Data::setLinearConstraintsMatrix] The linear constraint " 62 | "matrix was already set. Please use clearLinearConstraintsMatrix() method " 63 | "to deallocate memory." 64 | << std::endl; 65 | return false; 66 | } 67 | 68 | if (!m_isNumberOfConstraintsSet) 69 | { 70 | debugStream() << "[OsqpEigen::Data::setLinearConstraintsMatrix] Please set the number of " 71 | "constraints before add the constraint matrix." 72 | << std::endl; 73 | return false; 74 | } 75 | 76 | if (!m_isNumberOfVariablesSet) 77 | { 78 | debugStream() << "[OsqpEigen::Data::setLinearConstraintsMatrix] Please set the number of " 79 | "variables before add the constraint matrix." 80 | << std::endl; 81 | return false; 82 | } 83 | 84 | if ((linearConstraintsMatrix.rows() != m_data->m) 85 | || (linearConstraintsMatrix.cols() != m_data->n)) 86 | { 87 | debugStream() << "[OsqpEigen::Data::setLinearConstraintsMatrix] The Linear constraints " 88 | "matrix has to be a m x n size matrix." 89 | << std::endl; 90 | return false; 91 | } 92 | 93 | // set the hessian matrix 94 | if (!OsqpEigen::SparseMatrixHelper::createOsqpSparseMatrix(linearConstraintsMatrix, m_data->A)) 95 | { 96 | debugStream() << "[OsqpEigen::Data::setLinearConstraintsMatrix] osqp sparse matrix not " 97 | "created." 98 | << std::endl; 99 | return false; 100 | } 101 | 102 | m_isLinearConstraintsMatrixSet = true; 103 | 104 | return true; 105 | } 106 | -------------------------------------------------------------------------------- /include/OsqpEigen/Debug.hpp: -------------------------------------------------------------------------------- 1 | #ifndef OSQPEIGEN_DEBUG_HPP 2 | #define OSQPEIGEN_DEBUG_HPP 3 | 4 | #include 5 | 6 | namespace OsqpEigen 7 | { 8 | std::ostream& debugStream(); 9 | } // namespace OsqpEigen 10 | 11 | #endif /* OSQPEIGEN_DEBUG_HPP */ 12 | -------------------------------------------------------------------------------- /include/OsqpEigen/OsqpEigen.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file OsqpEigen.h 3 | * @author Giulio Romualdi, Stefano Dafarra 4 | * @copyright Released under the terms of the BSD 3-Clause License 5 | * @date 2018 6 | */ 7 | #ifndef OSQPEIGEN_OSQPEIGEN_H 8 | #define OSQPEIGEN_OSQPEIGEN_H 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #endif // OSQPEIGEN_OSQPEIGEN_H 17 | -------------------------------------------------------------------------------- /include/OsqpEigen/Settings.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Settings.hpp 3 | * @author Giulio Romualdi 4 | * @copyright Released under the terms of the BSD 3-Clause License 5 | * @date 2018 6 | */ 7 | 8 | #ifndef OSQPEIGEN_SETTINGS_HPP 9 | #define OSQPEIGEN_SETTINGS_HPP 10 | 11 | // OSQP 12 | #include 13 | 14 | /** 15 | * OsqpEigen namespace. 16 | */ 17 | namespace OsqpEigen 18 | { 19 | /** 20 | * settings class is a wrapper of the OSQP OSQPSettings struct. 21 | * All the setter methods refer to this particular kind of optimizer. 22 | * Here 23 | * you can find further information. 24 | */ 25 | class Settings 26 | { 27 | OSQPSettings* m_settings; /**< OSQPSettings struct. */ 28 | public: 29 | /** 30 | * Constructor. 31 | */ 32 | Settings(); 33 | 34 | /** 35 | * Deconstructor. 36 | */ 37 | ~Settings(); 38 | 39 | /** 40 | * Reset the default settings for the optimization problem. 41 | */ 42 | void resetDefaultSettings(); 43 | 44 | /** 45 | * Set the ADMM step rho. 46 | * @param rho a ADMM step constant. 47 | */ 48 | void setRho(const double rho); 49 | 50 | /** 51 | * Set the ADMM step sigma. 52 | * @param sigma a ADMM step constant. 53 | */ 54 | void setSigma(const double sigma); 55 | 56 | /** 57 | * Set the heuristic data scaling iterations. If 0, scaling disabled. 58 | * @param scaling is the heuristic data scaling iteration. 59 | */ 60 | void setScaling(const int scaling); 61 | 62 | /** 63 | * Set if the rho step size adaptive feature is active. 64 | * @param isRhoStepSizeAdactive if True the feature is active. 65 | */ 66 | void setAdaptiveRho(const bool isRhoStepSizeAdactive); 67 | 68 | /** 69 | * Set the number of iterations between rho adaptations rho. If 0, it is automatic. 70 | * @param rhoInterval number of iterations. 71 | */ 72 | void setAdaptiveRhoInterval(const int rhoInterval); 73 | 74 | /** 75 | * Set the tolerance for adapting rho. The new rho has to be X times larger or 1/X times 76 | * smaller than the current one to trigger a new factorization. 77 | * @param adaptiveRhoTolerance is the tolerance. 78 | */ 79 | void setAdaptiveRhoTolerance(const double adaptiveRhoTolerance); 80 | 81 | /** 82 | * Set the interval for adapting rho (fraction of the setup time). 83 | * @param adaptiveRhoFraction interval of the adapting rho. 84 | */ 85 | void setAdaptiveRhoFraction(const double adaptiveRhoFraction); 86 | 87 | /** 88 | * Set the max number of iterations. 89 | * @param maxIteration max number of iteration 90 | */ 91 | [[deprecated("Use setMaxIteration(int) instead.")]] void 92 | setMaxIteraction(const int maxIteration); 93 | 94 | /** 95 | * Set the max number of iterations. 96 | * @param maxIteration max number of iteration 97 | */ 98 | void setMaxIteration(const int maxIteration); 99 | 100 | /** 101 | * Set the absolute convergence tolerance. 102 | * @param absoluteTolerance absolute tolerance of the solver. 103 | */ 104 | void setAbsoluteTolerance(const double absoluteTolerance); 105 | 106 | /** 107 | * Set the relative convergence tolerance. 108 | * @param relativeTolerance relative tolerance of the solver. 109 | */ 110 | void setRelativeTolerance(const double relativeTolerance); 111 | 112 | /** 113 | * Set the primal infeasibility tolerance. 114 | * @param primalInfeasibilityTolerance tolerance of the primal variables. 115 | */ 116 | [[deprecated("Use setPrimalInfeasibilityTolerance() instead.")]] void 117 | setPrimalInfeasibilityTollerance(const double primalInfeasibilityTolerance); 118 | 119 | /** 120 | * Set the primal infeasibility tolerance. 121 | * @param primalInfeasibilityTolerance tolerance of the primal variables. 122 | */ 123 | void setPrimalInfeasibilityTolerance(const double primalInfeasibilityTolerance); 124 | 125 | /** 126 | * Set the dual infeasibility tolerance. 127 | * @param dualInfeasibilityTolerance tolerance of the dual variables. 128 | */ 129 | [[deprecated("Use setDualInfeasibilityTolerance() instead.")]] void 130 | setDualInfeasibilityTollerance(const double dualInfeasibilityTolerance); 131 | 132 | /** 133 | * Set the dual infeasibility tolerance. 134 | * @param dualInfeasibilityTolerance tolerance of the dual variables. 135 | */ 136 | void setDualInfeasibilityTolerance(const double dualInfeasibilityTolerance); 137 | 138 | /** 139 | * Set the relaxation parameter. 140 | * @param alpha is the relaxation parameter. 141 | */ 142 | void setAlpha(const double alpha); 143 | 144 | /** 145 | * Set linear solver 146 | * @param linsysSolver is the name of the solver 147 | */ 148 | void setLinearSystemSolver(const int linsysSolver); 149 | 150 | /** 151 | * Set the relaxation parameter for polish. 152 | * @param delta is the relaxation parameter. 153 | */ 154 | void setDelta(const double delta); 155 | 156 | /** 157 | * Set if the polish feature is active. 158 | * @param polish if True the feature is active. 159 | */ 160 | void setPolish(const bool polish); 161 | 162 | /** 163 | * Set the iterative refinement steps in polish. 164 | * @param polishRefineIter iterative refinement step. 165 | */ 166 | void setPolishRefineIter(const int polishRefineIter); 167 | 168 | /** 169 | * Set the Verbose mode. 170 | * @param isVerbose if true the verbose mode is activate. 171 | */ 172 | void setVerbosity(const bool isVerbose); 173 | 174 | /** 175 | * Set the scaled termination criteria. 176 | * @param scaledTermination if true the scaled termination criteria is used. 177 | */ 178 | void setScaledTerimination(const bool scaledTermination); 179 | 180 | /** 181 | * Set check termination interval. If 0, termination checking is disabled. 182 | * @param checkTermination if 0 the termination checking is disabled. 183 | */ 184 | void setCheckTermination(const int checkTermination); 185 | 186 | /** 187 | * Set check duality gap termination criteria. 188 | * @param checkDualGap If true, duality gap checking is enabled. 189 | */ 190 | void setCheckDualGap(const bool checkDualGap); 191 | 192 | /** 193 | * Set warm start. 194 | * @param warmStart if true the warm start is set. 195 | */ 196 | void setWarmStart(const bool warmStart); 197 | 198 | /** 199 | * Set the maximum number of seconds allowed to solve the problem. 200 | * @param timeLimit is the time limit in seconds. If 0, then disabled. 201 | */ 202 | void setTimeLimit(const double timeLimit); 203 | 204 | /** 205 | * Get a pointer to Settings struct. 206 | * @return a const pointer to OSQPSettings struct. 207 | */ 208 | OSQPSettings* const& getSettings() const; 209 | }; 210 | } // namespace OsqpEigen 211 | 212 | #endif 213 | -------------------------------------------------------------------------------- /include/OsqpEigen/Solver.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Solver.hpp 3 | * @author Giulio Romualdi 4 | * @copyright Released under the terms of the BSD 3-Clause License 5 | * @date 2018 6 | */ 7 | #ifndef OSQPEIGEN_SOLVER_HPP 8 | #define OSQPEIGEN_SOLVER_HPP 9 | 10 | // Std 11 | #include 12 | 13 | // Eigen 14 | #include 15 | 16 | // OSQP 17 | #include 18 | 19 | // OsqpEigen 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | /** 26 | * OsqpEigen namespace. 27 | */ 28 | namespace OsqpEigen 29 | { 30 | /** 31 | * Solver class is a wrapper of the OSQP OSQPWorkspace struct. 32 | */ 33 | class Solver 34 | { 35 | bool m_isSolverInitialized; /**< Boolean true if solver is initialized. */ 36 | #ifdef OSQP_EIGEN_OSQP_IS_V1 37 | std::unique_ptr> m_solver; /**< Pointer to 38 | OSQPSolver struct. */ 39 | #else 40 | std::unique_ptr> m_workspace; /**< Pointer to 41 | OSQPWorkspace 42 | struct. */ 43 | #endif 44 | std::unique_ptr m_settings; /**< Pointer to Settings class. */ 45 | std::unique_ptr m_data; /**< Pointer to Data class. */ 46 | Eigen::Matrix m_primalVariables; 47 | Eigen::Matrix m_dualVariables; 48 | Eigen::Matrix m_solution; 49 | Eigen::Matrix m_dualSolution; 50 | 51 | std::vector m_hessianNewIndices; 52 | std::vector m_hessianNewValues; 53 | 54 | std::vector m_constraintsNewIndices; 55 | std::vector m_constraintsNewValues; 56 | 57 | std::vector> m_oldHessianTriplet, m_newHessianTriplet, 58 | m_newUpperTriangularHessianTriplets; 59 | std::vector> m_oldLinearConstraintsTriplet, 60 | m_newLinearConstraintsTriplet; 61 | 62 | /** 63 | * Evaluate the position and the values of the new elements of a sparse matrix. 64 | * @param oldMatrixTriplet vector containing the triplets of the old sparse matrix; 65 | * @param newMatrixTriplet vector containing the triplets of the mew sparse matrix; 66 | * @param newIndices vector of the index mapping new elements 67 | * to position in the sparse matrix; 68 | * @param newValues vector of new elements in the sparse matrix. 69 | * @return true if the sparsity pattern is not changed false otherwise. 70 | */ 71 | template 72 | bool evaluateNewValues(const std::vector>& oldMatrixTriplet, 73 | const std::vector>& newMatrixTriplet, 74 | std::vector& newIndices, 75 | std::vector& newValues) const; 76 | 77 | /** 78 | * Takes only the triplets which belongs to the upper triangular part of the matrix. 79 | * @param fullMatrixTriplets vector containing the triplets of the sparse matrix; 80 | * @param upperTriangularMatrixTriplets vector containing the triplets of the mew sparse matrix; 81 | */ 82 | template 83 | void selectUpperTriangularTriplets( 84 | const std::vector>& fullMatrixTriplets, 85 | std::vector>& upperTriangularMatrixTriplets) const; 86 | 87 | #ifdef OSQP_EIGEN_OSQP_IS_V1 88 | /** 89 | * Custom Deleter for the OSQPSolver. It is required to free the @ref m_workspace unique_ptr 90 | * @param ptr raw pointer to the workspace 91 | */ 92 | static void OSQPSolverDeleter(OSQPSolver* ptr) noexcept; 93 | #else 94 | /** 95 | * Custom Deleter for the OSQPWorkspace. It is required to free the @ref m_workspace unique_ptr 96 | * @param ptr raw pointer to the workspace 97 | */ 98 | static void OSQPWorkspaceDeleter(OSQPWorkspace* ptr) noexcept; 99 | #endif 100 | 101 | inline const OSQPData* getData() const noexcept 102 | { 103 | #ifdef OSQP_EIGEN_OSQP_IS_V1 104 | return m_data->getData(); 105 | #else 106 | return m_workspace->data; 107 | #endif 108 | } 109 | 110 | inline const OSQPInfo* getInfo() const noexcept 111 | { 112 | #ifdef OSQP_EIGEN_OSQP_IS_V1 113 | return m_solver->info; 114 | #else 115 | return m_workspace->info; 116 | #endif 117 | } 118 | 119 | inline const OSQPSolution* getOSQPSolution() const noexcept 120 | { 121 | #ifdef OSQP_EIGEN_OSQP_IS_V1 122 | return m_solver->solution; 123 | #else 124 | return m_workspace->solution; 125 | #endif 126 | } 127 | 128 | public: 129 | /** 130 | * Constructor. 131 | */ 132 | Solver(); 133 | 134 | /** 135 | * Initialize the solver with the actual initial data and settings. 136 | * @return true/false in case of success/failure. 137 | */ 138 | bool initSolver(); 139 | 140 | /** 141 | * Check if the solver is initialized. 142 | * @return true if the solver is initialized. 143 | */ 144 | bool isInitialized(); 145 | 146 | /** 147 | * Deallocate memory. 148 | */ 149 | void clearSolver(); 150 | 151 | /** 152 | * Set to zero all the solver variables. 153 | * @return true/false in case of success/failure. 154 | */ 155 | bool clearSolverVariables(); 156 | 157 | /** 158 | * Solve the QP optimization problem. 159 | * @return true/false in case of success/failure. 160 | */ 161 | [[deprecated("Use solveProblem() instead.")]] bool solve(); 162 | 163 | /** 164 | * Solve the QP optimization problem. 165 | * @return the error exit flag 166 | */ 167 | OsqpEigen::ErrorExitFlag solveProblem(); 168 | 169 | /** 170 | * Get the status of the solver 171 | * @return The inner solver status 172 | */ 173 | OsqpEigen::Status getStatus() const; 174 | 175 | /** 176 | * Get the primal objective value 177 | * @return The primal objective value 178 | */ 179 | c_float getObjValue() const; 180 | 181 | /** 182 | * Get the optimization problem solution. 183 | * @return an Eigen::Vector containing the optimization result. 184 | */ 185 | const Eigen::Matrix& getSolution(); 186 | 187 | /** 188 | * Get the dual optimization problem solution. 189 | * @return an Eigen::Vector containing the optimization result. 190 | */ 191 | const Eigen::Matrix& getDualSolution(); 192 | 193 | /** 194 | * Update the linear part of the cost function (Gradient). 195 | * @param gradient is the Gradient vector. 196 | * @note the elements of the gradient are not copied inside the library. 197 | * The user has to guarantee that the lifetime of the objects passed is the same of the 198 | * OsqpEigen object. 199 | * @return true/false in case of success/failure. 200 | */ 201 | bool 202 | updateGradient(const Eigen::Ref>& gradient); 203 | 204 | /** 205 | * Update the lower bounds limit (size m). 206 | * @param lowerBound is the lower bound constraint vector. 207 | * @note the elements of the lowerBound are not copied inside the library. 208 | * The user has to guarantee that the lifetime of the object passed is the same of the 209 | * OsqpEigen object. 210 | * @return true/false in case of success/failure. 211 | */ 212 | bool 213 | updateLowerBound(const Eigen::Ref>& lowerBound); 214 | 215 | /** 216 | * Update the upper bounds limit (size m). 217 | * @param upperBound is the upper bound constraint vector. 218 | * @note the elements of the upperBound are not copied inside the library. 219 | * The user has to guarantee that the lifetime of the object passed is the same of the 220 | * OsqpEigen object. 221 | * @return true/false in case of success/failure. 222 | */ 223 | bool 224 | updateUpperBound(const Eigen::Ref>& upperBound); 225 | 226 | /** 227 | * Update both upper and lower bounds (size m). 228 | * @param lowerBound is the lower bound constraint vector; 229 | * @param upperBound is the upper bound constraint vector. 230 | * @note the elements of the lowerBound and upperBound are not copied inside the library. 231 | * The user has to guarantee that the lifetime of the objects passed is the same of the 232 | * OsqpEigen object 233 | * @return true/false in case of success/failure. 234 | */ 235 | bool 236 | updateBounds(const Eigen::Ref>& lowerBound, 237 | const Eigen::Ref>& upperBound); 238 | 239 | /** 240 | * Update the quadratic part of the cost function (Hessian). 241 | * It is assumed to be a symmetric matrix. 242 | * \note 243 | * If the sparsity pattern is preserved the matrix is simply update 244 | * otherwise the entire solver will be reinitialized. In this case 245 | * the primal and dual variable are copied in the new workspace. 246 | * 247 | * @param hessian is the Hessian matrix. 248 | * @return true/false in case of success/failure. 249 | */ 250 | template 251 | bool updateHessianMatrix(const Eigen::SparseCompressedBase& hessianMatrix); 252 | 253 | /** 254 | * Update the linear constraints matrix (A) 255 | * \note 256 | * If the sparsity pattern is preserved the matrix is simply update 257 | * otherwise the entire solver will be reinitialized. In this case 258 | * the primal and dual variable are copied in the new workspace. 259 | * 260 | * @param linearConstraintsMatrix is the linear constraint matrix A 261 | * @return true/false in case of success/failure. 262 | */ 263 | template 264 | bool updateLinearConstraintsMatrix( 265 | const Eigen::SparseCompressedBase& linearConstraintsMatrix); 266 | 267 | /** 268 | * Set the entire 269 | * @param linearConstraintsMatrix is the linear constraint matrix A 270 | * @return true/false in case of success/failure. 271 | */ 272 | template 273 | bool setWarmStart(const Eigen::Matrix& primalVariable, 274 | const Eigen::Matrix& dualVariable); 275 | 276 | template 277 | bool setPrimalVariable(const Eigen::Matrix& primalVariable); 278 | 279 | template bool setDualVariable(const Eigen::Matrix& dualVariable); 280 | 281 | template bool getPrimalVariable(Eigen::Matrix& primalVariable); 282 | 283 | template bool getDualVariable(Eigen::Matrix& dualVariable); 284 | 285 | /** 286 | * Get the solver settings pointer. 287 | * @return the pointer to Settings object. 288 | */ 289 | const std::unique_ptr& settings() const; 290 | 291 | /** 292 | * Get the pointer to the solver initial data. 293 | * @return the pointer to Data object. 294 | */ 295 | const std::unique_ptr& data() const; 296 | 297 | #ifdef OSQP_EIGEN_OSQP_IS_V1 298 | /** 299 | * Get the pointer to the OSQP solver. 300 | * @return the pointer to Solver object. 301 | */ 302 | const std::unique_ptr>& solver() const; 303 | #else 304 | /** 305 | * Get the pointer to the OSQP workspace. 306 | * @return the pointer to Workspace object. 307 | */ 308 | const std::unique_ptr>& workspace() const; 309 | #endif 310 | }; 311 | 312 | #include 313 | } // namespace OsqpEigen 314 | 315 | #endif 316 | -------------------------------------------------------------------------------- /include/OsqpEigen/Solver.tpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Solver.tpp 3 | * @author Giulio Romualdi 4 | * @copyright Released under the terms of the BSD 3-Clause License 5 | * @date 2018 6 | */ 7 | 8 | #include 9 | #ifndef OSQP_EIGEN_OSQP_IS_V1 10 | #include 11 | #include 12 | #endif 13 | 14 | #include "Debug.hpp" 15 | 16 | template 17 | bool OsqpEigen::Solver::updateHessianMatrix( 18 | const Eigen::SparseCompressedBase& hessianMatrix) 19 | { 20 | if (!m_isSolverInitialized) 21 | { 22 | debugStream() << "[OsqpEigen::Solver::updateHessianMatrix] The solver has not been " 23 | "initialized." 24 | << std::endl; 25 | return false; 26 | } 27 | 28 | if (((c_int)hessianMatrix.rows() != getData()->n) 29 | || ((c_int)hessianMatrix.cols() != getData()->n)) 30 | { 31 | debugStream() << "[OsqpEigen::Solver::updateHessianMatrix] The hessian matrix has to be a " 32 | "nxn matrix" 33 | << std::endl; 34 | return false; 35 | } 36 | 37 | // evaluate the triplets from old and new hessian sparse matrices 38 | if (!OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToTriplets(getData()->P, 39 | m_oldHessianTriplet)) 40 | { 41 | debugStream() << "[OsqpEigen::Solver::updateHessianMatrix] Unable to evaluate triplets " 42 | "from the old hessian matrix." 43 | << std::endl; 44 | return false; 45 | } 46 | if (!OsqpEigen::SparseMatrixHelper::eigenSparseMatrixToTriplets(hessianMatrix, 47 | m_newHessianTriplet)) 48 | { 49 | debugStream() << "[OsqpEigen::Solver::updateHessianMatrix] Unable to evaluate triplets " 50 | "from the old hessian matrix." 51 | << std::endl; 52 | return false; 53 | } 54 | 55 | selectUpperTriangularTriplets(m_newHessianTriplet, m_newUpperTriangularHessianTriplets); 56 | 57 | // try to update the hessian matrix without reinitialize the solver 58 | // according to the osqp library it can be done only if the sparsity pattern of the hessian 59 | // matrix does not change. 60 | 61 | if (evaluateNewValues(m_oldHessianTriplet, 62 | m_newUpperTriangularHessianTriplets, 63 | m_hessianNewIndices, 64 | m_hessianNewValues)) 65 | { 66 | if (m_hessianNewValues.size() > 0) 67 | { 68 | #ifdef OSQP_EIGEN_OSQP_IS_V1 69 | if (osqp_update_data_mat(m_solver.get(), 70 | m_hessianNewValues.data(), 71 | m_hessianNewIndices.data(), 72 | m_hessianNewIndices.size(), 73 | nullptr, 74 | nullptr, 75 | 0) 76 | != 0) 77 | { 78 | #else 79 | if (osqp_update_P(m_workspace.get(), 80 | m_hessianNewValues.data(), 81 | m_hessianNewIndices.data(), 82 | m_hessianNewIndices.size()) 83 | != 0) 84 | { 85 | #endif 86 | debugStream() << "[OsqpEigen::Solver::updateHessianMatrix] Unable to update " 87 | "hessian matrix." 88 | << std::endl; 89 | return false; 90 | } 91 | } 92 | } else 93 | { 94 | // the sparsity pattern has changed 95 | // the solver has to be setup again 96 | 97 | // get the primal and the dual variables 98 | 99 | if (!getPrimalVariable(m_primalVariables)) 100 | { 101 | debugStream() << "[OsqpEigen::Solver::updateHessianMatrix] Unable to get the primal " 102 | "variable." 103 | << std::endl; 104 | return false; 105 | } 106 | 107 | if (!getDualVariable(m_dualVariables)) 108 | { 109 | debugStream() << "[OsqpEigen::Solver::updateHessianMatrix] Unable to get the dual " 110 | "variable." 111 | << std::endl; 112 | return false; 113 | } 114 | 115 | // clear old hessian matrix 116 | m_data->clearHessianMatrix(); 117 | 118 | // set new hessian matrix 119 | if (!m_data->setHessianMatrix(hessianMatrix)) 120 | { 121 | debugStream() << "[OsqpEigen::Solver::updateHessianMatrix] Unable to update the " 122 | "hessian matrix in " 123 | << "OptimizaroData object." << std::endl; 124 | return false; 125 | } 126 | 127 | // clear the old solver 128 | clearSolver(); 129 | 130 | // initialize a new solver 131 | if (!initSolver()) 132 | { 133 | debugStream() << "[OsqpEigen::Solver::updateHessianMatrix] Unable to Initialize the " 134 | "solver." 135 | << std::endl; 136 | return false; 137 | } 138 | 139 | // set the old primal and dual variables 140 | if (!setPrimalVariable(m_primalVariables)) 141 | { 142 | debugStream() << "[OsqpEigen::Solver::updateHessianMatrix] Unable to set the primal " 143 | "variable." 144 | << std::endl; 145 | return false; 146 | } 147 | 148 | if (!setDualVariable(m_dualVariables)) 149 | { 150 | debugStream() << "[OsqpEigen::Solver::updateHessianMatrix] Unable to set the dual " 151 | "variable." 152 | << std::endl; 153 | return false; 154 | } 155 | } 156 | return true; 157 | } 158 | 159 | template 160 | bool OsqpEigen::Solver::updateLinearConstraintsMatrix( 161 | const Eigen::SparseCompressedBase& linearConstraintsMatrix) 162 | { 163 | if (!m_isSolverInitialized) 164 | { 165 | debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] The solver has not " 166 | "been initialized." 167 | << std::endl; 168 | return false; 169 | } 170 | 171 | if (((c_int)linearConstraintsMatrix.rows() != getData()->m) 172 | || ((c_int)linearConstraintsMatrix.cols() != getData()->n)) 173 | { 174 | debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] The constraints " 175 | "matrix has to be a mxn matrix" 176 | << std::endl; 177 | return false; 178 | } 179 | 180 | // evaluate the triplets from old and new hessian sparse matrices 181 | 182 | if (!OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToTriplets(getData()->A, 183 | m_oldLinearConstraintsTriplet)) 184 | { 185 | debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] Unable to evaluate " 186 | "triplets from the old hessian matrix." 187 | << std::endl; 188 | return false; 189 | } 190 | if (!OsqpEigen::SparseMatrixHelper::eigenSparseMatrixToTriplets(linearConstraintsMatrix, 191 | m_newLinearConstraintsTriplet)) 192 | { 193 | debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] Unable to evaluate " 194 | "triplets from the old hessian matrix." 195 | << std::endl; 196 | return false; 197 | } 198 | 199 | // try to update the linear constraints matrix without reinitialize the solver 200 | // according to the osqp library it can be done only if the sparsity pattern of the 201 | // matrix does not change. 202 | 203 | if (evaluateNewValues(m_oldLinearConstraintsTriplet, 204 | m_newLinearConstraintsTriplet, 205 | m_constraintsNewIndices, 206 | m_constraintsNewValues)) 207 | { 208 | if (m_constraintsNewValues.size() > 0) 209 | { 210 | #ifdef OSQP_EIGEN_OSQP_IS_V1 211 | if (osqp_update_data_mat(m_solver.get(), 212 | nullptr, 213 | nullptr, 214 | 0, 215 | m_constraintsNewValues.data(), 216 | m_constraintsNewIndices.data(), 217 | m_constraintsNewIndices.size()) 218 | != 0) 219 | { 220 | #else 221 | if (osqp_update_A(m_workspace.get(), 222 | m_constraintsNewValues.data(), 223 | m_constraintsNewIndices.data(), 224 | m_constraintsNewIndices.size()) 225 | != 0) 226 | { 227 | #endif 228 | debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] Unable to " 229 | "update linear constraints matrix." 230 | << std::endl; 231 | return false; 232 | } 233 | } 234 | } else 235 | { 236 | // the sparsity pattern has changed 237 | // the solver has to be setup again 238 | 239 | // get the primal and the dual variables 240 | 241 | if (!getPrimalVariable(m_primalVariables)) 242 | { 243 | debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] Unable to get the " 244 | "primal variable." 245 | << std::endl; 246 | return false; 247 | } 248 | 249 | if (!getDualVariable(m_dualVariables)) 250 | { 251 | debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] Unable to get the " 252 | "dual variable." 253 | << std::endl; 254 | return false; 255 | } 256 | 257 | // clear old linear constraints matrix 258 | m_data->clearLinearConstraintsMatrix(); 259 | 260 | // set new linear constraints matrix 261 | if (!m_data->setLinearConstraintsMatrix(linearConstraintsMatrix)) 262 | { 263 | debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] Unable to update " 264 | "the hessian matrix in " 265 | << "Data object." << std::endl; 266 | return false; 267 | } 268 | 269 | // clear the old solver 270 | clearSolver(); 271 | 272 | if (!initSolver()) 273 | { 274 | debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] Unable to " 275 | "Initialize the solver." 276 | << std::endl; 277 | return false; 278 | } 279 | 280 | // set the old primal and dual variables 281 | if (!setPrimalVariable(m_primalVariables)) 282 | { 283 | debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] Unable to set the " 284 | "primal variable." 285 | << std::endl; 286 | return false; 287 | } 288 | 289 | if (!setDualVariable(m_dualVariables)) 290 | { 291 | debugStream() << "[OsqpEigen::Solver::updateLinearConstraintsMatrix] Unable to set the " 292 | "dual variable." 293 | << std::endl; 294 | return false; 295 | } 296 | } 297 | return true; 298 | } 299 | 300 | template 301 | bool OsqpEigen::Solver::setWarmStart(const Eigen::Matrix& primalVariable, 302 | const Eigen::Matrix& dualVariable) 303 | { 304 | if (!m_isSolverInitialized) 305 | { 306 | debugStream() << "[OsqpEigen::Solver::setWarmStart] The solver is not initialized" 307 | << std::endl; 308 | return false; 309 | } 310 | 311 | if (primalVariable.rows() != getData()->n) 312 | { 313 | debugStream() << "[OsqpEigen::Solver::setWarmStart] The size of the primal variable vector " 314 | "has to be equal to " 315 | << " the number of variables." << std::endl; 316 | return false; 317 | } 318 | 319 | if (dualVariable.rows() != getData()->m) 320 | { 321 | debugStream() << "[OsqpEigen::Solver::setWarmStart] The size of the dual variable vector " 322 | "has to be equal to " 323 | << " the number of constraints." << std::endl; 324 | return false; 325 | } 326 | 327 | m_primalVariables = primalVariable.template cast(); 328 | m_dualVariables = dualVariable.template cast(); 329 | 330 | #ifdef OSQP_EIGEN_OSQP_IS_V1 331 | return (osqp_warm_start(m_solver.get(), m_primalVariables.data(), m_dualVariables.data()) == 0); 332 | #else 333 | return (osqp_warm_start(m_workspace.get(), m_primalVariables.data(), m_dualVariables.data()) 334 | == 0); 335 | #endif 336 | } 337 | 338 | template 339 | bool OsqpEigen::Solver::setPrimalVariable(const Eigen::Matrix& primalVariable) 340 | { 341 | if (!m_isSolverInitialized) 342 | { 343 | debugStream() << "[OsqpEigen::Solver::setPrimalVariable] The solver is not initialized" 344 | << std::endl; 345 | return false; 346 | } 347 | 348 | if (primalVariable.rows() != getData()->n) 349 | { 350 | debugStream() << "[OsqpEigen::Solver::setPrimalVariable] The size of the primal variable " 351 | "vector has to be equal to " 352 | << " the number of variables." << std::endl; 353 | return false; 354 | } 355 | 356 | m_primalVariables = primalVariable.template cast(); 357 | 358 | #ifdef OSQP_EIGEN_OSQP_IS_V1 359 | return (osqp_warm_start(m_solver.get(), m_primalVariables.data(), nullptr) == 0); 360 | #else 361 | return (osqp_warm_start_x(m_workspace.get(), m_primalVariables.data()) == 0); 362 | #endif 363 | } 364 | 365 | template 366 | bool OsqpEigen::Solver::setDualVariable(const Eigen::Matrix& dualVariable) 367 | { 368 | if (dualVariable.rows() != getData()->m) 369 | { 370 | debugStream() << "[OsqpEigen::Solver::setDualVariable] The size of the dual variable " 371 | "vector has to be equal to " 372 | << " the number of constraints." << std::endl; 373 | return false; 374 | } 375 | 376 | m_dualVariables = dualVariable.template cast(); 377 | 378 | #ifdef OSQP_EIGEN_OSQP_IS_V1 379 | return (osqp_warm_start(m_solver.get(), nullptr, m_dualVariables.data()) == 0); 380 | #else 381 | return (osqp_warm_start_y(m_workspace.get(), m_dualVariables.data()) == 0); 382 | #endif 383 | } 384 | 385 | template 386 | bool OsqpEigen::Solver::getPrimalVariable(Eigen::Matrix& primalVariable) 387 | { 388 | if (!m_isSolverInitialized) 389 | { 390 | debugStream() << "[OsqpEigen::Solver::getPrimalVariable] The solver is not initialized" 391 | << std::endl; 392 | return false; 393 | } 394 | 395 | if (n == Eigen::Dynamic) 396 | { 397 | primalVariable.resize(getData()->n, 1); 398 | } else 399 | { 400 | if (n != getData()->n) 401 | { 402 | debugStream() << "[OsqpEigen::Solver::getPrimalVariable] The size of the vector has to " 403 | "be equal to the number of variables. (You can use an eigen dynamic " 404 | "vector)" 405 | << std::endl; 406 | return false; 407 | } 408 | } 409 | 410 | #ifdef OSQP_EIGEN_OSQP_IS_V1 411 | primalVariable = Eigen::Map>(m_solver->solution->x, getData()->n) 412 | .template cast(); 413 | #else 414 | primalVariable 415 | = Eigen::Map>(m_workspace->x, getData()->n).template cast(); 416 | #endif 417 | 418 | return true; 419 | } 420 | 421 | template 422 | bool OsqpEigen::Solver::getDualVariable(Eigen::Matrix& dualVariable) 423 | { 424 | if (!m_isSolverInitialized) 425 | { 426 | debugStream() << "[OsqpEigen::Solver::getDualVariable] The solver is not initialized" 427 | << std::endl; 428 | return false; 429 | } 430 | 431 | if (m == Eigen::Dynamic) 432 | { 433 | dualVariable.resize(getData()->m, 1); 434 | } else 435 | { 436 | if (m != getData()->m) 437 | { 438 | debugStream() << "[OsqpEigen::Solver::getDualVariable] The size of the vector has to " 439 | "be equal to the number of constraints. (You can use an eigen dynamic " 440 | "vector)" 441 | << std::endl; 442 | return false; 443 | } 444 | } 445 | 446 | #ifdef OSQP_EIGEN_OSQP_IS_V1 447 | dualVariable = Eigen::Map>(m_solver->solution->y, getData()->m) 448 | .template cast(); 449 | #else 450 | dualVariable 451 | = Eigen::Map>(m_workspace->y, getData()->m).template cast(); 452 | #endif 453 | 454 | return true; 455 | } 456 | 457 | template 458 | bool OsqpEigen::Solver::evaluateNewValues(const std::vector>& oldMatrixTriplet, 459 | const std::vector>& newMatrixTriplet, 460 | std::vector& newIndices, 461 | std::vector& newValues) const 462 | { 463 | // When updating the matrices for osqp, we need to provide the indices to modify of the value 464 | // vector. The following can work since, when extracting triplets from osqp sparse matrices, the 465 | // order of the triplets follows the same order of the value vector. 466 | // check if the sparsity pattern is changed 467 | size_t valuesAdded = 0; 468 | if (newMatrixTriplet.size() == oldMatrixTriplet.size()) 469 | { 470 | for (int i = 0; i < newMatrixTriplet.size(); i++) 471 | { 472 | // check if the sparsity pattern is changed 473 | if ((newMatrixTriplet[i].row() != oldMatrixTriplet[i].row()) 474 | || (newMatrixTriplet[i].col() != oldMatrixTriplet[i].col())) 475 | return false; 476 | 477 | // check if an old value is changed 478 | if (newMatrixTriplet[i].value() != oldMatrixTriplet[i].value()) 479 | { 480 | if (valuesAdded >= newValues.size()) 481 | { 482 | newValues.push_back((c_float)newMatrixTriplet[i].value()); 483 | newIndices.push_back((c_int)i); 484 | valuesAdded++; 485 | } else 486 | { 487 | newValues[valuesAdded] = static_cast(newMatrixTriplet[i].value()); 488 | newIndices[valuesAdded] = static_cast(i); 489 | valuesAdded++; 490 | } 491 | } 492 | } 493 | newValues.erase(newValues.begin() + valuesAdded, newValues.end()); 494 | newIndices.erase(newIndices.begin() + valuesAdded, newIndices.end()); 495 | return true; 496 | } 497 | return false; 498 | } 499 | 500 | template 501 | void OsqpEigen::Solver::selectUpperTriangularTriplets( 502 | const std::vector>& fullMatrixTriplets, 503 | std::vector>& upperTriangularMatrixTriplets) const 504 | { 505 | 506 | int upperTriangularTriplets = 0; 507 | for (int i = 0; i < fullMatrixTriplets.size(); ++i) 508 | { 509 | if (fullMatrixTriplets[i].row() <= fullMatrixTriplets[i].col()) 510 | { 511 | if (upperTriangularTriplets < upperTriangularMatrixTriplets.size()) 512 | { 513 | upperTriangularMatrixTriplets[upperTriangularTriplets] = fullMatrixTriplets[i]; 514 | } else 515 | { 516 | upperTriangularMatrixTriplets.push_back(fullMatrixTriplets[i]); 517 | } 518 | upperTriangularTriplets++; 519 | } 520 | } 521 | 522 | upperTriangularMatrixTriplets.erase(upperTriangularMatrixTriplets.begin() 523 | + upperTriangularTriplets, 524 | upperTriangularMatrixTriplets.end()); 525 | } 526 | -------------------------------------------------------------------------------- /include/OsqpEigen/SparseMatrixHelper.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file SparseMatrixHelper.hpp 3 | * @author Giulio Romualdi 4 | * @copyright Released under the terms of the BSD 3-Clause License 5 | * @date 2018 6 | */ 7 | 8 | #ifndef SPARSE_MATRIX_HPP 9 | #define SPARSE_MATRIX_HPP 10 | 11 | // std 12 | #include 13 | 14 | // eigen 15 | #include 16 | 17 | #include 18 | 19 | /** 20 | * OsqpEigen namespace. 21 | */ 22 | namespace OsqpEigen 23 | { 24 | /** 25 | * SparseMatrixHelper namespace is a namespace that contains helper function to handle osqp matrix. 26 | * Use it to create to update or manage an osqp sparse matrix. 27 | * osqp sparse matrix in [compressed-column](https://people.sc.fsu.edu/~jburkardt/data/cc/cc.html) 28 | * or triplet form. 29 | */ 30 | namespace SparseMatrixHelper 31 | { 32 | 33 | /** 34 | * Allocate an osqpSparseMatrix struct. 35 | * NOTE: c_malloc function is used to allocate memory please call 36 | * c_free to deallcate memory. 37 | * @return a const point to the csc struct. 38 | */ 39 | template 40 | bool createOsqpSparseMatrix(const Eigen::SparseCompressedBase& eigenSparseMatrix, 41 | csc*& osqpSparseMatrix); 42 | 43 | /** 44 | * Convert an osqp sparse matrix into an eigen sparse matrix. 45 | * @param osqpSparseMatrix is a constant pointer to a constant csc struct; 46 | * @param eigenSparseMatrix is the eigen sparse matrix object. 47 | * @return a const point to the csc struct. 48 | */ 49 | template 50 | bool osqpSparseMatrixToEigenSparseMatrix(const csc* const& osqpSparseMatrix, 51 | Eigen::SparseMatrix& eigenSparseMatrix); 52 | 53 | /** 54 | * Convert an osqp sparse matrix into a eigen triplet list. 55 | * @param osqpSparseMatrix is reference to a constant pointer to a constant csc struct; 56 | * @param tripletList is a std::vector containing the triplet. 57 | * @return a const point to the csc struct. 58 | */ 59 | template 60 | bool osqpSparseMatrixToTriplets(const csc* const& osqpSparseMatrix, 61 | std::vector>& tripletList); 62 | 63 | /** 64 | * Convert an eigen sparse matrix into a eigen triplet list. 65 | * @param eigenSparseMatrix is the eigen sparse matrix object; 66 | * @param tripletList is a std::vector containing the triplet. 67 | * @return a const point to the csc struct. 68 | */ 69 | template 70 | bool eigenSparseMatrixToTriplets(const Eigen::SparseCompressedBase& eigenSparseMatrix, 71 | std::vector>& tripletList); 72 | }; // namespace SparseMatrixHelper 73 | } // namespace OsqpEigen 74 | 75 | #include 76 | 77 | #endif 78 | -------------------------------------------------------------------------------- /include/OsqpEigen/SparseMatrixHelper.tpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file SparseMatrixHelper.tpp 3 | * @author Giulio Romualdi 4 | * @copyright Released under the terms of the BSD 3-Clause License 5 | * @date 2018 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | template 12 | bool OsqpEigen::SparseMatrixHelper::createOsqpSparseMatrix( 13 | const Eigen::SparseCompressedBase& eigenSparseMatrix, csc*& osqpSparseMatrix) 14 | 15 | { 16 | // Copying into a new sparse matrix to be sure to use a CSC matrix 17 | Eigen::SparseMatrix colMajorCopy; 18 | 19 | // This may perform memory allocation, but this is already the case for allocating the 20 | // osqpSparseMatrix 21 | colMajorCopy = eigenSparseMatrix; 22 | 23 | // get number of row, columns and nonZeros from Eigen SparseMatrix 24 | c_int rows = colMajorCopy.rows(); 25 | c_int cols = colMajorCopy.cols(); 26 | c_int numberOfNonZeroCoeff = colMajorCopy.nonZeros(); 27 | 28 | // get innerr and outer index 29 | const int* outerIndexPtr = colMajorCopy.outerIndexPtr(); 30 | const int* innerNonZerosPtr = colMajorCopy.innerNonZeroPtr(); 31 | 32 | // instantiate csc matrix 33 | // MEMORY ALLOCATION!! 34 | if (osqpSparseMatrix != nullptr) 35 | { 36 | debugStream() << "[OsqpEigen::SparseMatrixHelper::createOsqpSparseMatrix] osqpSparseMatrix " 37 | "pointer is not a null pointer! " 38 | << std::endl; 39 | return false; 40 | } 41 | 42 | osqpSparseMatrix = OsqpEigen::spalloc(rows, cols, numberOfNonZeroCoeff); 43 | 44 | int innerOsqpPosition = 0; 45 | for (int k = 0; k < cols; k++) 46 | { 47 | if (colMajorCopy.isCompressed()) 48 | { 49 | osqpSparseMatrix->p[k] = static_cast(outerIndexPtr[k]); 50 | } else 51 | { 52 | if (k == 0) 53 | { 54 | osqpSparseMatrix->p[k] = 0; 55 | } else 56 | { 57 | osqpSparseMatrix->p[k] = osqpSparseMatrix->p[k - 1] + innerNonZerosPtr[k - 1]; 58 | } 59 | } 60 | for (typename Eigen::SparseMatrix::InnerIterator it(colMajorCopy, k); 62 | it; 63 | ++it) 64 | { 65 | osqpSparseMatrix->i[innerOsqpPosition] = static_cast(it.row()); 66 | osqpSparseMatrix->x[innerOsqpPosition] = static_cast(it.value()); 67 | innerOsqpPosition++; 68 | } 69 | } 70 | osqpSparseMatrix->p[static_cast(cols)] = static_cast(innerOsqpPosition); 71 | 72 | assert(innerOsqpPosition == numberOfNonZeroCoeff); 73 | 74 | return true; 75 | } 76 | 77 | template 78 | bool OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToTriplets( 79 | const csc* const& osqpSparseMatrix, std::vector>& tripletList) 80 | { 81 | // if the matrix is not instantiate the triplets vector is empty 82 | if (osqpSparseMatrix == nullptr) 83 | { 84 | debugStream() << "[OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToTriplets] the " 85 | "osqpSparseMatrix is not initialized." 86 | << std::endl; 87 | return false; 88 | } 89 | 90 | // get row and column data 91 | c_int* innerIndexPtr = osqpSparseMatrix->i; 92 | c_int* outerIndexPtr = osqpSparseMatrix->p; 93 | 94 | // get values data 95 | c_float* valuePtr = osqpSparseMatrix->x; 96 | c_int numberOfNonZeroCoeff = osqpSparseMatrix->p[osqpSparseMatrix->n]; 97 | 98 | // populate the tripletes vector 99 | int column = 0; 100 | int row; 101 | c_float value; 102 | 103 | tripletList.resize(numberOfNonZeroCoeff); 104 | for (int i = 0; i < numberOfNonZeroCoeff; i++) 105 | { 106 | row = innerIndexPtr[i]; 107 | value = valuePtr[i]; 108 | 109 | while (i >= outerIndexPtr[column + 1]) 110 | column++; 111 | 112 | tripletList[i] = Eigen::Triplet(row, column, static_cast(value)); 113 | } 114 | 115 | tripletList.erase(tripletList.begin() + numberOfNonZeroCoeff, tripletList.end()); 116 | 117 | return true; 118 | } 119 | 120 | template 121 | bool OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToEigenSparseMatrix( 122 | const csc* const& osqpSparseMatrix, Eigen::SparseMatrix& eigenSparseMatrix) 123 | { 124 | // if the matrix is not instantiate the eigen matrix is empty 125 | if (osqpSparseMatrix == nullptr) 126 | { 127 | debugStream() << "[OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToEigenSparseMatrix] the " 128 | "osqpSparseMatrix is not initialized." 129 | << std::endl; 130 | return false; 131 | } 132 | 133 | // get the number of rows and columns 134 | int rows = osqpSparseMatrix->m; 135 | int cols = osqpSparseMatrix->n; 136 | 137 | // get the triplets from the csc matrix 138 | std::vector> tripletList; 139 | 140 | OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToTriplets(osqpSparseMatrix, tripletList); 141 | 142 | // resize the eigen matrix 143 | eigenSparseMatrix.resize(rows, cols); 144 | 145 | // set the eigen matrix from triplets 146 | eigenSparseMatrix.setFromTriplets(tripletList.begin(), tripletList.end()); 147 | return true; 148 | } 149 | 150 | template 151 | bool OsqpEigen::SparseMatrixHelper::eigenSparseMatrixToTriplets( 152 | const Eigen::SparseCompressedBase& eigenSparseMatrix, 153 | std::vector>& tripletList) 154 | { 155 | if (eigenSparseMatrix.nonZeros() == 0) 156 | { 157 | debugStream() << "[OsqpEigen::SparseMatrixHelper::eigenSparseMatrixToTriplets] The " 158 | "eigenSparseMatrix is empty." 159 | << std::endl; 160 | return false; 161 | } 162 | 163 | tripletList.resize(eigenSparseMatrix.nonZeros()); 164 | // populate the triplet list 165 | int nonZero = 0; 166 | for (int k = 0; k < eigenSparseMatrix.outerSize(); ++k) 167 | { 168 | for (typename Eigen::SparseCompressedBase::InnerIterator it(eigenSparseMatrix, k); 169 | it; 170 | ++it) 171 | { 172 | tripletList[nonZero] 173 | = Eigen::Triplet(it.row(), it.col(), static_cast(it.value())); 174 | nonZero++; 175 | } 176 | } 177 | tripletList.erase(tripletList.begin() + eigenSparseMatrix.nonZeros(), tripletList.end()); 178 | 179 | return true; 180 | } 181 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | osqp-eigen 4 | 0.10.0 5 | Simple Eigen-C++ wrapper for OSQP library 6 | tbd 7 | 8 | BSD 9 | 10 | https://github.com/robotology/osqp-eigen 11 | 12 | git 13 | 14 | catkin 15 | osqp 16 | eigen 17 | 18 | cmake 19 | 20 | cmake 21 | 22 | 23 | -------------------------------------------------------------------------------- /pixi.toml: -------------------------------------------------------------------------------- 1 | [project] 2 | name = "osqp-eigen" 3 | authors = ["Stefano Dafarra "] 4 | # As this version is currently ignored, we do not 5 | # waste effort in mantain it in synch with the value 6 | # specified in CMakeLists.txt 7 | version = "0.0.0" 8 | description = "Simple Eigen-C++ wrapper for OSQP library." 9 | channels = ["conda-forge"] 10 | platforms = ["linux-64", "linux-aarch64", "win-64", "osx-64", "osx-arm64"] 11 | 12 | 13 | [target.win.activation.env] 14 | CMAKE_INSTALL_PREFIX = "%CONDA_PREFIX%\\Library" 15 | OSQPEIGEN_RUN_Valgrind_tests = "OFF" 16 | 17 | [target.unix.activation.env] 18 | CMAKE_INSTALL_PREFIX = "$CONDA_PREFIX" 19 | OSQPEIGEN_RUN_Valgrind_tests = "OFF" 20 | 21 | [target.linux.activation.env] 22 | OSQPEIGEN_RUN_Valgrind_tests = "ON" 23 | 24 | [tasks] 25 | configure = { cmd = [ 26 | "cmake", 27 | "-DCMAKE_BUILD_TYPE=Release", 28 | "-DOSQPEIGEN_RUN_Valgrind_tests=$OSQPEIGEN_RUN_Valgrind_tests", 29 | "-DBUILD_TESTING:BOOL=ON", 30 | # Use the cross-platform Ninja generator 31 | "-G", 32 | "Ninja", 33 | # The source is in the root directory 34 | "-S", 35 | ".", 36 | # We wanna build in the .build directory 37 | "-B", 38 | ".build", 39 | ]} 40 | 41 | build = { cmd = "cmake --build .build --config Release", depends-on = ["configure"] } 42 | test = { cmd = "ctest --test-dir .build --build-config Release --output-on-failure", depends-on = ["build"] } 43 | install = { cmd = ["cmake", "--install", ".build", "--config", "Release"], depends-on = ["build"] } 44 | uninstall = { cmd = ["cmake", "--build", ".build", "--target", "uninstall"]} 45 | 46 | [dependencies] 47 | cmake = "*" 48 | c-compiler = "*" 49 | cxx-compiler = "*" 50 | ninja = "*" 51 | pkg-config = "*" 52 | libosqp = "*" 53 | eigen = "*" 54 | ycm-cmake-modules = "*" 55 | catch2 = "*" 56 | 57 | [target.linux.dependencies] 58 | valgrind = "*" 59 | 60 | [feature.bazel.dependencies] 61 | bazel = "*" 62 | # Transitive dependencies of bazel-cmake-deps-override 63 | toml = "*" 64 | 65 | [feature.bazel.pypi-dependencies] 66 | bazel-cmake-deps-override = { git ="https://github.com/ami-iit/bazel-cmake-deps-override.git", rev = "8b8b9c9c22a67f76b8fe819e88c8f27899247007" } 67 | 68 | [feature.bazel.tasks] 69 | clean-bazel = "bazel clean" 70 | 71 | # Task to test and build osqp-eigen with bazel with bazel-provided dependencies 72 | test-bazel = "bazel test //..." 73 | 74 | # Tasks to test and build osqp-eigen with bazel with bazel-provided dependencies 75 | bazel-ext-deps-generate-metadata = "bazel-cmake-deps-override osqp eigen catch2" 76 | test-bazel-ext-deps = { cmd = "bazel --bazelrc=./bazel-cmake-deps-overrides/bazelrc test -s //...", depends-on = "bazel-ext-deps-generate-metadata" } 77 | 78 | [environments] 79 | # solve-group is meant to be sure that for common 80 | # dependencies the version used by the two envs are the same 81 | default = {solve-group = "common"} 82 | bazel = { features = ["bazel"], solve-group = "common"} 83 | -------------------------------------------------------------------------------- /src/Data.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Data.cpp 3 | * @author Giulio Romualdi 4 | * @copyright Released under the terms of the BSD 3-Clause License 5 | * @date 2018 6 | */ 7 | 8 | // std 9 | #include 10 | 11 | // OsqpEigen 12 | #include 13 | #include 14 | 15 | OsqpEigen::Data::Data() 16 | : m_isNumberOfVariablesSet(false), 17 | m_isNumberOfConstraintsSet(false), 18 | m_isHessianMatrixSet(false), 19 | m_isGradientSet(false), 20 | m_isLinearConstraintsMatrixSet(false), 21 | m_isLowerBoundSet(false), 22 | m_isUpperBoundSet(false) 23 | { 24 | m_data = (OSQPData *)c_malloc(sizeof(OSQPData)); 25 | m_data->P = nullptr; 26 | m_data->A = nullptr; 27 | } 28 | 29 | OsqpEigen::Data::Data(int n, int m) 30 | : m_isNumberOfVariablesSet(true), 31 | m_isNumberOfConstraintsSet(true), 32 | m_isHessianMatrixSet(false), 33 | m_isGradientSet(false), 34 | m_isLinearConstraintsMatrixSet(false), 35 | m_isLowerBoundSet(false), 36 | m_isUpperBoundSet(false) 37 | { 38 | m_data = (OSQPData *)c_malloc(sizeof(OSQPData)); 39 | m_data->P = nullptr; 40 | m_data->A = nullptr; 41 | 42 | setNumberOfVariables(n); 43 | setNumberOfConstraints(m); 44 | } 45 | 46 | void OsqpEigen::Data::clearHessianMatrix() 47 | { 48 | if (m_isHessianMatrixSet) 49 | { 50 | m_isHessianMatrixSet = false; 51 | OsqpEigen::spfree(m_data->P); 52 | m_data->P = nullptr; 53 | } 54 | } 55 | 56 | void OsqpEigen::Data::clearLinearConstraintsMatrix() 57 | { 58 | if (m_isLinearConstraintsMatrixSet) 59 | { 60 | m_isLinearConstraintsMatrixSet = false; 61 | OsqpEigen::spfree(m_data->A); 62 | m_data->A = nullptr; 63 | } 64 | } 65 | 66 | OsqpEigen::Data::~Data() 67 | { 68 | clearHessianMatrix(); 69 | clearLinearConstraintsMatrix(); 70 | c_free(m_data); 71 | } 72 | 73 | void OsqpEigen::Data::setNumberOfVariables(int n) 74 | { 75 | m_isNumberOfVariablesSet = true; 76 | m_data->n = n; 77 | } 78 | 79 | void OsqpEigen::Data::setNumberOfConstraints(int m) 80 | { 81 | m_isNumberOfConstraintsSet = true; 82 | m_data->m = m; 83 | } 84 | 85 | auto OsqpEigen::Data::getData() const -> OSQPData *const & 86 | { 87 | return m_data; 88 | } 89 | 90 | bool OsqpEigen::Data::isSet() const 91 | { 92 | const bool areConstraintsOk = (m_data->m == 0) || 93 | (m_isLinearConstraintsMatrixSet && 94 | m_isLowerBoundSet && 95 | m_isUpperBoundSet); 96 | 97 | return m_isNumberOfVariablesSet && 98 | m_isNumberOfConstraintsSet && 99 | m_isHessianMatrixSet && 100 | m_isGradientSet && 101 | areConstraintsOk; 102 | } 103 | 104 | bool OsqpEigen::Data::setGradient(Eigen::Ref> gradient) 105 | { 106 | if (gradient.rows() != m_data->n) 107 | { 108 | debugStream() << "[OsqpEigen::Data::setGradient] The size of the gradient must be equal to the number of the variables." 109 | << std::endl; 110 | return false; 111 | } 112 | m_isGradientSet = true; 113 | m_data->q = gradient.data(); 114 | return true; 115 | } 116 | 117 | Eigen::Matrix OsqpEigen::Data::getGradient() 118 | { 119 | return Eigen::Map>(m_data->q, m_data->n); 120 | } 121 | 122 | bool OsqpEigen::Data::setLowerBound(Eigen::Ref> lowerBound) 123 | { 124 | if (lowerBound.rows() != m_data->m) 125 | { 126 | debugStream() << "[OsqpEigen::Data::setLowerBound] The size of the lower bound must be equal to the number of the variables." 127 | << std::endl; 128 | return false; 129 | } 130 | m_isLowerBoundSet = true; 131 | m_data->l = lowerBound.data(); 132 | return true; 133 | } 134 | 135 | bool OsqpEigen::Data::setUpperBound(Eigen::Ref> upperBound) 136 | { 137 | if (upperBound.rows() != m_data->m) 138 | { 139 | debugStream() << "[OsqpEigen::Data::setUpperBound] The size of the upper bound must be equal to the number of the variables." 140 | << std::endl; 141 | return false; 142 | } 143 | m_isUpperBoundSet = true; 144 | m_data->u = upperBound.data(); 145 | return true; 146 | } 147 | 148 | bool OsqpEigen::Data::setBounds(Eigen::Ref> lowerBound, 149 | Eigen::Ref> upperBound) 150 | { 151 | bool ok = true; 152 | 153 | ok = ok && this->setLowerBound(lowerBound); 154 | ok = ok && this->setUpperBound(upperBound); 155 | 156 | return ok; 157 | } 158 | -------------------------------------------------------------------------------- /src/Debug.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Debug.cpp 3 | * @copyright Released under the terms of the BSD 3-Clause License 4 | * @date 2023 5 | */ 6 | #include 7 | 8 | namespace OsqpEigen 9 | { 10 | 11 | // Taken from https://stackoverflow.com/a/8243866/2702753 12 | class NullStream : public std::ostream 13 | { 14 | public: 15 | NullStream() 16 | : std::ostream(nullptr) 17 | { 18 | } 19 | NullStream(const NullStream&) 20 | : std::ostream(nullptr) 21 | { 22 | } 23 | }; 24 | 25 | template const NullStream& operator<<(NullStream&& os, const T&) 26 | { 27 | return os; 28 | } 29 | 30 | NullStream theStream; 31 | 32 | std::ostream& debugStream() 33 | { 34 | #ifdef OSQP_EIGEN_DEBUG_OUTPUT 35 | return std::cerr; 36 | #else 37 | return theStream; 38 | #endif 39 | } 40 | } // namespace OsqpEigen 41 | -------------------------------------------------------------------------------- /src/Settings.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Settings.cpp 3 | * @author Giulio Romualdi 4 | * @copyright Released under the terms of the BSD 3-Clause License 5 | * @date 2018 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | template 14 | inline void unused(Args &&...) {} 15 | 16 | OsqpEigen::Settings::Settings() 17 | { 18 | m_settings = (OSQPSettings *)c_malloc(sizeof(OSQPSettings)); 19 | osqp_set_default_settings(m_settings); 20 | } 21 | 22 | OsqpEigen::Settings::~Settings() 23 | { 24 | c_free(m_settings); 25 | } 26 | 27 | void OsqpEigen::Settings::resetDefaultSettings() 28 | { 29 | osqp_set_default_settings(m_settings); 30 | } 31 | 32 | void OsqpEigen::Settings::setRho(const double rho) 33 | { 34 | m_settings->rho = rho; 35 | } 36 | 37 | void OsqpEigen::Settings::setSigma(const double sigma) 38 | { 39 | m_settings->sigma = sigma; 40 | } 41 | 42 | void OsqpEigen::Settings::setScaling(const int scaling) 43 | { 44 | m_settings->scaling = (c_int)scaling; 45 | } 46 | 47 | void OsqpEigen::Settings::setAdaptiveRho(const bool isRhoStepSizeAdactive) 48 | { 49 | #if EMBEDDED != 1 50 | m_settings->adaptive_rho = (c_int)isRhoStepSizeAdactive; 51 | #else 52 | debugStream() << "[OsqpEigen::Settings::setAdaptiveRho] OSPQ has been set to EMBEDDED, hence this setting is disabled." << std::endl; 53 | unused(isRhoStepSizeAdactive); 54 | #endif 55 | } 56 | 57 | void OsqpEigen::Settings::setAdaptiveRhoInterval(const int rhoInterval) 58 | { 59 | #if EMBEDDED != 1 60 | m_settings->adaptive_rho_interval = (c_int)rhoInterval; 61 | #else 62 | debugStream() << "[OsqpEigen::Settings::setAdaptiveRhoInterval] OSPQ has been set to EMBEDDED, hence this setting is disabled." << std::endl; 63 | unused(rhoInterval); 64 | #endif 65 | } 66 | 67 | void OsqpEigen::Settings::setAdaptiveRhoTolerance(const double adaptiveRhoTolerance) 68 | { 69 | #if EMBEDDED != 1 70 | m_settings->adaptive_rho_tolerance = (c_float)adaptiveRhoTolerance; 71 | #else 72 | debugStream() << "[OsqpEigen::Settings::setAdaptiveRhoTolerance] OSPQ has been set to EMBEDDED, hence this setting is disabled." << std::endl; 73 | unused(adaptiveRhoTolerance); 74 | #endif 75 | } 76 | 77 | void OsqpEigen::Settings::setAdaptiveRhoFraction(const double adaptiveRhoFraction) 78 | { 79 | #if EMBEDDED != 1 80 | #ifdef PROFILING 81 | m_settings->adaptive_rho_fraction = (c_float)adaptiveRhoFraction; 82 | #else 83 | debugStream() << "[OsqpEigen::Settings::setAdaptiveRhoFraction] OSPQ has been set without PROFILING, hence this setting is disabled." << std::endl; 84 | unused(adaptiveRhoFraction); 85 | #endif // ifdef PROFILING 86 | #else // # if EMBEDDED != 1 87 | debugStream() << "[OsqpEigen::Settings::setAdaptiveRhoFraction] OSPQ has been set to EMBEDDED, hence this setting is disabled." << std::endl; 88 | unused(adaptiveRhoFraction); 89 | #endif // # if EMBEDDED != 1 90 | } 91 | 92 | void OsqpEigen::Settings::setMaxIteraction(const int maxIteration) 93 | { 94 | setMaxIteration(maxIteration); 95 | } 96 | 97 | void OsqpEigen::Settings::setMaxIteration(const int maxIteration) 98 | { 99 | m_settings->max_iter = (c_int)maxIteration; 100 | } 101 | 102 | void OsqpEigen::Settings::setAbsoluteTolerance(const double absoluteTolerance) 103 | { 104 | m_settings->eps_abs = (c_float)absoluteTolerance; 105 | } 106 | 107 | void OsqpEigen::Settings::setRelativeTolerance(const double relativeTolerance) 108 | { 109 | m_settings->eps_rel = (c_float)relativeTolerance; 110 | } 111 | 112 | void OsqpEigen::Settings::setPrimalInfeasibilityTollerance(const double primalInfeasibilityTolerance) 113 | { 114 | this->setPrimalInfeasibilityTolerance(primalInfeasibilityTolerance); 115 | } 116 | 117 | void OsqpEigen::Settings::setPrimalInfeasibilityTolerance(const double primalInfeasibilityTolerance) 118 | { 119 | m_settings->eps_prim_inf = (c_float)primalInfeasibilityTolerance; 120 | } 121 | 122 | void OsqpEigen::Settings::setDualInfeasibilityTollerance(const double dualInfeasibilityTolerance) 123 | { 124 | this->setDualInfeasibilityTolerance(dualInfeasibilityTolerance); 125 | } 126 | 127 | void OsqpEigen::Settings::setDualInfeasibilityTolerance(const double dualInfeasibilityTolerance) 128 | { 129 | m_settings->eps_dual_inf = (c_float)dualInfeasibilityTolerance; 130 | } 131 | 132 | void OsqpEigen::Settings::setAlpha(const double alpha) 133 | { 134 | m_settings->alpha = (c_float)alpha; 135 | } 136 | 137 | void OsqpEigen::Settings::setLinearSystemSolver(const int linsysSolver) 138 | { 139 | #ifdef OSQP_EIGEN_OSQP_IS_V1 140 | m_settings->linsys_solver = (osqp_linsys_solver_type)linsysSolver; 141 | #else 142 | m_settings->linsys_solver = (linsys_solver_type)linsysSolver; 143 | #endif 144 | } 145 | 146 | void OsqpEigen::Settings::setDelta(const double delta) 147 | { 148 | #ifndef EMBEDDED 149 | m_settings->delta = (c_float)delta; 150 | #else 151 | debugStream() << "[OsqpEigen::Settings::setDelta] OSPQ has been set to EMBEDDED, hence this setting is disabled." << std::endl; 152 | unused(delta); 153 | #endif 154 | } 155 | 156 | void OsqpEigen::Settings::setPolish(const bool polish) 157 | { 158 | #ifndef EMBEDDED 159 | #ifdef OSQP_EIGEN_OSQP_IS_V1 160 | m_settings->polishing = (c_int)polish; 161 | #else 162 | m_settings->polish = (c_int)polish; 163 | #endif 164 | #else 165 | debugStream() << "[OsqpEigen::Settings::setPolish] OSPQ has been set to EMBEDDED, hence this setting is disabled." << std::endl; 166 | unused(polish); 167 | #endif 168 | } 169 | 170 | void OsqpEigen::Settings::setPolishRefineIter(const int polishRefineIter) 171 | { 172 | #ifndef EMBEDDED 173 | m_settings->polish_refine_iter = (c_int)polishRefineIter; 174 | #else 175 | debugStream() << "[OsqpEigen::Settings::setPolishRefineIter] OSPQ has been set to EMBEDDED, hence this setting is disabled." << std::endl; 176 | unused(polishRefineIter); 177 | #endif 178 | } 179 | 180 | void OsqpEigen::Settings::setVerbosity(const bool isVerbose) 181 | { 182 | #ifndef EMBEDDED 183 | m_settings->verbose = (c_int)isVerbose; 184 | #else 185 | debugStream() << "[OsqpEigen::Settings::setVerbosity] OSPQ has been set to EMBEDDED, hence this setting is disabled." << std::endl; 186 | unused(isVerbose); 187 | #endif 188 | } 189 | 190 | void OsqpEigen::Settings::setScaledTerimination(const bool scaledTermination) 191 | { 192 | m_settings->scaled_termination = (c_int)scaledTermination; 193 | } 194 | 195 | void OsqpEigen::Settings::setCheckTermination(const int checkTermination) 196 | { 197 | m_settings->check_termination = (c_int)checkTermination; 198 | } 199 | 200 | void OsqpEigen::Settings::setCheckDualGap(const bool checkDualGap) 201 | { 202 | #ifdef OSQP_EIGEN_OSQP_IS_V1_FINAL 203 | m_settings->check_dualgap = (c_int)checkDualGap; 204 | #else 205 | debugStream() << "[OsqpEigen::Settings::setCheckDualGap] OSQP version is lower than v1.0.0, this setting is not available." << std::endl; 206 | unused(checkDualGap); 207 | #endif 208 | } 209 | 210 | void OsqpEigen::Settings::setWarmStart(const bool warmStart) 211 | { 212 | #ifdef OSQP_EIGEN_OSQP_IS_V1 213 | m_settings->warm_starting = (c_int)warmStart; 214 | #else 215 | m_settings->warm_start = (c_int)warmStart; 216 | #endif 217 | } 218 | 219 | void OsqpEigen::Settings::setTimeLimit(const double timeLimit) 220 | { 221 | #ifdef PROFILING 222 | m_settings->time_limit = (c_float)timeLimit; 223 | #else 224 | debugStream() << "[OsqpEigen::Settings::setTimeLimit] OSPQ has been set without PROFILING, hence this setting is disabled." << std::endl; 225 | unused(timeLimit); 226 | #endif 227 | } 228 | 229 | OSQPSettings *const &OsqpEigen::Settings::getSettings() const 230 | { 231 | return m_settings; 232 | } 233 | -------------------------------------------------------------------------------- /src/Solver.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Solver.cpp 3 | * @author Giulio Romualdi 4 | * @copyright Released under the terms of the BSD 3-Clause License 5 | * @date 2018 6 | */ 7 | 8 | // OsqpEigen 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #ifdef OSQP_EIGEN_OSQP_IS_V1 15 | void OsqpEigen::Solver::OSQPSolverDeleter(OSQPSolver* ptr) noexcept 16 | #else 17 | void OsqpEigen::Solver::OSQPWorkspaceDeleter(OSQPWorkspace* ptr) noexcept 18 | #endif 19 | { 20 | if (ptr != nullptr) 21 | { 22 | osqp_cleanup(ptr); 23 | } 24 | } 25 | 26 | OsqpEigen::Solver::Solver() 27 | : m_isSolverInitialized(false) 28 | , 29 | #ifdef OSQP_EIGEN_OSQP_IS_V1 30 | m_solver{nullptr, Solver::OSQPSolverDeleter} 31 | #else 32 | m_workspace{nullptr, Solver::OSQPWorkspaceDeleter} 33 | #endif 34 | { 35 | m_settings = std::make_unique(); 36 | m_data = std::make_unique(); 37 | } 38 | 39 | bool OsqpEigen::Solver::clearSolverVariables() 40 | { 41 | if (!m_isSolverInitialized) 42 | { 43 | debugStream() << "[OsqpEigen::Solver::clearSolverVariables] Unable to clear the solver " 44 | "variables. " 45 | << "Are you sure that the solver is initialized?" << std::endl; 46 | return false; 47 | } 48 | 49 | #ifndef OSQP_EIGEN_OSQP_IS_V1 50 | for (int i = 0; i < getData()->n; i++) 51 | { 52 | m_workspace->x[i] = 0; 53 | m_workspace->x_prev[i] = 0; 54 | 55 | m_workspace->Px[i] = 0; 56 | m_workspace->Aty[i] = 0; 57 | m_workspace->Atdelta_y[i] = 0; 58 | 59 | m_workspace->delta_x[i] = 0; 60 | m_workspace->Pdelta_x[i] = 0; 61 | } 62 | 63 | for (int i = 0; i < getData()->m; i++) 64 | { 65 | m_workspace->z[i] = 0; 66 | m_workspace->z_prev[i] = 0; 67 | m_workspace->y[i] = 0; 68 | 69 | m_workspace->Ax[i] = 0; 70 | m_workspace->delta_y[i] = 0; 71 | 72 | m_workspace->Adelta_x[i] = 0; 73 | } 74 | 75 | for (int i = 0; i < getData()->n + getData()->m; i++) 76 | { 77 | m_workspace->xz_tilde[i] = 0; 78 | } 79 | #endif 80 | 81 | return true; 82 | } 83 | 84 | bool OsqpEigen::Solver::initSolver() 85 | { 86 | if (m_isSolverInitialized) 87 | { 88 | debugStream() << "[OsqpEigen::Solver::initSolver] The solver has been already initialized. " 89 | << "Please use clearSolver() method to deallocate memory." << std::endl; 90 | return false; 91 | } 92 | 93 | if (!m_data->isSet()) 94 | { 95 | debugStream() << "[OsqpEigen::Solver::initSolver] Some data are not set." << std::endl; 96 | return false; 97 | } 98 | 99 | // if the number of constraints is equal to zero the user may not 100 | // call setLinearConstraintsMatrix() 101 | if (m_data->getData()->m == 0) 102 | { 103 | if (m_data->getData()->A == nullptr) 104 | { 105 | // let's create the matrix manually. This is required by osqp. Please check 106 | // https://github.com/oxfordcontrol/osqp/issues/295 107 | Eigen::SparseMatrix A(m_data->getData()->m, m_data->getData()->n); 108 | if (!m_data->setLinearConstraintsMatrix(A)) 109 | { 110 | debugStream() << "[OsqpEigen::Solver::initSolver] Unable to set the empty linear " 111 | "constraint " 112 | << "matrix in case of unconstrained optimization problem" 113 | << std::endl; 114 | return false; 115 | } 116 | } 117 | } 118 | 119 | #ifdef OSQP_EIGEN_OSQP_IS_V1 120 | OSQPSolver* solver; 121 | auto data = m_data->getData(); 122 | if (osqp_setup(&solver, 123 | data->P, 124 | data->q, 125 | data->A, 126 | data->l, 127 | data->u, 128 | data->m, 129 | data->n, 130 | m_settings->getSettings()) 131 | != 0) 132 | { 133 | debugStream() << "[OsqpEigen::Solver::initSolver] Unable to setup the workspace." 134 | << std::endl; 135 | return false; 136 | } 137 | 138 | m_solver.reset(solver); 139 | #else 140 | OSQPWorkspace* workspace; 141 | if (osqp_setup(&workspace, m_data->getData(), m_settings->getSettings()) != 0) 142 | { 143 | debugStream() << "[OsqpEigen::Solver::initSolver] Unable to setup the workspace." 144 | << std::endl; 145 | return false; 146 | } 147 | 148 | m_workspace.reset(workspace); 149 | #endif 150 | 151 | m_isSolverInitialized = true; 152 | return true; 153 | } 154 | 155 | bool OsqpEigen::Solver::isInitialized() 156 | { 157 | return m_isSolverInitialized; 158 | } 159 | 160 | void OsqpEigen::Solver::clearSolver() 161 | { 162 | if (m_isSolverInitialized) 163 | { 164 | #ifdef OSQP_EIGEN_OSQP_IS_V1 165 | m_solver.reset(); 166 | #else 167 | m_workspace.reset(); 168 | #endif 169 | m_isSolverInitialized = false; 170 | } 171 | } 172 | 173 | bool OsqpEigen::Solver::solve() 174 | { 175 | if (this->solveProblem() != OsqpEigen::ErrorExitFlag::NoError) 176 | { 177 | debugStream() << "[OsqpEigen::Solver::solve] Unable to solve the problem." << std::endl; 178 | return false; 179 | } 180 | 181 | // check if the solution is feasible 182 | if (this->getStatus() != OsqpEigen::Status::Solved) 183 | { 184 | debugStream() << "[OsqpEigen::Solver::solve] The solution is unfeasible." << std::endl; 185 | return false; 186 | } 187 | 188 | return true; 189 | } 190 | 191 | OsqpEigen::Status OsqpEigen::Solver::getStatus() const 192 | { 193 | return static_cast(getInfo()->status_val); 194 | } 195 | 196 | c_float OsqpEigen::Solver::getObjValue() const 197 | { 198 | return getInfo()->obj_val; 199 | } 200 | 201 | OsqpEigen::ErrorExitFlag OsqpEigen::Solver::solveProblem() 202 | { 203 | if (!m_isSolverInitialized) 204 | { 205 | debugStream() << "[OsqpEigen::Solver::solveProblem] The solve has not been initialized " 206 | "yet. " 207 | << "Please call initSolver() method." << std::endl; 208 | return OsqpEigen::ErrorExitFlag::WorkspaceNotInitError; 209 | } 210 | 211 | #ifdef OSQP_EIGEN_OSQP_IS_V1 212 | return static_cast(osqp_solve(m_solver.get())); 213 | #else 214 | return static_cast(osqp_solve(m_workspace.get())); 215 | #endif 216 | } 217 | 218 | const Eigen::Matrix& OsqpEigen::Solver::getSolution() 219 | { 220 | // copy data from an array to Eigen vector 221 | c_float* solution = getOSQPSolution()->x; 222 | m_solution = Eigen::Map>(solution, getData()->n, 1); 223 | 224 | return m_solution; 225 | } 226 | 227 | const Eigen::Matrix& OsqpEigen::Solver::getDualSolution() 228 | { 229 | // copy data from an array to Eigen vector 230 | c_float* solution = getOSQPSolution()->y; 231 | m_dualSolution = Eigen::Map>(solution, getData()->m, 1); 232 | 233 | return m_dualSolution; 234 | } 235 | 236 | const std::unique_ptr& OsqpEigen::Solver::settings() const 237 | { 238 | return m_settings; 239 | } 240 | 241 | const std::unique_ptr& OsqpEigen::Solver::data() const 242 | { 243 | return m_data; 244 | } 245 | 246 | #ifdef OSQP_EIGEN_OSQP_IS_V1 247 | const std::unique_ptr>& 248 | OsqpEigen::Solver::solver() const 249 | { 250 | return m_solver; 251 | } 252 | #else 253 | const std::unique_ptr>& 254 | OsqpEigen::Solver::workspace() const 255 | { 256 | return m_workspace; 257 | } 258 | #endif 259 | 260 | bool OsqpEigen::Solver::updateGradient( 261 | const Eigen::Ref>& gradient) 262 | { 263 | if (!m_isSolverInitialized) 264 | { 265 | debugStream() << "[OsqpEigen::Solver::updateGradient] The solver is not initialized" 266 | << std::endl; 267 | return false; 268 | } 269 | 270 | // check if the dimension of the gradient is correct 271 | if (gradient.rows() != getData()->n) 272 | { 273 | debugStream() << "[OsqpEigen::Solver::updateGradient] The size of the gradient must be " 274 | "equal to the number of the variables." 275 | << std::endl; 276 | return false; 277 | } 278 | 279 | // update the gradient vector 280 | #ifdef OSQP_EIGEN_OSQP_IS_V1 281 | if (osqp_update_data_vec(m_solver.get(), gradient.data(), nullptr, nullptr)) 282 | { 283 | #else 284 | if (osqp_update_lin_cost(m_workspace.get(), gradient.data())) 285 | { 286 | #endif 287 | debugStream() << "[OsqpEigen::Solver::updateGradient] Error when the update gradient is " 288 | "called." 289 | << std::endl; 290 | return false; 291 | } 292 | return true; 293 | } 294 | 295 | bool OsqpEigen::Solver::updateLowerBound( 296 | const Eigen::Ref>& lowerBound) 297 | { 298 | if (!m_isSolverInitialized) 299 | { 300 | debugStream() << "[OsqpEigen::Solver::updateLowerBound] The solver is not initialized" 301 | << std::endl; 302 | return false; 303 | } 304 | 305 | // check if the dimension of the lowerBound vector is correct 306 | if (lowerBound.rows() != getData()->m) 307 | { 308 | debugStream() << "[OsqpEigen::Solver::updateLowerBound] The size of the lower bound must " 309 | "be equal to the number of the variables." 310 | << std::endl; 311 | return false; 312 | } 313 | 314 | // update the lower bound vector 315 | #ifdef OSQP_EIGEN_OSQP_IS_V1 316 | if (osqp_update_data_vec(m_solver.get(), nullptr, lowerBound.data(), nullptr)) 317 | { 318 | #else 319 | if (osqp_update_lower_bound(m_workspace.get(), lowerBound.data())) 320 | { 321 | #endif 322 | debugStream() << "[OsqpEigen::Solver::updateLowerBound] Error when the update lower bound " 323 | "is called." 324 | << std::endl; 325 | return false; 326 | } 327 | 328 | return true; 329 | } 330 | 331 | bool OsqpEigen::Solver::updateUpperBound( 332 | const Eigen::Ref>& upperBound) 333 | { 334 | if (!m_isSolverInitialized) 335 | { 336 | debugStream() << "[OsqpEigen::Solver::updateUpperBound] The solver is not initialized" 337 | << std::endl; 338 | return false; 339 | } 340 | 341 | // check if the dimension of the upperBound vector is correct 342 | if (upperBound.rows() != getData()->m) 343 | { 344 | debugStream() << "[OsqpEigen::Solver::updateUpperBound] The size of the upper bound must " 345 | "be equal to the number of the variables." 346 | << std::endl; 347 | return false; 348 | } 349 | 350 | // update the upper bound vector 351 | #ifdef OSQP_EIGEN_OSQP_IS_V1 352 | if (osqp_update_data_vec(m_solver.get(), nullptr, nullptr, upperBound.data())) 353 | { 354 | #else 355 | if (osqp_update_upper_bound(m_workspace.get(), upperBound.data())) 356 | { 357 | #endif 358 | debugStream() << "[OsqpEigen::Solver::updateUpperBound] Error when the update upper bound " 359 | "is called." 360 | << std::endl; 361 | return false; 362 | } 363 | return true; 364 | } 365 | 366 | bool OsqpEigen::Solver::updateBounds( 367 | const Eigen::Ref>& lowerBound, 368 | const Eigen::Ref>& upperBound) 369 | { 370 | if (!m_isSolverInitialized) 371 | { 372 | debugStream() << "[OsqpEigen::Solver::updateBounds] The solver is not initialized" 373 | << std::endl; 374 | return false; 375 | } 376 | 377 | // check if the dimension of the upperBound vector is correct 378 | if (upperBound.rows() != getData()->m) 379 | { 380 | debugStream() << "[OsqpEigen::Solver::updateBounds] The size of the upper bound must be " 381 | "equal to the number of the variables." 382 | << std::endl; 383 | return false; 384 | } 385 | 386 | // check if the dimension of the lowerBound vector is correct 387 | if (lowerBound.rows() != getData()->m) 388 | { 389 | debugStream() << "[OsqpEigen::Solver::updateBounds] The size of the lower bound must be " 390 | "equal to the number of the variables." 391 | << std::endl; 392 | return false; 393 | } 394 | 395 | // update lower and upper constraints 396 | #ifdef OSQP_EIGEN_OSQP_IS_V1 397 | if (osqp_update_data_vec(m_solver.get(), nullptr, lowerBound.data(), upperBound.data())) 398 | { 399 | #else 400 | if (osqp_update_bounds(m_workspace.get(), lowerBound.data(), upperBound.data())) 401 | { 402 | #endif 403 | debugStream() << "[OsqpEigen::Solver::updateBounds] Error when the update bounds is called." 404 | << std::endl; 405 | return false; 406 | } 407 | return true; 408 | } 409 | -------------------------------------------------------------------------------- /tests/BUILD.bazel: -------------------------------------------------------------------------------- 1 | _TESTS = [ 2 | "SparseMatrix", 3 | "QP", 4 | "UpdateMatrices", 5 | "MPC", 6 | "MPCUpdateMatrices", 7 | ] 8 | 9 | [ 10 | cc_test( 11 | name = test, 12 | size = "small", 13 | srcs = ["{}Test.cpp".format(test)], 14 | deps = [ 15 | "@catch2//:catch2_main", 16 | "@osqp-eigen", 17 | ], 18 | defines = select({ 19 | "@platforms//os:windows": ["_USE_MATH_DEFINES",], 20 | "//conditions:default": [], 21 | }), 22 | ) 23 | for test in _TESTS 24 | ] 25 | -------------------------------------------------------------------------------- /tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Authors: Giulio Romualdi 2 | # CopyPolicy: Released under the terms of the LGPLv2.1 or later 3 | 4 | add_osqpeigen_test( 5 | NAME SparseMatrix 6 | SOURCES SparseMatrixTest.cpp 7 | LINKS OsqpEigen::OsqpEigen) 8 | 9 | add_osqpeigen_test( 10 | NAME QP 11 | SOURCES QPTest.cpp 12 | LINKS OsqpEigen::OsqpEigen) 13 | 14 | add_osqpeigen_test( 15 | NAME UpdateMatrices 16 | SOURCES UpdateMatricesTest.cpp 17 | LINKS OsqpEigen::OsqpEigen) 18 | 19 | add_osqpeigen_test( 20 | NAME MPC 21 | SOURCES MPCTest.cpp 22 | LINKS OsqpEigen::OsqpEigen 23 | COMPILE_DEFINITIONS _USE_MATH_DEFINES) 24 | 25 | add_osqpeigen_test( 26 | NAME MPCUpdateMatrices 27 | SOURCES MPCUpdateMatricesTest.cpp 28 | LINKS OsqpEigen::OsqpEigen 29 | COMPILE_DEFINITIONS _USE_MATH_DEFINES) 30 | -------------------------------------------------------------------------------- /tests/MODULE.bazel: -------------------------------------------------------------------------------- 1 | bazel_dep(name = "catch2", version = "3.8.0") 2 | bazel_dep(name = "osqp-eigen") 3 | local_path_override( 4 | module_name = "osqp-eigen", 5 | path = "..", 6 | ) 7 | 8 | bazel_dep(name = "platforms", version = "0.0.11") 9 | bazel_dep(name = "rules_cc", version = "0.1.1") 10 | -------------------------------------------------------------------------------- /tests/MPCTest.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file MPCTest.cpp 3 | * @author Giulio Romualdi 4 | * @copyright Released under the terms of the BSD 3-Clause License 5 | * @date 2018 6 | */ 7 | 8 | // Catch2 9 | #include 10 | 11 | // OsqpEigen 12 | #include 13 | 14 | // eigen 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | // colors 22 | #define ANSI_TXT_GRN "\033[0;32m" 23 | #define ANSI_TXT_MGT "\033[0;35m" // Magenta 24 | #define ANSI_TXT_DFT "\033[0;0m" // Console default 25 | #define GTEST_BOX "[ cout ] " 26 | #define COUT_GTEST ANSI_TXT_GRN << GTEST_BOX // You could add the Default 27 | #define COUT_GTEST_MGT COUT_GTEST << ANSI_TXT_MGT 28 | 29 | void setDynamicsMatrices(Eigen::Matrix& a, Eigen::Matrix& b) 30 | { 31 | 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., 32 | 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0.0488, 0., 0., 1., 0., 0., 0.0016, 33 | 0., 0., 0.0992, 0., 0., 0., -0.0488, 0., 0., 1., 0., 0., -0.0016, 0., 0., 0.0992, 0., 0., 34 | 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.0992, 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 35 | 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 36 | 0., 0., 0.9734, 0., 0., 0., 0., 0., 0.0488, 0., 0., 0.9846, 0., 0., 0., -0.9734, 0., 0., 0., 37 | 0., 0., -0.0488, 0., 0., 0.9846, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.9846; 38 | 39 | b << 0., -0.0726, 0., 0.0726, -0.0726, 0., 0.0726, 0., -0.0152, 0.0152, -0.0152, 0.0152, -0., 40 | -0.0006, -0., 0.0006, 0.0006, 0., -0.0006, 0.0000, 0.0106, 0.0106, 0.0106, 0.0106, 0, 41 | -1.4512, 0., 1.4512, -1.4512, 0., 1.4512, 0., -0.3049, 0.3049, -0.3049, 0.3049, -0., 42 | -0.0236, 0., 0.0236, 0.0236, 0., -0.0236, 0., 0.2107, 0.2107, 0.2107, 0.2107; 43 | } 44 | 45 | void setInequalityConstraints(Eigen::Matrix& xMax, 46 | Eigen::Matrix& xMin, 47 | Eigen::Matrix& uMax, 48 | Eigen::Matrix& uMin) 49 | { 50 | c_float u0 = 10.5916; 51 | 52 | // input inequality constraints 53 | uMin << 9.6 - u0, 9.6 - u0, 9.6 - u0, 9.6 - u0; 54 | 55 | uMax << 13 - u0, 13 - u0, 13 - u0, 13 - u0; 56 | 57 | // state inequality constraints 58 | xMin << -M_PI / 6, -M_PI / 6, -OsqpEigen::INFTY, -OsqpEigen::INFTY, -OsqpEigen::INFTY, -1., 59 | -OsqpEigen::INFTY, -OsqpEigen::INFTY, -OsqpEigen::INFTY, -OsqpEigen::INFTY, 60 | -OsqpEigen::INFTY, -OsqpEigen::INFTY; 61 | 62 | xMax << M_PI / 6, M_PI / 6, OsqpEigen::INFTY, OsqpEigen::INFTY, OsqpEigen::INFTY, 63 | OsqpEigen::INFTY, OsqpEigen::INFTY, OsqpEigen::INFTY, OsqpEigen::INFTY, OsqpEigen::INFTY, 64 | OsqpEigen::INFTY, OsqpEigen::INFTY; 65 | } 66 | 67 | void setWeightMatrices(Eigen::DiagonalMatrix& Q, Eigen::DiagonalMatrix& R) 68 | { 69 | Q.diagonal() << 0, 0, 10., 10., 10., 10., 0, 0, 0, 5., 5., 5.; 70 | R.diagonal() << 0.1, 0.1, 0.1, 0.1; 71 | } 72 | 73 | void castMPCToQPHessian(const Eigen::DiagonalMatrix& Q, 74 | const Eigen::DiagonalMatrix& R, 75 | int mpcWindow, 76 | Eigen::SparseMatrix& hessianMatrix) 77 | { 78 | 79 | hessianMatrix.resize(12 * (mpcWindow + 1) + 4 * mpcWindow, 80 | 12 * (mpcWindow + 1) + 4 * mpcWindow); 81 | 82 | // populate hessian matrix 83 | for (int i = 0; i < 12 * (mpcWindow + 1) + 4 * mpcWindow; i++) 84 | { 85 | if (i < 12 * (mpcWindow + 1)) 86 | { 87 | int posQ = i % 12; 88 | float value = Q.diagonal()[posQ]; 89 | if (value != 0) 90 | hessianMatrix.insert(i, i) = value; 91 | } else 92 | { 93 | int posR = i % 4; 94 | float value = R.diagonal()[posR]; 95 | if (value != 0) 96 | hessianMatrix.insert(i, i) = value; 97 | } 98 | } 99 | } 100 | 101 | void castMPCToQPGradient(const Eigen::DiagonalMatrix& Q, 102 | const Eigen::Matrix& xRef, 103 | int mpcWindow, 104 | Eigen::Matrix& gradient) 105 | { 106 | 107 | Eigen::Matrix Qx_ref; 108 | Qx_ref = Q * (-xRef); 109 | 110 | // populate the gradient vector 111 | gradient = Eigen::Matrix::Zero(12 * (mpcWindow + 1) + 4 * mpcWindow, 1); 112 | for (int i = 0; i < 12 * (mpcWindow + 1); i++) 113 | { 114 | int posQ = i % 12; 115 | float value = Qx_ref(posQ, 0); 116 | gradient(i, 0) = value; 117 | } 118 | } 119 | 120 | void castMPCToQPConstraintMatrix(const Eigen::Matrix& dynamicMatrix, 121 | const Eigen::Matrix& controlMatrix, 122 | int mpcWindow, 123 | Eigen::SparseMatrix& constraintMatrix) 124 | { 125 | constraintMatrix.resize(12 * (mpcWindow + 1) + 12 * (mpcWindow + 1) + 4 * mpcWindow, 126 | 12 * (mpcWindow + 1) + 4 * mpcWindow); 127 | 128 | // populate linear constraint matrix 129 | for (int i = 0; i < 12 * (mpcWindow + 1); i++) 130 | { 131 | constraintMatrix.insert(i, i) = -1; 132 | } 133 | 134 | for (int i = 0; i < mpcWindow; i++) 135 | for (int j = 0; j < 12; j++) 136 | for (int k = 0; k < 12; k++) 137 | { 138 | float value = dynamicMatrix(j, k); 139 | if (value != 0) 140 | { 141 | constraintMatrix.insert(12 * (i + 1) + j, 12 * i + k) = value; 142 | } 143 | } 144 | 145 | for (int i = 0; i < mpcWindow; i++) 146 | for (int j = 0; j < 12; j++) 147 | for (int k = 0; k < 4; k++) 148 | { 149 | float value = controlMatrix(j, k); 150 | if (value != 0) 151 | { 152 | constraintMatrix.insert(12 * (i + 1) + j, 4 * i + k + 12 * (mpcWindow + 1)) 153 | = value; 154 | } 155 | } 156 | 157 | for (int i = 0; i < 12 * (mpcWindow + 1) + 4 * mpcWindow; i++) 158 | { 159 | constraintMatrix.insert(i + (mpcWindow + 1) * 12, i) = 1; 160 | } 161 | } 162 | 163 | void castMPCToQPConstraintVectors(const Eigen::Matrix& xMax, 164 | const Eigen::Matrix& xMin, 165 | const Eigen::Matrix& uMax, 166 | const Eigen::Matrix& uMin, 167 | const Eigen::Matrix& x0, 168 | int mpcWindow, 169 | Eigen::Matrix& lowerBound, 170 | Eigen::Matrix& upperBound) 171 | { 172 | // evaluate the lower and the upper inequality vectors 173 | Eigen::Matrix lowerInequality 174 | = Eigen::Matrix::Zero(12 * (mpcWindow + 1) + 4 * mpcWindow, 1); 175 | Eigen::Matrix upperInequality 176 | = Eigen::Matrix::Zero(12 * (mpcWindow + 1) + 4 * mpcWindow, 1); 177 | for (int i = 0; i < mpcWindow + 1; i++) 178 | { 179 | lowerInequality.block(12 * i, 0, 12, 1) = xMin; 180 | upperInequality.block(12 * i, 0, 12, 1) = xMax; 181 | } 182 | for (int i = 0; i < mpcWindow; i++) 183 | { 184 | lowerInequality.block(4 * i + 12 * (mpcWindow + 1), 0, 4, 1) = uMin; 185 | upperInequality.block(4 * i + 12 * (mpcWindow + 1), 0, 4, 1) = uMax; 186 | } 187 | 188 | // evaluate the lower and the upper equality vectors 189 | Eigen::Matrix lowerEquality 190 | = Eigen::Matrix::Zero(12 * (mpcWindow + 1), 1); 191 | Eigen::Matrix upperEquality; 192 | lowerEquality.block(0, 0, 12, 1) = -x0; 193 | upperEquality = lowerEquality; 194 | lowerEquality = lowerEquality; 195 | 196 | // merge inequality and equality vectors 197 | lowerBound = Eigen::Matrix::Zero(2 * 12 * (mpcWindow + 1) + 4 * mpcWindow, 1); 198 | lowerBound << lowerEquality, lowerInequality; 199 | 200 | upperBound = Eigen::Matrix::Zero(2 * 12 * (mpcWindow + 1) + 4 * mpcWindow, 1); 201 | upperBound << upperEquality, upperInequality; 202 | } 203 | 204 | void updateConstraintVectors(const Eigen::Matrix& x0, 205 | Eigen::Matrix& lowerBound, 206 | Eigen::Matrix& upperBound) 207 | { 208 | lowerBound.block(0, 0, 12, 1) = -x0; 209 | upperBound.block(0, 0, 12, 1) = -x0; 210 | } 211 | 212 | c_float 213 | getErrorNorm(const Eigen::Matrix& x, const Eigen::Matrix& xRef) 214 | { 215 | // evaluate the error 216 | Eigen::Matrix error = x - xRef; 217 | 218 | // return the norm 219 | return error.norm(); 220 | } 221 | 222 | TEST_CASE("MPCTest") 223 | { 224 | // open the ofstream 225 | std::ofstream dataStream; 226 | dataStream.open("output.txt"); 227 | 228 | // set the preview window 229 | int mpcWindow = 20; 230 | 231 | // allocate the dynamics matrices 232 | Eigen::Matrix a; 233 | Eigen::Matrix b; 234 | 235 | // allocate the constraints vector 236 | Eigen::Matrix xMax; 237 | Eigen::Matrix xMin; 238 | Eigen::Matrix uMax; 239 | Eigen::Matrix uMin; 240 | 241 | // allocate the weight matrices 242 | Eigen::DiagonalMatrix Q; 243 | Eigen::DiagonalMatrix R; 244 | 245 | // allocate the initial and the reference state space 246 | Eigen::Matrix x0; 247 | Eigen::Matrix xRef; 248 | 249 | // allocate QP problem matrices and vectors 250 | Eigen::SparseMatrix hessian; 251 | Eigen::Matrix gradient; 252 | Eigen::SparseMatrix linearMatrix; 253 | Eigen::Matrix lowerBound; 254 | Eigen::Matrix upperBound; 255 | 256 | // set the initial and the desired states 257 | x0 << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 258 | xRef << 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0; 259 | 260 | // set MPC problem quantities 261 | setDynamicsMatrices(a, b); 262 | setInequalityConstraints(xMax, xMin, uMax, uMin); 263 | setWeightMatrices(Q, R); 264 | 265 | // cast the MPC problem as QP problem 266 | castMPCToQPHessian(Q, R, mpcWindow, hessian); 267 | castMPCToQPGradient(Q, xRef, mpcWindow, gradient); 268 | castMPCToQPConstraintMatrix(a, b, mpcWindow, linearMatrix); 269 | castMPCToQPConstraintVectors(xMax, xMin, uMax, uMin, x0, mpcWindow, lowerBound, upperBound); 270 | 271 | // instantiate the solver 272 | OsqpEigen::Solver solver; 273 | 274 | // settings 275 | solver.settings()->setVerbosity(false); 276 | solver.settings()->setWarmStart(true); 277 | 278 | // set the initial data of the QP solver 279 | solver.data()->setNumberOfVariables(12 * (mpcWindow + 1) + 4 * mpcWindow); 280 | solver.data()->setNumberOfConstraints(2 * 12 * (mpcWindow + 1) + 4 * mpcWindow); 281 | REQUIRE(solver.data()->setHessianMatrix(hessian)); 282 | REQUIRE(solver.data()->setGradient(gradient)); 283 | REQUIRE(solver.data()->setLinearConstraintsMatrix(linearMatrix)); 284 | REQUIRE(solver.data()->setLowerBound(lowerBound)); 285 | REQUIRE(solver.data()->setUpperBound(upperBound)); 286 | 287 | // instantiate the solver 288 | REQUIRE(solver.initSolver()); 289 | 290 | // controller input and QPSolution vector 291 | Eigen::Matrix ctr; 292 | Eigen::Matrix QPSolution; 293 | 294 | // number of iteration steps 295 | int numberOfSteps = 50; 296 | 297 | // profiling quantities 298 | clock_t startTime, endTime; 299 | c_float averageTime = 0; 300 | 301 | for (int i = 0; i < numberOfSteps; i++) 302 | { 303 | startTime = clock(); 304 | 305 | // solve the QP problem 306 | REQUIRE(solver.solveProblem() == OsqpEigen::ErrorExitFlag::NoError); 307 | 308 | // get the controller input 309 | QPSolution = solver.getSolution(); 310 | ctr = QPSolution.block(12 * (mpcWindow + 1), 0, 4, 1); 311 | 312 | // save data into file 313 | auto x0Data = x0.data(); 314 | for (int j = 0; j < 12; j++) 315 | dataStream << x0Data[j] << " "; 316 | dataStream << std::endl; 317 | 318 | // propagate the model 319 | x0 = a * x0 + b * ctr; 320 | 321 | // update the constraint bound 322 | updateConstraintVectors(x0, lowerBound, upperBound); 323 | REQUIRE(solver.updateBounds(lowerBound, upperBound)); 324 | 325 | endTime = clock(); 326 | 327 | averageTime += static_cast(endTime - startTime) / CLOCKS_PER_SEC; 328 | } 329 | 330 | // close the stream 331 | dataStream.close(); 332 | 333 | std::cout << COUT_GTEST_MGT << "Average time = " << averageTime / numberOfSteps << " seconds." 334 | << ANSI_TXT_DFT << std::endl; 335 | 336 | constexpr double tolerance = 1e-2; 337 | REQUIRE(getErrorNorm(x0, xRef) <= tolerance); 338 | } 339 | -------------------------------------------------------------------------------- /tests/MPCUpdateMatricesTest.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file UpdateMatricesTest.cpp 3 | * @author Giulio Romualdi 4 | * @copyright Released under the terms of the BSD 3-Clause License 5 | * @date 2020 6 | */ 7 | 8 | // Catch2 9 | #include 10 | 11 | // OsqpEigen 12 | #include 13 | 14 | // eigen 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | // colors 22 | #define ANSI_TXT_GRN "\033[0;32m" 23 | #define ANSI_TXT_MGT "\033[0;35m" // Magenta 24 | #define ANSI_TXT_DFT "\033[0;0m" // Console default 25 | #define GTEST_BOX "[ cout ] " 26 | #define COUT_GTEST ANSI_TXT_GRN << GTEST_BOX // You could add the Default 27 | #define COUT_GTEST_MGT COUT_GTEST << ANSI_TXT_MGT 28 | 29 | #define T 0.1 30 | 31 | void setDynamicsMatrices(Eigen::Matrix& a, 32 | Eigen::Matrix& b, 33 | Eigen::Matrix& c, 34 | double t) 35 | { 36 | 37 | double omega = 0.1132; 38 | double alpha = 0.5 * sin(2 * M_PI * omega * t); 39 | double beta = 2 - 1 * sin(2 * M_PI * omega * t); 40 | 41 | a << alpha, 1, 0, alpha; 42 | 43 | b << 0, 1; 44 | 45 | c << beta, 0; 46 | } 47 | 48 | void setWeightMatrices(Eigen::DiagonalMatrix& Q, Eigen::DiagonalMatrix& R) 49 | { 50 | Q.diagonal() << 10; 51 | R.diagonal() << 1; 52 | } 53 | 54 | void castMPCToQPHessian(const Eigen::DiagonalMatrix& Q, 55 | const Eigen::DiagonalMatrix& R, 56 | int mpcWindow, 57 | int k, 58 | Eigen::SparseMatrix& hessianMatrix) 59 | { 60 | 61 | Eigen::Matrix a; 62 | Eigen::Matrix b; 63 | Eigen::Matrix c; 64 | 65 | hessianMatrix.resize(2 * (mpcWindow + 1) + 1 * mpcWindow, 2 * (mpcWindow + 1) + 1 * mpcWindow); 66 | 67 | // populate hessian matrix 68 | for (int i = 0; i < 2 * (mpcWindow + 1) + 1 * mpcWindow; i++) 69 | { 70 | double t = (k + i) * T; 71 | setDynamicsMatrices(a, b, c, t); 72 | if (i < 2 * (mpcWindow + 1)) 73 | { 74 | // here the structure of the matrix c is used! 75 | int pos = i % 2; 76 | float value = c(pos) * Q.diagonal()[0] * c(pos); 77 | if (value != 0) 78 | hessianMatrix.insert(i, i) = value; 79 | } else 80 | { 81 | float value = R.diagonal()[0]; 82 | if (value != 0) 83 | hessianMatrix.insert(i, i) = value; 84 | } 85 | } 86 | } 87 | 88 | void castMPCToQPGradient(const Eigen::DiagonalMatrix& Q, 89 | const Eigen::Matrix& yRef, 90 | int mpcWindow, 91 | int k, 92 | Eigen::Matrix& gradient) 93 | { 94 | 95 | Eigen::Matrix a; 96 | Eigen::Matrix b; 97 | Eigen::Matrix c; 98 | 99 | Eigen::Matrix Qy_ref; 100 | Qy_ref = Q * (-yRef); 101 | 102 | // populate the gradient vector 103 | gradient = Eigen::Matrix::Zero(2 * (mpcWindow + 1) + 1 * mpcWindow, 1); 104 | for (int i = 0; i < 2 * (mpcWindow + 1); i++) 105 | { 106 | double t = (k + i) * T; 107 | setDynamicsMatrices(a, b, c, t); 108 | 109 | int pos = i % 2; 110 | float value = Qy_ref(0, 0) * c(pos); 111 | gradient(i, 0) = value; 112 | } 113 | } 114 | 115 | void castMPCToQPConstraintMatrix(int mpcWindow, 116 | int k, 117 | Eigen::SparseMatrix& constraintMatrix) 118 | { 119 | constraintMatrix.resize(2 * (mpcWindow + 1), 2 * (mpcWindow + 1) + 1 * mpcWindow); 120 | 121 | // populate linear constraint matrix 122 | for (int i = 0; i < 2 * (mpcWindow + 1); i++) 123 | { 124 | constraintMatrix.insert(i, i) = -1; 125 | } 126 | 127 | Eigen::Matrix a; 128 | Eigen::Matrix b; 129 | Eigen::Matrix c; 130 | 131 | for (int i = 0; i < mpcWindow; i++) 132 | { 133 | double t = (k + i) * T; 134 | setDynamicsMatrices(a, b, c, t); 135 | for (int j = 0; j < 2; j++) 136 | for (int k = 0; k < 2; k++) 137 | { 138 | float value = a(j, k); 139 | if (value != 0) 140 | { 141 | constraintMatrix.insert(2 * (i + 1) + j, 2 * i + k) = value; 142 | } 143 | } 144 | } 145 | 146 | for (int i = 0; i < mpcWindow; i++) 147 | for (int j = 0; j < 2; j++) 148 | for (int k = 0; k < 1; k++) 149 | { 150 | // b is constant 151 | float value = b(j, k); 152 | if (value != 0) 153 | { 154 | constraintMatrix.insert(2 * (i + 1) + j, 1 * i + k + 2 * (mpcWindow + 1)) 155 | = value; 156 | } 157 | } 158 | } 159 | 160 | void castMPCToQPConstraintVectors(const Eigen::Matrix& x0, 161 | int mpcWindow, 162 | Eigen::Matrix& lowerBound, 163 | Eigen::Matrix& upperBound) 164 | { 165 | // evaluate the lower and the upper equality vectors 166 | lowerBound = Eigen::Matrix::Zero(2 * (mpcWindow + 1), 1); 167 | lowerBound.block(0, 0, 2, 1) = -x0; 168 | upperBound = lowerBound; 169 | } 170 | 171 | bool updateHessianMatrix(OsqpEigen::Solver& solver, 172 | const Eigen::DiagonalMatrix& Q, 173 | const Eigen::DiagonalMatrix& R, 174 | int mpcWindow, 175 | int k) 176 | { 177 | Eigen::SparseMatrix hessianMatrix; 178 | castMPCToQPHessian(Q, R, mpcWindow, k, hessianMatrix); 179 | 180 | if (!solver.updateHessianMatrix(hessianMatrix)) 181 | return false; 182 | 183 | return true; 184 | } 185 | 186 | bool updateLinearConstraintsMatrix(OsqpEigen::Solver& solver, int mpcWindow, int k) 187 | { 188 | Eigen::SparseMatrix constraintMatrix; 189 | castMPCToQPConstraintMatrix(mpcWindow, k, constraintMatrix); 190 | 191 | if (!solver.updateLinearConstraintsMatrix(constraintMatrix)) 192 | return false; 193 | 194 | return true; 195 | } 196 | 197 | void updateConstraintVectors(const Eigen::Matrix& x0, 198 | Eigen::Matrix& lowerBound, 199 | Eigen::Matrix& upperBound) 200 | { 201 | lowerBound.block(0, 0, 2, 1) = -x0; 202 | upperBound.block(0, 0, 2, 1) = -x0; 203 | } 204 | 205 | TEST_CASE("MPCTest Update matrices") 206 | { 207 | // open the ofstream 208 | std::ofstream dataStream; 209 | dataStream.open("output.txt"); 210 | 211 | // set the preview window 212 | int mpcWindow = 100; 213 | 214 | // allocate the dynamics matrices 215 | Eigen::Matrix a; 216 | Eigen::Matrix b; 217 | Eigen::Matrix c; 218 | 219 | // allocate the weight matrices 220 | Eigen::DiagonalMatrix Q; 221 | Eigen::DiagonalMatrix R; 222 | 223 | // allocate the initial and the reference state space 224 | Eigen::Matrix x0; 225 | Eigen::Matrix yRef; 226 | Eigen::Matrix y; 227 | 228 | // allocate QP problem matrices and vectors 229 | Eigen::SparseMatrix hessian; 230 | Eigen::Matrix gradient; 231 | Eigen::SparseMatrix linearMatrix; 232 | Eigen::Matrix lowerBound; 233 | Eigen::Matrix upperBound; 234 | 235 | // set the initial and the desired states 236 | x0 << 0, 0; 237 | yRef << 1; 238 | 239 | // set MPC problem quantities 240 | setWeightMatrices(Q, R); 241 | 242 | // cast the MPC problem as QP problem 243 | castMPCToQPHessian(Q, R, mpcWindow, 0, hessian); 244 | castMPCToQPGradient(Q, yRef, mpcWindow, 0, gradient); 245 | castMPCToQPConstraintMatrix(mpcWindow, 0, linearMatrix); 246 | castMPCToQPConstraintVectors(x0, mpcWindow, lowerBound, upperBound); 247 | 248 | // instantiate the solver 249 | OsqpEigen::Solver solver; 250 | 251 | // settings 252 | solver.settings()->setVerbosity(false); 253 | solver.settings()->setWarmStart(true); 254 | 255 | // set the initial data of the QP solver 256 | solver.data()->setNumberOfVariables(2 * (mpcWindow + 1) + 1 * mpcWindow); 257 | solver.data()->setNumberOfConstraints(2 * (mpcWindow + 1)); 258 | REQUIRE(solver.data()->setHessianMatrix(hessian)); 259 | REQUIRE(solver.data()->setGradient(gradient)); 260 | REQUIRE(solver.data()->setLinearConstraintsMatrix(linearMatrix)); 261 | REQUIRE(solver.data()->setLowerBound(lowerBound)); 262 | REQUIRE(solver.data()->setUpperBound(upperBound)); 263 | 264 | // instantiate the solver 265 | REQUIRE(solver.initSolver()); 266 | 267 | // controller input and QPSolution vector 268 | Eigen::Matrix ctr; 269 | Eigen::Matrix QPSolution; 270 | 271 | // number of iteration steps 272 | int numberOfSteps = 50; 273 | 274 | // profiling quantities 275 | clock_t startTime, endTime; 276 | double averageTime = 0; 277 | 278 | for (int i = 0; i < numberOfSteps; i++) 279 | { 280 | startTime = clock(); 281 | 282 | setDynamicsMatrices(a, b, c, i * T); 283 | 284 | // update the constraint bound 285 | REQUIRE(updateHessianMatrix(solver, Q, R, mpcWindow, i)); 286 | REQUIRE(updateLinearConstraintsMatrix(solver, mpcWindow, i)); 287 | 288 | castMPCToQPGradient(Q, yRef, mpcWindow, i, gradient); 289 | REQUIRE(solver.updateGradient(gradient)); 290 | 291 | updateConstraintVectors(x0, lowerBound, upperBound); 292 | REQUIRE(solver.updateBounds(lowerBound, upperBound)); 293 | 294 | // solve the QP problem 295 | REQUIRE(solver.solveProblem() == OsqpEigen::ErrorExitFlag::NoError); 296 | 297 | // get the controller input 298 | QPSolution = solver.getSolution(); 299 | ctr = QPSolution.block(2 * (mpcWindow + 1), 0, 1, 1); 300 | 301 | // save data into file 302 | auto x0Data = x0.data(); 303 | for (int j = 0; j < 2; j++) 304 | dataStream << x0Data[j] << " "; 305 | dataStream << std::endl; 306 | 307 | // propagate the model 308 | x0 = a * x0 + b * ctr; 309 | y = c * x0; 310 | 311 | endTime = clock(); 312 | 313 | averageTime += static_cast(endTime - startTime) / CLOCKS_PER_SEC; 314 | } 315 | 316 | // close the stream 317 | dataStream.close(); 318 | 319 | std::cout << COUT_GTEST_MGT << "Average time = " << averageTime / numberOfSteps << " seconds." 320 | << ANSI_TXT_DFT << std::endl; 321 | } 322 | -------------------------------------------------------------------------------- /tests/QPTest.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file QPTest.cpp 3 | * @author Giulio Romualdi 4 | * @copyright Released under the terms of the BSD 3-Clause License 5 | * @date 2020 6 | */ 7 | 8 | // Catch2 9 | #include 10 | 11 | #include 12 | 13 | TEST_CASE("QPProblem - Unconstrained") 14 | { 15 | constexpr double tolerance = 1e-4; 16 | 17 | Eigen::SparseMatrix H_s(2, 2); 18 | H_s.insert(0, 0) = 3; 19 | H_s.insert(0, 1) = 2; 20 | H_s.insert(1, 0) = 2; 21 | H_s.insert(1, 1) = 4; 22 | 23 | Eigen::Matrix gradient; 24 | gradient << 3, 1; 25 | 26 | OsqpEigen::Solver solver; 27 | solver.settings()->setVerbosity(true); 28 | solver.settings()->setAlpha(1.0); 29 | 30 | solver.data()->setNumberOfVariables(2); 31 | solver.data()->setNumberOfConstraints(0); 32 | 33 | REQUIRE(solver.data()->setHessianMatrix(H_s)); 34 | REQUIRE(solver.data()->setGradient(gradient)); 35 | 36 | REQUIRE(solver.initSolver()); 37 | REQUIRE(solver.solveProblem() == OsqpEigen::ErrorExitFlag::NoError); 38 | 39 | // expected solution 40 | Eigen::Matrix expectedSolution; 41 | expectedSolution << -1.2500, 0.3750; 42 | 43 | REQUIRE(solver.getSolution().isApprox(expectedSolution, tolerance)); 44 | } 45 | 46 | TEST_CASE("QPProblem") 47 | { 48 | constexpr double tolerance = 1e-4; 49 | 50 | Eigen::SparseMatrix H_s(2, 2); 51 | H_s.insert(0, 0) = 4; 52 | H_s.insert(0, 1) = 1; 53 | H_s.insert(1, 0) = 1; 54 | H_s.insert(1, 1) = 2; 55 | 56 | Eigen::SparseMatrix A_s(3, 2); 57 | A_s.insert(0, 0) = 1; 58 | A_s.insert(0, 1) = 1; 59 | A_s.insert(1, 0) = 1; 60 | A_s.insert(2, 1) = 1; 61 | 62 | Eigen::Matrix gradient; 63 | gradient << 1, 1; 64 | 65 | Eigen::Matrix lowerBound; 66 | lowerBound << 1, 0, 0; 67 | 68 | Eigen::Matrix upperBound; 69 | upperBound << 1, 0.7, 0.7; 70 | 71 | OsqpEigen::Solver solver; 72 | solver.settings()->setVerbosity(true); 73 | solver.settings()->setAlpha(1.0); 74 | // This is required to avoid non-deterministic non-accurate solutions 75 | solver.settings()->setPolish(true); 76 | 77 | REQUIRE_FALSE(solver.data()->setHessianMatrix(H_s)); 78 | solver.data()->setNumberOfVariables(2); 79 | 80 | solver.data()->setNumberOfConstraints(3); 81 | REQUIRE(solver.data()->setHessianMatrix(H_s)); 82 | REQUIRE(solver.data()->setGradient(gradient)); 83 | REQUIRE(solver.data()->setLinearConstraintsMatrix(A_s)); 84 | REQUIRE(solver.data()->setLowerBound(lowerBound)); 85 | REQUIRE(solver.data()->setUpperBound(upperBound)); 86 | 87 | REQUIRE(solver.initSolver()); 88 | 89 | REQUIRE(solver.solveProblem() == OsqpEigen::ErrorExitFlag::NoError); 90 | Eigen::Matrix expectedSolution; 91 | expectedSolution << 0.3, 0.7; 92 | 93 | REQUIRE(solver.getSolution().isApprox(expectedSolution, tolerance)); 94 | } 95 | -------------------------------------------------------------------------------- /tests/SparseMatrixTest.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file SparseMatrixTest.cpp 3 | * @author Giulio Romualdi 4 | * @copyright Released under the terms of the BSD 3-Clause License 5 | * @date 2020 6 | */ 7 | 8 | // Catch2 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | template bool computeTest(const Eigen::Matrix& mEigen) 15 | { 16 | Eigen::SparseMatrix matrix, newMatrix, newMatrixFromCSR; 17 | matrix = mEigen.sparseView(); 18 | 19 | csc* osqpSparseMatrix = nullptr; 20 | // NOTE: Dynamic memory allocation 21 | if (!OsqpEigen::SparseMatrixHelper::createOsqpSparseMatrix(matrix, osqpSparseMatrix)) 22 | return false; 23 | 24 | Eigen::SparseMatrix csrMatrix; 25 | csrMatrix = matrix; 26 | csc* otherOsqpSparseMatrix = nullptr; 27 | if (!OsqpEigen::SparseMatrixHelper::createOsqpSparseMatrix(csrMatrix, otherOsqpSparseMatrix)) 28 | return false; 29 | 30 | if (!OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToEigenSparseMatrix(osqpSparseMatrix, 31 | newMatrix)) 32 | return false; 33 | 34 | if (!OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToEigenSparseMatrix(otherOsqpSparseMatrix, 35 | newMatrixFromCSR)) 36 | return false; 37 | 38 | if (!newMatrixFromCSR.isApprox(newMatrix)) 39 | return false; 40 | 41 | std::vector> tripletListCsc; 42 | if (!OsqpEigen::SparseMatrixHelper::osqpSparseMatrixToTriplets(osqpSparseMatrix, 43 | tripletListCsc)) 44 | return false; 45 | 46 | for (const auto& a : tripletListCsc) 47 | std::cout << a.row() << " " << a.col() << " " << a.value() << std::endl; 48 | 49 | std::vector> tripletListEigen; 50 | OsqpEigen::SparseMatrixHelper::eigenSparseMatrixToTriplets(matrix, tripletListEigen); 51 | 52 | std::cout << "***********************************************" << std::endl; 53 | for (const auto& a : tripletListEigen) 54 | std::cout << a.row() << " " << a.col() << " " << a.value() << std::endl; 55 | 56 | constexpr double tolerance = 1e-4; 57 | bool outcome = matrix.isApprox(newMatrix, tolerance); 58 | 59 | OsqpEigen::spfree(osqpSparseMatrix); 60 | OsqpEigen::spfree(otherOsqpSparseMatrix); 61 | 62 | return outcome; 63 | } 64 | 65 | TEST_CASE("SparseMatrix") 66 | { 67 | SECTION("Data type - double") 68 | { 69 | 70 | Eigen::Matrix3d m; 71 | m << 0, 1.002311, 0, 0, 0, 0, 0, 0.90835435, 0; 72 | 73 | REQUIRE(computeTest(m)); 74 | } 75 | 76 | SECTION("Data type - float") 77 | { 78 | Eigen::Matrix3f m; 79 | m << 0, 1, 0, 0, 0, 0, 0, 1, 0; 80 | 81 | REQUIRE(computeTest(m)); 82 | } 83 | 84 | SECTION("Data type - int") 85 | { 86 | 87 | Eigen::Matrix3i m; 88 | m << 0, 1, 0, 0, 0, 0, 0, 1, 0; 89 | 90 | REQUIRE(computeTest(m)); 91 | } 92 | 93 | SECTION("Data type - double") 94 | { 95 | Eigen::Matrix m; 96 | m << 0, 0, 0, 4, 0, 0, 0, 0; 97 | 98 | REQUIRE(computeTest(m)); 99 | } 100 | } 101 | -------------------------------------------------------------------------------- /tests/UpdateMatricesTest.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file UpdateMatricesTest.cpp 3 | * @author Giulio Romualdi 4 | * @copyright Released under the terms of the BSD 3-Clause License 5 | * @date 2020 6 | */ 7 | 8 | // Catch2 9 | #include 10 | 11 | // OsqpEigen 12 | #include 13 | 14 | // colors 15 | #define ANSI_TXT_GRN "\033[0;32m" 16 | #define ANSI_TXT_MGT "\033[0;35m" // Magenta 17 | #define ANSI_TXT_DFT "\033[0;0m" // Console default 18 | #define GTEST_BOX "[ cout ] " 19 | #define COUT_GTEST ANSI_TXT_GRN << GTEST_BOX // You could add the Default 20 | #define COUT_GTEST_MGT COUT_GTEST << ANSI_TXT_MGT 21 | 22 | Eigen::Matrix H; 23 | Eigen::SparseMatrix H_s; 24 | Eigen::Matrix A; 25 | Eigen::SparseMatrix A_s; 26 | Eigen::Matrix gradient; 27 | Eigen::Matrix lowerBound; 28 | Eigen::Matrix upperBound; 29 | 30 | OsqpEigen::Solver solver; 31 | 32 | TEST_CASE("QPProblem - FirstRun") 33 | { 34 | // hessian matrix 35 | H << 4, 0, 0, 2; 36 | H_s = H.sparseView(); 37 | H_s.pruned(0.01); 38 | 39 | // linear constraint matrix 40 | A << 1, 1, 1, 0, 0, 1; 41 | A_s = A.sparseView(); 42 | 43 | gradient << 1, 1; 44 | lowerBound << 1, 0, 0; 45 | upperBound << 1, 0.7, 0.7; 46 | 47 | solver.settings()->setVerbosity(false); 48 | 49 | solver.data()->setNumberOfVariables(2); 50 | solver.data()->setNumberOfConstraints(3); 51 | solver.settings()->setScaling(0); 52 | REQUIRE(solver.data()->setHessianMatrix(H_s)); 53 | REQUIRE(solver.data()->setGradient(gradient)); 54 | REQUIRE(solver.data()->setLinearConstraintsMatrix(A_s)); 55 | REQUIRE(solver.data()->setLowerBound(lowerBound)); 56 | REQUIRE(solver.data()->setUpperBound(upperBound)); 57 | 58 | REQUIRE(solver.initSolver()); 59 | REQUIRE(solver.solveProblem() == OsqpEigen::ErrorExitFlag::NoError); 60 | 61 | auto solution = solver.getSolution(); 62 | std::cout << COUT_GTEST_MGT << "Solution [" << solution(0) << " " << solution(1) << "]" 63 | << ANSI_TXT_DFT << std::endl; 64 | } 65 | 66 | TEST_CASE("QPProblem - SparsityConstant") 67 | { 68 | // update hessian matrix 69 | H << 4, 0, 0, 2; 70 | H_s = H.sparseView(); 71 | A << 2, 1, 1, 0, 0, 1; 72 | A_s = A.sparseView(); 73 | 74 | REQUIRE(solver.updateHessianMatrix(H_s)); 75 | REQUIRE(solver.updateLinearConstraintsMatrix(A_s)); 76 | REQUIRE(solver.solveProblem() == OsqpEigen::ErrorExitFlag::NoError); 77 | 78 | auto solution = solver.getSolution(); 79 | std::cout << COUT_GTEST_MGT << "Solution [" << solution(0) << " " << solution(1) << "]" 80 | << ANSI_TXT_DFT << std::endl; 81 | }; 82 | 83 | TEST_CASE("QPProblem - SparsityChange") 84 | { 85 | // update hessian matrix 86 | H << 1, 1, 1, 2; 87 | H_s = H.sparseView(); 88 | A << 1, 1, 1, 0.4, 0, 1; 89 | A_s = A.sparseView(); 90 | 91 | REQUIRE(solver.updateHessianMatrix(H_s)); 92 | REQUIRE(solver.updateLinearConstraintsMatrix(A_s)); 93 | REQUIRE(solver.solveProblem() == OsqpEigen::ErrorExitFlag::NoError); 94 | 95 | auto solution = solver.getSolution(); 96 | std::cout << COUT_GTEST_MGT << "Solution [" << solution(0) << " " << solution(1) << "]" 97 | << ANSI_TXT_DFT << std::endl; 98 | }; 99 | --------------------------------------------------------------------------------