├── .gitignore
├── .gitattributes
├── core
    ├── QpSolversEigen
    │   ├── QpSolversEigen.hpp
    │   ├── Debug.hpp
    │   ├── Debug.cpp
    │   ├── Constants.hpp
    │   ├── NullSolver.hpp
    │   ├── SolverInterface.hpp
    │   ├── NullSolver.cpp
    │   ├── Solver.cpp
    │   └── Solver.hpp
    └── CMakeLists.txt
├── tests
    ├── QpSolversEigenCommonTestMacros.hpp
    ├── CMakeLists.txt
    ├── UpdateMatricesTest.cpp
    ├── QPTest.cpp
    ├── MPCUpdateMatricesTest.cpp
    ├── MPCTest.cpp
    └── MPCEqualityConstraintsTest.cpp
├── examples
    ├── simple
    │   ├── CMakeLists.txt
    │   └── SimpleExample.cpp
    └── mpc
    │   ├── CMakeLists.txt
    │   └── MPCExample.cpp
├── plugins
    ├── CMakeLists.txt
    ├── osqp
    │   ├── CMakeLists.txt
    │   ├── README.md
    │   └── QpSolversEigenOsqp.cpp
    └── proxqp
    │   ├── CMakeLists.txt
    │   ├── README.md
    │   └── QpSolversEigenProxqp.cpp
├── cmake
    ├── FindVALGRIND.cmake
    ├── AddQpSolversEigenUnitTest.cmake
    └── valgrind-macos.supp
├── .github
    └── workflows
    │   ├── update-pixi-lockfile.yaml
    │   └── test-pixi.yaml
├── LICENSE
├── pixi.toml
├── docs
    └── migrate_from_osqp_eigen.md
├── .clang-format
├── CMakeLists.txt
└── README.md


/.gitignore:
--------------------------------------------------------------------------------
1 | 
2 | .pixi
3 | *.egg-info
4 | .build
5 | build
6 | Testing/


--------------------------------------------------------------------------------
/.gitattributes:
--------------------------------------------------------------------------------
1 | # GitHub syntax highlighting
2 | pixi.lock linguist-language=YAML linguist-generated=true
3 | 


--------------------------------------------------------------------------------
/core/QpSolversEigen/QpSolversEigen.hpp:
--------------------------------------------------------------------------------
1 | #ifndef QPSOLVERSEIGEN_QPSOLVERSEIGEN_H
2 | #define QPSOLVERSEIGEN_QPSOLVERSEIGEN_H
3 | 
4 | #include <QpSolversEigen/Constants.hpp>
5 | #include <QpSolversEigen/Solver.hpp>
6 | 
7 | #endif
8 | 


--------------------------------------------------------------------------------
/core/QpSolversEigen/Debug.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef QPSOLVERSEIGEN_DEBUG_HPP
 2 | #define QPSOLVERSEIGEN_DEBUG_HPP
 3 | 
 4 | #include <iostream>
 5 | 
 6 | namespace QpSolversEigen
 7 | {
 8 | std::ostream& debugStream();
 9 | }
10 | 
11 | #endif
12 | 


--------------------------------------------------------------------------------
/tests/QpSolversEigenCommonTestMacros.hpp:
--------------------------------------------------------------------------------
1 | #ifndef QPSOLVERSEIGEN_QPSOLVERSEIGENCOMMONTESTMACROS_HPP
2 | #define QPSOLVERSEIGEN_QPSOLVERSEIGENCOMMONTESTMACROS_HPP
3 | 
4 | #define QPSOLVERSEIGEN_SOLVERS_TO_TEST GENERATE("osqp", "proxqp")
5 | 
6 | #endif


--------------------------------------------------------------------------------
/examples/simple/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 3.16...3.31)
2 | 
3 | project(QpSolversEigen-SimpleExample)
4 | 
5 | find_package(QpSolversEigen REQUIRED)
6 | 
7 | add_executable(QpSolversEigen-SimpleExample SimpleExample.cpp)
8 | target_link_libraries(QpSolversEigen-SimpleExample QpSolversEigen::QpSolversEigen)
9 | 


--------------------------------------------------------------------------------
/examples/mpc/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.16...3.31)
 2 | 
 3 | project(QpSolversEigen-MPCExample)
 4 | 
 5 | find_package(QpSolversEigen REQUIRED)
 6 | 
 7 | add_executable(QpSolversEigen-MPCExaple MPCExample.cpp)
 8 | target_link_libraries(QpSolversEigen-MPCExaple QpSolversEigen::QpSolversEigen)
 9 | target_compile_features(QpSolversEigen-MPCExaple PUBLIC cxx_std_20)
10 | 


--------------------------------------------------------------------------------
/core/QpSolversEigen/Debug.cpp:
--------------------------------------------------------------------------------
 1 | #include <QpSolversEigen/Debug.hpp>
 2 | 
 3 | namespace QpSolversEigen
 4 | {
 5 | 
 6 | // Taken from https://stackoverflow.com/a/8243866/2702753
 7 | class NullStream : public std::ostream
 8 | {
 9 | public:
10 |     NullStream()
11 |         : std::ostream(nullptr)
12 |     {
13 |     }
14 |     NullStream(const NullStream&)
15 |         : std::ostream(nullptr)
16 |     {
17 |     }
18 | };
19 | 
20 | template <class T> const NullStream& operator<<(NullStream&& os, const T& value)
21 | {
22 |     return os;
23 | }
24 | 
25 | NullStream theStream;
26 | 
27 | std::ostream& debugStream()
28 | {
29 |     return std::cerr;
30 | }
31 | }
32 | 


--------------------------------------------------------------------------------
/tests/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | 
 2 | add_qpsolverseigen_test(
 3 |   NAME QP
 4 |   SOURCES QPTest.cpp
 5 |   LINKS QpSolversEigen::QpSolversEigen)
 6 | 
 7 | add_qpsolverseigen_test(
 8 |   NAME UpdateMatrices
 9 |   SOURCES UpdateMatricesTest.cpp
10 |   LINKS QpSolversEigen::QpSolversEigen)
11 | 
12 | add_qpsolverseigen_test(
13 |   NAME MPC
14 |   SOURCES MPCTest.cpp
15 |   LINKS QpSolversEigen::QpSolversEigen
16 |   COMPILE_DEFINITIONS _USE_MATH_DEFINES)
17 | 
18 | add_qpsolverseigen_test(
19 |   NAME MPCUpdateMatrices
20 |   SOURCES MPCUpdateMatricesTest.cpp
21 |   LINKS QpSolversEigen::QpSolversEigen
22 |   COMPILE_DEFINITIONS _USE_MATH_DEFINES)
23 | 
24 |   add_qpsolverseigen_test(
25 |     NAME MPCEqualityConstraints
26 |     SOURCES MPCEqualityConstraintsTest.cpp
27 |     LINKS QpSolversEigen::QpSolversEigen
28 |     COMPILE_DEFINITIONS _USE_MATH_DEFINES)
29 | 
30 | 


--------------------------------------------------------------------------------
/plugins/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | set(QPSOLVERSEIGEN_ENABLE_OSQP_DEFAULT_VALUE OFF)
 2 | if(NOT DEFINED QPSOLVERSEIGEN_ENABLE_OSQP)
 3 |     find_package(OsqpEigen QUIET)
 4 |     set(QPSOLVERSEIGEN_ENABLE_OSQP_DEFAULT_VALUE ${OsqpEigen_FOUND})
 5 | endif()
 6 | option(QPSOLVERSEIGEN_ENABLE_OSQP "If ON, compile the osqp plugin based on osqp-eigen" ${QPSOLVERSEIGEN_ENABLE_OSQP_DEFAULT_VALUE})
 7 | if(QPSOLVERSEIGEN_ENABLE_OSQP)
 8 |     add_subdirectory(osqp)
 9 | endif()
10 | 
11 | 
12 | set(QPSOLVERSEIGEN_ENABLE_PROXQP_DEFAULT_VALUE OFF)
13 | if(NOT DEFINED QPSOLVERSEIGEN_ENABLE_PROXQP)
14 |     find_package(proxsuite QUIET)
15 |     set(QPSOLVERSEIGEN_ENABLE_PROXQP_DEFAULT_VALUE ${proxsuite_FOUND})
16 | endif()
17 | option(QPSOLVERSEIGEN_ENABLE_PROXQP "If ON, compile the proxqp plugin based on proxsuite" ${QPSOLVERSEIGEN_ENABLE_PROXQP_DEFAULT_VALUE})
18 | if(QPSOLVERSEIGEN_ENABLE_PROXQP)
19 |     add_subdirectory(proxqp)
20 | endif()
21 | 


--------------------------------------------------------------------------------
/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 | 


--------------------------------------------------------------------------------
/.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 | 
10 | jobs:
11 |   pixi-update:
12 |     runs-on: ubuntu-22.04
13 |     steps:
14 |       - uses: actions/checkout@v4
15 | 
16 |       - name: Set up pixi
17 |         uses: prefix-dev/setup-pixi@v0.8.8
18 |         with:
19 |           run-install: false
20 | 
21 |       - name: Install pixi-diff-to-markdown
22 |         run: pixi global install pixi-diff-to-markdown
23 | 
24 |       - name: Update lockfiles
25 |         run: |
26 |           set -o pipefail
27 |           pixi update --json | pixi exec pixi-diff-to-markdown >> diff.md
28 | 
29 |       - name: Create pull request
30 |         uses: peter-evans/create-pull-request@v6
31 |         with:
32 |           token: ${{ secrets.GITHUB_TOKEN }}
33 |           commit-message: Update pixi lockfile
34 |           title: Update pixi lockfile
35 |           body-path: diff.md
36 |           branch: update-pixi
37 |           base: main
38 |           labels: pixi
39 |           delete-branch: true
40 |           add-paths: pixi.lock
41 | 


--------------------------------------------------------------------------------
/plugins/osqp/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | # Detect if we are doing a standalone build of this plugin, using an external qpsolvers-eigen
 2 | if(CMAKE_SOURCE_DIR STREQUAL CMAKE_CURRENT_SOURCE_DIR)
 3 |   cmake_minimum_required(VERSION 3.16)
 4 |   project(qpsolvers-eigen-osqp)
 5 |   find_package(QpSolversEigen REQUIRED)
 6 |   find_package(sharedlibpp REQUIRED)
 7 |   include(GNUInstallDirs)
 8 |   option(BUILD_SHARED_LIBS "Build libraries as shared as opposed to static" ON)
 9 |   option(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS "Export all symbols in Windows" ON)
10 | endif()
11 | 
12 | find_package(OsqpEigen REQUIRED)
13 | 
14 | # The library name needs to be coherent with the scheme used in
15 | # getShlibppLibraryNameFromSolverName, i.e. qpsolvers-eigen-<solverName>
16 | add_library(qpsolvers-eigen-osqp SHARED
17 |     QpSolversEigenOsqp.cpp
18 | )
19 | 
20 | target_link_libraries(qpsolvers-eigen-osqp
21 |     PRIVATE
22 |     QpSolversEigen::QpSolversEigen
23 |     OsqpEigen::OsqpEigen
24 |     sharedlibpp::sharedlibpp
25 | )
26 | 
27 | install(TARGETS qpsolvers-eigen-osqp
28 |         LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}"
29 |         ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}"
30 |         RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}")
31 | 


--------------------------------------------------------------------------------
/plugins/proxqp/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | # Detect if we are doing a standalone build of this plugin, using an external qpsolvers-eigen
 2 | if(CMAKE_SOURCE_DIR STREQUAL CMAKE_CURRENT_SOURCE_DIR)
 3 |   cmake_minimum_required(VERSION 3.16)
 4 |   project(qpsolvers-eigen-proxqp)
 5 |   find_package(QpSolversEigen REQUIRED)
 6 |   find_package(sharedlibpp REQUIRED)
 7 |   include(GNUInstallDirs)
 8 |   option(BUILD_SHARED_LIBS "Build libraries as shared as opposed to static" ON)
 9 |   option(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS "Export all symbols in Windows" ON)
10 | endif()
11 | 
12 | find_package(proxsuite REQUIRED)
13 | 
14 | # The library name needs to be coherent with the scheme used in
15 | # getShlibppLibraryNameFromSolverName, i.e. qpsolvers-eigen-<solverName>
16 | add_library(qpsolvers-eigen-proxqp SHARED
17 |     QpSolversEigenProxqp.cpp
18 | )
19 | 
20 | target_compile_features(qpsolvers-eigen-proxqp PUBLIC cxx_std_17)
21 | 
22 | target_link_libraries(qpsolvers-eigen-proxqp
23 |     PRIVATE
24 |     QpSolversEigen::QpSolversEigen
25 |     proxsuite::proxsuite
26 |     sharedlibpp::sharedlibpp
27 | )
28 | 
29 | install(TARGETS qpsolvers-eigen-proxqp
30 |         LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}"
31 |         ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}"
32 |         RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}")
33 | 


--------------------------------------------------------------------------------
/core/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | # List of CPP (source) library files.
 2 | set(QpSolversEigen_SRC
 3 |   QpSolversEigen/Debug.cpp
 4 |   QpSolversEigen/NullSolver.cpp
 5 |   QpSolversEigen/Solver.cpp)
 6 | 
 7 | set(QpSolversEigen_HDR
 8 |   QpSolversEigen/Constants.hpp
 9 |   QpSolversEigen/Debug.hpp
10 |   QpSolversEigen/NullSolver.hpp
11 |   QpSolversEigen/Solver.hpp
12 |   QpSolversEigen/SolverInterface.hpp
13 |   QpSolversEigen/QpSolversEigen.hpp)
14 | 
15 | add_library(QpSolversEigen ${QpSolversEigen_SRC} ${QpSolversEigen_HDR})
16 | # We want to keep the naming of library consistent, i.e.
17 | # <prefix>qpsolvers-eigen<extension> for the core library and plugin loader
18 | # <prefix>qpsolvers-eigen-osqp<extension> for the osqp plugin
19 | # <prefix>qpsolvers-eigen-proxqp<extension> for the proxqp plugin and so on so forth
20 | set_target_properties(QpSolversEigen PROPERTIES OUTPUT_NAME "qpsolvers-eigen")
21 | 
22 | target_include_directories(QpSolversEigen PUBLIC
23 |   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>"
24 |   "$<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>")
25 | target_link_libraries(QpSolversEigen PUBLIC Eigen3::Eigen PRIVATE sharedlibpp::sharedlibpp)
26 | add_library(QpSolversEigen::QpSolversEigen ALIAS QpSolversEigen)
27 | 
28 | set_target_properties(QpSolversEigen PROPERTIES
29 |   PUBLIC_HEADER "${QpSolversEigen_HDR}")
30 | 
31 | target_compile_features(QpSolversEigen PUBLIC cxx_std_17)
32 | 
33 | install(TARGETS QpSolversEigen
34 |   EXPORT  ${PROJECT_NAME}
35 |   PUBLIC_HEADER DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/QpSolversEigen")
36 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | BSD 3-Clause License
 2 | 
 3 | Copyright (c) 2024, Artificial and Mechanical Intelligence 
 4 | 
 5 | Redistribution and use in source and binary forms, with or without
 6 | modification, are permitted provided that the following conditions are met:
 7 | 
 8 | 1. Redistributions of source code must retain the above copyright notice, this
 9 |    list of conditions and the following disclaimer.
10 | 
11 | 2. Redistributions in binary form must reproduce the above copyright notice,
12 |    this list of conditions and the following disclaimer in the documentation
13 |    and/or other materials provided with the distribution.
14 | 
15 | 3. Neither the name of the copyright holder nor the names of its
16 |    contributors may be used to endorse or promote products derived from
17 |    this software without specific prior written permission.
18 | 
19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
29 | 


--------------------------------------------------------------------------------
/.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.os }}]'
20 |     runs-on: ${{ matrix.os }}
21 |     strategy:
22 |       fail-fast: false
23 |       matrix:
24 |         os: [
25 |           ubuntu-24.04,
26 |           macos-latest,
27 |           windows-2022
28 |         ]
29 |     steps:
30 |     - uses: actions/checkout@v4
31 | 
32 |     # 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
33 |     - name: Delete pixi.lock on scheduled jobs or if delete_pixi_lock is true
34 |       if: github.event_name == 'schedule' || (github.event_name == 'workflow_dispatch' && github.event.inputs.delete_pixi_lock == 'true')
35 |       shell: bash
36 |       run: |
37 |         rm pixi.lock
38 | 
39 | 
40 |     # To use valgrind even with conda/pixi we still need to install libc6-dbg via apt on Debian-based distros
41 |     # See https://github.com/robotology/osqp-eigen/pull/171#issuecomment-2458149581
42 |     - name: Install libc6-dbg
43 |       if: contains(matrix.os, 'ubuntu')
44 |       run: |
45 |         sudo apt-get install libc6-dbg
46 | 
47 |     - name: Set up pixi
48 |       uses: prefix-dev/setup-pixi@v0.8.8
49 | 
50 |     - name: Print pixi info
51 |       run: pixi info
52 | 
53 |     - name: Build and test the project
54 |       run: pixi run test
55 | 


--------------------------------------------------------------------------------
/pixi.toml:
--------------------------------------------------------------------------------
 1 | [project]
 2 | name = "qpsolvers-eigen"
 3 | authors = ["Silvio Traversaro <silvio@traversaro.it>", "Stefano Dafarra <stefano.dafarra@iit.it", "Giulio Romualdi <giulio.romualdi@iit.it>"]
 4 | description = "C++ abstraction layers for Quadratic Programming Solvers."
 5 | channels = ["conda-forge"]
 6 | platforms = ["linux-64", "linux-aarch64", "win-64", "osx-64", "osx-arm64"]
 7 | 
 8 | [target.win.activation.env]
 9 | CMAKE_INSTALL_PREFIX = "%CONDA_PREFIX%\\Library"
10 | QPSOLVERSEIGEN_RUN_Valgrind_tests = "OFF"
11 | 
12 | [target.unix.activation.env]
13 | CMAKE_INSTALL_PREFIX = "$CONDA_PREFIX"
14 | QPSOLVERSEIGEN_RUN_Valgrind_tests = "OFF"
15 | 
16 | [target.linux.activation.env]
17 | QPSOLVERSEIGEN_RUN_Valgrind_tests = "ON"
18 | 
19 | 
20 | 
21 | [tasks]
22 | configure = { cmd = [
23 |     "cmake",
24 |     "-DCMAKE_BUILD_TYPE=Release",
25 |     "-DQPSOLVERSEIGEN_RUN_Valgrind_tests=$QPSOLVERSEIGEN_RUN_Valgrind_tests",
26 |     "-DBUILD_TESTING:BOOL=ON",
27 |     # Use the cross-platform Ninja generator
28 |     "-G",
29 |     "Ninja",
30 |     # The source is in the root directory
31 |     "-S",
32 |     ".",
33 |     # We wanna build in the .build directory
34 |     "-B",
35 |     ".build",
36 | ]}
37 | 
38 | build = { cmd = "cmake --build .build --config Release", depends-on = ["configure"] }
39 | test = { cmd = "ctest --test-dir .build --build-config Release", depends-on = ["build"] }
40 | install = { cmd = ["cmake", "--install", ".build", "--config", "Release"], depends-on = ["build"] }
41 | uninstall = { cmd = ["cmake", "--build", ".build", "--target", "uninstall"]}
42 | 
43 | 
44 | [dependencies]
45 | cmake = "*"
46 | c-compiler = "*"
47 | cxx-compiler = "*"
48 | ninja = "*"
49 | pkg-config = "*"
50 | osqp-eigen = "*"
51 | ycm-cmake-modules = "*"
52 | catch2 = "*"
53 | proxsuite = "*"
54 | 
55 | [target.linux.dependencies]
56 | valgrind = "*"
57 | 
58 | # MSVC 2022 is required here as proxqp requires it
59 | [target.win-64.dependencies]
60 | vs2022_win-64 = "*"
61 | 
62 | 


--------------------------------------------------------------------------------
/plugins/osqp/README.md:
--------------------------------------------------------------------------------
 1 | # qpsolvers-eigen-osqp
 2 | 
 3 | [`osqp`](https://github.com/osqp/osqp) plugin for `qpsolvers-eigen`, based on [`osqp-eigen`](https://github.com/robotology/osqp-eigen).
 4 | 
 5 | ## Supported parameters
 6 | 
 7 | Unless a parameter has some qpsolvers-eigen specific notes, no documentation is reported here to avoid duplicating the official documentation. For the description of each parameter, check the official osqp documentation:
 8 | * osqp 0.6: https://osqp.org/docs/release-0.6.3/interfaces/solver_settings.html#solver-settings
 9 | * osqp 1.0: https://osqp.org/docs/interfaces/solver_settings.html
10 | 
11 | If you need support for more parameters, please open an issue.
12 | 
13 | ### Boolean parameters
14 | 
15 | | Name         | Notes                     |
16 | |:------------:|:-------------------------:|
17 | | `polish`     |                           |
18 | | `verbose`    |                           |
19 | | `scaled_termination`    |                |
20 | | `warm_start` |  `warm_start` is the name of the parameter in osqp 0.6.3, `warm_starting` in osqp 1.0.0, both are supported in qpsolvers-eigen |
21 | | `warm_starting` |  `warm_start` is the name of the parameter in osqp 0.6.3, `warm_starting` in osqp 1.0.0, both are supported in qpsolvers-eigen |
22 | | `adaptive_rho` |  |
23 | 
24 | ### Integer parameters
25 | 
26 | | Name         | Notes                     |
27 | |:------------:|:-------------------------:|
28 | | `scaling`     |                           |
29 | | `adaptive_rho_interval` |  |
30 | | `max_iter` |  |
31 | | `polish_refine_iter` | | 
32 | | `linsys_solver` | Documentation available at https://osqp.org/docs/release-0.6.3/interfaces/solver_settings.html#solver-settings (osqp 0.6) https://osqp.org/docs/interfaces/linear_systems_solvers.html#linear-system-solvers-setting (osqp 1.0) |
33 | | `check_termination` | |
34 | 
35 | ### Real Number parameters
36 | 
37 | | Name         | Notes                     |
38 | |:------------:|:-------------------------:|
39 | | `rho`     |                           |
40 | | `sigma`   | |
41 | | `adaptive_rho_tolerance`        | |
42 | | `adaptive_rho_fraction`        | |
43 | | `eps_abs` | |
44 | | `eps_rel` | |
45 | | `eps_prim_inf` | |
46 | | `eps_dual_inf` | |
47 | | `alpha` |  |
48 | | `delta` |  |
49 | 


--------------------------------------------------------------------------------
/examples/simple/SimpleExample.cpp:
--------------------------------------------------------------------------------
 1 | #include <cstdlib>
 2 | #include <iostream>
 3 | 
 4 | #include <QpSolversEigen/QpSolversEigen.hpp>
 5 | #include <Eigen/Dense>
 6 | #include <Eigen/Sparse>
 7 | 
 8 | 
 9 | int main()
10 | {
11 |     Eigen::SparseMatrix<double> H_s(2, 2);
12 |     H_s.insert(0, 0) = 4;
13 |     H_s.insert(0, 1) = 1;
14 |     H_s.insert(1, 0) = 1;
15 |     H_s.insert(1, 1) = 2;
16 | 
17 |     Eigen::SparseMatrix<double> A_s(3, 2);
18 |     A_s.insert(0, 0) = 1;
19 |     A_s.insert(0, 1) = 1;
20 |     A_s.insert(1, 0) = 1;
21 |     A_s.insert(2, 1) = 1;
22 | 
23 |     Eigen::Matrix<double, 2, 1> gradient;
24 |     gradient << 1, 1;
25 | 
26 |     Eigen::Matrix<double, 3, 1> lowerBound;
27 |     lowerBound << 1, 0, 0;
28 | 
29 |     Eigen::Matrix<double, 3, 1> upperBound;
30 |     upperBound << 1, 0.7, 0.7;
31 | 
32 |     QpSolversEigen::Solver solver;
33 | 
34 |     // Here you select the solver, possible options are:
35 |     // * osqp
36 |     // * proxqp
37 |     bool ok = solver.instantiateSolver("proxqp");
38 | 
39 |     if (!ok)
40 |     {
41 |         std::cerr << "Error in instantiating the solver" << std::endl;
42 |         return EXIT_FAILURE;
43 |     }
44 | 
45 |     // Set osqp-specific parameters
46 |     if (solver.getSolverName() == "osqp")
47 |     {
48 |         solver.setBooleanParameter("verbose", true);
49 |         solver.setRealNumberParameter("alpha", 1.0);
50 |         // See https://github.com/robotology/osqp-eigen/pull/172
51 |         solver.setBooleanParameter("polish", true);
52 |     }
53 | 
54 |     solver.data()->setNumberOfVariables(2);
55 |     solver.data()->setNumberOfInequalityConstraints(3);
56 |     ok = ok && solver.data()->setHessianMatrix(H_s);
57 |     ok = ok && solver.data()->setGradient(gradient);
58 |     ok = ok && solver.data()->setInequalityConstraintsMatrix(A_s);
59 |     ok = ok && solver.data()->setLowerBound(lowerBound);
60 |     ok = ok && solver.data()->setUpperBound(upperBound);
61 |     ok = ok && solver.initSolver();
62 | 
63 |     if (!ok)
64 |     {
65 |         std::cerr << "Error in solver initialization" << std::endl;
66 |         return EXIT_FAILURE;
67 |     }
68 | 
69 |     ok = ok && (solver.solveProblem() == QpSolversEigen::ErrorExitFlag::NoError);
70 | 
71 |     if (!ok)
72 |     {
73 |         std::cerr << "Error in solving the problem" << std::endl;
74 |         return EXIT_FAILURE;
75 |     }
76 | 
77 |     std::cerr << "Solution: " << solver.getSolution() << std::endl;
78 | }
79 | 


--------------------------------------------------------------------------------
/core/QpSolversEigen/Constants.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef QPSOLVERSEIGEN_CONSTANTS_HPP
 2 | #define QPSOLVERSEIGEN_CONSTANTS_HPP
 3 | 
 4 | #include <limits>
 5 | #include <string>
 6 | 
 7 | namespace QpSolversEigen
 8 | {
 9 | constexpr double INFTY = std::numeric_limits<double>::infinity(); /**< Infinity constant for QpSolversEigen, will be translated to solver specific constants if necessary */
10 | 
11 | /**
12 |  * Status of the solver.
13 |  *
14 |  * For historical reasons, the status code resemble the one of OSQP, however this is not something that 
15 |  * is guaranteed to be true in the future. Each solver status will be mapped to this QpSolversEigen specific
16 |  * status code. If the solver status of the solver does not match any of the listed status, the status reported
17 |  * will be SolverSpecificUnknownStatus.
18 |  */
19 | enum class Status : int
20 | {
21 |     Solved = 1,
22 |     SolvedInaccurate = 2,
23 |     PrimalInfeasible = 3,
24 |     PrimalInfeasibleInaccurate = 4,
25 |     DualInfeasible = 5,
26 |     DualInfeasibleInaccurate = 6,
27 |     MaxIterReached = 7,
28 |     TimeLimitReached = 8,
29 |     NonCvx = 9,
30 |     Sigint = 10,
31 |     Unsolved = 11,
32 |     SolverNotInitialized = 12,
33 |     SolvedClosestPrimalFeasible = 13,
34 |     SolverSpecificUnknownStatus = 1000,
35 | };
36 | 
37 | /**
38 |  * Error exit flag of the Solver
39 |  * 
40 |  * For historical reasons, the error exit flag code resemble the one of OSQP, however this is not something that 
41 |  * is guaranteed to be true in the future. Each solver exit flag will be mapped to this QpSolversEigen specific
42 |  * ErrorExitFlag code. If the exit code status of the solver does not match any of the listed status, the status reported
43 |  * will be SolverSpecificUnknownError.
44 |  */
45 | enum class ErrorExitFlag : int
46 | {
47 |     NoError = 0,
48 |     DataValidationError = 1,
49 |     SettingsValidationError = 2,
50 |     LinsysSolverLoadError = 3,
51 |     LinsysSolverInitError = 4,
52 |     NonCvxError = 5,
53 |     MemAllocError = 6,
54 |     WorkspaceNotInitError = 7,
55 |     SolverSpecificUnknownError = 1000,
56 | };
57 | 
58 | /**
59 |  * Get the library name from the solver name
60 |  */
61 | inline std::string getSharedlibppLibraryNameFromSolverName(const std::string& solverName)
62 | {
63 |     return "qpsolvers-eigen-" + solverName;
64 | }
65 | 
66 | /**
67 |  * Get the factory name from the solver name
68 |  */
69 | inline std::string getSharedlibppFactoryNameFromSolverName(const std::string& solverName)
70 | {
71 |     return "qpsolvers_eigen_" + solverName;
72 | }
73 | 
74 | }
75 | 
76 | #endif
77 | 


--------------------------------------------------------------------------------
/plugins/proxqp/README.md:
--------------------------------------------------------------------------------
 1 | # qpsolvers-eigen-proxqp
 2 | 
 3 | `proxqp` plugin for `qpsolvers-eigen`, the `proxqp` solver is contained in the [`proxsuite`](https://github.com/Simple-Robotics/proxsuite) library.
 4 | 
 5 | ## Supported parameters
 6 | 
 7 | Unless a parameter has some qpsolvers-eigen specific notes, no documentation is reported here to avoid duplicating the official documentation. For the description of each parameter, check the official proxqp documentation:
 8 | * https://simple-robotics.github.io/proxsuite/structproxsuite_1_1proxqp_1_1Settings.html
 9 | 
10 | If you need support for more parameters, please open an issue.
11 | 
12 | ### Boolean parameters
13 | 
14 | | Name         | Notes                     |
15 | |:------------:|:-------------------------:|
16 | | `verbose`    |                           |
17 | | `update_preconditioner`    |                |
18 | | `compute_preconditioner` |  |
19 | | `compute_timings` |   |
20 | | `check_duality_gap` |  |
21 | | `bcl_update` |  |
22 | | `primal_infeasibility_solving` | |
23 | 
24 | ### Integer parameters
25 | 
26 | | Name         | Notes                     |
27 | |:------------:|:-------------------------:|
28 | | `max_iter`     |                           |
29 | | `max_iter_in `     |                           |
30 | | `safe_guard`     |                           |
31 | | `nb_iterative_refinement`     |                           |
32 | | `preconditioner_max_iter`     |                           |
33 | | `frequence_infeasibility_check` | |
34 | 
35 | 
36 | ### Real Number parameters
37 | 
38 | | Name         | Notes                     |
39 | |:------------:|:-------------------------:|
40 | | `default_mu_eq`   |     |
41 | | `default_mu_in`   | |
42 | | `alpha_bcl`        | |
43 | | `beta_bcl`        | |
44 | | `refactor_dual_feasibility_threshold` | |
45 | | `refactor_rho_threshold` | |
46 | | `mu_min_eq` | |
47 | | `mu_min_in` | |
48 | | `mu_max_eq_inv` | |
49 | | `mu_max_in_inv` | |
50 | | `mu_update_factor` | |
51 | | `mu_update_inv_factor` | |
52 | | `cold_reset_mu_eq` | |
53 | | `cold_reset_mu_in` | |
54 | | `cold_reset_mu_eq_inv` | |
55 | | `cold_reset_mu_in_inv` | |
56 | | `eps_abs` | |
57 | | `eps_rel` |  |
58 | | `eps_refact` |  |
59 | | `eps_duality_gap_abs` | |
60 | | `eps_duality_gap_rel` | |
61 | | `preconditioner_accuracy` | |
62 | | `alpha_gpdal` | |
63 | | `default_H_eigenvalue_estimate` | |
64 | 
65 | ### String parameters
66 | 
67 | | Name         | Notes                     |
68 | |:------------:|:-------------------------:|
69 | | `initial_guess`   | Possible values are `NO_INITIAL_GUESS`, `EQUALITY_CONSTRAINED_INITIAL_GUESS`, `WARM_START_WITH_PREVIOUS_RESULT`, `WARM_START` or `COLD_START_WITH_PREVIOUS_RESULT`.  See https://simple-robotics.github.io/proxsuite/md_doc_22-ProxQP__api.html#OverviewInitialGuess for more details. |
70 | 


--------------------------------------------------------------------------------
/docs/migrate_from_osqp_eigen.md:
--------------------------------------------------------------------------------
 1 | # How to migrate from osqp-eigen to qpsolvers-eigen
 2 | 
 3 | This document list the changes that you need to do to migrate from `osqp-eigen` to `qpsolvers-eigen`.
 4 | 
 5 | ## CMake
 6 | 
 7 | Change any instance of `find_package(OsqpEigen)` to `find_package(QpSolversEigen)` and any instance of `OsqpEigen::OsqpEigen` to `QpSolversEigen::QpSolversEigen`.
 8 | 
 9 | ## Header inclusion
10 | 
11 | Remove any line that include OsqpEigen headers:
12 | 
13 | ~~~cxx
14 | #include <OsqpEigen/...>
15 | ~~~
16 | 
17 | and instead include:
18 | 
19 | ~~~cxx
20 | #include <QpSolversEigen/QpSolversEigen.hpp>
21 | ~~~
22 | 
23 | ## Solver instantiation
24 | 
25 | With osqp-eigen, the solver was instantiated as:
26 | 
27 | ~~~cxx
28 | OsqpEigen::Solver solver;
29 | ~~~
30 | 
31 | while for qpsolvers-eigen you need to instantiate the solver with:
32 | 
33 | ~~~cxx
34 | QpSolversEigen::Solver solver;
35 | // Change "osqp" to any solver you want to use
36 | solver.instantiateSolver("osqp");
37 | ~~~
38 | 
39 | ## solver.data() methods
40 | 
41 | All calls to methods like:
42 | 
43 | ~~~cxx
44 | solver.data()->setHessianMatrix(hessian)
45 | solver.data()->setGradient(gradient)
46 | ~~~
47 | 
48 | Need to be changed by removing `data()->`, i.e. converted to:
49 | 
50 | ~~~cxx
51 | solver.setHessianMatrix(hessian)
52 | solver.setGradient(gradient)
53 | ~~~
54 | 
55 | One difference between osqp-eigen and qpsolvers-eigen is that qpsolvers-eigen support explicit equality constraints, so the method related to set inequality constraints have new names:
56 | 
57 | | `OsqpEigen` name | `QpSolversEigen` name |
58 | |:----------------:|:----------------------:|
59 | | `updateLinearConstraintsMatrix()` | `updateInequalityConstraintsMatrix()`  |
60 | | `setLinearConstraintsMatrix()`    | `setInequalityConstraintsMatrix()`     |
61 | | `setNumberOfConstraints()`        | `setNumberOfInequalityConstraints()`   |
62 | 
63 | If in your osqp-eigen code you were converting equality constraints to inequality constraints, you can now directly set your equality constraints.
64 | 
65 | ## solver.settings() methods
66 | 
67 | As `QpSolversEigen` has a generic solver-agnostic interface, to set parameters you need to specify the parameter to set by name, so you need to port code such as:
68 | 
69 | ~~~cxx
70 | solver.settings()->setVerbosity(true);
71 | solver.settings()->setAlpha(1.0);
72 | ~~~
73 | 
74 | to 
75 | 
76 | ~~~cxx
77 | // verbose is an option common to all
78 | solver.setBooleanParameter("verbose", false);
79 | 
80 | // alpha is a osqp specific parameter, so we only set if the solver used is indeed osqp
81 | if (solver.getSolverName() == "osqp")
82 | {
83 |     REQUIRE(solver.setRealNumberParameter("alpha", 1.0));
84 | }
85 | ~~~
86 | 
87 | ### Constants
88 | 
89 | Change any instance of `OsqpEigen::INFTY` to `QpSolversEigen::INFTY`, `OsqpEigen::Status` to `QpSolversEigen::Status` and `OsqpEigen::ErrorExitFlag` to `QpSolversEigen::ErrorExitFlag` .
90 | 


--------------------------------------------------------------------------------
/cmake/AddQpSolversEigenUnitTest.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 | include(CMakeDependentOption)
 9 | 
10 | cmake_dependent_option(QPSOLVERSEIGEN_RUN_Valgrind_tests
11 |   "Run Valgrind tests?" OFF
12 |   "VALGRIND_FOUND" OFF)
13 | 
14 | if (QPSOLVERSEIGEN_RUN_Valgrind_tests)
15 |     set(CTEST_MEMORYCHECK_COMMAND ${VALGRIND_PROGRAM})
16 |     set(MEMORYCHECK_COMMAND ${VALGRIND_PROGRAM})
17 |     if (APPLE)
18 |         set(MEMORYCHECK_SUPPRESSIONS "--suppressions=${PROJECT_SOURCE_DIR}/cmake/valgrind-macos.supp")
19 |     else ()
20 |         set(MEMORYCHECK_SUPPRESSIONS "")
21 |     endif ()
22 |     set(MEMORYCHECK_COMMAND_OPTIONS "--leak-check=full --error-exitcode=1 ${MEMORYCHECK_SUPPRESSIONS}"  CACHE STRING "Options to pass to the memory checker")
23 |     mark_as_advanced(MEMORYCHECK_COMMAND_OPTIONS)
24 |     set(MEMCHECK_COMMAND_COMPLETE "${MEMORYCHECK_COMMAND} ${MEMORYCHECK_COMMAND_OPTIONS}")
25 |     separate_arguments(MEMCHECK_COMMAND_COMPLETE)
26 | endif()
27 | 
28 | if (BUILD_TESTING)
29 |     find_package(Catch2 REQUIRED)
30 | endif()
31 | 
32 | 
33 | function(add_qpsolverseigen_test)
34 | 
35 |     if(BUILD_TESTING)
36 | 
37 |       set(options)
38 |       set(oneValueArgs NAME)
39 |       set(multiValueArgs SOURCES LINKS COMPILE_DEFINITIONS)
40 | 
41 |       set(prefix "qpsolvers_eigen")
42 | 
43 |       cmake_parse_arguments(${prefix}
44 |           "${options}"
45 |           "${oneValueArgs}"
46 |           "${multiValueArgs}"
47 |           ${ARGN})
48 | 
49 |       set(name ${${prefix}_NAME})
50 |       set(unit_test_files ${${prefix}_SOURCES})
51 | 
52 |       set(targetname ${name}UnitTests)
53 |       add_executable(${targetname}
54 |           "${unit_test_files}")
55 | 
56 |       target_link_libraries(${targetname} PRIVATE Catch2::Catch2WithMain ${${prefix}_LINKS})
57 |       target_compile_definitions(${targetname} PRIVATE CATCH_CONFIG_FAST_COMPILE CATCH_CONFIG_DISABLE_MATCHERS)
58 |       target_compile_features(${targetname} PUBLIC cxx_std_17)
59 | 
60 |       add_test(NAME ${targetname} COMMAND ${targetname})
61 |       target_compile_definitions(${targetname} PRIVATE ${${prefix}_COMPILE_DEFINITIONS})
62 | 
63 |       # Append the value to the SHLIBPP_PLUGIN_PATH environment variable
64 |       if(WIN32)
65 |         set_property(TEST ${targetname} APPEND PROPERTY
66 |             ENVIRONMENT_MODIFICATION "SHLIBPP_PLUGIN_PATH=path_list_prepend:${CMAKE_RUNTIME_OUTPUT_DIRECTORY}")
67 |       else()
68 |         set_property(TEST ${targetname} APPEND PROPERTY
69 |             ENVIRONMENT_MODIFICATION "SHLIBPP_PLUGIN_PATH=path_list_prepend:${CMAKE_LIBRARY_OUTPUT_DIRECTORY}")
70 |       endif()
71 | 
72 |       if(QPSOLVERSEIGEN_RUN_Valgrind_tests)
73 |         add_test(NAME memcheck_${targetname} COMMAND ${MEMCHECK_COMMAND_COMPLETE} $<TARGET_FILE:${targetname}>)
74 |       endif()
75 | 
76 |     endif()
77 | 
78 | endfunction()
79 | 


--------------------------------------------------------------------------------
/.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 | 


--------------------------------------------------------------------------------
/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 | // Include macros common to all test
  9 | #include "QpSolversEigenCommonTestMacros.hpp"
 10 | 
 11 | // Catch2
 12 | #include <catch2/catch_test_macros.hpp>
 13 | #include <catch2/generators/catch_generators_all.hpp>
 14 | 
 15 | // QpSolversEigen
 16 | #include <QpSolversEigen/QpSolversEigen.hpp>
 17 | 
 18 | #include <iostream>
 19 | 
 20 | // colors
 21 | #define ANSI_TXT_GRN "\033[0;32m"
 22 | #define ANSI_TXT_MGT "\033[0;35m" // Magenta
 23 | #define ANSI_TXT_DFT "\033[0;0m" // Console default
 24 | #define GTEST_BOX "[     cout ] "
 25 | #define COUT_GTEST ANSI_TXT_GRN << GTEST_BOX // You could add the Default
 26 | #define COUT_GTEST_MGT COUT_GTEST << ANSI_TXT_MGT
 27 | 
 28 | 
 29 | TEST_CASE("QPProblem - UpdateMatricesTest")
 30 | {
 31 |     Eigen::Matrix<double, 2, 2> H;
 32 |     Eigen::SparseMatrix<double> H_s;
 33 |     Eigen::Matrix<double, 3, 2> A;
 34 |     Eigen::SparseMatrix<double> A_s;
 35 |     Eigen::Matrix<double, 3, 2> C;
 36 |     Eigen::SparseMatrix<double> C_s;
 37 |     Eigen::Matrix<double, 2, 1> gradient;
 38 |     Eigen::Matrix<double, 3, 1> lowerBound;
 39 |     Eigen::Matrix<double, 3, 1> upperBound;
 40 |     Eigen::Matrix<double, 3, 1> equalityConstraintVector;
 41 | 
 42 |     std::string solverName = QPSOLVERSEIGEN_SOLVERS_TO_TEST;
 43 |     QpSolversEigen::Solver solver;
 44 |     REQUIRE(solver.instantiateSolver(solverName));
 45 | 
 46 |     // hessian matrix
 47 |     H << 4, 0, 0, 2;
 48 |     H_s = H.sparseView();
 49 |     H_s.pruned(0.01);
 50 | 
 51 |     // inequality constraint matrix
 52 |     A << 1, 1, 1, 0, 0, 1;
 53 |     A_s = A.sparseView();
 54 | 
 55 |     // equality constraint matrix
 56 |     C << 1, 1, 1, 0.5, 0, 1;
 57 |     C_s = C.sparseView();
 58 | 
 59 |     gradient << 1, 1;
 60 |     lowerBound << 1, 0, 0;
 61 |     upperBound << 1, 0.7, 0.7;
 62 |     equalityConstraintVector << 1, 0.5, 0;
 63 | 
 64 |     // Set osqp-specific parameters
 65 |     if (solver.getSolverName() == "osqp")
 66 |     {
 67 |         solver.setBooleanParameter("verbose", false);
 68 |         solver.setIntegerParameter("scaling", 0);
 69 |     }
 70 |     solver.setNumberOfVariables(2);
 71 |     solver.setNumberOfInequalityConstraints(3);
 72 |     solver.setNumberOfEqualityConstraints(3);
 73 |     REQUIRE(solver.setHessianMatrix(H_s));
 74 |     REQUIRE(solver.setGradient(gradient));
 75 |     REQUIRE(solver.setInequalityConstraintsMatrix(A_s));
 76 |     REQUIRE(solver.setLowerBound(lowerBound));
 77 |     REQUIRE(solver.setUpperBound(upperBound));
 78 |     REQUIRE(solver.setEqualityConstraintsMatrix(C_s));
 79 |     REQUIRE(solver.setEqualityConstraintsVector(equalityConstraintVector));
 80 | 
 81 |     REQUIRE(solver.initSolver());
 82 |     REQUIRE(solver.solveProblem() == QpSolversEigen::ErrorExitFlag::NoError);
 83 | 
 84 |     auto solution = solver.getSolution();
 85 |     std::cout << COUT_GTEST_MGT << "Solution [" << solution(0) << " " << solution(1) << "]"
 86 |               << ANSI_TXT_DFT << std::endl;
 87 | 
 88 |     // update hessian matrix
 89 |     H << 4, 0, 0, 2;
 90 |     H_s = H.sparseView();
 91 |     A << 2, 1, 1, 0, 0, 1;
 92 |     A_s = A.sparseView();
 93 |     C << 2, 0, 2, 0.5, 0, 1;
 94 |     C_s = C.sparseView();
 95 | 
 96 |     REQUIRE(solver.updateHessianMatrix(H_s));
 97 |     REQUIRE(solver.updateInequalityConstraintsMatrix(A_s));
 98 |     REQUIRE(solver.updateEqualityConstraintsMatrix(C_s));
 99 |     REQUIRE(solver.solveProblem() == QpSolversEigen::ErrorExitFlag::NoError);
100 | 
101 |     solution = solver.getSolution();
102 |     std::cout << COUT_GTEST_MGT << "Solution [" << solution(0) << " " << solution(1) << "]"
103 |               << ANSI_TXT_DFT << std::endl;
104 | 
105 |     // update hessian matrix
106 |     H << 1, 1, 1, 2;
107 |     H_s = H.sparseView();
108 |     A << 1, 1, 1, 0.4, 0, 1;
109 |     A_s = A.sparseView();
110 |     C << 1, 1, 0.1, 0.5, 0, 0.1;
111 | 
112 |     REQUIRE(solver.updateHessianMatrix(H_s));
113 |     REQUIRE(solver.updateInequalityConstraintsMatrix(A_s));
114 |     REQUIRE(solver.updateEqualityConstraintsMatrix(C_s));
115 |     REQUIRE(solver.solveProblem() == QpSolversEigen::ErrorExitFlag::NoError);
116 | 
117 |     solution = solver.getSolution();
118 |     std::cout << COUT_GTEST_MGT << "Solution [" << solution(0) << " " << solution(1) << "]"
119 |               << ANSI_TXT_DFT << std::endl;
120 | };
121 | 


--------------------------------------------------------------------------------
/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 | // Include macros common to all test
  9 | #include "QpSolversEigenCommonTestMacros.hpp"
 10 | 
 11 | #include <iostream>
 12 | 
 13 | // Catch2
 14 | #include <catch2/catch_test_macros.hpp>
 15 | #include <catch2/generators/catch_generators_all.hpp>
 16 | 
 17 | #include <QpSolversEigen/QpSolversEigen.hpp>
 18 | 
 19 | TEST_CASE("QPProblem - Unconstrained")
 20 | {
 21 |     constexpr double tolerance = 1e-4;
 22 | 
 23 |     Eigen::SparseMatrix<double> H_s(2, 2);
 24 |     H_s.insert(0, 0) = 3;
 25 |     H_s.insert(0, 1) = 2;
 26 |     H_s.insert(1, 0) = 2;
 27 |     H_s.insert(1, 1) = 4;
 28 | 
 29 |     Eigen::Matrix<double, 2, 1> gradient;
 30 |     gradient << 3, 1;
 31 | 
 32 |     QpSolversEigen::Solver solver;
 33 |     std::string solverName = QPSOLVERSEIGEN_SOLVERS_TO_TEST;
 34 |     REQUIRE(solver.instantiateSolver(solverName));
 35 | 
 36 |     REQUIRE(solver.setBooleanParameter("verbose", true));
 37 | 
 38 |     // Verify that verbose parameters is reported as a supperted parameter
 39 |     std::vector<std::string> parametersNames;
 40 |     REQUIRE(solver.getBooleanParametersNames(parametersNames));
 41 |     REQUIRE(std::find(parametersNames.begin(), parametersNames.end(), "verbose") != parametersNames.end());
 42 | 
 43 |     // Check that a non-supported parameter is not reported
 44 |     REQUIRE(std::find(parametersNames.begin(), parametersNames.end(), "this_is_not_a_supported_parameter") == parametersNames.end());
 45 | 
 46 |     if (solver.getSolverName() == "osqp")
 47 |     {
 48 |         REQUIRE(solver.setRealNumberParameter("alpha", 1.0));
 49 | 
 50 |         REQUIRE(solver.getRealNumberParametersNames(parametersNames));
 51 |         REQUIRE(std::find(parametersNames.begin(), parametersNames.end(), "alpha") != parametersNames.end());
 52 | 
 53 |     }
 54 | 
 55 |     solver.setNumberOfVariables(2);
 56 |     solver.setNumberOfInequalityConstraints(0);
 57 | 
 58 |     REQUIRE(solver.data()->setHessianMatrix(H_s));
 59 |     REQUIRE(solver.data()->setGradient(gradient));
 60 | 
 61 |     REQUIRE(solver.initSolver());
 62 |     REQUIRE(solver.solveProblem() == QpSolversEigen::ErrorExitFlag::NoError);
 63 | 
 64 |     // expected solution
 65 |     Eigen::Matrix<double, 2, 1> expectedSolution;
 66 |     expectedSolution << -1.2500, 0.3750;
 67 | 
 68 |     REQUIRE(solver.getSolution().isApprox(expectedSolution, tolerance));
 69 | }
 70 | 
 71 | TEST_CASE("QPProblem")
 72 | {
 73 |     constexpr double tolerance = 1e-4;
 74 | 
 75 |     Eigen::SparseMatrix<double> H_s(2, 2);
 76 |     H_s.insert(0, 0) = 4;
 77 |     H_s.insert(0, 1) = 1;
 78 |     H_s.insert(1, 0) = 1;
 79 |     H_s.insert(1, 1) = 2;
 80 | 
 81 |     Eigen::SparseMatrix<double> A_s(3, 2);
 82 |     A_s.insert(0, 0) = 1;
 83 |     A_s.insert(0, 1) = 1;
 84 |     A_s.insert(1, 0) = 1;
 85 |     A_s.insert(2, 1) = 1;
 86 | 
 87 |     Eigen::Matrix<double, 2, 1> gradient;
 88 |     gradient << 1, 1;
 89 | 
 90 |     Eigen::Matrix<double, 3, 1> lowerBound;
 91 |     lowerBound << 1, 0, 0;
 92 | 
 93 |     Eigen::Matrix<double, 3, 1> upperBound;
 94 |     upperBound << 1, 0.7, 0.7;
 95 | 
 96 |     QpSolversEigen::Solver solver;
 97 |     std::string solverName = QPSOLVERSEIGEN_SOLVERS_TO_TEST;
 98 |     REQUIRE(solver.instantiateSolver(solverName));
 99 | 
100 |     if (solver.getSolverName() == "osqp")
101 |     {
102 |         REQUIRE(solver.setBooleanParameter("verbose", true));
103 |         REQUIRE(solver.setRealNumberParameter("alpha", 1.0));
104 |         // This is required to avoid non-deterministic non-accurate solutions
105 |         // See https://github.com/robotology/osqp-eigen/pull/172
106 |         REQUIRE(solver.setBooleanParameter("polish", true));
107 |     }
108 | 
109 |     solver.data()->setNumberOfVariables(2);
110 | 
111 |     solver.data()->setNumberOfInequalityConstraints(3);
112 |     REQUIRE(solver.data()->setHessianMatrix(H_s));
113 |     REQUIRE(solver.data()->setGradient(gradient));
114 |     REQUIRE(solver.data()->setInequalityConstraintsMatrix(A_s));
115 |     REQUIRE(solver.data()->setLowerBound(lowerBound));
116 |     REQUIRE(solver.data()->setUpperBound(upperBound));
117 | 
118 |     REQUIRE(solver.initSolver());
119 | 
120 |     REQUIRE(solver.solveProblem() == QpSolversEigen::ErrorExitFlag::NoError);
121 |     Eigen::Matrix<double, 2, 1> expectedSolution;
122 |     expectedSolution << 0.3, 0.7;
123 | 
124 |     REQUIRE(solver.getSolution().isApprox(expectedSolution, tolerance));
125 | }
126 | 


--------------------------------------------------------------------------------
/core/QpSolversEigen/NullSolver.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef QPSOLVERSEIGEN_NULLSOLVER_HPP
 2 | #define QPSOLVERSEIGEN_NULLSOLVER_HPP
 3 | 
 4 | // Std
 5 | #include <memory>
 6 | #include <string>
 7 | 
 8 | // Eigen
 9 | #include <Eigen/Dense>
10 | #include <Eigen/Sparse>
11 | 
12 | // QpSolversEigen
13 | #include <QpSolversEigen/SolverInterface.hpp>
14 | 
15 | namespace QpSolversEigen
16 | {
17 | 
18 | /**
19 |  * NullSolver class is a class that implements the SolverInterface but does nothing.
20 |  *
21 |  * The class is used to provide a default implementation of the SolverInterface that is used inside Solver class.
22 |  */
23 | class NullSolver final: public SolverInterface
24 | {
25 | private:
26 |     // Dummy variable for methods that return a reference to a matrix
27 |     Eigen::Matrix<double, Eigen::Dynamic, 1> m_dummy;
28 | public:
29 |     virtual ~NullSolver() = default;
30 | 
31 |     std::string getSolverName() const override;
32 |     bool initSolver() override;
33 |     bool isInitialized() override;
34 |     void clearSolver() override;
35 |     bool clearSolverVariables() override;
36 |     QpSolversEigen::ErrorExitFlag solveProblem() override;
37 |     QpSolversEigen::Status getStatus() const override;
38 |     const double getObjValue() const override;
39 |     const Eigen::Matrix<double, Eigen::Dynamic, 1>& getSolution() override;
40 |     const Eigen::Matrix<double, Eigen::Dynamic, 1>& getDualSolution() override;
41 |     bool updateHessianMatrix(const Eigen::SparseMatrix<double> &hessianMatrix) override;
42 |     bool updateInequalityConstraintsMatrix(const Eigen::SparseMatrix<double> &linearConstraintsMatrix) override;
43 |     bool updateGradient(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& gradient) override;
44 |     bool updateLowerBound(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& lowerBound) override;
45 |     bool updateUpperBound(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& upperBound) override;
46 |     bool updateBounds(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& lowerBound,
47 |                  const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& upperBound) override;
48 |     bool updateEqualityConstraintsMatrix(const Eigen::SparseMatrix<double>& equalityConstraintsMatrix) override;
49 |     bool updateEqualityConstraintsVector(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& equalityConstraintsVector) override;
50 | 
51 |     void clearHessianMatrix() override;
52 |     void clearLinearConstraintsMatrix() override;
53 |     void setNumberOfVariables(int n) override;
54 |     void setNumberOfInequalityConstraints(int m) override;
55 |     void setNumberOfEqualityConstraints(int m) override;
56 |     bool setHessianMatrix(const Eigen::SparseMatrix<double>& hessianMatrix) override;
57 |     bool setGradient(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> gradientVector) override;
58 | 
59 |     Eigen::Matrix<double, Eigen::Dynamic, 1> getGradient() override;
60 | 
61 |     bool
62 |     setInequalityConstraintsMatrix(const Eigen::SparseMatrix<double>& linearConstraintsMatrix) override;
63 |     bool setLowerBound(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> lowerBoundVector) override;
64 |     bool setUpperBound(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> upperBoundVector) override;
65 |     bool setBounds(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> lowerBound,
66 |                    Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> upperBound) override;
67 |     bool setEqualityConstraintsMatrix(const Eigen::SparseMatrix<double>& equalityConstraintsMatrix) override;
68 |     bool setEqualityConstraintsVector(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& equalityConstraintsVector) override;
69 | 
70 |     bool setBooleanParameter(const std::string& settingName, bool value) override;
71 |     bool setIntegerParameter(const std::string& settingName, int64_t value) override;
72 |     bool setRealNumberParameter(const std::string& settingName, double value) override;
73 |     bool setStringParameter(const std::string& parameterName, const std::string& value) override;
74 | 
75 |     bool getBooleanParametersNames(std::vector<std::string>& parametersNames) const override;
76 |     bool getIntegerParametersNames(std::vector<std::string>& parameterNames) const override;
77 |     bool getRealNumberParametersNames(std::vector<std::string>& parametersNames) const override;
78 |     bool getStringParametersNames(std::vector<std::string>& parametersNames) const override;
79 | 
80 |     SolverInterface* allocateInstance() const override;
81 | };
82 | 
83 | }
84 | 
85 | #endif
86 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
  1 | cmake_minimum_required(VERSION 3.16...3.31)
  2 | 
  3 | project(QpSolversEigen VERSION 0.2.0)
  4 | 
  5 | include(GNUInstallDirs)
  6 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_BINDIR}")
  7 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}")
  8 | set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}")
  9 | 
 10 | # Build shared libs on Windows
 11 | set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
 12 | 
 13 | if(MSVC)
 14 |   set(CMAKE_DEBUG_POSTFIX "d")
 15 | endif()
 16 | 
 17 | list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake)
 18 | 
 19 | option(BUILD_SHARED_LIBS "Build libraries as shared as opposed to static" ON)
 20 | 
 21 | # Disable C and C++ compiler extensions.
 22 | # C/CXX_EXTENSIONS are ON by default to allow the compilers to use extended
 23 | # variants of the C/CXX language.
 24 | # However, this could expose cross-platform bugs in user code or in the headers
 25 | # of third-party dependencies and thus it is strongly suggested to turn
 26 | # extensions off.
 27 | set(CMAKE_C_EXTENSIONS OFF)
 28 | set(CMAKE_CXX_EXTENSIONS OFF)
 29 | 
 30 | 
 31 | # Dependencies
 32 | find_package(Eigen3 REQUIRED)
 33 | 
 34 | set(QPSOLVERSEIGEN_USES_SYSTEM_SHAREDLIBPP_DEFAULT_VALUE OFF)
 35 | if(NOT DEFINED QPSOLVERSEIGEN_USES_SYSTEM_SHAREDLIBPP)
 36 |     find_package(sharedlibpp QUIET)
 37 |     set(QPSOLVERSEIGEN_USES_SYSTEM_SHAREDLIBPP_DEFAULT_VALUE ${sharedlibpp_FOUND})
 38 | endif()
 39 | option(QPSOLVERSEIGEN_USES_SYSTEM_SHAREDLIBPP "If ON, find sharedlibpp with find_package(sharedlibpp)" ${QPSOLVERSEIGEN_USES_SYSTEM_SHAREDLIBPP_DEFAULT_VALUE})
 40 | if(QPSOLVERSEIGEN_USES_SYSTEM_SHAREDLIBPP)
 41 |     find_package(sharedlibpp REQUIRED)
 42 | else()
 43 |     include(FetchContent)
 44 |     FetchContent_Declare(
 45 |         sharedlibpp
 46 |         URL https://github.com/ami-iit/sharedlibpp/archive/refs/tags/v0.0.3.zip
 47 |     )
 48 |     FetchContent_MakeAvailable(sharedlibpp)
 49 | endif()
 50 | 
 51 | set(QPSOLVERSEIGEN_USES_SYSTEM_YCM_DEFAULT_VALUE OFF)
 52 | if(NOT DEFINED QPSOLVERSEIGEN_USES_SYSTEM_YCM)
 53 |     find_package(YCM QUIET)
 54 |     set(QPSOLVERSEIGEN_USES_SYSTEM_YCM_DEFAULT_VALUE ${YCM_FOUND})
 55 | endif()
 56 | option(QPSOLVERSEIGEN_USES_SYSTEM_YCM "If ON, find ycm-cmake-modules with find_package(YCM)" ${QPSOLVERSEIGEN_USES_SYSTEM_YCM_DEFAULT_VALUE})
 57 | if(QPSOLVERSEIGEN_USES_SYSTEM_YCM)
 58 |     find_package(YCM REQUIRED)
 59 | else()
 60 |     include(FetchContent)
 61 |     FetchContent_Declare(
 62 |         YCM
 63 |         URL https://github.com/robotology/ycm-cmake-modules/archive/refs/tags/v0.16.9.zip
 64 |     )
 65 |     FetchContent_GetProperties(YCM)
 66 |     if(NOT YCM_POPULATED)
 67 |         FetchContent_Populate(YCM)
 68 |         include(${ycm_SOURCE_DIR}/tools/UseYCMFromSource.cmake)
 69 |     endif()
 70 | endif()
 71 | 
 72 | include(CMakePackageConfigHelpers)
 73 | 
 74 | option(QPSOLVERS_EIGEN_ENABLE_RPATH "Enable RPATH for this library" ON)
 75 | mark_as_advanced(QPSOLVERS_EIGEN_ENABLE_RPATH)
 76 | include(AddInstallRPATHSupport)
 77 | add_install_rpath_support(BIN_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_BINDIR}"
 78 |   LIB_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}"
 79 |   DEPENDS QPSOLVERS_EIGEN_ENABLE_RPATH
 80 |   USE_LINK_PATH)
 81 | 
 82 | # Encourage user to specify a build type (e.g. Release, Debug, etc.), otherwise set it to Release.
 83 | if(NOT CMAKE_CONFIGURATION_TYPES)
 84 |   if(NOT CMAKE_BUILD_TYPE)
 85 |       message(STATUS "Setting build type to 'Release' as none was specified.")
 86 |       set_property(CACHE CMAKE_BUILD_TYPE PROPERTY VALUE "Release")
 87 |   endif()
 88 | endif()
 89 | 
 90 | option(BUILD_TESTING "Create tests using CMake" OFF)
 91 | include(CTest)
 92 | 
 93 | # Set default build type to "Release" in single-config generators
 94 | if(NOT CMAKE_CONFIGURATION_TYPES)
 95 |     if(NOT CMAKE_BUILD_TYPE)
 96 |     set(CMAKE_BUILD_TYPE "Release" CACHE STRING
 97 |         "Choose the type of build, recommended options are: Debug or Release" FORCE)
 98 |     endif()
 99 |     set(QPSOLVERSEIGEN_BUILD_TYPES "Debug" "Release" "MinSizeRel" "RelWithDebInfo")
100 |     set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS ${QPSOLVERSEIGEN_BUILD_TYPES})
101 | endif()
102 | 
103 | # Add core library
104 | add_subdirectory(core)
105 | 
106 | # Add plugins
107 | add_subdirectory(plugins)
108 | 
109 | include(InstallBasicPackageFiles)
110 | install_basic_package_files(${PROJECT_NAME}
111 |   NAMESPACE QpSolversEigen::
112 |   VERSION ${${PROJECT_NAME}_VERSION}
113 |   COMPATIBILITY AnyNewerVersion
114 |   VARS_PREFIX ${PROJECT_NAME}
115 |   NO_CHECK_REQUIRED_COMPONENTS_MACRO
116 |   DEPENDENCIES Eigen3)
117 | 
118 | ## Testing
119 | include(AddQpSolversEigenUnitTest)
120 | add_subdirectory(tests)
121 | 


--------------------------------------------------------------------------------
/core/QpSolversEigen/SolverInterface.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef QPSOLVERSEIGEN_SOLVERINTERFACE_HPP
 2 | #define QPSOLVERSEIGEN_SOLVERINTERFACE_HPP
 3 | 
 4 | // Std
 5 | #include <memory>
 6 | #include <string>
 7 | 
 8 | // Eigen
 9 | #include <Eigen/Dense>
10 | #include <Eigen/Sparse>
11 | 
12 | // QpSolversEigen
13 | #include <QpSolversEigen/Constants.hpp>
14 | 
15 | namespace QpSolversEigen
16 | {
17 | /**
18 |  * SolverInterface class is the virtual class implemented by the solver-specific classes.
19 |  *
20 |  * Given the specific structure of the library, where the QpSolversEigen::Solver class 
21 |  * is acting both as a factory and as a proxy for each solver methods, most of the methods
22 |  * are duplicated between the Solver class and the SolverInterface class. For this
23 |  * reason, the methods are documented only once in the QpSolversEigen::Solver class.
24 |  */
25 | class SolverInterface
26 | {
27 | public:
28 |     virtual ~SolverInterface() = default;
29 | 
30 |     virtual std::string getSolverName() const = 0;
31 |     virtual bool initSolver() = 0;
32 |     virtual bool isInitialized() = 0;
33 |     virtual void clearSolver() = 0;
34 |     virtual bool clearSolverVariables() = 0;
35 |     virtual QpSolversEigen::ErrorExitFlag solveProblem() = 0;
36 |     virtual QpSolversEigen::Status getStatus() const = 0;
37 |     virtual const double getObjValue() const = 0;
38 |     virtual const Eigen::Matrix<double, Eigen::Dynamic, 1>& getSolution() = 0;
39 |     virtual  const Eigen::Matrix<double, Eigen::Dynamic, 1>& getDualSolution() = 0;
40 |     virtual bool updateHessianMatrix(const Eigen::SparseMatrix<double> &hessianMatrix) = 0;
41 |     virtual bool updateInequalityConstraintsMatrix(const Eigen::SparseMatrix<double> &linearConstraintsMatrix) = 0;
42 |     virtual bool updateGradient(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& gradient) = 0;
43 |     virtual bool updateLowerBound(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& lowerBound) = 0;
44 |     virtual bool updateUpperBound(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& upperBound) = 0;
45 |     virtual bool updateBounds(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& lowerBound,
46 |                  const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& upperBound) = 0;
47 |     virtual bool updateEqualityConstraintsMatrix(const Eigen::SparseMatrix<double>& equalityConstraintsMatrix) = 0;
48 |     virtual bool updateEqualityConstraintsVector(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& equalityConstraintsVector) = 0;
49 | 
50 |     virtual void clearHessianMatrix() = 0;
51 |     virtual void clearLinearConstraintsMatrix() = 0;
52 |     virtual void setNumberOfVariables(int n) = 0;
53 |     virtual void setNumberOfInequalityConstraints(int m) = 0;
54 |     virtual void setNumberOfEqualityConstraints(int m) = 0;
55 |     virtual bool setHessianMatrix(const Eigen::SparseMatrix<double>& hessianMatrix) = 0;
56 |     virtual bool setGradient(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> gradientVector) = 0;
57 | 
58 |     virtual Eigen::Matrix<double, Eigen::Dynamic, 1> getGradient() = 0;
59 | 
60 |     virtual bool
61 |     setInequalityConstraintsMatrix(const Eigen::SparseMatrix<double>& linearConstraintsMatrix) = 0;
62 |     virtual bool setLowerBound(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> lowerBoundVector) = 0;
63 |     virtual bool setUpperBound(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> upperBoundVector) = 0;
64 |     virtual bool setBounds(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> lowerBound,
65 |                    Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> upperBound) = 0;
66 |     virtual bool setEqualityConstraintsMatrix(const Eigen::SparseMatrix<double>& equalityConstraintsMatrix) = 0;
67 |     virtual bool setEqualityConstraintsVector(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& equalityConstraintsVector) = 0;
68 | 
69 |     virtual bool setBooleanParameter(const std::string& settingName, bool value) = 0;
70 |     virtual bool setIntegerParameter(const std::string& settingName, int64_t value) = 0;
71 |     virtual bool setRealNumberParameter(const std::string& settingName, double value) = 0;
72 |     virtual bool setStringParameter(const std::string& settingName, const std::string& value) = 0;
73 | 
74 |     virtual bool getBooleanParametersNames(std::vector<std::string>& parametersNames) const = 0;
75 |     virtual bool getIntegerParametersNames(std::vector<std::string>& parameterNames) const = 0;
76 |     virtual bool getRealNumberParametersNames(std::vector<std::string>& parametersNames) const = 0;
77 |     virtual bool getStringParametersNames(std::vector<std::string>& parametersNames) const = 0;
78 | 
79 |     /**
80 |      * Allocate a new instance of this class, and return a pointer to it.
81 |      * The ownership of the pointer is transferred to the caller.
82 |      */
83 |     virtual SolverInterface* allocateInstance() const = 0;
84 | };
85 | 
86 | }
87 | 
88 | #endif
89 | 


--------------------------------------------------------------------------------
/core/QpSolversEigen/NullSolver.cpp:
--------------------------------------------------------------------------------
  1 | 
  2 | #include <QpSolversEigen/NullSolver.hpp>
  3 | 
  4 | namespace QpSolversEigen
  5 | {
  6 | 
  7 | std::string NullSolver::getSolverName() const
  8 | {
  9 |     return "null";
 10 | }
 11 | 
 12 | bool NullSolver::initSolver()
 13 | {
 14 |     return false;
 15 | }
 16 | 
 17 | bool NullSolver::isInitialized()
 18 | {
 19 |     return false;
 20 | }
 21 | 
 22 | void NullSolver::clearSolver()
 23 | {
 24 |     return;
 25 | }
 26 | 
 27 | bool NullSolver::clearSolverVariables()
 28 | {
 29 |     return false;
 30 | }
 31 | 
 32 | QpSolversEigen::ErrorExitFlag NullSolver::solveProblem()
 33 | {
 34 |     return QpSolversEigen::ErrorExitFlag::SolverSpecificUnknownError;
 35 | }
 36 | 
 37 | QpSolversEigen::Status NullSolver::getStatus() const
 38 | {
 39 |     return QpSolversEigen::Status::SolverSpecificUnknownStatus;
 40 | }
 41 | 
 42 | const double NullSolver::getObjValue() const
 43 | {
 44 |     return -std::numeric_limits<double>::infinity();
 45 | }
 46 | 
 47 | const Eigen::Matrix<double, Eigen::Dynamic, 1>& NullSolver::getSolution()
 48 | {
 49 |     return m_dummy;
 50 | }
 51 | 
 52 | const Eigen::Matrix<double, Eigen::Dynamic, 1>& NullSolver::getDualSolution()
 53 | {
 54 |     return m_dummy;
 55 | }
 56 | 
 57 | bool NullSolver::updateHessianMatrix(const Eigen::SparseMatrix<double> &hessianMatrix)
 58 | {
 59 |     return false;
 60 | }
 61 | 
 62 | bool NullSolver::updateInequalityConstraintsMatrix(const Eigen::SparseMatrix<double> &linearConstraintsMatrix)
 63 | {
 64 |     return false;
 65 | }
 66 | 
 67 | bool NullSolver::updateGradient(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& gradient)
 68 | {
 69 |     return false;
 70 | }
 71 | 
 72 | bool NullSolver::updateLowerBound(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& lowerBound)
 73 | {
 74 |     return false;
 75 | }
 76 | 
 77 | bool NullSolver::updateUpperBound(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& upperBound)
 78 | {
 79 |     return false;
 80 | }
 81 | 
 82 | bool NullSolver::updateBounds(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& lowerBound,
 83 |              const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& upperBound)
 84 | {
 85 |     return false;
 86 | }
 87 | 
 88 | bool NullSolver::updateEqualityConstraintsMatrix(const Eigen::SparseMatrix<double>& equalityConstraintsMatrix)
 89 | {
 90 |     return false;
 91 | }
 92 | 
 93 | bool NullSolver::updateEqualityConstraintsVector(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& equalityConstraintsVector)
 94 | {
 95 |     return false;
 96 | }
 97 | 
 98 | void NullSolver::clearHessianMatrix()
 99 | {
100 |     return;
101 | }
102 | 
103 | void NullSolver::clearLinearConstraintsMatrix()
104 | {
105 |     return;
106 | }
107 | 
108 | void NullSolver::setNumberOfVariables(int n)
109 | {
110 |     return;
111 | }
112 | 
113 | void NullSolver::setNumberOfInequalityConstraints(int m)
114 | {
115 |     return;
116 | }
117 | 
118 | void NullSolver::setNumberOfEqualityConstraints(int m)
119 | {
120 |     return;
121 | }
122 | 
123 | bool NullSolver::setHessianMatrix(const Eigen::SparseMatrix<double>& hessianMatrix)
124 | {
125 |     return false;
126 | }
127 | 
128 | bool NullSolver::setGradient(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> gradientVector)
129 | {
130 |     return false;
131 | }
132 | 
133 | Eigen::Matrix<double, Eigen::Dynamic, 1> NullSolver::getGradient()
134 | {
135 |     return Eigen::Matrix<double, Eigen::Dynamic, 1>();
136 | }
137 | 
138 | bool
139 | NullSolver::setInequalityConstraintsMatrix(const Eigen::SparseMatrix<double>& linearConstraintsMatrix)
140 | {
141 |     return false;
142 | }
143 | 
144 | bool NullSolver::setLowerBound(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> lowerBoundVector)
145 | {
146 |     return false;
147 | }
148 | 
149 | bool NullSolver::setUpperBound(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> upperBoundVector)
150 | {
151 |     return false;
152 | }
153 | 
154 | bool NullSolver::setBounds(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> lowerBound,
155 |                Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> upperBound)
156 | {
157 |     return false;
158 | }
159 | 
160 | bool NullSolver::setEqualityConstraintsMatrix(const Eigen::SparseMatrix<double>& equalityConstraintsMatrix)
161 | {
162 |     return false;
163 | }
164 | 
165 | bool NullSolver::setEqualityConstraintsVector(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& equalityConstraintsVector)
166 | {
167 |     return false;
168 | }
169 | 
170 | bool NullSolver::setBooleanParameter(const std::string& settingName, bool value)
171 | {
172 |     return false;
173 | }
174 | 
175 | bool NullSolver::setIntegerParameter(const std::string& settingName, int64_t value)
176 | {
177 |     return false;
178 | }
179 | 
180 | bool NullSolver::setRealNumberParameter(const std::string& settingName, double value)
181 | {
182 |     return false;
183 | }
184 | 
185 | bool NullSolver::setStringParameter(const std::string& parameterName, const std::string& value)
186 | {
187 |     return false;
188 | }
189 | 
190 | bool NullSolver::getBooleanParametersNames(std::vector<std::string>& parametersNames) const
191 | {
192 |     return false;
193 | }
194 | 
195 | bool NullSolver::getIntegerParametersNames(std::vector<std::string>& parametersNames) const
196 | {
197 |     return false;
198 | }
199 | 
200 | bool NullSolver::getRealNumberParametersNames(std::vector<std::string>& parametersNames) const
201 | {
202 |     return false;
203 | }
204 | 
205 | bool NullSolver::getStringParametersNames(std::vector<std::string>& parametersNames) const
206 | {
207 |     return false;
208 | }
209 | 
210 | SolverInterface* NullSolver::allocateInstance() const
211 | {
212 |     return new NullSolver();
213 | }
214 | 
215 | }
216 | 


--------------------------------------------------------------------------------
/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 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
  1 | # qpsolvers-eigen
  2 | 
  3 | Simple C++ abstraction layer for quadratic programming solvers using [Eigen](http://eigen.tuxfamily.org/index.php?title=Main_Page).
  4 | 
  5 | ## 🛠️ Usage
  6 | 
  7 | Please install the library following one (and just one) method listed below.
  8 | 
  9 | #### 📦 Install with conda or pixi (recommended)
 10 | 
 11 | You can easily the library with [`conda`](https://github.com/conda-forge/qpsolvers-eigen-feedstock) in a new conda environment with
 12 | ```
 13 | conda create -n newenvname -c conda-forge qpsolvers-eigen
 14 | ```
 15 | `conda` will automatically install all the supported dependencies.
 16 | 
 17 | To add qpsolvers-eigen to a `pixi` project, just run:
 18 | 
 19 | ```
 20 | pixi add qpsolvers-eigen
 21 | ```
 22 | 
 23 | #### ⚙️ Install via build from source for internal development (advanced)
 24 | 
 25 | If you just want to modify `qpsolvers-eigen` and run the tests again your modification,
 26 | the easiest way to do that is to use [`pixi`](https://pixi.sh/latest/), in particular you just need to run:
 27 | 
 28 | ~~~
 29 | git clone https://github.com/ami-iit/qpsolvers-eigen.git
 30 | cd qpsolvers-eigen
 31 | pixi run test
 32 | ~~~
 33 | 
 34 | #### ⚙️ Build from source (advanced)
 35 | 
 36 | If you want to use a package manager that does not provide `qpsolvers-eigen` packages, youc can do that
 37 | as qpsolvers-eigen is a fairly standard CMake project. To do that, first of all install either via a package
 38 | manager or manually the following depencies:
 39 | 
 40 | Required dependencies:
 41 | * [eigen](https://eigen.tuxfamily.org/index.php?title=Main_Page)
 42 | * [cmake](https://cmake.org/)
 43 | * C and C++ compiler
 44 | 
 45 | Optional dependencies:
 46 | * [sharedlibpp](https://github.com/ami-iit/sharedlibpp) (if not found an internal copy is used and installed)
 47 | * [osqp-eigen](https://github.com/robotology/osqp-eigen) (if not found the `osqp` plugin is not compiled)
 48 | * [proxsuite](https://github.com/Simple-Robotics/proxsuite) (if not found the `proxqp` plugin is not compiled)
 49 | 
 50 | Test dependencies:
 51 | * [catch2](https://github.com/catchorg/Catch2)
 52 | 
 53 | Then follow the instructions
 54 | 
 55 | 1. Clone the repository
 56 |    ```
 57 |    git clone https://github.com/ami-iit/qpsolvers-eigen.git
 58 |    ```
 59 | 2. Build it
 60 |    ```
 61 |    cd osqp-eigen
 62 |    mkdir build
 63 |    cd build
 64 |    cmake -GNinja -DCMAKE_INSTALL_PREFIX:PATH=<custom-folder> ../
 65 |    ninja
 66 |    ninja install
 67 |    ```
 68 | 3. Add the following environmental variable to ensure that `find_package(QpSolversEigen REQUIRED)` is successful:
 69 |    ```
 70 |    QpSolversEigen_DIR=/path/where/you/installed/
 71 |    ```
 72 | 
 73 | ## 🖥️ How to use the library
 74 | 
 75 | **qpsolvers-eigen** provides native `CMake` support which allows the library to be easily used in `CMake` projects.
 76 | 
 77 | **qpsolvers-eigen** exports a CMake target called `QpSolversEigen::QpSolversEigen` which can be imported using the `find_package` CMake command and used by calling `target_link_libraries` as in the following example:
 78 | 
 79 | ```cmake
 80 | cmake_minimum_required(VERSION 3.16)
 81 | project(myproject)
 82 | find_package(QpSolversEigen REQUIRED)
 83 | add_executable(example example.cpp)
 84 | target_link_libraries(example QpSolversEigen::QpSolversEigen)
 85 | ```
 86 | 
 87 | A minimal `example.cpp` is:
 88 | 
 89 | ~~~cxx
 90 | #include <cstdlib>
 91 | #include <iostream>
 92 | 
 93 | #include <QpSolversEigen/QpSolversEigen.hpp>
 94 | #include <Eigen/Dense>
 95 | #include <Eigen/Sparse>
 96 | 
 97 | 
 98 | int main()
 99 | {
100 |     Eigen::SparseMatrix<double> H_s(2, 2);
101 |     H_s.insert(0, 0) = 4;
102 |     H_s.insert(0, 1) = 1;
103 |     H_s.insert(1, 0) = 1;
104 |     H_s.insert(1, 1) = 2;
105 | 
106 |     Eigen::SparseMatrix<double> A_s(3, 2);
107 |     A_s.insert(0, 0) = 1;
108 |     A_s.insert(0, 1) = 1;
109 |     A_s.insert(1, 0) = 1;
110 |     A_s.insert(2, 1) = 1;
111 | 
112 |     Eigen::Matrix<double, 2, 1> gradient;
113 |     gradient << 1, 1;
114 | 
115 |     Eigen::Matrix<double, 3, 1> lowerBound;
116 |     lowerBound << 1, 0, 0;
117 | 
118 |     Eigen::Matrix<double, 3, 1> upperBound;
119 |     upperBound << 1, 0.7, 0.7;
120 | 
121 |     QpSolversEigen::Solver solver;
122 | 
123 |     // Here you select the solver, possible options are:
124 |     // * osqp
125 |     // * proxqp
126 |     bool ok = solver.instantiateSolver("osqp");
127 | 
128 |     if (!ok)
129 |     {
130 |         std::cerr << "Error in instantiating the solver" << std::endl;
131 |         return EXIT_FAILURE;
132 |     }
133 | 
134 |     // Set osqp-specific parameters
135 |     if (solver.getSolverName() == "osqp")
136 |     {
137 |         solver.setBooleanParameter("verbose", true);
138 |         solver.setRealNumberParameter("alpha", 1.0);
139 |         // See https://github.com/robotology/osqp-eigen/pull/172
140 |         solver.setBooleanParameter("polish", true);
141 |     }
142 | 
143 |     solver.data()->setNumberOfVariables(2);
144 |     solver.data()->setNumberOfInequalityConstraints(3);
145 |     ok = ok && solver.data()->setHessianMatrix(H_s);
146 |     ok = ok && solver.data()->setGradient(gradient);
147 |     ok = ok && solver.data()->setInequalityConstraintsMatrix(A_s);
148 |     ok = ok && solver.data()->setLowerBound(lowerBound);
149 |     ok = ok && solver.data()->setUpperBound(upperBound);
150 |     ok = ok && solver.initSolver();
151 | 
152 |     if (!ok)
153 |     {
154 |         std::cerr << "Error in solver initialization" << std::endl;
155 |         return EXIT_FAILURE;
156 |     }
157 | 
158 |     ok = ok && (solver.solveProblem() == QpSolversEigen::ErrorExitFlag::NoError);
159 | 
160 |     if (!ok)
161 |     {
162 |         std::cerr << "Error in solving the problem" << std::endl;
163 |         return EXIT_FAILURE;
164 |     }
165 | 
166 |     std::cerr << "Solution: " << solver.getSolution() << std::endl;
167 | }
168 | ~~~
169 | 
170 | For more examples, check the content of the [./examples](./examples) folder in this repo.
171 | 
172 | 
173 | ### Migrate from osqp-eigen
174 | 
175 | If you are already using `osqp-eigen` and you want to understand how to migrate your code to `qpsolvers-eigen`, check the [`./docs/migrate_from_osqp_eigen.md`](./docs/migrate_from_osqp_eigen.md) document.
176 | 
177 | ### Related projects
178 | 
179 | If you are interested to other projects that provide abstraction over QP solvers, you can check also this other projects:
180 | 
181 | * [Python's `qpsolvers`](https://qpsolvers.github.io/qpsolvers/) : Python abstraction layer over QPs, quite complete and definitely an inspiration for `qpsolvers-eigen`.
182 | * [`isri-aist/QpSolverCollection`](https://github.com/isri-aist/QpSolverCollection) : Another C++ QP standalone abstraction layer, that supports more solvers w.r.t. to `qpsolvers-eigen`, but does not permit to easily set parameters to the underlying solvers.
183 | * [`RobotLocomotion/drake`](https://drake.mit.edu/doxygen_cxx/group__solvers.html) : Drake is a collection of tools for analyzing the dynamics of our robots and building control systems for them, with a heavy emphasis on optimization-based design/analysis. As part of its extensive capabilities, it also provide abstraction over QP solvers. However, it is quite an heavyweight dependency, and does not support Windows.
184 | * [`casadi`](https://web.casadi.org/docs/#quadratic-programming) CasADi is an open-source tool for nonlinear optimization and algorithmic differentiation. As part of its extensive capabilities, it also provide abstraction over QP solvers.
185 | * [`roboptim`](http://roboptim.net/) RobOptim is a C++ Library for Numerical Optimization applied to Robotics. Similarly to `casadi` or `qpsolvers-eigen`, it permits to write solvers as dynamically loadable plugins that can be loaded without modifying the core library. Mantainance of the library seems to be stopped around 2019.
186 | 
187 | ## 🐛 Bug reports and support
188 | 
189 | All types of [issues](https://github.com/ami-iit/qpsolvers-eigen/issues/new) are welcome.
190 | 
191 | ## Versioning policy
192 | 
193 | Any ABI or API incompatible change will result in a minor release bump.
194 | 
195 | ## 📝 License
196 | 
197 | Materials in this repository are distributed under the following license:
198 | 
199 | > All software is licensed under the BSD 3-Clause License. See [LICENSE](https://github.com/robotology/osqp-eigen/blob/master/LICENSE) file for details.
200 | 


--------------------------------------------------------------------------------
/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 | // Include macros common to all test
  9 | #include "QpSolversEigenCommonTestMacros.hpp"
 10 | 
 11 | // Catch2
 12 | #include <catch2/catch_test_macros.hpp>
 13 | #include <catch2/generators/catch_generators_all.hpp>
 14 | 
 15 | // QpSolversEigen
 16 | #include <QpSolversEigen/QpSolversEigen.hpp>
 17 | 
 18 | // eigen
 19 | #include <Eigen/Dense>
 20 | 
 21 | #include <cmath>
 22 | #include <fstream>
 23 | #include <iostream>
 24 | 
 25 | // colors
 26 | #define ANSI_TXT_GRN "\033[0;32m"
 27 | #define ANSI_TXT_MGT "\033[0;35m" // Magenta
 28 | #define ANSI_TXT_DFT "\033[0;0m" // Console default
 29 | #define GTEST_BOX "[     cout ] "
 30 | #define COUT_GTEST ANSI_TXT_GRN << GTEST_BOX // You could add the Default
 31 | #define COUT_GTEST_MGT COUT_GTEST << ANSI_TXT_MGT
 32 | 
 33 | #define T 0.1
 34 | 
 35 | void setDynamicsMatrices(Eigen::Matrix<double, 2, 2>& a,
 36 |                          Eigen::Matrix<double, 2, 1>& b,
 37 |                          Eigen::Matrix<double, 1, 2>& c,
 38 |                          double t)
 39 | {
 40 | 
 41 |     double omega = 0.1132;
 42 |     double alpha = 0.5 * sin(2 * M_PI * omega * t);
 43 |     double beta = 2 - 1 * sin(2 * M_PI * omega * t);
 44 | 
 45 |     a << alpha, 1, 0, alpha;
 46 | 
 47 |     b << 0, 1;
 48 | 
 49 |     c << beta, 0;
 50 | }
 51 | 
 52 | void setWeightMatrices(Eigen::DiagonalMatrix<double, 1>& Q, Eigen::DiagonalMatrix<double, 1>& R)
 53 | {
 54 |     Q.diagonal() << 10;
 55 |     R.diagonal() << 1;
 56 | }
 57 | 
 58 | void castMPCToQPHessian(const Eigen::DiagonalMatrix<double, 1>& Q,
 59 |                         const Eigen::DiagonalMatrix<double, 1>& R,
 60 |                         int mpcWindow,
 61 |                         int k,
 62 |                         Eigen::SparseMatrix<double>& hessianMatrix)
 63 | {
 64 | 
 65 |     Eigen::Matrix<double, 2, 2> a;
 66 |     Eigen::Matrix<double, 2, 1> b;
 67 |     Eigen::Matrix<double, 1, 2> c;
 68 | 
 69 |     hessianMatrix.resize(2 * (mpcWindow + 1) + 1 * mpcWindow, 2 * (mpcWindow + 1) + 1 * mpcWindow);
 70 | 
 71 |     // populate hessian matrix
 72 |     for (int i = 0; i < 2 * (mpcWindow + 1) + 1 * mpcWindow; i++)
 73 |     {
 74 |         double t = (k + i) * T;
 75 |         setDynamicsMatrices(a, b, c, t);
 76 |         if (i < 2 * (mpcWindow + 1))
 77 |         {
 78 |             // here the structure of the matrix c is used!
 79 |             int pos = i % 2;
 80 |             float value = c(pos) * Q.diagonal()[0] * c(pos);
 81 |             if (value != 0)
 82 |                 hessianMatrix.insert(i, i) = value;
 83 |         } else
 84 |         {
 85 |             float value = R.diagonal()[0];
 86 |             if (value != 0)
 87 |                 hessianMatrix.insert(i, i) = value;
 88 |         }
 89 |     }
 90 | }
 91 | 
 92 | void castMPCToQPGradient(const Eigen::DiagonalMatrix<double, 1>& Q,
 93 |                          const Eigen::Matrix<double, 1, 1>& yRef,
 94 |                          int mpcWindow,
 95 |                          int k,
 96 |                          Eigen::Matrix<double, -1, 1>& gradient)
 97 | {
 98 | 
 99 |     Eigen::Matrix<double, 2, 2> a;
100 |     Eigen::Matrix<double, 2, 1> b;
101 |     Eigen::Matrix<double, 1, 2> c;
102 | 
103 |     Eigen::Matrix<double, 1, 1> Qy_ref;
104 |     Qy_ref = Q * (-yRef);
105 | 
106 |     // populate the gradient vector
107 |     gradient = Eigen::Matrix<double, -1, 1>::Zero(2 * (mpcWindow + 1) + 1 * mpcWindow, 1);
108 |     for (int i = 0; i < 2 * (mpcWindow + 1); i++)
109 |     {
110 |         double t = (k + i) * T;
111 |         setDynamicsMatrices(a, b, c, t);
112 | 
113 |         int pos = i % 2;
114 |         float value = Qy_ref(0, 0) * c(pos);
115 |         gradient(i, 0) = value;
116 |     }
117 | }
118 | 
119 | void castMPCToQPConstraintMatrix(int mpcWindow,
120 |                                  int k,
121 |                                  Eigen::SparseMatrix<double>& constraintMatrix)
122 | {
123 |     constraintMatrix.resize(2 * (mpcWindow + 1), 2 * (mpcWindow + 1) + 1 * mpcWindow);
124 | 
125 |     // populate linear constraint matrix
126 |     for (int i = 0; i < 2 * (mpcWindow + 1); i++)
127 |     {
128 |         constraintMatrix.insert(i, i) = -1;
129 |     }
130 | 
131 |     Eigen::Matrix<double, 2, 2> a;
132 |     Eigen::Matrix<double, 2, 1> b;
133 |     Eigen::Matrix<double, 1, 2> c;
134 | 
135 |     for (int i = 0; i < mpcWindow; i++)
136 |     {
137 |         double t = (k + i) * T;
138 |         setDynamicsMatrices(a, b, c, t);
139 |         for (int j = 0; j < 2; j++)
140 |             for (int k = 0; k < 2; k++)
141 |             {
142 |                 float value = a(j, k);
143 |                 if (value != 0)
144 |                 {
145 |                     constraintMatrix.insert(2 * (i + 1) + j, 2 * i + k) = value;
146 |                 }
147 |             }
148 |     }
149 | 
150 |     for (int i = 0; i < mpcWindow; i++)
151 |         for (int j = 0; j < 2; j++)
152 |             for (int k = 0; k < 1; k++)
153 |             {
154 |                 // b is constant
155 |                 float value = b(j, k);
156 |                 if (value != 0)
157 |                 {
158 |                     constraintMatrix.insert(2 * (i + 1) + j, 1 * i + k + 2 * (mpcWindow + 1))
159 |                         = value;
160 |                 }
161 |             }
162 | }
163 | 
164 | void castMPCToQPConstraintVectors(const Eigen::Matrix<double, 2, 1>& x0,
165 |                                   int mpcWindow,
166 |                                   Eigen::Matrix<double, -1, 1>& lowerBound,
167 |                                   Eigen::Matrix<double, -1, 1>& upperBound)
168 | {
169 |     // evaluate the lower and the upper equality vectors
170 |     lowerBound = Eigen::Matrix<double, -1, -1>::Zero(2 * (mpcWindow + 1), 1);
171 |     lowerBound.block(0, 0, 2, 1) = -x0;
172 |     upperBound = lowerBound;
173 | }
174 | 
175 | bool updateHessianMatrix(QpSolversEigen::Solver& solver,
176 |                          const Eigen::DiagonalMatrix<double, 1>& Q,
177 |                          const Eigen::DiagonalMatrix<double, 1>& R,
178 |                          int mpcWindow,
179 |                          int k)
180 | {
181 |     Eigen::SparseMatrix<double> hessianMatrix;
182 |     castMPCToQPHessian(Q, R, mpcWindow, k, hessianMatrix);
183 | 
184 |     if (!solver.updateHessianMatrix(hessianMatrix))
185 |         return false;
186 | 
187 |     return true;
188 | }
189 | 
190 | bool updateLinearConstraintsMatrix(QpSolversEigen::Solver& solver, int mpcWindow, int k)
191 | {
192 |     Eigen::SparseMatrix<double> constraintMatrix;
193 |     castMPCToQPConstraintMatrix(mpcWindow, k, constraintMatrix);
194 | 
195 |     if (!solver.updateInequalityConstraintsMatrix(constraintMatrix))
196 |         return false;
197 | 
198 |     return true;
199 | }
200 | 
201 | void updateConstraintVectors(const Eigen::Matrix<double, 2, 1>& x0,
202 |                              Eigen::Matrix<double, -1, 1>& lowerBound,
203 |                              Eigen::Matrix<double, -1, 1>& upperBound)
204 | {
205 |     lowerBound.block(0, 0, 2, 1) = -x0;
206 |     upperBound.block(0, 0, 2, 1) = -x0;
207 | }
208 | 
209 | TEST_CASE("MPCTest Update matrices")
210 | {
211 |     // open the ofstream
212 |     std::ofstream dataStream;
213 |     dataStream.open("output.txt");
214 | 
215 |     // set the preview window
216 |     int mpcWindow = 100;
217 | 
218 |     // allocate the dynamics matrices
219 |     Eigen::Matrix<double, 2, 2> a;
220 |     Eigen::Matrix<double, 2, 1> b;
221 |     Eigen::Matrix<double, 1, 2> c;
222 | 
223 |     // allocate the weight matrices
224 |     Eigen::DiagonalMatrix<double, 1> Q;
225 |     Eigen::DiagonalMatrix<double, 1> R;
226 | 
227 |     // allocate the initial and the reference state space
228 |     Eigen::Matrix<double, 2, 1> x0;
229 |     Eigen::Matrix<double, 1, 1> yRef;
230 |     Eigen::Matrix<double, 1, 1> y;
231 | 
232 |     // allocate QP problem matrices and vectors
233 |     Eigen::SparseMatrix<double> hessian;
234 |     Eigen::Matrix<double, -1, 1> gradient;
235 |     Eigen::SparseMatrix<double> linearMatrix;
236 |     Eigen::Matrix<double, -1, 1> lowerBound;
237 |     Eigen::Matrix<double, -1, 1> upperBound;
238 | 
239 |     // set the initial and the desired states
240 |     x0 << 0, 0;
241 |     yRef << 1;
242 | 
243 |     // set MPC problem quantities
244 |     setWeightMatrices(Q, R);
245 | 
246 |     // cast the MPC problem as QP problem
247 |     castMPCToQPHessian(Q, R, mpcWindow, 0, hessian);
248 |     castMPCToQPGradient(Q, yRef, mpcWindow, 0, gradient);
249 |     castMPCToQPConstraintMatrix(mpcWindow, 0, linearMatrix);
250 |     castMPCToQPConstraintVectors(x0, mpcWindow, lowerBound, upperBound);
251 | 
252 |     // instantiate the solver
253 |     std::string solverName = QPSOLVERSEIGEN_SOLVERS_TO_TEST;
254 |     QpSolversEigen::Solver solver;
255 |     REQUIRE(solver.instantiateSolver(solverName));
256 |     std::cout << COUT_GTEST_MGT << "Testing solver " << solver.getSolverName()
257 |               << ANSI_TXT_DFT << std::endl;
258 |     // settings
259 |     REQUIRE(solver.setBooleanParameter("verbose", false));
260 | 
261 |     if (solver.getSolverName() == "osqp")
262 |     {
263 |         REQUIRE(solver.setBooleanParameter("warm_start", true));
264 |     }
265 | 
266 |     if (solver.getSolverName() == "proxqp")
267 |     {
268 |         REQUIRE(solver.setStringParameter("initial_guess", "WARM_START_WITH_PREVIOUS_RESULT"));
269 |         // Check that setStringParameter fail for unknown setting or unknown value
270 |         REQUIRE_FALSE(solver.setStringParameter("initial_guess", "THIS_IS_NOT_A_VALID_INITIAL_GUESS_VALUE"));
271 |         REQUIRE_FALSE(solver.setStringParameter("this_is_not_a_valid_proqp_parameter_name", "THIS_IS_NOT_A_VALID_INITIAL_GUESS_VALUE"));
272 |     }
273 | 
274 |     // set the initial data of the QP solver
275 |     solver.data()->setNumberOfVariables(2 * (mpcWindow + 1) + 1 * mpcWindow);
276 |     solver.data()->setNumberOfInequalityConstraints(2 * (mpcWindow + 1));
277 |     REQUIRE(solver.data()->setHessianMatrix(hessian));
278 |     REQUIRE(solver.data()->setGradient(gradient));
279 |     REQUIRE(solver.data()->setInequalityConstraintsMatrix(linearMatrix));
280 |     REQUIRE(solver.data()->setLowerBound(lowerBound));
281 |     REQUIRE(solver.data()->setUpperBound(upperBound));
282 | 
283 |     // instantiate the solver
284 |     REQUIRE(solver.initSolver());
285 | 
286 |     // controller input and QPSolution vector
287 |     Eigen::Matrix<double, -1, 1> ctr;
288 |     Eigen::Matrix<double, -1, 1> QPSolution;
289 | 
290 |     // number of iteration steps
291 |     int numberOfSteps = 50;
292 | 
293 |     // profiling quantities
294 |     clock_t startTime, endTime;
295 |     double averageTime = 0;
296 | 
297 |     for (int i = 0; i < numberOfSteps; i++)
298 |     {
299 |         startTime = clock();
300 | 
301 |         setDynamicsMatrices(a, b, c, i * T);
302 | 
303 |         // update the constraint bound
304 |         REQUIRE(updateHessianMatrix(solver, Q, R, mpcWindow, i));
305 |         REQUIRE(updateLinearConstraintsMatrix(solver, mpcWindow, i));
306 | 
307 |         castMPCToQPGradient(Q, yRef, mpcWindow, i, gradient);
308 |         REQUIRE(solver.updateGradient(gradient));
309 | 
310 |         updateConstraintVectors(x0, lowerBound, upperBound);
311 |         REQUIRE(solver.updateBounds(lowerBound, upperBound));
312 | 
313 |         // solve the QP problem
314 |         REQUIRE(solver.solveProblem() == QpSolversEigen::ErrorExitFlag::NoError);
315 | 
316 |         // get the controller input
317 |         QPSolution = solver.getSolution();
318 |         ctr = QPSolution.block(2 * (mpcWindow + 1), 0, 1, 1);
319 | 
320 |         // save data into file
321 |         auto x0Data = x0.data();
322 |         for (int j = 0; j < 2; j++)
323 |             dataStream << x0Data[j] << " ";
324 |         dataStream << std::endl;
325 | 
326 |         // propagate the model
327 |         x0 = a * x0 + b * ctr;
328 |         y = c * x0;
329 | 
330 |         endTime = clock();
331 | 
332 |         averageTime += static_cast<double>(endTime - startTime) / CLOCKS_PER_SEC;
333 |     }
334 | 
335 |     // close the stream
336 |     dataStream.close();
337 | 
338 |     std::cout << COUT_GTEST_MGT << "Average time = " << averageTime / numberOfSteps << " seconds."
339 |               << ANSI_TXT_DFT << std::endl;
340 | }
341 | 


--------------------------------------------------------------------------------
/core/QpSolversEigen/Solver.cpp:
--------------------------------------------------------------------------------
  1 | #include <QpSolversEigen/Solver.hpp>
  2 | #include <QpSolversEigen/SolverInterface.hpp>
  3 | #include <QpSolversEigen/NullSolver.hpp>
  4 | #include <QpSolversEigen/Debug.hpp>
  5 | 
  6 | #include <sharedlibpp/SharedLibraryClassFactory.h>
  7 | #include <sharedlibpp/SharedLibraryClass.h>
  8 | 
  9 | #include <optional>
 10 | #include <filesystem>
 11 | 
 12 | #ifdef _WIN32
 13 | #include <windows.h>
 14 | #else
 15 | #include <dlfcn.h>
 16 | #endif
 17 | 
 18 | namespace QpSolversEigen
 19 | {
 20 | 
 21 | // Inspired by https://github.com/ami-iit/reloc-cpp, but a bit different (and simpler)
 22 | // as in this case we just need the location of the qpsolvers-eigen shared library
 23 | std::optional<std::string> getPathOfQpSolversEigenSharedLibrary()
 24 | {
 25 |     std::error_code fs_error;
 26 | 
 27 |     // Get location of the library
 28 |     std::filesystem::path library_location;
 29 | #ifndef _WIN32
 30 |     Dl_info address_info;
 31 |     int res_val = dladdr(reinterpret_cast<void *>(QpSolversEigen::getPathOfQpSolversEigenSharedLibrary), &address_info);
 32 |     if (address_info.dli_fname && res_val > 0)
 33 |     {
 34 |       library_location = address_info.dli_fname;
 35 |     }
 36 |     else
 37 |     {
 38 |       return {};
 39 |     }
 40 | #else
 41 |     // See
 42 |     char module_path[MAX_PATH];
 43 |     HMODULE hm = NULL;
 44 | 
 45 |     if (GetModuleHandleEx(GET_MODULE_HANDLE_EX_FLAG_FROM_ADDRESS |
 46 |         GET_MODULE_HANDLE_EX_FLAG_UNCHANGED_REFCOUNT,
 47 |         (LPCSTR) &QpSolversEigen::getPathOfQpSolversEigenSharedLibrary, &hm) == 0)
 48 |     {
 49 |         return {};
 50 |     }
 51 |     if (GetModuleFileNameA(hm, module_path, sizeof(module_path)) == 0)
 52 |     {
 53 |         return {};
 54 |     }
 55 | 
 56 |     library_location = std::string(module_path);
 57 | #endif
 58 | 
 59 |     const std::filesystem::path library_directory = library_location.parent_path();
 60 |     return library_directory.string();
 61 | }
 62 | 
 63 | struct Solver::Impl
 64 | {
 65 |     bool solverInitialized;
 66 |     std::unique_ptr<QpSolversEigen::SolverInterface> solver;
 67 |     std::unique_ptr<sharedlibpp::SharedLibraryClassFactory<QpSolversEigen::SolverInterface>> solverFactory;
 68 |     Impl();
 69 | };
 70 | 
 71 | Solver::Impl::Impl()
 72 |     : solverInitialized{false},
 73 |       solver{std::make_unique<QpSolversEigen::NullSolver>()}
 74 | {
 75 | }
 76 | 
 77 | Solver::Solver()
 78 |     : m_pimpl{std::make_unique<Impl>()}
 79 | {
 80 | }
 81 | 
 82 | Solver::~Solver()
 83 | {
 84 |     // Ensure that the solver is deallocated before the factory
 85 |     m_pimpl->solver.reset();
 86 |     m_pimpl->solverFactory.reset();
 87 | }
 88 | 
 89 | bool Solver::instantiateSolver(std::string solverName)
 90 | {
 91 |     // Ensure that the solver is deallocated before the factory
 92 |     m_pimpl->solver.reset();
 93 | 
 94 |     // Get the expected library name and factory name given the solver name
 95 |     std::string libraryName = getSharedlibppLibraryNameFromSolverName(solverName);
 96 |     std::string factoryName = getSharedlibppFactoryNameFromSolverName(solverName);
 97 |     m_pimpl->solverFactory = std::make_unique<sharedlibpp::SharedLibraryClassFactory<QpSolversEigen::SolverInterface>>(SHLIBPP_DEFAULT_START_CHECK,
 98 |                                                                                                                        SHLIBPP_DEFAULT_END_CHECK,
 99 |                                                                                                                        SHLIBPP_DEFAULT_SYSTEM_VERSION,
100 |                                                                                                                        factoryName.c_str());
101 | 
102 |     // Extend the search path of the plugins to include the install prefix of the library, this make sure that
103 |     // if the plugins and the core library are installed in the same directory, they are found out of the box
104 |     // if there are plugins in any other directory, the directory needs to be added to the SHLIBPP_PLUGIN_PATH
105 |     // env variable to be found
106 |     std::optional<std::string> pathOfQpSolversEigenSharedLib = getPathOfQpSolversEigenSharedLibrary();
107 |     if (pathOfQpSolversEigenSharedLib.has_value())
108 |     {
109 |         m_pimpl->solverFactory->extendSearchPath(pathOfQpSolversEigenSharedLib.value());
110 |     }
111 | 
112 |     bool ok = m_pimpl->solverFactory->open(libraryName.c_str(), factoryName.c_str());
113 |     ok = ok && m_pimpl->solverFactory->isValid();
114 | 
115 |     if (!ok)
116 |     {
117 |         debugStream() << "QpSolversEigen::Solver::initSolver: Failed to create factory for solver " << solverName << std::endl;
118 |         debugStream() << "Factory error (" << static_cast<std::uint32_t>(m_pimpl->solverFactory->getStatus())
119 |                     << "): " << m_pimpl->solverFactory->getError().c_str() << std::endl;
120 |         debugStream() << "Make sure that the library name contains " << libraryName << " and the factory name is called " << factoryName << std::endl;
121 |         return false;
122 |     }
123 | 
124 |     sharedlibpp::SharedLibraryClass<QpSolversEigen::SolverInterface> solver(*(m_pimpl->solverFactory));
125 |     m_pimpl->solver.reset(solver.getContent().allocateInstance());
126 |     return true;
127 | }
128 | 
129 | std::string Solver::getSolverName() const
130 | {
131 |     return m_pimpl->solver->getSolverName();
132 | }
133 | 
134 | bool Solver::initSolver()
135 | {
136 |     return m_pimpl->solver->initSolver();
137 | }
138 | 
139 | 
140 | bool Solver::isInitialized()
141 | {
142 |     return m_pimpl->solver->isInitialized();
143 | }
144 | 
145 | void Solver::clearSolver()
146 | {
147 |     m_pimpl->solver->clearSolver();
148 | }
149 | 
150 | bool Solver::clearSolverVariables()
151 | {
152 |     return m_pimpl->solver->clearSolverVariables();
153 | }
154 | 
155 | QpSolversEigen::ErrorExitFlag Solver::solveProblem()
156 | {
157 |     return m_pimpl->solver->solveProblem();
158 | }
159 | 
160 | QpSolversEigen::Status Solver::getStatus() const
161 | {
162 |     return m_pimpl->solver->getStatus();
163 | }
164 | 
165 | const double Solver::getObjValue() const
166 | {
167 |     return m_pimpl->solver->getObjValue();
168 | }
169 | 
170 | const Eigen::Matrix<double, -1, 1>& Solver::getSolution()
171 | {
172 |     return m_pimpl->solver->getSolution();
173 | }
174 | 
175 | const Eigen::Matrix<double, -1, 1>& Solver::getDualSolution()
176 | {
177 |     return m_pimpl->solver->getDualSolution();
178 | }
179 | 
180 | bool Solver::updateHessianMatrix(const Eigen::SparseMatrix<double> &hessianMatrix)
181 | {
182 |     return m_pimpl->solver->updateHessianMatrix(hessianMatrix);
183 | }
184 | 
185 | bool Solver::updateLinearConstraintsMatrix(const Eigen::SparseMatrix<double> &linearConstraintsMatrix)
186 | {
187 |     return m_pimpl->solver->updateInequalityConstraintsMatrix(linearConstraintsMatrix);
188 | }
189 | 
190 | bool Solver::updateInequalityConstraintsMatrix(const Eigen::SparseMatrix<double> &linearConstraintsMatrix)
191 | {
192 |     return m_pimpl->solver->updateInequalityConstraintsMatrix(linearConstraintsMatrix);
193 | }
194 | 
195 | bool
196 | Solver::updateGradient(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& gradient)
197 | {
198 |     return m_pimpl->solver->updateGradient(gradient);
199 | }
200 | 
201 | bool
202 | Solver::updateLowerBound(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& lowerBound)
203 | {
204 |     return m_pimpl->solver->updateLowerBound(lowerBound);
205 | }
206 | 
207 | bool
208 | Solver::updateUpperBound(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& upperBound)
209 | {
210 |     return m_pimpl->solver->updateUpperBound(upperBound);
211 | }
212 | 
213 | bool
214 | Solver::updateBounds(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& lowerBound,
215 |              const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& upperBound)
216 | {
217 |     return m_pimpl->solver->updateBounds(lowerBound, upperBound);
218 | }
219 | 
220 | bool Solver::updateEqualityConstraintsMatrix(const Eigen::SparseMatrix<double>& equalityConstraintsMatrix)
221 | {
222 |     return m_pimpl->solver->updateEqualityConstraintsMatrix(equalityConstraintsMatrix);
223 | }
224 | 
225 | bool Solver::updateEqualityConstraintsVector(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& equalityConstraintsVector)
226 | {
227 |     return m_pimpl->solver->updateEqualityConstraintsVector(equalityConstraintsVector);
228 | }
229 | 
230 | void Solver::clearHessianMatrix()
231 | {
232 |     return m_pimpl->solver->clearHessianMatrix();
233 | }
234 | 
235 | void Solver::clearLinearConstraintsMatrix()
236 | {
237 |     return m_pimpl->solver->clearLinearConstraintsMatrix();
238 | }
239 | 
240 | void Solver::setNumberOfVariables(int n)
241 | {
242 |     return m_pimpl->solver->setNumberOfVariables(n);
243 | }
244 | 
245 | void Solver::setNumberOfConstraints(int m)
246 | {
247 |     return m_pimpl->solver->setNumberOfInequalityConstraints(m);
248 | }
249 | 
250 | void Solver::setNumberOfInequalityConstraints(int m)
251 | {
252 |     return m_pimpl->solver->setNumberOfInequalityConstraints(m);
253 | }
254 | 
255 | void Solver::setNumberOfEqualityConstraints(int m)
256 | {
257 |     return m_pimpl->solver->setNumberOfEqualityConstraints(m);
258 | }
259 | 
260 | bool Solver::setHessianMatrix(const Eigen::SparseMatrix<double>& hessianMatrix)
261 | {
262 |     return m_pimpl->solver->setHessianMatrix(hessianMatrix);
263 | }
264 | 
265 | bool Solver::setGradient(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> gradientVector)
266 | {
267 |     return m_pimpl->solver->setGradient(gradientVector);
268 | }
269 | 
270 | Eigen::Matrix<double, Eigen::Dynamic, 1> Solver::getGradient()
271 | {
272 |     return m_pimpl->solver->getGradient();
273 | }
274 | 
275 | bool
276 | Solver::setLinearConstraintsMatrix(const Eigen::SparseMatrix<double>& inequalityConstraintsMatrix)
277 | {
278 |     return m_pimpl->solver->setInequalityConstraintsMatrix(inequalityConstraintsMatrix);
279 | }
280 | 
281 | bool Solver::setInequalityConstraintsMatrix(const Eigen::SparseMatrix<double>& inequalityConstraintsMatrix)
282 | {
283 |     return m_pimpl->solver->setInequalityConstraintsMatrix(inequalityConstraintsMatrix);
284 | }
285 | 
286 | bool Solver::setLowerBound(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> lowerBoundVector)
287 | {
288 |     return m_pimpl->solver->setLowerBound(lowerBoundVector);
289 | }
290 | 
291 | bool Solver::setUpperBound(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> upperBoundVector)
292 | {
293 |     return m_pimpl->solver->setUpperBound(upperBoundVector);
294 | }
295 | 
296 | bool Solver::setBounds(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> lowerBound,
297 |                Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> upperBound)
298 | {
299 |     return m_pimpl->solver->setBounds(lowerBound, upperBound);
300 | }
301 | 
302 | bool Solver::setEqualityConstraintsMatrix(const Eigen::SparseMatrix<double>& equalityConstraintsMatrix)
303 | {
304 |     return m_pimpl->solver->setEqualityConstraintsMatrix(equalityConstraintsMatrix);
305 | }
306 | 
307 | bool Solver::setEqualityConstraintsVector(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& equalityConstraintsVector)
308 | {
309 |     return m_pimpl->solver->setEqualityConstraintsVector(equalityConstraintsVector);
310 | }
311 | 
312 | bool Solver::setBooleanParameter(const std::string& parameterName, bool value)
313 | {
314 |     return m_pimpl->solver->setBooleanParameter(parameterName, value);
315 | }
316 | 
317 | bool Solver::setIntegerParameter(const std::string& parameterName, int64_t value)
318 | {
319 |     return m_pimpl->solver->setIntegerParameter(parameterName, value);
320 | }
321 | 
322 | bool Solver::setRealNumberParameter(const std::string& parameterName, double value)
323 | {
324 |     return m_pimpl->solver->setRealNumberParameter(parameterName, value);
325 | }
326 | 
327 | bool Solver::setStringParameter(const std::string& parameterName, const std::string& value)
328 | {
329 |     return m_pimpl->solver->setStringParameter(parameterName, value);
330 | }
331 | 
332 | bool Solver::getBooleanParametersNames(std::vector<std::string>& parametersNames) const
333 | {
334 |     return m_pimpl->solver->getBooleanParametersNames(parametersNames);
335 | }
336 | 
337 | bool Solver::getIntegerParametersNames(std::vector<std::string>& parameterNames) const
338 | {
339 |     return m_pimpl->solver->getIntegerParametersNames(parameterNames);
340 | }
341 | 
342 | bool Solver::getRealNumberParametersNames(std::vector<std::string>& parametersNames) const
343 | {
344 |     return m_pimpl->solver->getRealNumberParametersNames(parametersNames);
345 | }
346 | 
347 | bool Solver::getStringParametersNames(std::vector<std::string>& parametersNames) const
348 | {
349 |     return m_pimpl->solver->getStringParametersNames(parametersNames);
350 | }
351 | 
352 | 
353 | Solver* Solver::data()
354 | {
355 |     return this;
356 | }
357 | 
358 | }
359 | 
360 | 


--------------------------------------------------------------------------------
/examples/mpc/MPCExample.cpp:
--------------------------------------------------------------------------------
  1 | #include <QpSolversEigen/QpSolversEigen.hpp>
  2 | #include <Eigen/Dense>
  3 | 
  4 | #include <iostream>
  5 | #include <numbers>
  6 | 
  7 | void setDynamicsMatrices(Eigen::Matrix<double, 12, 12>& a, Eigen::Matrix<double, 12, 4>& b)
  8 | {
  9 |     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.,
 10 |         0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0.0488, 0., 0., 1., 0., 0., 0.0016,
 11 |         0., 0., 0.0992, 0., 0., 0., -0.0488, 0., 0., 1., 0., 0., -0.0016, 0., 0., 0.0992, 0., 0.,
 12 |         0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.0992, 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0.,
 13 |         0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0.,
 14 |         0., 0., 0.9734, 0., 0., 0., 0., 0., 0.0488, 0., 0., 0.9846, 0., 0., 0., -0.9734, 0., 0., 0.,
 15 |         0., 0., -0.0488, 0., 0., 0.9846, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.9846;
 16 | 
 17 |     b << 0., -0.0726, 0., 0.0726, -0.0726, 0., 0.0726, 0., -0.0152, 0.0152, -0.0152, 0.0152, -0.,
 18 |         -0.0006, -0., 0.0006, 0.0006, 0., -0.0006, 0.0000, 0.0106, 0.0106, 0.0106, 0.0106, 0,
 19 |         -1.4512, 0., 1.4512, -1.4512, 0., 1.4512, 0., -0.3049, 0.3049, -0.3049, 0.3049, -0.,
 20 |         -0.0236, 0., 0.0236, 0.0236, 0., -0.0236, 0., 0.2107, 0.2107, 0.2107, 0.2107;
 21 | }
 22 | 
 23 | void setInequalityConstraints(Eigen::Matrix<double, 12, 1>& xMax,
 24 |                               Eigen::Matrix<double, 12, 1>& xMin,
 25 |                               Eigen::Matrix<double, 4, 1>& uMax,
 26 |                               Eigen::Matrix<double, 4, 1>& uMin)
 27 | {
 28 |     double u0 = 10.5916;
 29 | 
 30 |     // input inequality constraints
 31 |     uMin << 9.6 - u0, 9.6 - u0, 9.6 - u0, 9.6 - u0;
 32 | 
 33 |     uMax << 13 - u0, 13 - u0, 13 - u0, 13 - u0;
 34 | 
 35 |     // state inequality constraints
 36 |     xMin << -std::numbers::pi / 6, -std::numbers::pi / 6, -QpSolversEigen::INFTY, -QpSolversEigen::INFTY, -QpSolversEigen::INFTY, -1.,
 37 |         -QpSolversEigen::INFTY, -QpSolversEigen::INFTY, -QpSolversEigen::INFTY, -QpSolversEigen::INFTY,
 38 |         -QpSolversEigen::INFTY, -QpSolversEigen::INFTY;
 39 | 
 40 |     xMax << std::numbers::pi / 6, std::numbers::pi / 6, QpSolversEigen::INFTY, QpSolversEigen::INFTY, QpSolversEigen::INFTY,
 41 |         QpSolversEigen::INFTY, QpSolversEigen::INFTY, QpSolversEigen::INFTY, QpSolversEigen::INFTY, QpSolversEigen::INFTY,
 42 |         QpSolversEigen::INFTY, QpSolversEigen::INFTY;
 43 | }
 44 | 
 45 | void setWeightMatrices(Eigen::DiagonalMatrix<double, 12>& Q, Eigen::DiagonalMatrix<double, 4>& R)
 46 | {
 47 |     Q.diagonal() << 0, 0, 10., 10., 10., 10., 0, 0, 0, 5., 5., 5.;
 48 |     R.diagonal() << 0.1, 0.1, 0.1, 0.1;
 49 | }
 50 | 
 51 | void castMPCToQPHessian(const Eigen::DiagonalMatrix<double, 12>& Q,
 52 |                         const Eigen::DiagonalMatrix<double, 4>& R,
 53 |                         int mpcWindow,
 54 |                         Eigen::SparseMatrix<double>& hessianMatrix)
 55 | {
 56 | 
 57 |     hessianMatrix.resize(12 * (mpcWindow + 1) + 4 * mpcWindow,
 58 |                          12 * (mpcWindow + 1) + 4 * mpcWindow);
 59 | 
 60 |     // populate hessian matrix
 61 |     for (int i = 0; i < 12 * (mpcWindow + 1) + 4 * mpcWindow; i++)
 62 |     {
 63 |         if (i < 12 * (mpcWindow + 1))
 64 |         {
 65 |             int posQ = i % 12;
 66 |             float value = Q.diagonal()[posQ];
 67 |             if (value != 0)
 68 |                 hessianMatrix.insert(i, i) = value;
 69 |         } else
 70 |         {
 71 |             int posR = i % 4;
 72 |             float value = R.diagonal()[posR];
 73 |             if (value != 0)
 74 |                 hessianMatrix.insert(i, i) = value;
 75 |         }
 76 |     }
 77 | }
 78 | 
 79 | void castMPCToQPGradient(const Eigen::DiagonalMatrix<double, 12>& Q,
 80 |                          const Eigen::Matrix<double, 12, 1>& xRef,
 81 |                          int mpcWindow,
 82 |                          Eigen::VectorXd& gradient)
 83 | {
 84 | 
 85 |     Eigen::Matrix<double, 12, 1> Qx_ref;
 86 |     Qx_ref = Q * (-xRef);
 87 | 
 88 |     // populate the gradient vector
 89 |     gradient = Eigen::VectorXd::Zero(12 * (mpcWindow + 1) + 4 * mpcWindow, 1);
 90 |     for (int i = 0; i < 12 * (mpcWindow + 1); i++)
 91 |     {
 92 |         int posQ = i % 12;
 93 |         float value = Qx_ref(posQ, 0);
 94 |         gradient(i, 0) = value;
 95 |     }
 96 | }
 97 | 
 98 | void castMPCToQPLinearConstraintMatrix(const Eigen::Matrix<double, 12, 12>& dynamicMatrix,
 99 |                                  const Eigen::Matrix<double, 12, 4>& controlMatrix,
100 |                                  int mpcWindow,
101 |                                  Eigen::SparseMatrix<double>& constraintMatrix)
102 | {
103 |     constraintMatrix.resize( 12 * (mpcWindow + 1),
104 |                             12 * (mpcWindow + 1) + 4 * mpcWindow);
105 | 
106 |     // populate linear constraint matrix
107 |     for (int i = 0; i < 12 * (mpcWindow + 1); i++)
108 |     {
109 |         constraintMatrix.insert(i, i) = -1;
110 |     }
111 | 
112 |     for (int i = 0; i < mpcWindow; i++)
113 |         for (int j = 0; j < 12; j++)
114 |             for (int k = 0; k < 12; k++)
115 |             {
116 |                 float value = dynamicMatrix(j, k);
117 |                 if (value != 0)
118 |                 {
119 |                     constraintMatrix.insert(12 * (i + 1) + j, 12 * i + k) = value;
120 |                 }
121 |             }
122 | 
123 |     for (int i = 0; i < mpcWindow; i++)
124 |         for (int j = 0; j < 12; j++)
125 |             for (int k = 0; k < 4; k++)
126 |             {
127 |                 float value = controlMatrix(j, k);
128 |                 if (value != 0)
129 |                 {
130 |                     constraintMatrix.insert(12 * (i + 1) + j, 4 * i + k + 12 * (mpcWindow + 1))
131 |                         = value;
132 |                 }
133 |             }
134 | }
135 | 
136 | void castMPCToQPInequalityConstraintMatrix(int mpcWindow, Eigen::SparseMatrix<double>& constraintMatrix)
137 | {
138 |     constraintMatrix.resize(12 * (mpcWindow + 1) + 4 * mpcWindow,
139 |                             12 * (mpcWindow + 1) + 4 * mpcWindow);
140 | 
141 |     // populate linear constraint matrix
142 |     for (int i = 0; i < 12 * (mpcWindow + 1) + 4 * mpcWindow; i++)
143 |     {
144 |         constraintMatrix.insert(i, i) = 1;
145 |     }
146 | }
147 | 
148 | void castMPCToQPConstraintVectors(const Eigen::Matrix<double, 12, 1>& xMax,
149 |                                   const Eigen::Matrix<double, 12, 1>& xMin,
150 |                                   const Eigen::Matrix<double, 4, 1>& uMax,
151 |                                   const Eigen::Matrix<double, 4, 1>& uMin,
152 |                                   const Eigen::Matrix<double, 12, 1>& x0,
153 |                                   int mpcWindow,
154 |                                   Eigen::VectorXd& equalityVectorConstraints,
155 |                                   Eigen::VectorXd& lowerInequality,
156 |                                   Eigen::VectorXd& upperInequality)
157 | {
158 |     // evaluate the lower and the upper inequality vectors
159 |     lowerInequality
160 |         = Eigen::MatrixXd::Zero(12 * (mpcWindow + 1) + 4 * mpcWindow, 1);
161 |     upperInequality
162 |         = Eigen::MatrixXd::Zero(12 * (mpcWindow + 1) + 4 * mpcWindow, 1);
163 |     for (int i = 0; i < mpcWindow + 1; i++)
164 |     {
165 |         lowerInequality.block(12 * i, 0, 12, 1) = xMin;
166 |         upperInequality.block(12 * i, 0, 12, 1) = xMax;
167 |     }
168 |     for (int i = 0; i < mpcWindow; i++)
169 |     {
170 |         lowerInequality.block(4 * i + 12 * (mpcWindow + 1), 0, 4, 1) = uMin;
171 |         upperInequality.block(4 * i + 12 * (mpcWindow + 1), 0, 4, 1) = uMax;
172 |     }
173 | 
174 |     // evaluate the lower and the upper equality vectors
175 |     equalityVectorConstraints = Eigen::MatrixXd::Zero(12 * (mpcWindow + 1), 1);
176 |     equalityVectorConstraints.block(0, 0, 12, 1) = -x0;
177 | }
178 | 
179 | void updateLinearConstraintVectors(const Eigen::Matrix<double, 12, 1>& x0,
180 |                              Eigen::VectorXd& equalityVectorConstraints)
181 | {
182 |     equalityVectorConstraints.block(0, 0, 12, 1) = -x0;
183 | }
184 | 
185 | double getErrorNorm(const Eigen::Matrix<double, 12, 1>& x, const Eigen::Matrix<double, 12, 1>& xRef)
186 | {
187 |     // evaluate the error
188 |     Eigen::Matrix<double, 12, 1> error = x - xRef;
189 | 
190 |     // return the norm
191 |     return error.norm();
192 | }
193 | 
194 | int main()
195 | {
196 |     // set the preview window
197 |     int mpcWindow = 20;
198 | 
199 |     // allocate the dynamics matrices
200 |     Eigen::Matrix<double, 12, 12> a;
201 |     Eigen::Matrix<double, 12, 4> b;
202 | 
203 |     // allocate the constraints vector
204 |     Eigen::Matrix<double, 12, 1> xMax;
205 |     Eigen::Matrix<double, 12, 1> xMin;
206 |     Eigen::Matrix<double, 4, 1> uMax;
207 |     Eigen::Matrix<double, 4, 1> uMin;
208 | 
209 |     // allocate the weight matrices
210 |     Eigen::DiagonalMatrix<double, 12> Q;
211 |     Eigen::DiagonalMatrix<double, 4> R;
212 | 
213 |     // allocate the initial and the reference state space
214 |     Eigen::Matrix<double, 12, 1> x0;
215 |     Eigen::Matrix<double, 12, 1> xRef;
216 | 
217 |     // allocate QP problem matrices and vectors
218 |     Eigen::SparseMatrix<double> hessian;
219 |     Eigen::VectorXd gradient;
220 |     Eigen::SparseMatrix<double> linearEqMatrix;
221 |     Eigen::SparseMatrix<double> linearInqeMatrix;
222 |     Eigen::VectorXd equalityVectorConstraints;
223 |     Eigen::VectorXd lowerBound;
224 |     Eigen::VectorXd upperBound;
225 | 
226 |     // set the initial and the desired states
227 |     x0 << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
228 |     xRef << 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0;
229 | 
230 |     // set MPC problem quantities
231 |     setDynamicsMatrices(a, b);
232 |     setInequalityConstraints(xMax, xMin, uMax, uMin);
233 |     setWeightMatrices(Q, R);
234 | 
235 |     // cast the MPC problem as QP problem
236 |     castMPCToQPHessian(Q, R, mpcWindow, hessian);
237 |     castMPCToQPGradient(Q, xRef, mpcWindow, gradient);
238 |     castMPCToQPLinearConstraintMatrix(a, b, mpcWindow, linearEqMatrix);
239 |     castMPCToQPInequalityConstraintMatrix(mpcWindow, linearInqeMatrix);
240 |     castMPCToQPConstraintVectors(xMax, xMin, uMax, uMin, x0, mpcWindow, equalityVectorConstraints, lowerBound, upperBound);
241 | 
242 |     // instantiate the solver
243 |     QpSolversEigen::Solver solver;
244 |     bool ok = solver.instantiateSolver("proxqp");
245 |     if (!ok)
246 |     {
247 |         std::cerr << "QpSolverEigenMPCExample: Failed to instantiate the solver." << std::endl;
248 |         return EXIT_FAILURE;
249 |     }
250 | 
251 |     // settings
252 | 
253 |     // solver.setBooleanParameter("verbose", false);
254 | 
255 |     // Set osqp-specific parameters
256 |     if (solver.getSolverName() == "osqp")
257 |     {
258 |         solver.setBooleanParameter("warm_starting", true);
259 |     }
260 | 
261 |     // Set proxqp-specific parameters
262 |     if (solver.getSolverName() == "proxqp")
263 |     {
264 |         solver.setBooleanParameter("verbose", true);
265 |         solver.setBooleanParameter("compute_timings", true);
266 |         solver.setStringParameter("initial_guess", "WARM_START_WITH_PREVIOUS_RESULT");
267 |     }
268 | 
269 |     // set the initial data of the QP solver
270 |     solver.setNumberOfVariables(12 * (mpcWindow + 1) + 4 * mpcWindow);
271 |     solver.setNumberOfInequalityConstraints(linearInqeMatrix.rows());
272 |     solver.setNumberOfEqualityConstraints(linearEqMatrix.rows());
273 | 
274 |     if (!solver.setHessianMatrix(hessian))
275 |         return 1;
276 |     if (!solver.setGradient(gradient))
277 |         return 1;
278 |     if (!solver.setInequalityConstraintsMatrix(linearInqeMatrix))
279 |         return 1;
280 |     if (!solver.setEqualityConstraintsMatrix(linearEqMatrix))
281 |         return 1;
282 |     if (!solver.setEqualityConstraintsVector(equalityVectorConstraints))
283 |         return 1;
284 |     if (!solver.setLowerBound(lowerBound))
285 |         return 1;
286 |     if (!solver.setUpperBound(upperBound))
287 |         return 1;
288 | 
289 |     // instantiate the solver
290 |     if (!solver.initSolver())
291 |         return 1;
292 | 
293 |     // controller input and QPSolution vector
294 |     Eigen::Vector4d ctr;
295 |     Eigen::VectorXd QPSolution;
296 | 
297 |     // number of iteration steps
298 |     int numberOfSteps = 50;
299 | 
300 |     for (int i = 0; i < numberOfSteps; i++)
301 |     {
302 | 
303 |         // solve the QP problem
304 |         if (solver.solveProblem() != QpSolversEigen::ErrorExitFlag::NoError)
305 |             return 1;
306 | 
307 |         // get the controller input
308 |         QPSolution = solver.getSolution();
309 |         ctr = QPSolution.block(12 * (mpcWindow + 1), 0, 4, 1);
310 | 
311 |         // save data into file
312 |         auto x0Data = x0.data();
313 | 
314 |         // propagate the model
315 |         x0 = a * x0 + b * ctr;
316 | 
317 |         // update the constraint bound
318 |         updateLinearConstraintVectors(x0, equalityVectorConstraints);
319 |         if (!solver.updateEqualityConstraintsVector(equalityVectorConstraints))
320 |             return 1;
321 |     }
322 |     return 0;
323 | }
324 | 


--------------------------------------------------------------------------------
/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-2024
  6 |  */
  7 | 
  8 | // Include macros common to all test
  9 | #include "QpSolversEigenCommonTestMacros.hpp"
 10 | 
 11 | // Catch2
 12 | #include <catch2/catch_test_macros.hpp>
 13 | #include <catch2/generators/catch_generators_all.hpp>
 14 | 
 15 | // QpSolversEigen
 16 | #include <QpSolversEigen/QpSolversEigen.hpp>
 17 | 
 18 | // eigen
 19 | #include <Eigen/Dense>
 20 | 
 21 | #include <cmath>
 22 | #include <fstream>
 23 | #include <iostream>
 24 | 
 25 | // colors
 26 | #define ANSI_TXT_GRN "\033[0;32m"
 27 | #define ANSI_TXT_MGT "\033[0;35m" // Magenta
 28 | #define ANSI_TXT_DFT "\033[0;0m" // Console default
 29 | #define GTEST_BOX "[     cout ] "
 30 | #define COUT_GTEST ANSI_TXT_GRN << GTEST_BOX // You could add the Default
 31 | #define COUT_GTEST_MGT COUT_GTEST << ANSI_TXT_MGT
 32 | 
 33 | void setDynamicsMatrices(Eigen::Matrix<double, 12, 12>& a, Eigen::Matrix<double, 12, 4>& b)
 34 | {
 35 |     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.,
 36 |         0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0.0488, 0., 0., 1., 0., 0., 0.0016,
 37 |         0., 0., 0.0992, 0., 0., 0., -0.0488, 0., 0., 1., 0., 0., -0.0016, 0., 0., 0.0992, 0., 0.,
 38 |         0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.0992, 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0.,
 39 |         0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0.,
 40 |         0., 0., 0.9734, 0., 0., 0., 0., 0., 0.0488, 0., 0., 0.9846, 0., 0., 0., -0.9734, 0., 0., 0.,
 41 |         0., 0., -0.0488, 0., 0., 0.9846, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.9846;
 42 | 
 43 |     b << 0., -0.0726, 0., 0.0726, -0.0726, 0., 0.0726, 0., -0.0152, 0.0152, -0.0152, 0.0152, -0.,
 44 |         -0.0006, -0., 0.0006, 0.0006, 0., -0.0006, 0.0000, 0.0106, 0.0106, 0.0106, 0.0106, 0,
 45 |         -1.4512, 0., 1.4512, -1.4512, 0., 1.4512, 0., -0.3049, 0.3049, -0.3049, 0.3049, -0.,
 46 |         -0.0236, 0., 0.0236, 0.0236, 0., -0.0236, 0., 0.2107, 0.2107, 0.2107, 0.2107;
 47 | }
 48 | 
 49 | void setInequalityConstraints(Eigen::Matrix<double, 12, 1>& xMax,
 50 |                               Eigen::Matrix<double, 12, 1>& xMin,
 51 |                               Eigen::Matrix<double, 4, 1>& uMax,
 52 |                               Eigen::Matrix<double, 4, 1>& uMin)
 53 | {
 54 |     double u0 = 10.5916;
 55 | 
 56 |     // input inequality constraints
 57 |     uMin << 9.6 - u0, 9.6 - u0, 9.6 - u0, 9.6 - u0;
 58 | 
 59 |     uMax << 13 - u0, 13 - u0, 13 - u0, 13 - u0;
 60 | 
 61 |     // state inequality constraints
 62 |     xMin << -M_PI / 6, -M_PI / 6, -QpSolversEigen::INFTY, -QpSolversEigen::INFTY, -QpSolversEigen::INFTY, -1.,
 63 |         -QpSolversEigen::INFTY, -QpSolversEigen::INFTY, -QpSolversEigen::INFTY, -QpSolversEigen::INFTY,
 64 |         -QpSolversEigen::INFTY, -QpSolversEigen::INFTY;
 65 | 
 66 |     xMax << M_PI / 6, M_PI / 6, QpSolversEigen::INFTY, QpSolversEigen::INFTY, QpSolversEigen::INFTY,
 67 |         QpSolversEigen::INFTY, QpSolversEigen::INFTY, QpSolversEigen::INFTY, QpSolversEigen::INFTY, QpSolversEigen::INFTY,
 68 |         QpSolversEigen::INFTY, QpSolversEigen::INFTY;
 69 | }
 70 | 
 71 | void setWeightMatrices(Eigen::DiagonalMatrix<double, 12>& Q, Eigen::DiagonalMatrix<double, 4>& R)
 72 | {
 73 |     Q.diagonal() << 0, 0, 10., 10., 10., 10., 0, 0, 0, 5., 5., 5.;
 74 |     R.diagonal() << 0.1, 0.1, 0.1, 0.1;
 75 | }
 76 | 
 77 | void castMPCToQPHessian(const Eigen::DiagonalMatrix<double, 12>& Q,
 78 |                         const Eigen::DiagonalMatrix<double, 4>& R,
 79 |                         int mpcWindow,
 80 |                         Eigen::SparseMatrix<double>& hessianMatrix)
 81 | {
 82 | 
 83 |     hessianMatrix.resize(12 * (mpcWindow + 1) + 4 * mpcWindow,
 84 |                          12 * (mpcWindow + 1) + 4 * mpcWindow);
 85 | 
 86 |     // populate hessian matrix
 87 |     for (int i = 0; i < 12 * (mpcWindow + 1) + 4 * mpcWindow; i++)
 88 |     {
 89 |         if (i < 12 * (mpcWindow + 1))
 90 |         {
 91 |             int posQ = i % 12;
 92 |             float value = Q.diagonal()[posQ];
 93 |             if (value != 0)
 94 |                 hessianMatrix.insert(i, i) = value;
 95 |         } else
 96 |         {
 97 |             int posR = i % 4;
 98 |             float value = R.diagonal()[posR];
 99 |             if (value != 0)
100 |                 hessianMatrix.insert(i, i) = value;
101 |         }
102 |     }
103 | }
104 | 
105 | void castMPCToQPGradient(const Eigen::DiagonalMatrix<double, 12>& Q,
106 |                          const Eigen::Matrix<double, 12, 1>& xRef,
107 |                          int mpcWindow,
108 |                          Eigen::Matrix<double, -1, 1>& gradient)
109 | {
110 | 
111 |     Eigen::Matrix<double, 12, 1> Qx_ref;
112 |     Qx_ref = Q * (-xRef);
113 | 
114 |     // populate the gradient vector
115 |     gradient = Eigen::Matrix<double, -1, 1>::Zero(12 * (mpcWindow + 1) + 4 * mpcWindow, 1);
116 |     for (int i = 0; i < 12 * (mpcWindow + 1); i++)
117 |     {
118 |         int posQ = i % 12;
119 |         float value = Qx_ref(posQ, 0);
120 |         gradient(i, 0) = value;
121 |     }
122 | }
123 | 
124 | void castMPCToQPConstraintMatrix(const Eigen::Matrix<double, 12, 12>& dynamicMatrix,
125 |                                  const Eigen::Matrix<double, 12, 4>& controlMatrix,
126 |                                  int mpcWindow,
127 |                                  Eigen::SparseMatrix<double>& constraintMatrix)
128 | {
129 |     constraintMatrix.resize(12 * (mpcWindow + 1) + 12 * (mpcWindow + 1) + 4 * mpcWindow,
130 |                             12 * (mpcWindow + 1) + 4 * mpcWindow);
131 | 
132 |     // populate linear constraint matrix
133 |     for (int i = 0; i < 12 * (mpcWindow + 1); i++)
134 |     {
135 |         constraintMatrix.insert(i, i) = -1;
136 |     }
137 | 
138 |     for (int i = 0; i < mpcWindow; i++)
139 |         for (int j = 0; j < 12; j++)
140 |             for (int k = 0; k < 12; k++)
141 |             {
142 |                 float value = dynamicMatrix(j, k);
143 |                 if (value != 0)
144 |                 {
145 |                     constraintMatrix.insert(12 * (i + 1) + j, 12 * i + k) = value;
146 |                 }
147 |             }
148 | 
149 |     for (int i = 0; i < mpcWindow; i++)
150 |         for (int j = 0; j < 12; j++)
151 |             for (int k = 0; k < 4; k++)
152 |             {
153 |                 float value = controlMatrix(j, k);
154 |                 if (value != 0)
155 |                 {
156 |                     constraintMatrix.insert(12 * (i + 1) + j, 4 * i + k + 12 * (mpcWindow + 1))
157 |                         = value;
158 |                 }
159 |             }
160 | 
161 |     for (int i = 0; i < 12 * (mpcWindow + 1) + 4 * mpcWindow; i++)
162 |     {
163 |         constraintMatrix.insert(i + (mpcWindow + 1) * 12, i) = 1;
164 |     }
165 | }
166 | 
167 | void castMPCToQPConstraintVectors(const Eigen::Matrix<double, 12, 1>& xMax,
168 |                                   const Eigen::Matrix<double, 12, 1>& xMin,
169 |                                   const Eigen::Matrix<double, 4, 1>& uMax,
170 |                                   const Eigen::Matrix<double, 4, 1>& uMin,
171 |                                   const Eigen::Matrix<double, 12, 1>& x0,
172 |                                   int mpcWindow,
173 |                                   Eigen::Matrix<double, -1, 1>& lowerBound,
174 |                                   Eigen::Matrix<double, -1, 1>& upperBound)
175 | {
176 |     // evaluate the lower and the upper inequality vectors
177 |     Eigen::Matrix<double, -1, 1> lowerInequality
178 |         = Eigen::Matrix<double, -1, -1>::Zero(12 * (mpcWindow + 1) + 4 * mpcWindow, 1);
179 |     Eigen::Matrix<double, -1, 1> upperInequality
180 |         = Eigen::Matrix<double, -1, -1>::Zero(12 * (mpcWindow + 1) + 4 * mpcWindow, 1);
181 |     for (int i = 0; i < mpcWindow + 1; i++)
182 |     {
183 |         lowerInequality.block(12 * i, 0, 12, 1) = xMin;
184 |         upperInequality.block(12 * i, 0, 12, 1) = xMax;
185 |     }
186 |     for (int i = 0; i < mpcWindow; i++)
187 |     {
188 |         lowerInequality.block(4 * i + 12 * (mpcWindow + 1), 0, 4, 1) = uMin;
189 |         upperInequality.block(4 * i + 12 * (mpcWindow + 1), 0, 4, 1) = uMax;
190 |     }
191 | 
192 |     // evaluate the lower and the upper equality vectors
193 |     Eigen::Matrix<double, -1, 1> lowerEquality
194 |         = Eigen::Matrix<double, -1, -1>::Zero(12 * (mpcWindow + 1), 1);
195 |     Eigen::Matrix<double, -1, 1> upperEquality;
196 |     lowerEquality.block(0, 0, 12, 1) = -x0;
197 |     upperEquality = lowerEquality;
198 |     lowerEquality = lowerEquality;
199 | 
200 |     // merge inequality and equality vectors
201 |     lowerBound = Eigen::Matrix<double, -1, -1>::Zero(2 * 12 * (mpcWindow + 1) + 4 * mpcWindow, 1);
202 |     lowerBound << lowerEquality, lowerInequality;
203 | 
204 |     upperBound = Eigen::Matrix<double, -1, -1>::Zero(2 * 12 * (mpcWindow + 1) + 4 * mpcWindow, 1);
205 |     upperBound << upperEquality, upperInequality;
206 | }
207 | 
208 | void updateConstraintVectors(const Eigen::Matrix<double, 12, 1>& x0,
209 |                              Eigen::Matrix<double, -1, 1>& lowerBound,
210 |                              Eigen::Matrix<double, -1, 1>& upperBound)
211 | {
212 |     lowerBound.block(0, 0, 12, 1) = -x0;
213 |     upperBound.block(0, 0, 12, 1) = -x0;
214 | }
215 | 
216 | double
217 | getErrorNorm(const Eigen::Matrix<double, 12, 1>& x, const Eigen::Matrix<double, 12, 1>& xRef)
218 | {
219 |     // evaluate the error
220 |     Eigen::Matrix<double, 12, 1> error = x - xRef;
221 | 
222 |     // return the norm
223 |     return error.norm();
224 | }
225 | 
226 | TEST_CASE("MPCTest")
227 | {
228 |     // open the ofstream
229 |     std::ofstream dataStream;
230 |     dataStream.open("output.txt");
231 | 
232 |     // set the preview window
233 |     int mpcWindow = 20;
234 | 
235 |     // allocate the dynamics matrices
236 |     Eigen::Matrix<double, 12, 12> a;
237 |     Eigen::Matrix<double, 12, 4> b;
238 | 
239 |     // allocate the constraints vector
240 |     Eigen::Matrix<double, 12, 1> xMax;
241 |     Eigen::Matrix<double, 12, 1> xMin;
242 |     Eigen::Matrix<double, 4, 1> uMax;
243 |     Eigen::Matrix<double, 4, 1> uMin;
244 | 
245 |     // allocate the weight matrices
246 |     Eigen::DiagonalMatrix<double, 12> Q;
247 |     Eigen::DiagonalMatrix<double, 4> R;
248 | 
249 |     // allocate the initial and the reference state space
250 |     Eigen::Matrix<double, 12, 1> x0;
251 |     Eigen::Matrix<double, 12, 1> xRef;
252 | 
253 |     // allocate QP problem matrices and vectors
254 |     Eigen::SparseMatrix<double> hessian;
255 |     Eigen::Matrix<double, -1, 1> gradient;
256 |     Eigen::SparseMatrix<double> linearMatrix;
257 |     Eigen::Matrix<double, -1, 1> lowerBound;
258 |     Eigen::Matrix<double, -1, 1> upperBound;
259 | 
260 | 
261 |     // set the initial and the desired states
262 |     x0 << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
263 |     xRef << 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0;
264 | 
265 |     // set MPC problem quantities
266 |     setDynamicsMatrices(a, b);
267 |     setInequalityConstraints(xMax, xMin, uMax, uMin);
268 |     setWeightMatrices(Q, R);
269 | 
270 |     // cast the MPC problem as QP problem
271 |     castMPCToQPHessian(Q, R, mpcWindow, hessian);
272 |     castMPCToQPGradient(Q, xRef, mpcWindow, gradient);
273 |     castMPCToQPConstraintMatrix(a, b, mpcWindow, linearMatrix);
274 |     castMPCToQPConstraintVectors(xMax, xMin, uMax, uMin, x0, mpcWindow, lowerBound, upperBound);
275 | 
276 | 
277 |     // instantiate the solver
278 |     std::string solverName = QPSOLVERSEIGEN_SOLVERS_TO_TEST;
279 |     QpSolversEigen::Solver solver;
280 |     REQUIRE(solver.instantiateSolver(solverName));
281 | 
282 |     std::cout << COUT_GTEST_MGT << "Testing solver " << solver.getSolverName()
283 |               << ANSI_TXT_DFT << std::endl;
284 | 
285 |     // settings
286 |     REQUIRE(solver.setBooleanParameter("verbose", false));
287 | 
288 |     // Set solver-specific parameters
289 |     if (solver.getSolverName() == "osqp")
290 |     {
291 |         REQUIRE(solver.setBooleanParameter("warm_start", true));
292 |     }
293 |     if (solver.getSolverName() == "proxqp")
294 |     {
295 |         REQUIRE(solver.setStringParameter("initial_guess", "WARM_START_WITH_PREVIOUS_RESULT"));
296 |     }
297 | 
298 |     // set the initial data of the QP solver
299 |     solver.data()->setNumberOfVariables(12 * (mpcWindow + 1) + 4 * mpcWindow);
300 |     solver.data()->setNumberOfInequalityConstraints(2 * 12 * (mpcWindow + 1) + 4 * mpcWindow);
301 |     REQUIRE(solver.data()->setHessianMatrix(hessian));
302 |     REQUIRE(solver.data()->setGradient(gradient));
303 |     REQUIRE(solver.data()->setInequalityConstraintsMatrix(linearMatrix));
304 |     REQUIRE(solver.data()->setLowerBound(lowerBound));
305 |     REQUIRE(solver.data()->setUpperBound(upperBound));
306 | 
307 |     // instantiate the solver
308 |     REQUIRE(solver.initSolver());
309 | 
310 |     // controller input and QPSolution vector
311 |     Eigen::Matrix<double, 4, 1> ctr;
312 |     Eigen::Matrix<double, -1, 1> QPSolution;
313 | 
314 |     // number of iteration steps
315 |     int numberOfSteps = 50;
316 | 
317 |     // profiling quantities
318 |     clock_t startTime, endTime;
319 |     double averageTime = 0;
320 | 
321 |     for (int i = 0; i < numberOfSteps; i++)
322 |     {
323 |         startTime = clock();
324 | 
325 |         // solve the QP problem
326 |         REQUIRE(solver.solveProblem() == QpSolversEigen::ErrorExitFlag::NoError);
327 | 
328 |         // get the controller input
329 |         QPSolution = solver.getSolution();
330 |         ctr = QPSolution.block(12 * (mpcWindow + 1), 0, 4, 1);
331 | 
332 |         // save data into file
333 |         auto x0Data = x0.data();
334 |         for (int j = 0; j < 12; j++)
335 |             dataStream << x0Data[j] << " ";
336 |         dataStream << std::endl;
337 | 
338 |         // propagate the model
339 |         x0 = a * x0 + b * ctr;
340 | 
341 |         // update the constraint bound
342 |         updateConstraintVectors(x0, lowerBound, upperBound);
343 |         REQUIRE(solver.updateBounds(lowerBound, upperBound));
344 | 
345 |         endTime = clock();
346 | 
347 |         averageTime += static_cast<double>(endTime - startTime) / CLOCKS_PER_SEC;
348 |     }
349 | 
350 |     // close the stream
351 |     dataStream.close();
352 | 
353 |     std::cout << COUT_GTEST_MGT << "Average time = " << averageTime / numberOfSteps << " seconds."
354 |               << ANSI_TXT_DFT << std::endl;
355 | 
356 |     constexpr double tolerance = 1e-2;
357 |     REQUIRE(getErrorNorm(x0, xRef) <= tolerance);
358 | }
359 | 


--------------------------------------------------------------------------------
/tests/MPCEqualityConstraintsTest.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-2024
  6 |  */
  7 | 
  8 | // Include macros common to all test
  9 | #include "QpSolversEigenCommonTestMacros.hpp"
 10 | 
 11 | // Catch2
 12 | #include <catch2/catch_test_macros.hpp>
 13 | #include <catch2/generators/catch_generators_all.hpp>
 14 | 
 15 | // QpSolversEigen
 16 | #include <QpSolversEigen/QpSolversEigen.hpp>
 17 | 
 18 | // eigen
 19 | #include <Eigen/Dense>
 20 | 
 21 | #include <cmath>
 22 | #include <fstream>
 23 | #include <iostream>
 24 | 
 25 | // colors
 26 | #define ANSI_TXT_GRN "\033[0;32m"
 27 | #define ANSI_TXT_MGT "\033[0;35m" // Magenta
 28 | #define ANSI_TXT_DFT "\033[0;0m" // Console default
 29 | #define GTEST_BOX "[     cout ] "
 30 | #define COUT_GTEST ANSI_TXT_GRN << GTEST_BOX // You could add the Default
 31 | #define COUT_GTEST_MGT COUT_GTEST << ANSI_TXT_MGT
 32 | 
 33 | void setDynamicsMatrices(Eigen::Matrix<double, 12, 12>& a, Eigen::Matrix<double, 12, 4>& b)
 34 | {
 35 |     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.,
 36 |         0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0.0488, 0., 0., 1., 0., 0., 0.0016,
 37 |         0., 0., 0.0992, 0., 0., 0., -0.0488, 0., 0., 1., 0., 0., -0.0016, 0., 0., 0.0992, 0., 0.,
 38 |         0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.0992, 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0.,
 39 |         0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0.,
 40 |         0., 0., 0.9734, 0., 0., 0., 0., 0., 0.0488, 0., 0., 0.9846, 0., 0., 0., -0.9734, 0., 0., 0.,
 41 |         0., 0., -0.0488, 0., 0., 0.9846, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.9846;
 42 | 
 43 |     b << 0., -0.0726, 0., 0.0726, -0.0726, 0., 0.0726, 0., -0.0152, 0.0152, -0.0152, 0.0152, -0.,
 44 |         -0.0006, -0., 0.0006, 0.0006, 0., -0.0006, 0.0000, 0.0106, 0.0106, 0.0106, 0.0106, 0,
 45 |         -1.4512, 0., 1.4512, -1.4512, 0., 1.4512, 0., -0.3049, 0.3049, -0.3049, 0.3049, -0.,
 46 |         -0.0236, 0., 0.0236, 0.0236, 0., -0.0236, 0., 0.2107, 0.2107, 0.2107, 0.2107;
 47 | }
 48 | 
 49 | void setInequalityConstraints(Eigen::Matrix<double, 12, 1>& xMax,
 50 |                               Eigen::Matrix<double, 12, 1>& xMin,
 51 |                               Eigen::Matrix<double, 4, 1>& uMax,
 52 |                               Eigen::Matrix<double, 4, 1>& uMin)
 53 | {
 54 |     double u0 = 10.5916;
 55 | 
 56 |     // input inequality constraints
 57 |     uMin << 9.6 - u0, 9.6 - u0, 9.6 - u0, 9.6 - u0;
 58 | 
 59 |     uMax << 13 - u0, 13 - u0, 13 - u0, 13 - u0;
 60 | 
 61 |     // state inequality constraints
 62 |     xMin << -M_PI / 6, -M_PI / 6, -QpSolversEigen::INFTY, -QpSolversEigen::INFTY, -QpSolversEigen::INFTY, -1.,
 63 |         -QpSolversEigen::INFTY, -QpSolversEigen::INFTY, -QpSolversEigen::INFTY, -QpSolversEigen::INFTY,
 64 |         -QpSolversEigen::INFTY, -QpSolversEigen::INFTY;
 65 | 
 66 |     xMax << M_PI / 6, M_PI / 6, QpSolversEigen::INFTY, QpSolversEigen::INFTY, QpSolversEigen::INFTY,
 67 |         QpSolversEigen::INFTY, QpSolversEigen::INFTY, QpSolversEigen::INFTY, QpSolversEigen::INFTY, QpSolversEigen::INFTY,
 68 |         QpSolversEigen::INFTY, QpSolversEigen::INFTY;
 69 | }
 70 | 
 71 | void setWeightMatrices(Eigen::DiagonalMatrix<double, 12>& Q, Eigen::DiagonalMatrix<double, 4>& R)
 72 | {
 73 |     Q.diagonal() << 0, 0, 10., 10., 10., 10., 0, 0, 0, 5., 5., 5.;
 74 |     R.diagonal() << 0.1, 0.1, 0.1, 0.1;
 75 | }
 76 | 
 77 | void castMPCToQPHessian(const Eigen::DiagonalMatrix<double, 12>& Q,
 78 |                         const Eigen::DiagonalMatrix<double, 4>& R,
 79 |                         int mpcWindow,
 80 |                         Eigen::SparseMatrix<double>& hessianMatrix)
 81 | {
 82 | 
 83 |     hessianMatrix.resize(12 * (mpcWindow + 1) + 4 * mpcWindow,
 84 |                          12 * (mpcWindow + 1) + 4 * mpcWindow);
 85 | 
 86 |     // populate hessian matrix
 87 |     for (int i = 0; i < 12 * (mpcWindow + 1) + 4 * mpcWindow; i++)
 88 |     {
 89 |         if (i < 12 * (mpcWindow + 1))
 90 |         {
 91 |             int posQ = i % 12;
 92 |             float value = Q.diagonal()[posQ];
 93 |             if (value != 0)
 94 |                 hessianMatrix.insert(i, i) = value;
 95 |         } else
 96 |         {
 97 |             int posR = i % 4;
 98 |             float value = R.diagonal()[posR];
 99 |             if (value != 0)
100 |                 hessianMatrix.insert(i, i) = value;
101 |         }
102 |     }
103 | }
104 | 
105 | void castMPCToQPGradient(const Eigen::DiagonalMatrix<double, 12>& Q,
106 |                          const Eigen::Matrix<double, 12, 1>& xRef,
107 |                          int mpcWindow,
108 |                          Eigen::Matrix<double, -1, 1>& gradient)
109 | {
110 | 
111 |     Eigen::Matrix<double, 12, 1> Qx_ref;
112 |     Qx_ref = Q * (-xRef);
113 | 
114 |     // populate the gradient vector
115 |     gradient = Eigen::Matrix<double, -1, 1>::Zero(12 * (mpcWindow + 1) + 4 * mpcWindow, 1);
116 |     for (int i = 0; i < 12 * (mpcWindow + 1); i++)
117 |     {
118 |         int posQ = i % 12;
119 |         float value = Qx_ref(posQ, 0);
120 |         gradient(i, 0) = value;
121 |     }
122 | }
123 | 
124 | void castMPCToQPLinearConstraintMatrix(const Eigen::Matrix<double, 12, 12>& dynamicMatrix,
125 |                                  const Eigen::Matrix<double, 12, 4>& controlMatrix,
126 |                                  int mpcWindow,
127 |                                  Eigen::SparseMatrix<double>& constraintMatrix)
128 | {
129 |     constraintMatrix.resize( 12 * (mpcWindow + 1),
130 |                             12 * (mpcWindow + 1) + 4 * mpcWindow);
131 | 
132 |     // populate linear constraint matrix
133 |     for (int i = 0; i < 12 * (mpcWindow + 1); i++)
134 |     {
135 |         constraintMatrix.insert(i, i) = -1;
136 |     }
137 | 
138 |     for (int i = 0; i < mpcWindow; i++)
139 |         for (int j = 0; j < 12; j++)
140 |             for (int k = 0; k < 12; k++)
141 |             {
142 |                 float value = dynamicMatrix(j, k);
143 |                 if (value != 0)
144 |                 {
145 |                     constraintMatrix.insert(12 * (i + 1) + j, 12 * i + k) = value;
146 |                 }
147 |             }
148 | 
149 |     for (int i = 0; i < mpcWindow; i++)
150 |         for (int j = 0; j < 12; j++)
151 |             for (int k = 0; k < 4; k++)
152 |             {
153 |                 float value = controlMatrix(j, k);
154 |                 if (value != 0)
155 |                 {
156 |                     constraintMatrix.insert(12 * (i + 1) + j, 4 * i + k + 12 * (mpcWindow + 1))
157 |                         = value;
158 |                 }
159 |             }
160 | }
161 | 
162 | 
163 | void castMPCToQPInequalityConstraintMatrix(int mpcWindow, Eigen::SparseMatrix<double>& constraintMatrix)
164 | {
165 |     constraintMatrix.resize(12 * (mpcWindow + 1) + 4 * mpcWindow,
166 |                             12 * (mpcWindow + 1) + 4 * mpcWindow);
167 | 
168 |     // populate linear constraint matrix
169 |     for (int i = 0; i < 12 * (mpcWindow + 1) + 4 * mpcWindow; i++)
170 |     {
171 |         constraintMatrix.insert(i, i) = 1;
172 |     }
173 | }
174 | 
175 | void castMPCToQPConstraintVectors(const Eigen::Matrix<double, 12, 1>& xMax,
176 |                                   const Eigen::Matrix<double, 12, 1>& xMin,
177 |                                   const Eigen::Matrix<double, 4, 1>& uMax,
178 |                                   const Eigen::Matrix<double, 4, 1>& uMin,
179 |                                   const Eigen::Matrix<double, 12, 1>& x0,
180 |                                   int mpcWindow,
181 |                                   Eigen::VectorXd& equalityVectorConstraints,
182 |                                   Eigen::VectorXd& lowerInequality,
183 |                                   Eigen::VectorXd& upperInequality)
184 | {
185 |     // evaluate the lower and the upper inequality vectors
186 |     lowerInequality
187 |         = Eigen::MatrixXd::Zero(12 * (mpcWindow + 1) + 4 * mpcWindow, 1);
188 |     upperInequality
189 |         = Eigen::MatrixXd::Zero(12 * (mpcWindow + 1) + 4 * mpcWindow, 1);
190 |     for (int i = 0; i < mpcWindow + 1; i++)
191 |     {
192 |         lowerInequality.block(12 * i, 0, 12, 1) = xMin;
193 |         upperInequality.block(12 * i, 0, 12, 1) = xMax;
194 |     }
195 |     for (int i = 0; i < mpcWindow; i++)
196 |     {
197 |         lowerInequality.block(4 * i + 12 * (mpcWindow + 1), 0, 4, 1) = uMin;
198 |         upperInequality.block(4 * i + 12 * (mpcWindow + 1), 0, 4, 1) = uMax;
199 |     }
200 | 
201 |     // evaluate the lower and the upper equality vectors
202 |     equalityVectorConstraints = Eigen::MatrixXd::Zero(12 * (mpcWindow + 1), 1);
203 |     equalityVectorConstraints.block(0, 0, 12, 1) = -x0;
204 | }
205 | 
206 | void updateLinearConstraintVectors(const Eigen::Matrix<double, 12, 1>& x0,
207 |                              Eigen::VectorXd& equalityVectorConstraints)
208 | {
209 |     equalityVectorConstraints.block(0, 0, 12, 1) = -x0;
210 | }
211 | 
212 | double
213 | getErrorNorm(const Eigen::Matrix<double, 12, 1>& x, const Eigen::Matrix<double, 12, 1>& xRef)
214 | {
215 |     // evaluate the error
216 |     Eigen::Matrix<double, 12, 1> 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<double, 12, 12> a;
233 |     Eigen::Matrix<double, 12, 4> b;
234 | 
235 |     // allocate the constraints vector
236 |     Eigen::Matrix<double, 12, 1> xMax;
237 |     Eigen::Matrix<double, 12, 1> xMin;
238 |     Eigen::Matrix<double, 4, 1> uMax;
239 |     Eigen::Matrix<double, 4, 1> uMin;
240 | 
241 |     // allocate the weight matrices
242 |     Eigen::DiagonalMatrix<double, 12> Q;
243 |     Eigen::DiagonalMatrix<double, 4> R;
244 | 
245 |     // allocate the initial and the reference state space
246 |     Eigen::Matrix<double, 12, 1> x0;
247 |     Eigen::Matrix<double, 12, 1> xRef;
248 | 
249 |     // allocate QP problem matrices and vectors
250 |     Eigen::SparseMatrix<double> hessian;
251 |     Eigen::Matrix<double, -1, 1> gradient;
252 |     Eigen::SparseMatrix<double> linearEqMatrix;
253 |     Eigen::SparseMatrix<double> linearInqeMatrix;
254 |     Eigen::VectorXd equalityVectorConstraints;
255 |     Eigen::Matrix<double, -1, 1> lowerBound;
256 |     Eigen::Matrix<double, -1, 1> upperBound;
257 | 
258 | 
259 |     // set the initial and the desired states
260 |     x0 << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
261 |     xRef << 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0;
262 | 
263 |     // set MPC problem quantities
264 |     setDynamicsMatrices(a, b);
265 |     setInequalityConstraints(xMax, xMin, uMax, uMin);
266 |     setWeightMatrices(Q, R);
267 | 
268 |     // cast the MPC problem as QP problem
269 |     castMPCToQPHessian(Q, R, mpcWindow, hessian);
270 |     castMPCToQPGradient(Q, xRef, mpcWindow, gradient);
271 |     castMPCToQPLinearConstraintMatrix(a, b, mpcWindow, linearEqMatrix);
272 |     castMPCToQPInequalityConstraintMatrix(mpcWindow, linearInqeMatrix);
273 |     castMPCToQPConstraintVectors(xMax, xMin, uMax, uMin, x0, mpcWindow, equalityVectorConstraints, lowerBound, upperBound);
274 | 
275 | 
276 |     // instantiate the solver
277 |     std::string solverName = QPSOLVERSEIGEN_SOLVERS_TO_TEST;
278 |     QpSolversEigen::Solver solver;
279 |     REQUIRE(solver.instantiateSolver(solverName));
280 | 
281 |     std::cout << COUT_GTEST_MGT << "Testing solver " << solver.getSolverName()
282 |               << ANSI_TXT_DFT << std::endl;
283 | 
284 |     // settings
285 |     REQUIRE(solver.setBooleanParameter("verbose", false));
286 | 
287 |     // Set solver-specific parameters
288 |     if (solver.getSolverName() == "osqp")
289 |     {
290 |         REQUIRE(solver.setBooleanParameter("warm_start", true));
291 |     }
292 |     if (solver.getSolverName() == "proxqp")
293 |     {
294 |         REQUIRE(solver.setStringParameter("initial_guess", "WARM_START_WITH_PREVIOUS_RESULT"));
295 |     }
296 | 
297 |     // set the initial data of the QP solver
298 |     solver.data()->setNumberOfVariables(12 * (mpcWindow + 1) + 4 * mpcWindow);
299 |     solver.data()->setNumberOfInequalityConstraints(12 * (mpcWindow + 1) + 4 * mpcWindow);
300 |     solver.data()->setNumberOfEqualityConstraints(12 * (mpcWindow + 1));
301 |     REQUIRE(solver.data()->setHessianMatrix(hessian));
302 |     REQUIRE(solver.data()->setGradient(gradient));
303 |     REQUIRE(solver.data()->setInequalityConstraintsMatrix(linearInqeMatrix));
304 |     REQUIRE(solver.data()->setEqualityConstraintsMatrix(linearEqMatrix));
305 |     REQUIRE(solver.data()->setEqualityConstraintsVector(equalityVectorConstraints));
306 |     REQUIRE(solver.data()->setLowerBound(lowerBound));
307 |     REQUIRE(solver.data()->setUpperBound(upperBound));
308 | 
309 |     // instantiate the solver
310 |     REQUIRE(solver.initSolver());
311 | 
312 |     // controller input and QPSolution vector
313 |     Eigen::Matrix<double, 4, 1> ctr;
314 |     Eigen::Matrix<double, -1, 1> QPSolution;
315 | 
316 |     // number of iteration steps
317 |     int numberOfSteps = 50;
318 | 
319 |     // profiling quantities
320 |     clock_t startTime, endTime;
321 |     double averageTime = 0;
322 | 
323 |     for (int i = 0; i < numberOfSteps; i++)
324 |     {
325 |         startTime = clock();
326 | 
327 |         // solve the QP problem
328 |         REQUIRE(solver.solveProblem() == QpSolversEigen::ErrorExitFlag::NoError);
329 | 
330 |         // get the controller input
331 |         QPSolution = solver.getSolution();
332 |         ctr = QPSolution.block(12 * (mpcWindow + 1), 0, 4, 1);
333 | 
334 |         // save data into file
335 |         auto x0Data = x0.data();
336 |         for (int j = 0; j < 12; j++)
337 |             dataStream << x0Data[j] << " ";
338 |         dataStream << std::endl;
339 | 
340 |         // propagate the model
341 |         x0 = a * x0 + b * ctr;
342 | 
343 |         // update the constraint bound
344 |         updateLinearConstraintVectors(x0, equalityVectorConstraints);
345 |         REQUIRE(solver.updateEqualityConstraintsVector(equalityVectorConstraints));
346 | 
347 |         endTime = clock();
348 | 
349 |         averageTime += static_cast<double>(endTime - startTime) / CLOCKS_PER_SEC;
350 |     }
351 | 
352 |     // close the stream
353 |     dataStream.close();
354 | 
355 |     std::cout << COUT_GTEST_MGT << "Average time = " << averageTime / numberOfSteps << " seconds."
356 |               << ANSI_TXT_DFT << std::endl;
357 | 
358 |     constexpr double tolerance = 1e-2;
359 |     REQUIRE(getErrorNorm(x0, xRef) <= tolerance);
360 | }
361 | 


--------------------------------------------------------------------------------
/core/QpSolversEigen/Solver.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef QPSOLVERSEIGEN_SOLVER_HPP
  2 | #define QPSOLVERSEIGEN_SOLVER_HPP
  3 | 
  4 | // Std
  5 | #include <memory>
  6 | #include <string>
  7 | 
  8 | // Eigen
  9 | #include <Eigen/Dense>
 10 | #include <Eigen/Sparse>
 11 | 
 12 | // QpSolversEigen
 13 | #include <QpSolversEigen/Constants.hpp>
 14 | #include <QpSolversEigen/SolverInterface.hpp>
 15 | 
 16 | namespace QpSolversEigen
 17 | {
 18 | /**
 19 |  * Solver class is the main class used to wrap the QP solvers.
 20 |  *
 21 |  * The class is used to both instantiate the solver and then interact with it.
 22 |  */
 23 | class Solver
 24 | {
 25 | public:
 26 |     /**
 27 |      * Constructor.
 28 |      */
 29 |     Solver();
 30 | 
 31 |     /**
 32 |      * Destructor. It is required by the pimpl implementation.
 33 |      */
 34 |     ~Solver();
 35 | 
 36 |     /**
 37 |      * Instantiate the solver. This is compulsory before calling any other method.
 38 |      * @param[in] solverName is the name of the solver to be used (default: osqp)
 39 |      * @return true/false in case of success/failure.
 40 |      */
 41 |     bool instantiateSolver(std::string solverName = "osqp");
 42 | 
 43 |     /**
 44 |      * Get the name of the solver.
 45 |      *
 46 |      * @return the value passed to the instantiateSolver method if the instantiateSolver was successful, or "null" otherwise.
 47 |      */
 48 |     std::string getSolverName() const;
 49 | 
 50 |     /**
 51 |      * Initialize the solver with the actual initial data and settings.
 52 |      * @return true/false in case of success/failure.
 53 |      */
 54 |     bool initSolver();
 55 | 
 56 |     /**
 57 |      * Check if the solver is initialized.
 58 |      * @return true if the solver is initialized.
 59 |      */
 60 |     bool isInitialized();
 61 | 
 62 |     /**
 63 |      * Deallocate memory.
 64 |      */
 65 |     void clearSolver();
 66 | 
 67 |     /**
 68 |      * Set to zero all the solver variables.
 69 |      * @return true/false in case of success/failure.
 70 |      */
 71 |     bool clearSolverVariables();
 72 | 
 73 |     /**
 74 |      * Solve the QP optimization problem.
 75 |      * @return the error exit flag
 76 |      */
 77 |     QpSolversEigen::ErrorExitFlag solveProblem();
 78 | 
 79 |     /**
 80 |      * Get the status of the solver
 81 |      * @return The inner solver status
 82 |      */
 83 |     QpSolversEigen::Status getStatus() const;
 84 | 
 85 |     /**
 86 |      * Get the primal objective value
 87 |      * @return The primal objective value
 88 |      */
 89 |     const double getObjValue() const;
 90 | 
 91 |     /**
 92 |      * Get the optimization problem solution.
 93 |      * @return an Eigen::Vector containing the optimization result.
 94 |      */
 95 |     const Eigen::Matrix<double, -1, 1>& getSolution();
 96 | 
 97 |     /**
 98 |      * Get the dual optimization problem solution.
 99 |      * @return an Eigen::Vector containing the optimization result.
100 |      */
101 |     const Eigen::Matrix<double, -1, 1>& getDualSolution();
102 | 
103 | 
104 |     /**
105 |      * Update the quadratic part of the cost function (Hessian).
106 |      * It is assumed to be a simmetric matrix.
107 |      * \note
108 |      * If the sparsity pattern is preserved the matrix is simply update
109 |      * otherwise the entire solver will be reinitialized. In this case
110 |      * the primal and dual variable are copied in the new workspace.
111 |      *
112 |      * @param hessian is the Hessian matrix.
113 |      * @return true/false in case of success/failure.
114 |      */
115 |     bool updateHessianMatrix(const Eigen::SparseMatrix<double> &hessianMatrix);
116 | 
117 |     /**
118 |      * Update the linear constraints matrix (A)
119 |      * \note
120 |      * If the sparsity pattern is preserved the matrix is simply update
121 |      * otherwise the entire solver will be reinitialized. In this case
122 |      * the primal and dual variable are copied in the new workspace.
123 |      *
124 |      * @param linearConstraintsMatrix is the linear constraint matrix A
125 |      * @return true/false in case of success/failure.
126 |      */
127 |     [[deprecated("Use updateInequalityConstraintsMatrix() instead.")]]
128 |     bool updateLinearConstraintsMatrix(const Eigen::SparseMatrix<double> &linearConstraintsMatrix);
129 | 
130 |     /**
131 |      * Update the linear constraints matrix (A)
132 |      * \note
133 |      * If the sparsity pattern is preserved the matrix is simply update
134 |      * otherwise the entire solver will be reinitialized. In this case
135 |      * the primal and dual variable are copied in the new workspace.
136 |      *
137 |      * @param linearConstraintsMatrix is the linear constraint matrix A
138 |      * @return true/false in case of success/failure.
139 |      */
140 |     bool updateInequalityConstraintsMatrix(const Eigen::SparseMatrix<double> &linearConstraintsMatrix);
141 | 
142 |     /**
143 |      * Update the linear part of the cost function (Gradient).
144 |      * @param gradient is the Gradient vector.
145 |      * @note the elements of the gradient are not copied inside the library.
146 |      * The user has to guarantee that the lifetime of the objects passed is the same of the
147 |      * OsqpEigen object.
148 |      * @return true/false in case of success/failure.
149 |      */
150 |     bool
151 |     updateGradient(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& gradient);
152 | 
153 |     /**
154 |      * Update the lower bounds limit (size m).
155 |      * @param lowerBound is the lower bound constraint vector.
156 |      * @note the elements of the lowerBound are not copied inside the library.
157 |      * The user has to guarantee that the lifetime of the object passed is the same of the
158 |      * OsqpEigen object.
159 |      * @return true/false in case of success/failure.
160 |      */
161 |     bool
162 |     updateLowerBound(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& lowerBound);
163 | 
164 |     /**
165 |      * Update the upper bounds limit (size m).
166 |      * @param upperBound is the upper bound constraint vector.
167 |      * @note the elements of the upperBound are not copied inside the library.
168 |      * The user has to guarantee that the lifetime of the object passed is the same of the
169 |      * OsqpEigen object.
170 |      * @return true/false in case of success/failure.
171 |      */
172 |     bool
173 |     updateUpperBound(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& upperBound);
174 | 
175 |     /**
176 |      * Update both upper and lower bounds (size m).
177 |      * @param lowerBound is the lower bound constraint vector;
178 |      * @param upperBound is the upper bound constraint vector.
179 |      * @note the elements of the lowerBound and upperBound are not copied inside the library.
180 |      * The user has to guarantee that the lifetime of the objects passed is the same of the
181 |      * OsqpEigen object
182 |      * @return true/false in case of success/failure.
183 |      */
184 |     bool
185 |     updateBounds(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& lowerBound,
186 |                  const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& upperBound);
187 | 
188 |     bool updateEqualityConstraintsMatrix(const Eigen::SparseMatrix<double>& equalityConstraintsMatrix);
189 | 
190 |     bool updateEqualityConstraintsVector(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& equalityConstraintsVector);
191 | 
192 |     /**
193 |      * Clear the hessian matrix.
194 |      */
195 |     void clearHessianMatrix();
196 | 
197 |     /**
198 |      * Clear the linear constraints matrix.
199 |      */
200 |     void clearLinearConstraintsMatrix();
201 | 
202 |     /**
203 |      * Set the number of variables.
204 |      * @param n is the number of variables.
205 |      */
206 |     void setNumberOfVariables(int n);
207 | 
208 |     /**
209 |      * Set the number of inequality constraints.
210 |      * @param m is the number of inequality constraints.
211 |      */
212 |     [[deprecated("Use setNumberOfInequalityConstraints() instead.")]]
213 |     void setNumberOfConstraints(int m);
214 | 
215 |     /**
216 |      * Set the number of inequality constraints.
217 |      * @param m is the number of inequality constraints.
218 |      */
219 |     void setNumberOfInequalityConstraints(int m);
220 | 
221 |     /**
222 |      * Set the number of equality constraints.
223 |      * @param m is the number of equality constraints.
224 |      */
225 |     void setNumberOfEqualityConstraints(int m);
226 | 
227 |     /**
228 |      * Set the quadratic part of the cost function (Hessian).
229 |      * It is assumed to be a symmetric matrix.
230 |      * @param hessianMatrix is the Hessian matrix.
231 |      * @return true/false in case of success/failure.
232 |      */
233 |     bool setHessianMatrix(const Eigen::SparseMatrix<double>& hessianMatrix);
234 | 
235 |     /**
236 |      * Set the linear part of the cost function (Gradient).
237 |      * @param gradientVector is the Gradient vector.
238 |      * @note the elements of the gradient are not copied inside the library.
239 |      * The user has to guarantee that the lifetime of the object passed is the same of the
240 |      * OsqpEigen object
241 |      * @return true/false in case of success/failure.
242 |      */
243 |     bool setGradient(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> gradientVector);
244 | 
245 |     Eigen::Matrix<double, Eigen::Dynamic, 1> getGradient();
246 | 
247 |     /**
248 |      * Set the linear constraint matrix A (size m x n)
249 |      * @param inequalityConstraintsMatrix is the linear constraints matrix A.
250 |      * @return true/false in case of success/failure.
251 |      */
252 |     [[deprecated("Use setInequalityConstraintsMatrix() instead.")]]
253 |     bool
254 |     setLinearConstraintsMatrix(const Eigen::SparseMatrix<double>& inequalityConstraintsMatrix);
255 | 
256 |     /**
257 |      * Set the linear constraints matrix A (size m x n)
258 |      * @param inequalityConstraintsMatrix is the linear constraints matrix A.
259 |      * @return true/false in case of success/failure.
260 |      */
261 |     bool
262 |     setInequalityConstraintsMatrix(const Eigen::SparseMatrix<double>& inequalityConstraintsMatrix);
263 | 
264 |     /**
265 |      * Set the array for lower bound (size m).
266 |      * @param lowerBoundVector is the lower bound constraint.
267 |      * @note the elements of the lowerBoundVector are not copied inside the library.
268 |      * The user has to guarantee that the lifetime of the object passed is the same of the
269 |      * OsqpEigen object
270 |      * @return true/false in case of success/failure.
271 |      */
272 |     bool setLowerBound(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> lowerBoundVector);
273 | 
274 |     /**
275 |      * Set the array for upper bound (size m).
276 |      * @param upperBoundVector is the upper bound constraint.
277 |      * @note the elements of the upperBoundVector are not copied inside the library.
278 |      * The user has to guarantee that the lifetime of the object passed is the same of the
279 |      * OsqpEigen object.
280 |      * @return true/false in case of success/failure.
281 |      */
282 |     bool setUpperBound(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> upperBoundVector);
283 | 
284 |     /**
285 |      * Set the array for upper and lower bounds (size m).
286 |      * @param lowerBound is the lower bound constraint.
287 |      * @param upperBound is the upper bound constraint.
288 |      * @note the elements of the upperBound and lowerBound are not copied inside the library.
289 |      * The user has to guarantee that the lifetime of the object passed is the same of the
290 |      * OsqpEigen object.
291 |      * @return true/false in case of success/failure.
292 |      */
293 |     bool setBounds(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> lowerBound,
294 |                    Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> upperBound);
295 | 
296 |     /**
297 |      * Set the equality constraints matrix (size p x n).
298 |      * @param equalityConstraintsMatrix is the equality constraints matrix A.
299 |      * @return true/false in case of success/failure.
300 |      */
301 |     bool setEqualityConstraintsMatrix(const Eigen::SparseMatrix<double>& equalityConstraintsMatrix);
302 | 
303 |     /**
304 |      * Set the equality constraints vector (size p x 1).
305 |      * @param equalityConstraintsVector is the equality constraints vector.
306 |      * @return true/false in case of success/failure.
307 |      */
308 |     bool setEqualityConstraintsVector(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& equalityConstraintsVector);
309 | 
310 |     /**
311 |      * Set a boolean parameter.
312 |      * @param parameterName the name of the parameter to bet set.
313 |      * @return true/false in case of success/failure.
314 |      */
315 |     bool setBooleanParameter(const std::string& parameterName, bool value);
316 | 
317 |     /**
318 |      * Set a integer parameter.
319 |      * @param parameterName the name of the parameter to bet set.
320 |      * @return true/false in case of success/failure.
321 |      */
322 |     bool setIntegerParameter(const std::string& parameterName, int64_t value);
323 | 
324 |     /**
325 |      * Set a real number parameter.
326 |      * @param parameterName the name of the parameter to bet set.
327 |      * @return true/false in case of success/failure.
328 |      */
329 |     bool setRealNumberParameter(const std::string& parameterName, double value);
330 | 
331 |     /**
332 |      * Set a string parameter.
333 |      *
334 |      * @note Sometimes Enum parameters in a specific solvers are wrapped as a string parameter.
335 |      *
336 |      * @param parameterName the name of the parameter to bet set.
337 |      * @return true/false in case of success/failure.
338 |      */
339 |     bool setStringParameter(const std::string& parameterName, const std::string& value);
340 | 
341 |     /**
342 |      * Get the names of all the boolean parameters supported by the solver.
343 |      * @param parametersNames the names of all the boolean parameters supported by the solver.
344 |      * @return true/false in case of success/failure.
345 |      */
346 |     bool getBooleanParametersNames(std::vector<std::string>& parametersNames) const;
347 | 
348 |     /**
349 |      * Get the names of all the integer parameters supported by the solver.
350 |      * @param parametersNames the names of all the integer parameters supported by the solver.
351 |      * @return true/false in case of success/failure.
352 |      */
353 |     bool getIntegerParametersNames(std::vector<std::string>& parameterNames) const;
354 | 
355 |     /**
356 |      * Get the names of all the real number parameters supported by the solver.
357 |      * @param parametersNames the names of all the real number parameters supported by the solver.
358 |      * @return true/false in case of success/failure.
359 |      */
360 |     bool getRealNumberParametersNames(std::vector<std::string>& parametersNames) const;
361 | 
362 |     /**
363 |      * Get the names of all the string parameters supported by the solver.
364 |      * @param parametersNames the names of all the string parameters supported by the solver.
365 |      * @return true/false in case of success/failure.
366 |      */
367 |     bool getStringParametersNames(std::vector<std::string>& parametersNames) const ;
368 | 
369 | 
370 |     /**
371 |      * Return a pointer to this class.
372 |      *
373 |      * This is just for backward compatibility with OsqpEigen::Solver::data() method,
374 |      * please do not use in QpSolversEigen-specific code.
375 |      */
376 |     Solver* data();
377 | 
378 | private:
379 |     /** Private implementation */
380 |     struct Impl;
381 |     std::unique_ptr<Impl> m_pimpl;
382 | };
383 | 
384 | }
385 | 
386 | #endif
387 | 


--------------------------------------------------------------------------------
/plugins/osqp/QpSolversEigenOsqp.cpp:
--------------------------------------------------------------------------------
  1 | // QpSolversEigen 
  2 | #include <QpSolversEigen/SolverInterface.hpp>
  3 | #include <QpSolversEigen/Debug.hpp>
  4 | 
  5 | // Class factory API
  6 | #include <sharedlibpp/SharedLibraryClassApi.h>
  7 | 
  8 | // OsqpEigen
  9 | #include <OsqpEigen/OsqpEigen.h>
 10 | 
 11 | namespace QpSolversEigen
 12 | {
 13 | 
 14 | class OsqpSolver final: public SolverInterface
 15 | {
 16 | private:
 17 |     // OSQP solver
 18 |     OsqpEigen::Solver osqpEigenSolver;
 19 | 
 20 |     // Helper class to convert OsqpEigen::ErrorExitFlag to QpSolversEigen::ErrorExitFlag
 21 |     QpSolversEigen::ErrorExitFlag convertErrorExitFlag(const OsqpEigen::ErrorExitFlag errorExitFlag) const;
 22 | 
 23 |     // Helper class to convert OsqpEigen::Status to QpSolversEigen::Status
 24 |     QpSolversEigen::Status convertStatus(const OsqpEigen::Status status) const;
 25 | 
 26 |     // Helper class to convert to convert QpSolversEigen::INFTY to OsqpEigen::INFTY in bounds
 27 |     bool convertQpSolversEigenInftyToOsqpEigenInfty(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& inputBound,
 28 |                                                     Eigen::VectorXd& outputBound);
 29 | 
 30 |     // Buffers used to store the bound data with infinity values compatible with OsqpEigen
 31 |     Eigen::VectorXd lowerBoundBufferWithOsqpEigenInfty;
 32 |     Eigen::VectorXd upperBoundBufferWithOsqpEigenInfty;
 33 |     int numberOfEqualityConstraints = 0;
 34 |     int numberOfInequalityConstraints = 0;
 35 |     int numberOfVariables = 0;
 36 |     Eigen::SparseMatrix<double> totalConstraintsMatrix;
 37 |     Eigen::VectorXd totalLowerConstraintsVector;
 38 |     Eigen::VectorXd totalUpperConstraintsVector;
 39 |     bool updateConstraintMatrix = false;
 40 |     bool updateConstraintVectors = false;
 41 | 
 42 | public:
 43 |     virtual ~OsqpSolver() = default;
 44 | 
 45 |     std::string getSolverName() const override;
 46 |     bool initSolver() override;
 47 |     bool isInitialized() override;
 48 |     void clearSolver() override;
 49 |     bool clearSolverVariables() override;
 50 |     QpSolversEigen::ErrorExitFlag solveProblem() override;
 51 |     QpSolversEigen::Status getStatus() const override;
 52 |     const double getObjValue() const override;
 53 |     const Eigen::Matrix<double, Eigen::Dynamic, 1>& getSolution() override;
 54 |     const Eigen::Matrix<double, Eigen::Dynamic, 1>& getDualSolution() override;
 55 |     bool updateHessianMatrix(const Eigen::SparseMatrix<double> &hessianMatrix) override;
 56 |     bool updateInequalityConstraintsMatrix(const Eigen::SparseMatrix<double> &linearConstraintsMatrix) override;
 57 |     bool updateGradient(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& gradient) override;
 58 |     bool updateLowerBound(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& lowerBound) override;
 59 |     bool updateUpperBound(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& upperBound) override;
 60 |     bool updateBounds(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& lowerBound,
 61 |                  const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& upperBound) override;
 62 |     bool updateEqualityConstraintsMatrix(const Eigen::SparseMatrix<double>& equalityConstraintsMatrix) override;
 63 |     bool updateEqualityConstraintsVector(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& equalityConstraintsVector) override;
 64 | 
 65 |     void clearHessianMatrix() override;
 66 |     void clearLinearConstraintsMatrix() override;
 67 |     void setNumberOfVariables(int n) override;
 68 |     void setNumberOfInequalityConstraints(int m) override;
 69 |     void setNumberOfEqualityConstraints(int m) override;
 70 |     bool setHessianMatrix(const Eigen::SparseMatrix<double>& hessianMatrix) override;
 71 |     bool setGradient(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> gradientVector) override;
 72 | 
 73 |     Eigen::Matrix<double, Eigen::Dynamic, 1> getGradient() override;
 74 | 
 75 |     bool
 76 |     setInequalityConstraintsMatrix(const Eigen::SparseMatrix<double>& linearConstraintsMatrix) override;
 77 |     bool setLowerBound(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> lowerBoundVector) override;
 78 |     bool setUpperBound(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> upperBoundVector) override;
 79 |     bool setBounds(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> lowerBound,
 80 |                    Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> upperBound) override;
 81 |     bool setEqualityConstraintsMatrix(const Eigen::SparseMatrix<double>& equalityConstraintsMatrix) override;
 82 |     bool setEqualityConstraintsVector(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& equalityConstraintsVector) override;
 83 | 
 84 |     bool setBooleanParameter(const std::string& settingName, bool value) override;
 85 |     bool setIntegerParameter(const std::string& settingName, int64_t value) override;
 86 |     bool setRealNumberParameter(const std::string& settingName, double value) override;
 87 |     bool setStringParameter(const std::string& parameterName, const std::string& value) override;
 88 | 
 89 |     bool getBooleanParametersNames(std::vector<std::string>& parametersNames) const override;
 90 |     bool getIntegerParametersNames(std::vector<std::string>& parameterNames) const override;
 91 |     bool getRealNumberParametersNames(std::vector<std::string>& parametersNames) const override;
 92 |     bool getStringParametersNames(std::vector<std::string>& parametersNames) const override;
 93 | 
 94 |     SolverInterface* allocateInstance() const override;
 95 | };
 96 | 
 97 | // The first argument needs to be coherent with the scheme used in
 98 | // getShlibppFactoryNameFromSolverName, i.e. qpsolvers_eigen_<solverName>
 99 | SHLIBPP_DEFINE_SHARED_SUBCLASS(qpsolvers_eigen_osqp, QpSolversEigen::OsqpSolver, QpSolversEigen::SolverInterface);
100 | 
101 | QpSolversEigen::ErrorExitFlag OsqpSolver::convertErrorExitFlag(const OsqpEigen::ErrorExitFlag errorExitFlag) const
102 | {
103 |     switch (errorExitFlag)
104 |     {
105 |     case OsqpEigen::ErrorExitFlag::NoError:
106 |         return QpSolversEigen::ErrorExitFlag::NoError;
107 |     case OsqpEigen::ErrorExitFlag::DataValidationError:
108 |         return QpSolversEigen::ErrorExitFlag::DataValidationError;
109 |     case OsqpEigen::ErrorExitFlag::SettingsValidationError:
110 |         return QpSolversEigen::ErrorExitFlag::SettingsValidationError;
111 |     case OsqpEigen::ErrorExitFlag::LinsysSolverLoadError:
112 |         return QpSolversEigen::ErrorExitFlag::LinsysSolverLoadError;
113 |     case OsqpEigen::ErrorExitFlag::LinsysSolverInitError:
114 |         return QpSolversEigen::ErrorExitFlag::LinsysSolverInitError;
115 |     case OsqpEigen::ErrorExitFlag::NonCvxError:
116 |         return QpSolversEigen::ErrorExitFlag::NonCvxError;
117 |     case OsqpEigen::ErrorExitFlag::MemAllocError:
118 |         return QpSolversEigen::ErrorExitFlag::MemAllocError;
119 |     case OsqpEigen::ErrorExitFlag::WorkspaceNotInitError:
120 |         return QpSolversEigen::ErrorExitFlag::WorkspaceNotInitError;
121 |     default:
122 |         return QpSolversEigen::ErrorExitFlag::SolverSpecificUnknownError;
123 |     }
124 | }
125 | 
126 | QpSolversEigen::Status OsqpSolver::convertStatus(const OsqpEigen::Status status) const
127 | {
128 |     switch (status)
129 |     {
130 |     case OsqpEigen::Status::Solved:
131 |         return QpSolversEigen::Status::Solved;
132 |     case OsqpEigen::Status::SolvedInaccurate:
133 |         return QpSolversEigen::Status::SolvedInaccurate;
134 |     case OsqpEigen::Status::PrimalInfeasible:
135 |         return QpSolversEigen::Status::PrimalInfeasible;
136 |     case OsqpEigen::Status::PrimalInfeasibleInaccurate:
137 |         return QpSolversEigen::Status::PrimalInfeasibleInaccurate;
138 |     case OsqpEigen::Status::DualInfeasible:
139 |         return QpSolversEigen::Status::DualInfeasible;
140 |     case OsqpEigen::Status::DualInfeasibleInaccurate:
141 |         return QpSolversEigen::Status::DualInfeasibleInaccurate;
142 |     case OsqpEigen::Status::MaxIterReached:
143 |         return QpSolversEigen::Status::MaxIterReached;
144 |     case OsqpEigen::Status::TimeLimitReached:
145 |         return QpSolversEigen::Status::TimeLimitReached;
146 |     case OsqpEigen::Status::NonCvx:
147 |         return QpSolversEigen::Status::NonCvx;
148 |     case OsqpEigen::Status::Sigint:
149 |         return QpSolversEigen::Status::Sigint;
150 |     case OsqpEigen::Status::Unsolved:
151 |         return QpSolversEigen::Status::Unsolved;
152 |     default:
153 |         return QpSolversEigen::Status::SolverSpecificUnknownStatus;
154 |     }
155 | }
156 | 
157 | bool OsqpSolver::convertQpSolversEigenInftyToOsqpEigenInfty(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& inputBound,
158 |                                                             Eigen::VectorXd& outputBound)
159 | {
160 |     outputBound = inputBound;
161 |     for (size_t i = 0; i < inputBound.size(); i++)
162 |     {
163 |         if (inputBound[i] == QpSolversEigen::INFTY)
164 |         {
165 |             outputBound[i] = OsqpEigen::INFTY;
166 |         }
167 | 
168 |         if (inputBound[i] == -QpSolversEigen::INFTY)
169 |         {
170 |             outputBound[i] = -OsqpEigen::INFTY;
171 |         }
172 |     }
173 |     return true;
174 | }
175 | 
176 | std::string OsqpSolver::getSolverName() const
177 | {
178 |     return "osqp";
179 | }
180 | 
181 | bool OsqpSolver::initSolver()
182 | {
183 |     osqpEigenSolver.data()->setLinearConstraintsMatrix(totalConstraintsMatrix);
184 |     return osqpEigenSolver.initSolver();
185 | }
186 | 
187 | bool OsqpSolver::isInitialized()
188 | {
189 |     return osqpEigenSolver.isInitialized();
190 | }
191 | 
192 | void OsqpSolver::clearSolver()
193 | {
194 |     return osqpEigenSolver.clearSolver();
195 | }
196 | 
197 | bool OsqpSolver::clearSolverVariables()
198 | {
199 |     return osqpEigenSolver.clearSolverVariables();
200 | }
201 | 
202 | QpSolversEigen::ErrorExitFlag OsqpSolver::solveProblem()
203 | {
204 |     if (updateConstraintMatrix)
205 |     {
206 |         if(!osqpEigenSolver.updateLinearConstraintsMatrix(totalConstraintsMatrix))
207 |         {
208 |             QpSolversEigen::debugStream() << "QpSolversEigen::OsqpSolver::solveProblem: Error setting linear constraints matrix." << std::endl;
209 |             return QpSolversEigen::ErrorExitFlag::DataValidationError;
210 |         }
211 |         updateConstraintMatrix = false;
212 |     }
213 |     if (updateConstraintVectors)
214 |     {
215 |         if(!osqpEigenSolver.updateBounds(totalLowerConstraintsVector, totalUpperConstraintsVector))
216 |         {
217 |             QpSolversEigen::debugStream() << "QpSolversEigen::OsqpSolver::solveProblem: Error setting bounds." << std::endl;
218 |             return QpSolversEigen::ErrorExitFlag::DataValidationError;
219 |         }
220 |         updateConstraintVectors = false;
221 |     }
222 |     return this->convertErrorExitFlag(osqpEigenSolver.solveProblem());
223 | }
224 | 
225 | QpSolversEigen::Status OsqpSolver::getStatus() const
226 | {
227 |     return this->convertStatus(osqpEigenSolver.getStatus());
228 | }
229 | 
230 | const double OsqpSolver::getObjValue() const
231 | {
232 |     return osqpEigenSolver.getObjValue();
233 | }
234 | 
235 | const Eigen::Matrix<double, Eigen::Dynamic, 1>& OsqpSolver::getSolution()
236 | {
237 |     return osqpEigenSolver.getSolution();
238 | }
239 | 
240 | const Eigen::Matrix<double, Eigen::Dynamic, 1>& OsqpSolver::getDualSolution()
241 | {
242 |     return osqpEigenSolver.getDualSolution();
243 | }
244 | 
245 | bool OsqpSolver::updateHessianMatrix(const Eigen::SparseMatrix<double> &hessianMatrix)
246 | {
247 |     return osqpEigenSolver.updateHessianMatrix(hessianMatrix);
248 | }
249 | 
250 | bool OsqpSolver::updateInequalityConstraintsMatrix(const Eigen::SparseMatrix<double> &linearConstraintsMatrix)
251 | {
252 |     if (linearConstraintsMatrix.rows() != numberOfInequalityConstraints)
253 |     {
254 |         QpSolversEigen::debugStream() << "QpSolversEigen::OsqpSolver::updateLinearConstraintsMatrix: number of rows in linear constraints matrix does not match the number of inequality constraints." << std::endl;
255 |         return false;
256 |     }
257 |     for (int k=0; k < linearConstraintsMatrix.outerSize(); ++k) {
258 |         for (Eigen::SparseMatrix<double>::InnerIterator it(linearConstraintsMatrix,k); it; ++it) {
259 |             totalConstraintsMatrix.coeffRef(it.row(), it.col()) = it.value();
260 |         }
261 |     }
262 |     updateConstraintMatrix = true;
263 |     return true;
264 | }
265 | 
266 | bool OsqpSolver::updateGradient(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& gradient)
267 | {
268 |     return osqpEigenSolver.updateGradient(gradient);
269 | }
270 | 
271 | bool OsqpSolver::updateLowerBound(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& lowerBound)
272 | {
273 |     if (lowerBound.size() != numberOfInequalityConstraints)
274 |     {
275 |         QpSolversEigen::debugStream() << "QpSolversEigen::OsqpSolver::updateLowerBound: size of lower bound vector does not match the number of inequality constraints." << std::endl;
276 |         return false;
277 |     }
278 |     bool ok = convertQpSolversEigenInftyToOsqpEigenInfty(lowerBound, lowerBoundBufferWithOsqpEigenInfty);
279 |     totalLowerConstraintsVector.head(numberOfInequalityConstraints) = lowerBoundBufferWithOsqpEigenInfty;
280 |     updateConstraintVectors = true;
281 |     return true;
282 | }
283 | 
284 | bool OsqpSolver::updateUpperBound(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& upperBound)
285 | {
286 |     bool ok = convertQpSolversEigenInftyToOsqpEigenInfty(upperBound, upperBoundBufferWithOsqpEigenInfty);
287 |     totalUpperConstraintsVector.head(numberOfInequalityConstraints) = upperBoundBufferWithOsqpEigenInfty;
288 |     updateConstraintVectors = true;
289 |     return true;
290 | }
291 | 
292 | bool OsqpSolver::updateBounds(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& lowerBound,
293 |              const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& upperBound)
294 | {
295 |     if (lowerBound.size() != numberOfInequalityConstraints || upperBound.size() != numberOfInequalityConstraints)
296 |     {
297 |         QpSolversEigen::debugStream() << "QpSolversEigen::OsqpSolver::updateBounds: size of lower or upper bound vector does not match the number of inequality constraints." << std::endl;
298 |         return false;
299 |     }
300 |     bool ok = convertQpSolversEigenInftyToOsqpEigenInfty(lowerBound, lowerBoundBufferWithOsqpEigenInfty);
301 |     ok = ok &&  convertQpSolversEigenInftyToOsqpEigenInfty(upperBound, upperBoundBufferWithOsqpEigenInfty);
302 |     totalLowerConstraintsVector.head(numberOfInequalityConstraints) = lowerBoundBufferWithOsqpEigenInfty;
303 |     totalUpperConstraintsVector.head(numberOfInequalityConstraints) = upperBoundBufferWithOsqpEigenInfty;
304 |     updateConstraintVectors = true;
305 |     return true;
306 | }
307 | 
308 | bool OsqpSolver::updateEqualityConstraintsMatrix(const Eigen::SparseMatrix<double>& equalityConstraintsMatrix)
309 | {
310 |     if (equalityConstraintsMatrix.rows() != numberOfEqualityConstraints)
311 |     {
312 |         QpSolversEigen::debugStream() << "QpSolversEigen::OsqpSolver::updateEqualityConstraintsMatrix: number of rows in equality constraints matrix does not match the number of equality constraints." << std::endl;
313 |         return false;
314 |     }
315 |     const int startRow = numberOfInequalityConstraints;
316 |     for (int k=0; k < equalityConstraintsMatrix.outerSize(); ++k) {
317 |         for (Eigen::SparseMatrix<double>::InnerIterator it(equalityConstraintsMatrix,k); it; ++it) {
318 |             totalConstraintsMatrix.coeffRef(it.row() + startRow, it.col()) = it.value();
319 |         }
320 |     }
321 |     updateConstraintMatrix = true;
322 |     return true;
323 | }
324 | 
325 | bool OsqpSolver::updateEqualityConstraintsVector(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& equalityConstraintsVector)
326 | {
327 |     if (equalityConstraintsVector.size() != numberOfEqualityConstraints)
328 |     {
329 |         QpSolversEigen::debugStream() << "QpSolversEigen::OsqpSolver::updateEqualityConstraintsVector: size of equality constraints vector does not match the number of equality constraints." << std::endl;
330 |         return false;
331 |     }
332 |     bool ok = convertQpSolversEigenInftyToOsqpEigenInfty(equalityConstraintsVector, lowerBoundBufferWithOsqpEigenInfty);
333 |     totalLowerConstraintsVector.tail(numberOfEqualityConstraints) = lowerBoundBufferWithOsqpEigenInfty;
334 |     totalUpperConstraintsVector.tail(numberOfEqualityConstraints) = lowerBoundBufferWithOsqpEigenInfty;
335 |     updateConstraintVectors = true;
336 |     return true;
337 | }
338 | 
339 | void OsqpSolver::clearHessianMatrix()
340 | {
341 |     return osqpEigenSolver.data()->clearHessianMatrix();
342 | }
343 | 
344 | void OsqpSolver::clearLinearConstraintsMatrix()
345 | {
346 |     return osqpEigenSolver.data()->clearLinearConstraintsMatrix();
347 | }
348 | 
349 | void OsqpSolver::setNumberOfVariables(int n)
350 | {
351 |     numberOfVariables = n;
352 |     return osqpEigenSolver.data()->setNumberOfVariables(n);
353 | }
354 | 
355 | void OsqpSolver::setNumberOfInequalityConstraints(int m)
356 | {
357 |     if (numberOfVariables == 0)
358 |     {
359 |         QpSolversEigen::debugStream() << "QpSolversEigen::OsqpSolver::setNumberOfInequalityConstraints: number of variables is not set, cannot set number of constraints." << std::endl;
360 |         return;
361 |     }
362 |     numberOfInequalityConstraints = m;
363 |     totalConstraintsMatrix.resize(numberOfInequalityConstraints + numberOfEqualityConstraints, numberOfVariables);
364 |     totalLowerConstraintsVector.resize(numberOfInequalityConstraints + numberOfEqualityConstraints);
365 |     totalUpperConstraintsVector.resize(numberOfInequalityConstraints + numberOfEqualityConstraints);
366 |     return osqpEigenSolver.data()->setNumberOfConstraints(numberOfInequalityConstraints + numberOfEqualityConstraints);
367 | }
368 | 
369 | void OsqpSolver::setNumberOfEqualityConstraints(int m)
370 | {
371 |     numberOfEqualityConstraints = m;
372 |     totalConstraintsMatrix.resize(numberOfInequalityConstraints + numberOfEqualityConstraints, numberOfVariables);
373 |     totalLowerConstraintsVector.resize(numberOfInequalityConstraints + numberOfEqualityConstraints);
374 |     totalUpperConstraintsVector.resize(numberOfInequalityConstraints + numberOfEqualityConstraints);
375 |     return osqpEigenSolver.data()->setNumberOfConstraints(numberOfInequalityConstraints + numberOfEqualityConstraints);
376 | }
377 | 
378 | bool OsqpSolver::setHessianMatrix(const Eigen::SparseMatrix<double>& hessianMatrix)
379 | {
380 |     return osqpEigenSolver.data()->setHessianMatrix(hessianMatrix);
381 | }
382 | 
383 | bool OsqpSolver::setGradient(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> gradientVector)
384 | {
385 |     return  osqpEigenSolver.data()->setGradient(gradientVector);
386 | }
387 | 
388 | Eigen::Matrix<double, Eigen::Dynamic, 1> OsqpSolver::getGradient()
389 | {
390 |     return Eigen::Matrix<double, Eigen::Dynamic, 1>();
391 | }
392 | 
393 | bool
394 | OsqpSolver::setInequalityConstraintsMatrix(const Eigen::SparseMatrix<double>& linearConstraintsMatrix)
395 | {
396 |     if (linearConstraintsMatrix.rows() != numberOfInequalityConstraints)
397 |     {
398 |         QpSolversEigen::debugStream() << "QpSolversEigen::OsqpSolver::setLinearConstraintsMatrix: number of rows in linear constraints matrix does not match the number of inequality constraints." << std::endl;
399 |         return false;
400 |     }
401 |     for (int k=0; k < linearConstraintsMatrix.outerSize(); ++k) {
402 |         for (Eigen::SparseMatrix<double>::InnerIterator it(linearConstraintsMatrix,k); it; ++it) {
403 |             totalConstraintsMatrix.coeffRef(it.row(), it.col()) = it.value();
404 |         }
405 |     }
406 |     return true;
407 | }
408 | 
409 | bool OsqpSolver::setLowerBound(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> lowerBoundVector)
410 | {
411 |     if (lowerBoundVector.size() != numberOfInequalityConstraints)
412 |     {
413 |         QpSolversEigen::debugStream() << "QpSolversEigen::OsqpSolver::setLowerBound: size of lower bound vector does not match the number of inequality constraints." << std::endl;
414 |         return false;
415 |     }
416 |     bool ok = convertQpSolversEigenInftyToOsqpEigenInfty(lowerBoundVector, lowerBoundBufferWithOsqpEigenInfty);
417 |     totalLowerConstraintsVector.head(numberOfInequalityConstraints) = lowerBoundBufferWithOsqpEigenInfty;
418 |     return osqpEigenSolver.data()->setLowerBound(totalLowerConstraintsVector);
419 | }
420 | 
421 | bool OsqpSolver::setUpperBound(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> upperBoundVector)
422 | {
423 |     if (upperBoundVector.size() != numberOfInequalityConstraints)
424 |     {
425 |         QpSolversEigen::debugStream() << "QpSolversEigen::OsqpSolver::setUpperBound: size of upper bound vector does not match the number of inequality constraints." << std::endl;
426 |         return false;
427 |     }
428 |     bool ok = convertQpSolversEigenInftyToOsqpEigenInfty(upperBoundVector, upperBoundBufferWithOsqpEigenInfty);
429 |     totalUpperConstraintsVector.head(numberOfInequalityConstraints) = upperBoundBufferWithOsqpEigenInfty;
430 |     return osqpEigenSolver.data()->setUpperBound(totalUpperConstraintsVector);
431 | }
432 | 
433 | bool OsqpSolver::setBounds(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> lowerBound,
434 |                Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> upperBound)
435 | {
436 |     bool ok = convertQpSolversEigenInftyToOsqpEigenInfty(lowerBound, lowerBoundBufferWithOsqpEigenInfty);
437 |     ok = ok &&  convertQpSolversEigenInftyToOsqpEigenInfty(upperBound, upperBoundBufferWithOsqpEigenInfty);
438 |     return osqpEigenSolver.data()->setBounds(lowerBoundBufferWithOsqpEigenInfty, upperBoundBufferWithOsqpEigenInfty);
439 | }
440 | 
441 | bool OsqpSolver::setEqualityConstraintsMatrix(const Eigen::SparseMatrix<double>& equalityConstraintsMatrix)
442 | {
443 |     if (equalityConstraintsMatrix.rows() != numberOfEqualityConstraints)
444 |     {
445 |         QpSolversEigen::debugStream() << "QpSolversEigen::OsqpSolver::setEqualityConstraintsMatrix: number of rows in equality constraints matrix does not match the number of equality constraints." << std::endl;
446 |         return false;
447 |     }
448 |     const int startRow = numberOfInequalityConstraints;
449 |     for (int k=0; k < equalityConstraintsMatrix.outerSize(); ++k) {
450 |         for (Eigen::SparseMatrix<double>::InnerIterator it(equalityConstraintsMatrix,k); it; ++it) {
451 |             totalConstraintsMatrix.coeffRef(it.row() + startRow, it.col()) = it.value();
452 |         }
453 |     }
454 |     return true;
455 | }
456 | 
457 | bool OsqpSolver::setEqualityConstraintsVector(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& equalityConstraintsVector)
458 | {
459 |     if (equalityConstraintsVector.size() != numberOfEqualityConstraints)
460 |     {
461 |         QpSolversEigen::debugStream() << "QpSolversEigen::OsqpSolver::setEqualityConstraintsVector: size of equality constraints vector does not match the number of equality constraints." << std::endl;
462 |         return false;
463 |     }
464 |     totalLowerConstraintsVector.tail(numberOfEqualityConstraints) = equalityConstraintsVector;
465 |     totalUpperConstraintsVector.tail(numberOfEqualityConstraints) = equalityConstraintsVector;
466 |     return osqpEigenSolver.data()->setBounds(totalLowerConstraintsVector, totalUpperConstraintsVector);
467 | }
468 | 
469 | bool OsqpSolver::setBooleanParameter(const std::string& settingName, bool value)
470 | {
471 |     // If you edit this method, remember to update
472 |     // the documentation in the README of the osqp plugin
473 |     if (settingName == "polish")
474 |     {
475 |         osqpEigenSolver.settings()->setPolish(value);
476 |         return true;
477 |     } else if (settingName == "verbose")
478 |     {
479 |         osqpEigenSolver.settings()->setVerbosity(value);
480 |         return true;
481 |     } else if (settingName == "scaled_termination")
482 |     {
483 |         osqpEigenSolver.settings()->setScaledTerimination(value);
484 |         return true;
485 |     } else if (settingName == "warm_start")
486 |     {
487 |         osqpEigenSolver.settings()->setWarmStart(value);
488 |         return true;
489 |     } else if (settingName == "warm_starting")
490 |     {
491 |         osqpEigenSolver.settings()->setWarmStart(value);
492 |         return true;
493 |     } else if (settingName == "adaptive_rho")
494 |     {
495 |         osqpEigenSolver.settings()->setAdaptiveRho(value);
496 |         return true;
497 |     }
498 | 
499 |     QpSolversEigen::debugStream() << "QpSolversEigen::OsqpSolver::setBooleanParameter: unknown setting name: " << settingName << std::endl;
500 |     return false;
501 | }
502 | 
503 | bool OsqpSolver::setIntegerParameter(const std::string& settingName, int64_t value)
504 | {
505 |     // If you edit this method, remember to update
506 |     // the documentation in the README of the osqp plugin
507 |     if (settingName == "scaling")
508 |     {
509 |         osqpEigenSolver.settings()->setScaling(value);
510 |         return true;
511 |     } else if (settingName == "adaptive_rho_interval")
512 |     {
513 |         osqpEigenSolver.settings()->setAdaptiveRhoInterval(value);
514 |         return true;
515 |     } else if (settingName == "max_iter")
516 |     {
517 |         osqpEigenSolver.settings()->setMaxIteration(value);
518 |         return true;
519 |     } else if (settingName == "polish_refine_iter")
520 |     {
521 |         osqpEigenSolver.settings()->setPolishRefineIter(value);
522 |         return true;
523 |     } else if (settingName == "linsys_solver")
524 |     {
525 |         osqpEigenSolver.settings()->setLinearSystemSolver(value);
526 |         return true;
527 |     } else if (settingName == "check_termination")
528 |     {
529 |         osqpEigenSolver.settings()->setCheckTermination(value);
530 |         return true;
531 |     }
532 | 
533 |     QpSolversEigen::debugStream() << "QpSolversEigen::OsqpSolver::setIntegerParameter: unknown setting name: " << settingName << std::endl;
534 |     return false;
535 | }
536 | 
537 | bool OsqpSolver::setRealNumberParameter(const std::string& settingName, double value)
538 | {
539 |     // If you edit this method, remember to update
540 |     // the documentation in the README of the osqp plugin
541 |     if (settingName == "rho")
542 |     {
543 |         osqpEigenSolver.settings()->setRho(value);
544 |         return true;
545 |     } else if (settingName == "sigma")
546 |     {
547 |         osqpEigenSolver.settings()->setSigma(value);
548 |         return true;
549 |     } else if (settingName == "adaptive_rho_tolerance")
550 |     {
551 |         osqpEigenSolver.settings()->setAdaptiveRhoTolerance(value);
552 |         return true;
553 |     } else if (settingName == "adaptive_rho_fraction")
554 |     {
555 |         osqpEigenSolver.settings()->setAdaptiveRhoFraction(value);
556 |         return true;
557 |     } else if (settingName == "eps_abs")
558 |     {
559 |         osqpEigenSolver.settings()->setAbsoluteTolerance(value);
560 |         return true;
561 |     } else if (settingName == "eps_rel")
562 |     {
563 |         osqpEigenSolver.settings()->setRelativeTolerance(value);
564 |         return true;
565 |     } else if (settingName == "eps_prim_inf")
566 |     {
567 |         osqpEigenSolver.settings()->setPrimalInfeasibilityTolerance(value);
568 |         return true;
569 |     } else if (settingName == "eps_dual_inf")
570 |     {
571 |         osqpEigenSolver.settings()->setDualInfeasibilityTolerance(value);
572 |         return true;
573 |     } else if (settingName == "alpha")
574 |     {
575 |         osqpEigenSolver.settings()->setAlpha(value);
576 |         return true;
577 |     } else if (settingName == "delta")
578 |     {
579 |         osqpEigenSolver.settings()->setDelta(value);
580 |         return true;
581 |     }
582 | 
583 |     QpSolversEigen::debugStream() << "QpSolversEigen::OsqpSolver::setRealNumberParameter: unknown setting name: " << settingName << std::endl;
584 |     return false;
585 | }
586 | 
587 | bool OsqpSolver::setStringParameter(const std::string& parameterName, const std::string& value)
588 | {
589 |     QpSolversEigen::debugStream() << "QpSolversEigen::OsqpSolver::setStringParameter: unknown setting name: " << parameterName << std::endl;
590 |     return false;
591 | }
592 | 
593 | bool OsqpSolver::getBooleanParametersNames(std::vector<std::string>& parametersNames) const
594 | {
595 |     parametersNames.clear();
596 |     parametersNames.push_back("polish");
597 |     parametersNames.push_back("verbose");
598 |     parametersNames.push_back("scaled_termination");
599 |     parametersNames.push_back("warm_start");
600 |     parametersNames.push_back("warm_starting");
601 |     parametersNames.push_back("adaptive_rho");
602 |     return true;
603 | }
604 | 
605 | bool OsqpSolver::getIntegerParametersNames(std::vector<std::string>& parametersNames) const
606 | {
607 |     parametersNames.clear();
608 |     parametersNames.push_back("scaling");
609 |     parametersNames.push_back("adaptive_rho_interval");
610 |     parametersNames.push_back("max_iter");
611 |     parametersNames.push_back("polish_refine_iter");
612 |     parametersNames.push_back("linsys_solver");
613 |     parametersNames.push_back("check_termination");
614 |     return true;
615 | }
616 | 
617 | bool OsqpSolver::getRealNumberParametersNames(std::vector<std::string>& parametersNames) const
618 | {
619 |     parametersNames.clear();
620 |     parametersNames.push_back("rho");
621 |     parametersNames.push_back("sigma");
622 |     parametersNames.push_back("adaptive_rho_tolerance");
623 |     parametersNames.push_back("adaptive_rho_fraction");
624 |     parametersNames.push_back("eps_abs");
625 |     parametersNames.push_back("eps_rel");
626 |     parametersNames.push_back("eps_prim_inf");
627 |     parametersNames.push_back("eps_dual_inf");
628 |     parametersNames.push_back("alpha");
629 |     parametersNames.push_back("delta");
630 |     return true;
631 | }
632 | 
633 | bool OsqpSolver::getStringParametersNames(std::vector<std::string>& parametersNames) const
634 | {
635 |     parametersNames.clear();
636 |     return true;
637 | }
638 | 
639 | SolverInterface* OsqpSolver::allocateInstance() const
640 | {
641 |     return new OsqpSolver();
642 | }
643 | 
644 | }
645 | 


--------------------------------------------------------------------------------
/plugins/proxqp/QpSolversEigenProxqp.cpp:
--------------------------------------------------------------------------------
  1 | // QpSolversEigen
  2 | #include <QpSolversEigen/Debug.hpp>
  3 | #include <QpSolversEigen/SolverInterface.hpp>
  4 | 
  5 | // Class factory API
  6 | #include <sharedlibpp/SharedLibraryClassApi.h>
  7 | 
  8 | // proxsuite
  9 | #include <proxsuite/proxqp/sparse/sparse.hpp>
 10 | 
 11 | #include <memory>
 12 | 
 13 | namespace QpSolversEigen
 14 | {
 15 | 
 16 | class ProxqpSolver final : public SolverInterface
 17 | {
 18 | private:
 19 |     // proxqp settings
 20 |     proxsuite::proxqp::Settings<double> proxqpSettings;
 21 |     // proxqp sparse solver
 22 |     std::unique_ptr<proxsuite::proxqp::sparse::QP<double, long long>> proxqpSparseSolver;
 23 | 
 24 |     struct InitialSparseSolverData
 25 |     {
 26 |         int numberOfVariables = 0;
 27 |         int numberOfInequalityConstraints = 0;
 28 |         int numberOfEqualityConstraints = 0;
 29 | 
 30 |         // Hessian
 31 |         bool isHessianSet = false;
 32 |         Eigen::SparseMatrix<double> H;
 33 | 
 34 |         // Gradient
 35 |         bool isGradientSet = false;
 36 |         Eigen::VectorXd g;
 37 | 
 38 |         // Equality constraints data (unused)
 39 |         bool isEqualityConstraintsMatrixSet = false;
 40 |         Eigen::SparseMatrix<double> A;
 41 |         bool isEqualityConstraintsVectorSet = false;
 42 |         Eigen::VectorXd b;
 43 | 
 44 |         // Inequality constraints data
 45 |         bool isLinearConstraintsSet = false;
 46 |         Eigen::SparseMatrix<double> C;
 47 |         bool isLowerBoundSet = false;
 48 |         Eigen::VectorXd l;
 49 |         bool isUpperBoundSet = false;
 50 |         Eigen::VectorXd u;
 51 | 
 52 |         bool isSet()
 53 |         {
 54 |             return isHessianSet && isGradientSet &&
 55 |                    // If the problem is unconstrained there is no need to set the constraint-related
 56 |                    // variables
 57 |                    ((isLinearConstraintsSet && isLowerBoundSet && isUpperBoundSet)
 58 |                     || (numberOfInequalityConstraints + numberOfEqualityConstraints == 0));
 59 |         }
 60 |     } initialSparseSolverData;
 61 | 
 62 |     // If proxqpSparseSolver is set, copy proxqpSettings to the proxqpSparseSolver->settings
 63 |     void syncSettings();
 64 | 
 65 |     // Helper class to convert proxsuite::proxqp::QPSolverOutput to QpSolversEigen::Status
 66 |     QpSolversEigen::Status convertStatus(const proxsuite::proxqp::QPSolverOutput status) const;
 67 | 
 68 | public:
 69 |     virtual ~ProxqpSolver() = default;
 70 | 
 71 |     std::string getSolverName() const override;
 72 |     void setNumberOfVariables(int n) override;
 73 |     void setNumberOfInequalityConstraints(int m) override;
 74 |     void setNumberOfEqualityConstraints(int m) override;
 75 |     bool initSolver() override;
 76 |     bool isInitialized() override;
 77 |     void clearSolver() override;
 78 |     bool clearSolverVariables() override;
 79 |     QpSolversEigen::ErrorExitFlag solveProblem() override;
 80 |     QpSolversEigen::Status getStatus() const override;
 81 |     const double getObjValue() const override;
 82 |     const Eigen::Matrix<double, Eigen::Dynamic, 1>& getSolution() override;
 83 |     const Eigen::Matrix<double, Eigen::Dynamic, 1>& getDualSolution() override;
 84 |     bool updateHessianMatrix(const Eigen::SparseMatrix<double>& hessianMatrix) override;
 85 |     bool updateInequalityConstraintsMatrix(
 86 |         const Eigen::SparseMatrix<double>& linearConstraintsMatrix) override;
 87 |     bool updateGradient(
 88 |         const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& gradient) override;
 89 |     bool updateLowerBound(
 90 |         const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& lowerBound) override;
 91 |     bool updateUpperBound(
 92 |         const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& upperBound) override;
 93 |     bool updateBounds(
 94 |         const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& lowerBound,
 95 |         const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& upperBound) override;
 96 |     bool updateEqualityConstraintsMatrix(
 97 |         const Eigen::SparseMatrix<double>& equalityConstraintsMatrix) override;
 98 |     bool updateEqualityConstraintsVector(
 99 |         const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& equalityConstraintsVector)
100 |         override;
101 |     void clearHessianMatrix() override;
102 |     void clearLinearConstraintsMatrix() override;
103 | 
104 |     bool setHessianMatrix(const Eigen::SparseMatrix<double>& hessianMatrix) override;
105 |     bool setGradient(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> gradientVector) override;
106 | 
107 |     Eigen::Matrix<double, Eigen::Dynamic, 1> getGradient() override;
108 | 
109 |     bool setInequalityConstraintsMatrix(
110 |         const Eigen::SparseMatrix<double>& linearConstraintsMatrix) override;
111 |     bool
112 |     setLowerBound(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> lowerBoundVector) override;
113 |     bool
114 |     setUpperBound(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> upperBoundVector) override;
115 |     bool setBounds(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> lowerBound,
116 |                    Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> upperBound) override;
117 |     bool setEqualityConstraintsMatrix(
118 |         const Eigen::SparseMatrix<double>& equalityConstraintsMatrix) override;
119 |     bool
120 |     setEqualityConstraintsVector(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>&
121 |                                      equalityConstraintsVector) override;
122 | 
123 |     bool setBooleanParameter(const std::string& settingName, bool value) override;
124 |     bool setIntegerParameter(const std::string& settingName, int64_t value) override;
125 |     bool setRealNumberParameter(const std::string& settingName, double value) override;
126 |     bool setStringParameter(const std::string& parameterName, const std::string& value) override;
127 | 
128 |     bool getBooleanParametersNames(std::vector<std::string>& parametersNames) const override;
129 |     bool getIntegerParametersNames(std::vector<std::string>& parametersNames) const override;
130 |     bool getRealNumberParametersNames(std::vector<std::string>& parametersNames) const override;
131 |     bool getStringParametersNames(std::vector<std::string>& parametersNames) const override;
132 | 
133 |     SolverInterface* allocateInstance() const override;
134 | };
135 | 
136 | // The first argument needs to be coherent with the scheme used in
137 | // getSharedlibppFactoryNameFromSolverName, i.e. qpsolvers_eigen_<solverName>
138 | SHLIBPP_DEFINE_SHARED_SUBCLASS(qpsolvers_eigen_proxqp,
139 |                                QpSolversEigen::ProxqpSolver,
140 |                                QpSolversEigen::SolverInterface);
141 | 
142 | QpSolversEigen::Status
143 | ProxqpSolver::convertStatus(const proxsuite::proxqp::QPSolverOutput status) const
144 | {
145 |     switch (status)
146 |     {
147 |     case proxsuite::proxqp::QPSolverOutput::PROXQP_SOLVED:
148 |         return QpSolversEigen::Status::Solved;
149 |     case proxsuite::proxqp::QPSolverOutput::PROXQP_MAX_ITER_REACHED:
150 |         return QpSolversEigen::Status::MaxIterReached;
151 |     case proxsuite::proxqp::QPSolverOutput::PROXQP_PRIMAL_INFEASIBLE:
152 |         return QpSolversEigen::Status::PrimalInfeasible;
153 |     case proxsuite::proxqp::QPSolverOutput::PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE:
154 |         return QpSolversEigen::Status::SolvedClosestPrimalFeasible;
155 |     case proxsuite::proxqp::QPSolverOutput::PROXQP_DUAL_INFEASIBLE:
156 |         return QpSolversEigen::Status::DualInfeasible;
157 |     case proxsuite::proxqp::QPSolverOutput::PROXQP_NOT_RUN:
158 |         return QpSolversEigen::Status::Unsolved;
159 |     default:
160 |         return QpSolversEigen::Status::SolverSpecificUnknownStatus;
161 |     }
162 | }
163 | 
164 | void ProxqpSolver::syncSettings()
165 | {
166 |     if (proxqpSparseSolver)
167 |     {
168 |         proxqpSparseSolver->settings = proxqpSettings;
169 |     }
170 | }
171 | 
172 | std::string ProxqpSolver::getSolverName() const
173 | {
174 |     return "proxqp";
175 | }
176 | 
177 | void ProxqpSolver::setNumberOfVariables(int n)
178 | {
179 |     initialSparseSolverData.numberOfVariables = n;
180 | }
181 | 
182 | void ProxqpSolver::setNumberOfInequalityConstraints(int m)
183 | {
184 |     initialSparseSolverData.numberOfInequalityConstraints = m;
185 | }
186 | 
187 | void ProxqpSolver::setNumberOfEqualityConstraints(int m)
188 | {
189 |     initialSparseSolverData.numberOfEqualityConstraints = m;
190 | }
191 | 
192 | bool ProxqpSolver::initSolver()
193 | {
194 |     if (!initialSparseSolverData.isSet())
195 |     {
196 |         debugStream() << "QpSolversEigen::ProxqpSolver::initSolver: Some data are not set."
197 |                       << std::endl;
198 |         return false;
199 |     }
200 | 
201 |     proxqpSparseSolver = std::make_unique<
202 |         proxsuite::proxqp::sparse::QP<double, long long>>(initialSparseSolverData.numberOfVariables,
203 |                                                           initialSparseSolverData
204 |                                                               .numberOfEqualityConstraints,
205 |                                                           initialSparseSolverData
206 |                                                               .numberOfInequalityConstraints);
207 |     syncSettings();
208 |     proxqpSparseSolver->init(initialSparseSolverData.H,
209 |                              initialSparseSolverData.g,
210 |                              initialSparseSolverData.A,
211 |                              initialSparseSolverData.b,
212 |                              initialSparseSolverData.C,
213 |                              initialSparseSolverData.l,
214 |                              initialSparseSolverData.u);
215 | 
216 |     return isInitialized();
217 | }
218 | 
219 | bool ProxqpSolver::isInitialized()
220 | {
221 |     return static_cast<bool>(proxqpSparseSolver);
222 | }
223 | 
224 | void ProxqpSolver::clearSolver()
225 | {
226 |     proxqpSparseSolver.reset();
227 | }
228 | 
229 | bool ProxqpSolver::clearSolverVariables()
230 | {
231 |     QpSolversEigen::debugStream() << "QpSolversEigen::ProxqpSolver::clearSolverVariables: method "
232 |                                      "not supported in proxqp."
233 |                                   << std::endl;
234 |     return false;
235 | }
236 | 
237 | QpSolversEigen::ErrorExitFlag ProxqpSolver::solveProblem()
238 | {
239 |     if (!proxqpSparseSolver)
240 |     {
241 |         QpSolversEigen::debugStream()
242 |             << "QpSolversEigen::ProxqpSolver::solveProblem: solver not initialized." << std::endl;
243 |         return QpSolversEigen::ErrorExitFlag::WorkspaceNotInitError;
244 |     }
245 | 
246 |     proxqpSparseSolver->solve();
247 | 
248 |     return QpSolversEigen::ErrorExitFlag::NoError;
249 | }
250 | 
251 | QpSolversEigen::Status ProxqpSolver::getStatus() const
252 | {
253 |     if (!proxqpSparseSolver)
254 |     {
255 |         QpSolversEigen::debugStream()
256 |             << "QpSolversEigen::ProxqpSolver::solveProblem: solver not initialized." << std::endl;
257 |         return QpSolversEigen::Status::SolverNotInitialized;
258 |     }
259 | 
260 |     return this->convertStatus(proxqpSparseSolver->results.info.status);
261 | }
262 | 
263 | const double ProxqpSolver::getObjValue() const
264 | {
265 |     return proxqpSparseSolver->results.info.objValue;
266 | }
267 | 
268 | const Eigen::Matrix<double, Eigen::Dynamic, 1>& ProxqpSolver::getSolution()
269 | {
270 |     return proxqpSparseSolver->results.x;
271 | }
272 | 
273 | const Eigen::Matrix<double, Eigen::Dynamic, 1>& ProxqpSolver::getDualSolution()
274 | {
275 |     return proxqpSparseSolver->results.z;
276 | }
277 | 
278 | bool ProxqpSolver::updateHessianMatrix(const Eigen::SparseMatrix<double>& hessianMatrix)
279 | {
280 |     if (!isInitialized())
281 |     {
282 |         debugStream() << "QpSolversEigen::ProxqpSolver::updateHessianMatrix: solver was not "
283 |                          "initialized."
284 |                       << std::endl;
285 |         return false;
286 |     }
287 | 
288 |     proxqpSparseSolver->update(initialSparseSolverData.H,
289 |                                proxsuite::nullopt,
290 |                                proxsuite::nullopt,
291 |                                proxsuite::nullopt,
292 |                                proxsuite::nullopt,
293 |                                proxsuite::nullopt,
294 |                                proxsuite::nullopt);
295 | 
296 |     return true;
297 | }
298 | 
299 | bool ProxqpSolver::updateInequalityConstraintsMatrix(
300 |     const Eigen::SparseMatrix<double>& linearConstraintsMatrix)
301 | {
302 |     proxqpSparseSolver->update(proxsuite::nullopt,
303 |                                proxsuite::nullopt,
304 |                                proxsuite::nullopt,
305 |                                proxsuite::nullopt,
306 |                                linearConstraintsMatrix,
307 |                                proxsuite::nullopt,
308 |                                proxsuite::nullopt);
309 |     return true;
310 | }
311 | 
312 | bool ProxqpSolver::updateGradient(
313 |     const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& gradient)
314 | {
315 |     proxqpSparseSolver->update(proxsuite::nullopt,
316 |                                gradient,
317 |                                proxsuite::nullopt,
318 |                                proxsuite::nullopt,
319 |                                proxsuite::nullopt,
320 |                                proxsuite::nullopt,
321 |                                proxsuite::nullopt);
322 |     return true;
323 | }
324 | 
325 | bool ProxqpSolver::updateLowerBound(
326 |     const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& lowerBound)
327 | {
328 |     proxqpSparseSolver->update(proxsuite::nullopt,
329 |                                proxsuite::nullopt,
330 |                                proxsuite::nullopt,
331 |                                proxsuite::nullopt,
332 |                                proxsuite::nullopt,
333 |                                lowerBound,
334 |                                proxsuite::nullopt);
335 |     return true;
336 | }
337 | 
338 | bool ProxqpSolver::updateUpperBound(
339 |     const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& upperBound)
340 | {
341 |     proxqpSparseSolver->update(proxsuite::nullopt,
342 |                                proxsuite::nullopt,
343 |                                proxsuite::nullopt,
344 |                                proxsuite::nullopt,
345 |                                proxsuite::nullopt,
346 |                                proxsuite::nullopt,
347 |                                upperBound);
348 |     return true;
349 | }
350 | 
351 | bool ProxqpSolver::updateBounds(
352 |     const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& lowerBound,
353 |     const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& upperBound)
354 | {
355 |     proxqpSparseSolver->update(proxsuite::nullopt,
356 |                                proxsuite::nullopt,
357 |                                proxsuite::nullopt,
358 |                                proxsuite::nullopt,
359 |                                proxsuite::nullopt,
360 |                                lowerBound,
361 |                                upperBound);
362 |     return true;
363 | }
364 | 
365 | bool ProxqpSolver::updateEqualityConstraintsMatrix(
366 |     const Eigen::SparseMatrix<double>& equalityConstraintsMatrix)
367 | {
368 |     proxqpSparseSolver->update(proxsuite::nullopt,
369 |                                proxsuite::nullopt,
370 |                                equalityConstraintsMatrix,
371 |                                proxsuite::nullopt,
372 |                                proxsuite::nullopt,
373 |                                proxsuite::nullopt,
374 |                                proxsuite::nullopt);
375 |     return true;
376 | }
377 | 
378 | bool ProxqpSolver::updateEqualityConstraintsVector(
379 |     const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& equalityConstraintsVector)
380 | {
381 |     proxqpSparseSolver->update(proxsuite::nullopt,
382 |                                proxsuite::nullopt,
383 |                                proxsuite::nullopt,
384 |                                equalityConstraintsVector,
385 |                                proxsuite::nullopt,
386 |                                proxsuite::nullopt,
387 |                                proxsuite::nullopt);
388 |     return true;
389 | }
390 | 
391 | void ProxqpSolver::clearHessianMatrix()
392 | {
393 |     QpSolversEigen::debugStream() << "QpSolversEigen::ProxqpSolver::clearHessianMatrix: method "
394 |                                      "unsupported in proxqp plugin"
395 |                                   << std::endl;
396 |     return;
397 | }
398 | 
399 | void ProxqpSolver::clearLinearConstraintsMatrix()
400 | {
401 |     QpSolversEigen::debugStream() << "QpSolversEigen::ProxqpSolver::clearLinearConstraintsMatrix: "
402 |                                      "method unsupported in proxqp plugin"
403 |                                   << std::endl;
404 |     return;
405 | }
406 | 
407 | bool ProxqpSolver::setHessianMatrix(const Eigen::SparseMatrix<double>& hessianMatrix)
408 | {
409 |     initialSparseSolverData.H = hessianMatrix;
410 |     initialSparseSolverData.isHessianSet = true;
411 |     return true;
412 | }
413 | 
414 | bool ProxqpSolver::setGradient(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> gradientVector)
415 | {
416 |     initialSparseSolverData.g = gradientVector;
417 |     initialSparseSolverData.isGradientSet = true;
418 |     return true;
419 | }
420 | 
421 | Eigen::Matrix<double, Eigen::Dynamic, 1> ProxqpSolver::getGradient()
422 | {
423 |     return Eigen::Matrix<double, Eigen::Dynamic, 1>();
424 | }
425 | 
426 | bool ProxqpSolver::setInequalityConstraintsMatrix(
427 |     const Eigen::SparseMatrix<double>& linearConstraintsMatrix)
428 | {
429 |     initialSparseSolverData.C = linearConstraintsMatrix;
430 |     initialSparseSolverData.isLinearConstraintsSet = true;
431 |     return true;
432 | }
433 | 
434 | bool ProxqpSolver::setLowerBound(
435 |     Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> lowerBoundVector)
436 | {
437 |     initialSparseSolverData.l = lowerBoundVector;
438 |     initialSparseSolverData.isLowerBoundSet = true;
439 |     return true;
440 | }
441 | 
442 | bool ProxqpSolver::setUpperBound(
443 |     Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> upperBoundVector)
444 | {
445 |     initialSparseSolverData.u = upperBoundVector;
446 |     initialSparseSolverData.isUpperBoundSet = true;
447 |     return true;
448 | }
449 | 
450 | bool ProxqpSolver::setBounds(Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> lowerBound,
451 |                              Eigen::Ref<Eigen::Matrix<double, Eigen::Dynamic, 1>> upperBound)
452 | {
453 |     bool ok = setLowerBound(lowerBound) && setUpperBound(upperBound);
454 |     return ok;
455 | }
456 | 
457 | bool ProxqpSolver::setEqualityConstraintsMatrix(
458 |     const Eigen::SparseMatrix<double>& equalityConstraintsMatrix)
459 | {
460 |     initialSparseSolverData.A = equalityConstraintsMatrix;
461 |     initialSparseSolverData.isEqualityConstraintsMatrixSet = true;
462 |     return true;
463 | }
464 | 
465 | bool ProxqpSolver::setEqualityConstraintsVector(
466 |     const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& equalityConstraintsVector)
467 | {
468 |     initialSparseSolverData.b = equalityConstraintsVector;
469 |     initialSparseSolverData.isEqualityConstraintsVectorSet = true;
470 |     return true;
471 | }
472 | 
473 | bool ProxqpSolver::setBooleanParameter(const std::string& settingName, bool value)
474 | {
475 |     // If you edit this method, remember to update
476 |     // the documentation in the README of the proxqp plugin
477 |     bool settingFound = false;
478 |     if (settingName == "verbose")
479 |     {
480 |         proxqpSettings.verbose = value;
481 |         settingFound = true;
482 |     } else if (settingName == "update_preconditioner")
483 |     {
484 |         proxqpSettings.update_preconditioner = value;
485 |         settingFound = true;
486 |     } else if (settingName == "compute_preconditioner")
487 |     {
488 |         proxqpSettings.compute_preconditioner = value;
489 |         settingFound = true;
490 |     } else if (settingName == "compute_timings")
491 |     {
492 |         proxqpSettings.compute_timings = value;
493 |         settingFound = true;
494 |     } else if (settingName == "check_duality_gap")
495 |     {
496 |         proxqpSettings.check_duality_gap = value;
497 |         settingFound = true;
498 |     } else if (settingName == "bcl_update")
499 |     {
500 |         proxqpSettings.bcl_update = value;
501 |         settingFound = true;
502 |     } else if (settingName == "primal_infeasibility_solving")
503 |     {
504 |         proxqpSettings.primal_infeasibility_solving = value;
505 |         settingFound = true;
506 |     }
507 | 
508 |     if (settingFound)
509 |     {
510 |         syncSettings();
511 |         return true;
512 |     }
513 | 
514 |     QpSolversEigen::debugStream() << "QpSolversEigen::ProxqpSolver::setRealNumberParameter: "
515 |                                      "unknown setting name: "
516 |                                   << settingName << std::endl;
517 |     return false;
518 | }
519 | 
520 | bool ProxqpSolver::setIntegerParameter(const std::string& settingName, int64_t value)
521 | {
522 |     bool settingFound = false;
523 |     if (settingName == "max_iter")
524 |     {
525 |         proxqpSettings.max_iter = value;
526 |         settingFound = true;
527 |     } else if (settingName == "max_iter_in")
528 |     {
529 |         proxqpSettings.max_iter_in = value;
530 |         settingFound = true;
531 |     } else if (settingName == "safe_guard")
532 |     {
533 |         proxqpSettings.safe_guard = value;
534 |         settingFound = true;
535 |     } else if (settingName == "nb_iterative_refinement")
536 |     {
537 |         proxqpSettings.nb_iterative_refinement = value;
538 |         settingFound = true;
539 |     } else if (settingName == "preconditioner_max_iter")
540 |     {
541 |         proxqpSettings.preconditioner_max_iter = value;
542 |         settingFound = true;
543 |     } else if (settingName == "frequence_infeasibility_check")
544 |     {
545 |         proxqpSettings.frequence_infeasibility_check = value;
546 |         settingFound = true;
547 |     }
548 | 
549 |     if (settingFound)
550 |     {
551 |         syncSettings();
552 |         return true;
553 |     }
554 | 
555 |     QpSolversEigen::debugStream() << "QpSolversEigen::ProxqpSolver::setRealNumberParameter: "
556 |                                      "unknown setting name: "
557 |                                   << settingName << std::endl;
558 |     return false;
559 | }
560 | 
561 | bool ProxqpSolver::setRealNumberParameter(const std::string& settingName, double value)
562 | {
563 |     // If you edit this method, remember to update
564 |     // the documentation in the README of the osqp plugin
565 |     bool settingFound = false;
566 |     if (settingName == "default_mu_eq")
567 |     {
568 |         proxqpSettings.default_mu_eq = value;
569 |         settingFound = true;
570 |     } else if (settingName == "default_mu_in")
571 |     {
572 |         proxqpSettings.default_mu_in = value;
573 |         settingFound = true;
574 |     } else if (settingName == "alpha_bcl")
575 |     {
576 |         proxqpSettings.alpha_bcl = value;
577 |         settingFound = true;
578 |     } else if (settingName == "beta_bcl")
579 |     {
580 |         proxqpSettings.beta_bcl = value;
581 |         settingFound = true;
582 |     } else if (settingName == "refactor_dual_feasibility_threshold")
583 |     {
584 |         proxqpSettings.refactor_dual_feasibility_threshold = value;
585 |         settingFound = true;
586 |     } else if (settingName == "refactor_rho_threshold")
587 |     {
588 |         proxqpSettings.refactor_rho_threshold = value;
589 |         settingFound = true;
590 |     } else if (settingName == "mu_min_in")
591 |     {
592 |         proxqpSettings.mu_min_in = value;
593 |         settingFound = true;
594 |     } else if (settingName == "mu_min_eq")
595 |     {
596 |         proxqpSettings.mu_min_eq = value;
597 |         settingFound = true;
598 |     } else if (settingName == "mu_max_eq_inv")
599 |     {
600 |         proxqpSettings.mu_max_eq_inv = value;
601 |         settingFound = true;
602 |     } else if (settingName == "mu_max_in_inv")
603 |     {
604 |         proxqpSettings.mu_max_in_inv = value;
605 |         settingFound = true;
606 |     } else if (settingName == "mu_update_factor")
607 |     {
608 |         proxqpSettings.mu_update_factor = value;
609 |         settingFound = true;
610 |     } else if (settingName == "cold_reset_mu_eq")
611 |     {
612 |         proxqpSettings.cold_reset_mu_eq = value;
613 |         settingFound = true;
614 |     } else if (settingName == "cold_reset_mu_in")
615 |     {
616 |         proxqpSettings.cold_reset_mu_in = value;
617 |         settingFound = true;
618 |     } else if (settingName == "cold_reset_mu_eq_inv")
619 |     {
620 |         proxqpSettings.cold_reset_mu_eq_inv = value;
621 |         settingFound = true;
622 |     } else if (settingName == "cold_reset_mu_in_inv")
623 |     {
624 |         proxqpSettings.cold_reset_mu_in_inv = value;
625 |         settingFound = true;
626 |     } else if (settingName == "eps_abs")
627 |     {
628 |         proxqpSettings.eps_abs = value;
629 |         settingFound = true;
630 |     } else if (settingName == "eps_rel")
631 |     {
632 |         proxqpSettings.eps_rel = value;
633 |         settingFound = true;
634 |     } else if (settingName == "eps_refact")
635 |     {
636 |         proxqpSettings.eps_refact = value;
637 |         settingFound = true;
638 |     } else if (settingName == "eps_duality_gap_abs")
639 |     {
640 |         proxqpSettings.eps_duality_gap_abs = value;
641 |         settingFound = true;
642 |     } else if (settingName == "eps_duality_gap_rel")
643 |     {
644 |         proxqpSettings.eps_duality_gap_rel = value;
645 |         settingFound = true;
646 |     } else if (settingName == "preconditioner_accuracy")
647 |     {
648 |         proxqpSettings.preconditioner_accuracy = value;
649 |         settingFound = true;
650 |     } else if (settingName == "alpha_gpdal")
651 |     {
652 |         proxqpSettings.alpha_gpdal = value;
653 |         settingFound = true;
654 |     } else if (settingName == "default_H_eigenvalue_estimate")
655 |     {
656 |         proxqpSettings.default_H_eigenvalue_estimate = value;
657 |         settingFound = true;
658 |     }
659 | 
660 |     if (settingFound)
661 |     {
662 |         syncSettings();
663 |         return true;
664 |     }
665 | 
666 |     QpSolversEigen::debugStream() << "QpSolversEigen::ProxqpSolver::setRealNumberParameter: "
667 |                                      "unknown setting name: "
668 |                                   << settingName << std::endl;
669 |     return false;
670 | }
671 | 
672 | bool ProxqpSolver::setStringParameter(const std::string& parameterName, const std::string& value)
673 | {
674 |     bool settingFound = false;
675 |     bool valueFound = false;
676 | 
677 |     if (parameterName == "initial_guess")
678 |     {
679 |         settingFound = true;
680 |         if (value == "NO_INITIAL_GUESS")
681 |         {
682 |             proxqpSettings.initial_guess = proxsuite::proxqp::InitialGuessStatus::NO_INITIAL_GUESS;
683 |             valueFound = true;
684 |         } else if (value == "EQUALITY_CONSTRAINED_INITIAL_GUESS")
685 |         {
686 |             proxqpSettings.initial_guess
687 |                 = proxsuite::proxqp::InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS;
688 |             valueFound = true;
689 |         } else if (value == "WARM_START_WITH_PREVIOUS_RESULT")
690 |         {
691 |             proxqpSettings.initial_guess
692 |                 = proxsuite::proxqp::InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT;
693 |             valueFound = true;
694 |         } else if (value == "WARM_START")
695 |         {
696 |             proxqpSettings.initial_guess = proxsuite::proxqp::InitialGuessStatus::WARM_START;
697 |             valueFound = true;
698 |         } else if (value == "COLD_START_WITH_PREVIOUS_RESULT")
699 |         {
700 |             proxqpSettings.initial_guess
701 |                 = proxsuite::proxqp::InitialGuessStatus::COLD_START_WITH_PREVIOUS_RESULT;
702 |             valueFound = true;
703 |         }
704 |     }
705 | 
706 |     if (settingFound && valueFound)
707 |     {
708 |         syncSettings();
709 |         return true;
710 |     }
711 | 
712 |     if (!settingFound)
713 |     {
714 |         QpSolversEigen::debugStream() << "QpSolversEigen::ProxqpSolver::setStringParameter: "
715 |                                          "unknown setting name: "
716 |                                       << parameterName << std::endl;
717 |     } else if (!valueFound)
718 |     {
719 |         QpSolversEigen::debugStream()
720 |             << "QpSolversEigen::ProxqpSolver::setStringParameter: unknown value << " << value
721 |             << " for parameter with name: " << parameterName << std::endl;
722 |     }
723 |     return false;
724 | }
725 | 
726 | bool ProxqpSolver::getBooleanParametersNames(std::vector<std::string>& parametersNames) const
727 | {
728 |     parametersNames.clear();
729 |     parametersNames.push_back("verbose");
730 |     parametersNames.push_back("update_preconditioner");
731 |     parametersNames.push_back("compute_preconditioner");
732 |     parametersNames.push_back("compute_timings");
733 |     parametersNames.push_back("check_duality_gap");
734 |     parametersNames.push_back("bcl_update");
735 |     parametersNames.push_back("primal_infeasibility_solving");
736 |     return true;
737 | }
738 | 
739 | bool ProxqpSolver::getIntegerParametersNames(std::vector<std::string>& parametersNames) const
740 | {
741 |     parametersNames.clear();
742 |     parametersNames.push_back("max_iter");
743 |     parametersNames.push_back("max_iter_in");
744 |     parametersNames.push_back("safe_guard");
745 |     parametersNames.push_back("nb_iterative_refinement");
746 |     parametersNames.push_back("preconditioner_max_iter");
747 |     parametersNames.push_back("frequence_infeasibility_check");
748 |     return true;
749 | }
750 | 
751 | bool ProxqpSolver::getRealNumberParametersNames(std::vector<std::string>& parametersNames) const
752 | {
753 |     parametersNames.clear();
754 |     parametersNames.push_back("default_mu_eq");
755 |     parametersNames.push_back("default_mu_in");
756 |     parametersNames.push_back("alpha_bcl");
757 |     parametersNames.push_back("beta_bcl");
758 |     parametersNames.push_back("refactor_dual_feasibility_threshold");
759 |     parametersNames.push_back("refactor_rho_threshold");
760 |     parametersNames.push_back("mu_min_in");
761 |     parametersNames.push_back("mu_max_eq_inv");
762 |     parametersNames.push_back("mu_max_in_inv");
763 |     parametersNames.push_back("mu_update_factor");
764 |     parametersNames.push_back("cold_reset_mu_eq");
765 |     parametersNames.push_back("cold_reset_mu_in");
766 |     parametersNames.push_back("cold_reset_mu_eq_inv");
767 |     parametersNames.push_back("cold_reset_mu_in_inv");
768 |     parametersNames.push_back("eps_abs");
769 |     parametersNames.push_back("eps_rel");
770 |     parametersNames.push_back("eps_refact");
771 |     parametersNames.push_back("eps_duality_gap_abs");
772 |     parametersNames.push_back("eps_duality_gap_rel");
773 |     parametersNames.push_back("preconditioner_accuracy");
774 |     parametersNames.push_back("alpha_gpdal");
775 |     parametersNames.push_back("default_H_eigenvalue_estimate");
776 |     return true;
777 | }
778 | 
779 | bool ProxqpSolver::getStringParametersNames(std::vector<std::string>& parametersNames) const
780 | {
781 |     parametersNames.clear();
782 |     parametersNames.push_back("initial_guess");
783 |     return true;
784 | }
785 | 
786 | SolverInterface* ProxqpSolver::allocateInstance() const
787 | {
788 |     return new ProxqpSolver();
789 | }
790 | 
791 | } // namespace QpSolversEigen


--------------------------------------------------------------------------------