diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..e4fbfcd --- /dev/null +++ b/.gitignore @@ -0,0 +1,17 @@ +# Build artifacts +build/ +dist/ +*.egg-info/ +*.so + +# Python caches +__pycache__/ +*.py[cod] + +# npm (e.g. locally installed pyright) +node_modules/ + +# Editors / OS +.vscode/ +.idea/ +.DS_Store diff --git a/AGENTS.md b/AGENTS.md new file mode 100644 index 0000000..7cc06f5 --- /dev/null +++ b/AGENTS.md @@ -0,0 +1,130 @@ +# AGENTS.md + +Repository conventions for the `marinholab-solvers-osqp` project — a Python +(C++ via pybind11) wrapper around [OSQP](https://github.com/osqp/osqp), a +first-order ADMM solver for quadratic programs. + +## Project layout + +``` +marinholab/solvers/osqp/ + __init__.py Public API re-exports (Solver, Configuration, Info, enums) + solver.py numpy-friendly Solver wrapper (accepts None constraints, warm-starts) + example.py runnable example (console script: osqp_example) + example_kinematics.py OPTIONAL example (needs dqrobotics + dqrobotics-pyplot) + _core.pyi type stub for the compiled _core extension (ships in the wheel) + py.typed PEP 561 marker so stubs are picked up by type checkers +include/marinholab/solvers/osqp.h C++ header (Solver + Configuration + Info): `marinholab::solvers::osqp::Solver` + `Configuration` (doxygen-documented) +src/core.cpp pybind11 module (_core): binds `Solver` + `Configuration` + `Info` + enums +src/core_function.cpp C++ implementation (wraps OSQP's OSQPSolver); compiled into the `marinholab_osqp` static library +CMakeLists.txt CMake build: `marinholab_osqp` static lib, `_core` pybind11 module, optional C++ example (`-DBUILD_EXAMPLES=ON`) +example/example.cpp standalone C++ usage example (target: `example_osqp`) +osqp/ OSQP (git submodule) +pybind11/ pybind11 (git submodule) +setup.py PEP 517 build (CMake + pybind11) +pyrightconfig.json Pyright configuration (python 3.9, mode basic) +``` + +## Build & install + +Requires: a C++23 compiler (e.g. `g++`), CMake, Ninja, `eigen3` (dev headers), +and Python >= 3.9 with `numpy` + `setuptools`/`wheel`. On Ubuntu: +`sudo apt-get install cmake libeigen3-dev`. + +```console +git submodule update --init --recursive # if cloning without submodules +pip install -e . # editable install (builds _core in build_ext) +# or produce a wheel: +python setup.py bdist_wheel +pip install dist/marinholab_solvers_osqp-*.whl +``` + +The extension name is `marinholab.solvers.osqp._core`. Builds are slow on first +run (OSQP is compiled from its submodule); CMake reuses the `build/` cache +across builds. + +## Run the example (smoke test) + +```console +osqp_example +``` + +(or `python -m marinholab.solvers.osqp.example`). Exits 0 and prints the +optimal `x`, the `None`-constraint paths, warm-starts, a hierarchical +(task-priority) example, and `get_info()` for the final solve. + +`example_kinematics.py` additionally needs the *optional* dependencies +`dqrobotics` and `dqrobotics-pyplot` (`pip install --pre dqrobotics +dqrobotics-pyplot`); it is not required for the core package to work. + +The standalone C++ example (`example/example.cpp`, target `example_osqp`) is +built only when `BUILD_EXAMPLES=ON`; it is *not* built by `pip install .`: + +```console +cmake -B build -GNinja -DCMAKE_BUILD_TYPE=Release -DBUILD_EXAMPLES=ON +cmake --build build --target example_osqp +./build/example/example_osqp # prints x ≈ [0.2, 1.0] and the residuals +``` + +## Type checking (Pyright) + +```console +pyright +``` + +Configuration lives in `pyrightconfig.json` (`pythonVersion` 3.9, +`typeCheckingMode` basic). It must pass with **0 errors / 0 warnings**. + +- The compiled extension `_core` is typed through the `_core.pyi` stub. + Keep the stub in sync with the pybind11 surface in `src/core.cpp`. +- `example_kinematics.py` depends on the untyped, *optional* third-party + `dqrobotics` package; its `import` lines carry targeted `# type: ignore` + comments (`reportMissingImports` for `matplotlib`, `reportAttributeAccessIssue` + for the `dqrobotics`/`dqrobotics_extensions` imports). Do not remove them + (they are the documented reason those lines are ignored, and keep the module + checking cleanly even when the optional deps are not installed). +- The package ships `py.typed` + `_core.pyi` in the wheel (`package_data` in + `setup.py`) so downstream projects can be checked against it. + +## Conventions + +- **Defaults match OSQP.** `Configuration` defaults mirror OSQP's own + `osqp_set_default_settings()` for a standard double-precision, direct-solver + build (see the defaults tables in `README.md`), with one documented + exception: `verbose` defaults to `0` (quiet) instead of OSQP's own + `OSQP_VERBOSE = 1`. Do not silently override them here; if a particular + problem needs a non-default option, set it on the `Configuration` in the + *caller* (e.g. `example.py` tightens `eps_abs`/`eps_rel` and raises + `max_iter`). Re-validate the `example.py` and `example_kinematics.py` solves + after any change to a default. +- **`Solver` API.** `Solver.solve_quadratic_program()` accepts `None` for + `A`/`b`/`Aeq`/`beq` (in the Python wrapper, which substitutes a single + trivially-satisfied zero row; the C++ `Solver` always takes all six + matrices) and optional `x0`/`y0` warm-starts. `Solver.get_info()` returns + `obj_val`, `dual_obj_val`, `prim_res`, `dual_res`, and the `dual_solution`. +- **Style.** Match the existing style: docstrings on the public API. + `Configuration` fields that map to OSQP `OSQPSettings` fields keep the + library's native snake_case spelling (e.g. `eps_abs`, `max_iter`, + `warm_starting`); the wrapper-specific field that has no OSQP counterpart is + also snake_case (`use_hotstart`). +- **Doxygen.** C++ types and members are documented with Doxygen + (`/** ... @brief ... @param ... @return ... @see ... */` blocks). Keep that + when adding fields or methods. +- **Annotations.** All public Python API is fully type-annotated and must + remain Pyright-clean. `requires-python` is `>= 3.9`; use + `from __future__ import annotations` where PEP 604 `X | Y` is used so the + module still parses on 3.9. + +## CI + +`.github/workflows/python-publish.yml` builds the wheel on `ubuntu-latest` and +`ubuntu-24.04-arm` for Python 3.12 and 3.13 and publishes to PyPI. Local +builds here mirror that (aarch64, Python 3.13). + +## Version + +The version is dynamic (`dynamic = ["version"]` in `pyproject.toml`) via +`setuptools-git-versioning`, computed from the git tag/commit. `setup.py` +computes a date+commit-derived fallback when the git tag isn't available, which +is why locally-built wheels may show a different distribution version — this +is pre-existing and unrelated to code changes. diff --git a/CMakeLists.txt b/CMakeLists.txt index 0db54ae..97ed281 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,18 +10,9 @@ project(marinholab_solver_osqp LANGUAGES CXX) set(CMAKE_CXX_STANDARD 23) set(CMAKE_CXX_STANDARD_REQUIRED ON) -find_package(Eigen3 REQUIRED) - -set(PYBIND11_FINDPYTHON ON) -add_subdirectory(pybind11) -pybind11_add_module(_core MODULE - src/core.cpp - src/core_function.cpp - ) +option(BUILD_EXAMPLES "Build the standalone C++ example (example/)." OFF) -target_include_directories(_core PRIVATE - ${PROJECT_SOURCE_DIR}/include - ) +find_package(Eigen3 REQUIRED) # https://stackoverflow.com/questions/38296756/what-is-the-idiomatic-way-in-cmake-to-add-the-fpic-compiler-option set(CMAKE_POSITION_INDEPENDENT_CODE ON) @@ -36,7 +27,37 @@ set(OSQP_BUILD_DEMO_EXE FALSE CACHE BOOL "x" FORCE) set(OSQP_BUILD_UNITTESTS FALSE CACHE BOOL "x" FORCE) add_subdirectory(osqp) -target_link_libraries(_core PRIVATE Eigen3::Eigen osqpstatic) +# Static library with the Solver/Configuration implementation. Both the +# pybind11 module and any standalone C++ consumer link against it, so the C++ +# code is compiled exactly once. +add_library(marinholab_osqp STATIC + src/core_function.cpp + ) + +target_include_directories(marinholab_osqp + PUBLIC + ${PROJECT_SOURCE_DIR}/include + osqp/include/public + ) + +target_link_libraries(marinholab_osqp PUBLIC Eigen3::Eigen osqpstatic) + +# Build the standalone C++ example (see example/example.cpp). +if (BUILD_EXAMPLES) + add_executable(example_osqp example/example.cpp) + target_link_libraries(example_osqp PRIVATE marinholab_osqp) + set_target_properties(example_osqp PROPERTIES + RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/example) +endif () + +# Python extension. +set(PYBIND11_FINDPYTHON ON) +add_subdirectory(pybind11) +pybind11_add_module(_core MODULE + src/core.cpp + ) + +target_link_libraries(_core PRIVATE marinholab_osqp) # Version is dynamically obtained. See pyproject.toml # Activating this causes issues with Windows compilation. # target_compile_definitions(_core PRIVATE VERSION_INFO=${PROJECT_VERSION}) diff --git a/README.md b/README.md index ab0ffe9..e4d6625 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,288 @@ # solver-osqp +A [OSQP](https://github.com/osqp/osqp) wrapper for Python that ships prebuilt +binaries. It exposes OSQP's first-order ADMM solver through a thin, +numpy-friendly, MATLAB-`quadprog`-like interface, in the C++ namespace +`marinholab::solvers::osqp` and the Python package `marinholab.solvers.osqp`. + ```console pip install marinholab-solvers-osqp -``` \ No newline at end of file +``` + +## Overview + +Given a symmetric matrix `H`, a vector `f`, and (optionally) inequality and +equality constraint matrices, the solver solves the quadratic program + +``` +min_x 0.5 * x' H x + f' x +s.t. A x <= b + Aeq x = beq +``` + +Internally the inequality and equality rows are stacked into OSQP's single +`l <= A x <= u` form. Once a problem has been solved once, subsequent solves on +the same `Solver` instance reuse the OSQP solver and update its data in place +by default (`Configuration.use_hotstart = True`), which is the main performance +benefit for repeated, related QPs. OSQP additionally warm-starts from the +previous iterate between `osqp_solve()` calls +(`Configuration.warm_starting = 1`). + +## Quickstart + +```python +import numpy as np +from marinholab.solvers import osqp + +solver = osqp.Solver() + +H = np.eye(2) # positive definite Hessian +f = np.array([-1.0, -1.0]) # linear term +A = np.array([[1.0, 0.0]]) # x[0] <= 0.2 +b = np.array([0.2]) + +x = solver.solve_quadratic_program(H, f, A, b, + Aeq=np.zeros((1, 2)), + beq=np.zeros((1,))) +# x ≈ [0.2, 1.0] +``` + +### Omitting constraints + +Any of the four constraint arguments (`A`, `b`, `Aeq`, `beq`) can be `None`, +meaning "no such constraint". The matrix and its right-hand side must be +omitted (or provided) together: + +```python +# Unconstrained +x = solver.solve_quadratic_program(H, f, None, None, None, None) + +# Equality constraints only +x = solver.solve_quadratic_program(H, f, None, None, Aeq, beq) +``` + +### Warm-starting + +`solve_quadratic_program` accepts optional `x0` and `y0` warm-starts for the +primal and dual variables. The dual solution returned by `get_info()` can be +used to warm-start the next solve: + +```python +x = solver.solve_quadratic_program(H, f, A, b, None, None) +y0 = solver.get_info().dual_solution +x = solver.solve_quadratic_program(H, f, A, b, None, None, x0=x, y0=y0) +``` + +### Solution information + +`solver.get_info()` returns an `Info` with the objective and dual-objective +values, the primal and dual residual norms, and the dual solution +(`dual_solution`): + +```python +solver.solve_quadratic_program(H, f, A, b, Aeq, beq) +info = solver.get_info() +info.obj_val, info.dual_obj_val, info.prim_res, info.dual_res, info.dual_solution +``` + +## C++ API + +The same solver is available directly in C++ (the Python wrapper is a thin +pybind11 layer over it). It is built with +[Eigen](https://eigen.tuxfamily.org/) and +[OSQP](https://github.com/osqp/osqp): + +```cpp +#include + +namespace osqp = marinholab::solvers::osqp; + +osqp::Configuration config; +config.eps_abs = 1.0e-9; // the rest keeps its defaults +config.polishing = 1; + +osqp::Solver solver(config); + +Eigen::MatrixXd H = Eigen::MatrixXd::Identity(2, 2); +Eigen::VectorXd f(2); +f << -1.0, -1.0; + +Eigen::MatrixXd A(1, 2); +A << 1.0, 0.0; // x[0] <= 0.2 +Eigen::VectorXd b(1); +b << 0.2; + +Eigen::MatrixXd Aeq = Eigen::MatrixXd::Zero(1, 2); +Eigen::VectorXd beq = Eigen::VectorXd::Zero(1); + +Eigen::VectorXd x = solver.solve_quadratic_program(H, f, A, b, Aeq, beq); +// x ≈ [0.2, 1.0] + +osqp::Solver::Info info = solver.get_info(); // obj_val, prim_res, dual_solution, ... +``` + +The API mirrors the Python one: `solve_quadratic_program(H, f, A, b, Aeq, beq, +x0, y0)` solves the QP above and `get_info()` reports the solution quality. +`Configuration` exposes the same fields documented below. + +A standalone, self-contained C++ program using this API lives in +[`example/example.cpp`](example/example.cpp) (target `example_osqp`). It is +built only when `BUILD_EXAMPLES=ON` so it does not affect `pip install .`: + +```console +cmake -B build -GNinja -DCMAKE_BUILD_TYPE=Release -DBUILD_EXAMPLES=ON +cmake --build build --target example_osqp +./build/example/example_osqp # prints x ≈ [0.2, 1.0] and the residuals +``` + +## Type checking (Pyright) + +The Python wrapper is fully type-annotated and ships a PEP 561 `py.typed` +marker plus a `_core.pyi` type stub for the compiled `_core` extension, so +downstream projects can be checked against it. To type-check the package: + +```console +pyright # configuration in pyrightconfig.json (python 3.9, mode basic) +``` + +This must pass with **0 errors / 0 warnings**. Keep `_core.pyi` in sync with +the pybind11 surface in `src/core.cpp`. + +## Configuration + +All of OSQP's `OSQPSettings` fields are exposed, plus one wrapper-specific +setting. Create a `Configuration`, tweak the fields you need, and pass it to +the solver: + +```python +config = osqp.Configuration() +config.eps_abs = 1.0e-9 # tighter absolute tolerance +config.eps_rel = 1.0e-9 # tighter relative tolerance +config.max_iter = 20000 # more ADMM iterations +config.polishing = 1 # polish the ADMM solution +solver = osqp.Solver(config) +``` + +The enum types are re-exported for convenience: `osqp.LinsysSolverType`, +`osqp.PreconditionerType`, and `osqp.Status`. + +### Wrapper-specific option + +| Option | Default | Type | Description | +|---|---|---|---| +| `use_hotstart` | `True` | `bool` | Reuse the existing OSQP solver and update its data in place instead of re-running `osqp_setup()` when the problem shape is unchanged. | + +### OSQP options (linear algebra & control) + +These map 1:1 onto OSQP's `OSQPSettings` fields. Defaults match OSQP's own +defaults for a standard double-precision, direct-solver build (see +`osqp_set_default_settings()`), except `verbose`, which defaults to off so the +solver is quiet by default. See the +[OSQP documentation](https://osqp.org/docs/) for a full description of each +option. + +| Option | Default | Type | Description | +|---|---|---|---| +| `device` | `0` | `int` | Device identifier; currently used for CUDA devices. | +| `linsys_solver` | `OSQP_DIRECT_SOLVER` | `LinsysSolverType` | Linear system solver to use. | +| `allocate_solution` | `1` | `int` | Whether the solution is allocated during `osqp_setup()`. | +| `verbose` | `0` | `int` | Whether solver progress is written out (quiet by default). | +| `profiler_level` | `0` | `int` | Level of detail for profiler annotations. | +| `warm_starting` | `1` | `int` | Warm-start from the previous solution between consecutive solves. | +| `scaling` | `10` | `int` | Heuristic data-scaling iterations; `0` disables scaling. | +| `polishing` | `0` | `int` | Whether the ADMM solution is polished to improve accuracy. | + +### OSQP options (ADMM parameters) + +| Option | Default | Type | Description | +|---|---|---|---| +| `rho` | `0.1` | `float` | ADMM penalty parameter (scalar). | +| `rho_is_vec` | `1` | `int` | Whether `rho` is a scalar or a vector. | +| `sigma` | `1e-06` | `float` | ADMM regularization parameter (improves conditioning). | +| `alpha` | `1.6` | `float` | ADMM relaxation parameter. | + +### OSQP options (CG settings) + +| Option | Default | Type | Description | +|---|---|---|---| +| `cg_max_iter` | `20` | `int` | Maximum number of CG iterations per solve. | +| `cg_tol_reduction` | `10` | `int` | Consecutive zero CG iterations before the tolerance is halved. | +| `cg_tol_fraction` | `0.15` | `float` | CG tolerance, as a fraction of the ADMM residuals. | +| `cg_precond` | `OSQP_DIAGONAL_PRECONDITIONER` | `PreconditionerType` | Preconditioner used by the CG method. | + +### OSQP options (adaptive rho) + +| Option | Default | Type | Description | +|---|---|---|---| +| `adaptive_rho` | `1` (`..._ITERATIONS`) | `int` | `rho` stepsize adaptation method (`0` disabled, `1` iterations, `2` time, `3` KKT error). | +| `adaptive_rho_interval` | `50` | `int` | Interval between `rho` adaptations (iterations-based method). | +| `adaptive_rho_fraction` | `0.4` | `float` | Fraction controlling when non-fixed `rho` adaptations occur. | +| `adaptive_rho_tolerance` | `5.0` | `float` | Min ratio between new and current `rho` for it to be adopted. | + +### OSQP options (termination) + +| Option | Default | Type | Description | +|---|---|---|---| +| `max_iter` | `4000` | `int` | Maximum number of ADMM iterations. | +| `eps_abs` | `1e-3` | `float` | Absolute solution tolerance. | +| `eps_rel` | `1e-3` | `float` | Relative solution tolerance. | +| `eps_prim_inf` | `1e-4` | `float` | Primal infeasibility detection tolerance. | +| `eps_dual_inf` | `1e-4` | `float` | Dual infeasibility detection tolerance. | +| `scaled_termination` | `0` | `int` | Whether the scaled termination criteria are used. | +| `check_termination` | `25` | `int` | Interval at which termination is checked; `0` disables the periodic check. | +| `check_dualgap` | `1` | `int` | Whether the duality-gap termination criteria are used. | +| `time_limit` | `1e10` | `float` | Maximum solve time, in seconds. | + +### OSQP options (polishing) + +| Option | Default | Type | Description | +|---|---|---|---| +| `delta` | `1e-6` | `float` | Regularization parameter used by polishing. | +| `polish_refine_iter` | `3` | `int` | Number of iterative refinement steps during polishing. | + +### Enums + +**Linear system solvers** (`LinsysSolverType`): `OSQP_UNKNOWN_SOLVER`, +`OSQP_DIRECT_SOLVER`, `OSQP_INDIRECT_SOLVER`. + +**CG preconditioners** (`PreconditionerType`): `OSQP_NO_PRECONDITIONER`, +`OSQP_DIAGONAL_PRECONDITIONER`. + +**Solver status** (`Status`): `OSQP_SOLVED`, `OSQP_SOLVED_INACCURATE`, +`OSQP_PRIMAL_INFEASIBLE`, `OSQP_PRIMAL_INFEASIBLE_INACCURATE`, +`OSQP_DUAL_INFEASIBLE`, `OSQP_DUAL_INFEASIBLE_INACCURATE`, +`OSQP_MAX_ITER_REACHED`, `OSQP_TIME_LIMIT_REACHED`, `OSQP_NON_CVX`, +`OSQP_SIGINT`, `OSQP_UNSOLVED`. + +## Examples + +* `marinholab/solvers/osqp/example.py` — positive-definite solves, the + `None`-constraint path, warm-starting, a hierarchical (task-priority) + example, and `get_info()`. Run it with `osqp_example` (installed as a + console script). +* `marinholab/solvers/osqp/example_kinematics.py` — an *optional* example + showing the solver used in a hierarchical (task-priority) controller for a + kinematically redundant robot, built on + [`dqrobotics`](https://pypi.org/project/dqrobotics/). It requires the + optional dependencies `dqrobotics` and `dqrobotics-pyplot` + (`pip install --pre dqrobotics dqrobotics-pyplot`). + +## Building from source + +The package builds a C++ extension (via CMake + pybind11) and vendors +[OSQP](https://github.com/osqp/osqp) and +[pybind11](https://github.com/pybind/pybind11) as git submodules. + +```console +git clone --recurse-submodules +pip install . +``` + +Prerequisites: a C++23 compiler, CMake, an Eigen3 installation, and Python. +On Ubuntu: `sudo apt-get install cmake libeigen3-dev`. + +## License + +The wrapper is under the GNU Lesser General Public License v2.1 (see the +included `LICENSE` file); the bundled +[OSQP](https://github.com/osqp/osqp) library is Apache-2.0. \ No newline at end of file diff --git a/evaluation/compare_qpoases_osqp.py b/evaluation/compare_qpoases_osqp.py index 7bfe9d3..5d039fe 100644 --- a/evaluation/compare_qpoases_osqp.py +++ b/evaluation/compare_qpoases_osqp.py @@ -119,9 +119,9 @@ def main(): osqp_config = osqp.Configuration() # Tighten OSQP's tolerances/enable polishing so that its ADMM solution is # directly comparable to qpOASES' active-set solution. - osqp_config.eps_absolute = 1e-7 - osqp_config.eps_relative = 1e-7 - osqp_config.maximum_iterations = 20000 + osqp_config.eps_abs = 1e-7 + osqp_config.eps_rel = 1e-7 + osqp_config.max_iter = 20000 osqp_config.polishing = 1 all_passed = True diff --git a/example/example.cpp b/example/example.cpp new file mode 100644 index 0000000..37de075 --- /dev/null +++ b/example/example.cpp @@ -0,0 +1,65 @@ +/** + * @brief Example usage of the C++ API of `marinholab-solvers-osqp`. + * + * Builds a small quadratic program and solves it with + * `marinholab::solvers::osqp::Solver`, mirroring the Python quickstart in the + * README. + * + * Build and run it with the `BUILD_EXAMPLES` option (OFF by default so the + * normal `pip install .` build is unaffected): + * + * cmake -B build -GNinja -DBUILD_EXAMPLES=ON + * cmake --build build + * ./build/example/example_osqp + */ +#include + +#include + +namespace osqp = marinholab::solvers::osqp; + +int main() +{ + // 1. Configure the solver. Only a couple of fields are set here; the rest + // keep their defaults (which mirror OSQP's own defaults for a standard + // double-precision, direct-solver build; see osqp_set_default_settings()). + osqp::Configuration config; + config.eps_abs = 1.0e-9; // tighter absolute tolerance + config.eps_rel = 1.0e-9; // tighter relative tolerance + + osqp::Solver solver(config); + + // 2. The problem: + // + // min_x 0.5 * x' H x + f' x + // s.t. A x <= b + // Aeq x = beq + // + // H = I, f = [-1, -1], x[0] <= 0.2, plus one trivially-satisfied equality. + Eigen::MatrixXd H = Eigen::MatrixXd::Identity(2, 2); + Eigen::VectorXd f(2); + f << -1.0, -1.0; + + Eigen::MatrixXd A(1, 2); + A << 1.0, 0.0; + Eigen::VectorXd b(1); + b << 0.2; + + Eigen::MatrixXd Aeq = Eigen::MatrixXd::Zero(1, 2); + Eigen::VectorXd beq = Eigen::VectorXd::Zero(1); + + // 3. Solve. The Solver keeps the underlying OSQP problem, so repeated + // calls on the same instance are warm-started by default + // (`Configuration.use_hotstart = true` / `warm_starting = 1`). + Eigen::VectorXd x = solver.solve_quadratic_program(H, f, A, b, Aeq, beq); + + // 4. Inspect the result and the solution quality. `.transpose()` makes + // Eigen print the (column) vector as a single horizontal line. + std::cout << "x = " << x.transpose() << "\n"; + const osqp::Solver::Info info = solver.get_info(); + std::cout << "obj_val = " << info.obj_val << "\n"; + std::cout << "prim_res = " << info.prim_res << "\n"; + std::cout << "dual_res = " << info.dual_res << "\n"; + + return 0; +} diff --git a/include/OSQP_solver.h b/include/OSQP_solver.h deleted file mode 100644 index c08f982..0000000 --- a/include/OSQP_solver.h +++ /dev/null @@ -1,144 +0,0 @@ -#pragma once - -#include -#include -using namespace Eigen; - -#include - -namespace M3 -{ - -class OSQP_Solver -{ - public: - struct Configuration - { - //Maximum number of ADMM iterations. See OSQPSettings::max_iter. - OSQPInt maximum_iterations = OSQP_MAX_ITER; - //Absolute/relative solution tolerances. See OSQPSettings::eps_abs/eps_rel. - OSQPFloat eps_absolute = OSQP_EPS_ABS; - OSQPFloat eps_relative = OSQP_EPS_REL; - //Primal/dual infeasibility tolerances. See OSQPSettings::eps_prim_inf/eps_dual_inf. - OSQPFloat eps_primal_infeasibility = OSQP_EPS_PRIM_INF; - OSQPFloat eps_dual_infeasibility = OSQP_EPS_DUAL_INF; - //boolean; write out solver progress. See OSQPSettings::verbose. - OSQPInt verbose = 0; - //boolean; polish ADMM solution. See OSQPSettings::polishing. - OSQPInt polishing = OSQP_POLISHING; - //boolean; use OSQP's own warm-starting between consecutive osqp_solve() calls. See OSQPSettings::warm_starting. - OSQPInt warm_starting = OSQP_WARM_STARTING; - //If true, and the problem dimensions/sparsity pattern are unchanged since the last call, - //solve_quadratic_program() will reuse the existing OSQPSolver instance and update its data - //in place (osqp_update_data_vec/osqp_update_data_mat) instead of calling osqp_setup() again. - bool use_hotstart = true; - Configuration(); //https://stackoverflow.com/questions/53408962/try-to-understand-compiler-error-message-default-member-initializer-required-be - }; - - /** - * @brief Named solution-quality values obtained from the last successful call to - * solve_quadratic_program(). See OSQPInfo in osqp_api_types.h for further details. - */ - struct Info - { - //Primal objective value. See OSQPInfo::obj_val. - OSQPFloat obj_val = 0.0; - //Dual objective value. See OSQPInfo::dual_obj_val. - OSQPFloat dual_obj_val = 0.0; - //Norm of the primal residual. See OSQPInfo::prim_res. - OSQPFloat prim_res = 0.0; - //Norm of the dual residual. See OSQPInfo::dual_res. - OSQPFloat dual_res = 0.0; - //Dual solution, i.e. the Lagrange multiplier associated with l <= Ax <= u. - //See OSQPSolution::y. - VectorXd dual_solution; - Info(); //https://stackoverflow.com/questions/53408962/try-to-understand-compiler-error-message-default-member-initializer-required-be - }; - protected: - //Holds the CSC (compressed-sparse-column) arrays of a converted Eigen matrix. - //The vectors own the storage so that it outlives the temporary OSQPCscMatrix - //wrapper used when calling osqp_setup()/osqp_update_data_mat(). - struct CSCMatrixData - { - std::vector x; - std::vector i; - std::vector p; - OSQPInt rows{0}; - OSQPInt cols{0}; - }; - - bool osqp_solve_first_time_; - ::OSQPSolver* osqp_solver_; - Configuration configuration_; - - //Dimensions used in the last successful osqp_setup() call. Used to detect whether - //a new call to solve_quadratic_program() is compatible with hotstarting or requires - //a fresh osqp_setup(). - OSQPInt problem_size_; - OSQPInt constraint_size_; - - //https://github.com/SmartArmStack/sas_conversions/blob/master/src/eigen3_std_conversions.cpp - //A copy from sas - std::vector _vectorxd_to_std_vector_double(const VectorXd& vectorxd); - - //Another copy from sas - VectorXd _std_vector_double_to_vectorxd(std::vector std_vector_double) const; - - //Converts a dense n x n matrix into the upper-triangular CSC representation required by OSQP for P. - static CSCMatrixData _dense_to_csc_upper_triangular(const MatrixXd& M); - - //Converts a dense m x n matrix into a structurally-dense CSC representation (i.e. every - //entry, including zeros, is stored explicitly). Keeping the sparsity pattern stable - //across calls is what allows osqp_update_data_mat() to be used when hotstarting. - static CSCMatrixData _dense_to_csc(const MatrixXd& M); - - //Releases the current osqp_solver_ instance, if any. - void _cleanup(); - - public: - OSQP_Solver(const Configuration& configuration = OSQP_Solver::Configuration()); - ~OSQP_Solver(); - - //Not copyable, as this class owns a raw ::OSQPSolver* that is not reference counted. - OSQP_Solver(const OSQP_Solver&) = delete; - OSQP_Solver& operator=(const OSQP_Solver&) = delete; - - /** - * @brief - * Solves the following quadratic program - * min(x) 0.5*x'Hx + f'x - * s.t. Ax <= b - * Aeqx = beq. - * Method signature is compatible with MATLAB's 'quadprog'. - * @param H the n x n matrix of the quadratic coefficients of the decision variables. - * @param f the n x 1 vector of the linear coefficients of the decision variables. - * @param A the m x n matrix of inequality constraints. - * @param b the m x 1 value for the inequality constraints. - * @param Aeq the m x n matrix of equality constraints. - * @param beq the m x 1 value for the inequality constraints. - * @param x0 optional n x 1 warm-start for the primal variable x, e.g. a known feasible - * solution. Passed to OSQP via osqp_warm_start(). Pass an empty vector (the default) - * to skip warm-starting. Must be compatible with H.rows() when provided. - * @param y0 optional warm-start for the dual variable y, e.g. a dual solution obtained - * from get_info().dual_solution in a previous call. Passed to OSQP via - * osqp_warm_start(). Pass an empty vector (the default) to skip dual warm-starting. - * Must have b.size()+beq.size() entries when provided. - * @return the optimal x - */ - VectorXd solve_quadratic_program(const MatrixXd& H, const VectorXd& f, const MatrixXd& A, const VectorXd& b, const MatrixXd& Aeq, const VectorXd& beq, const VectorXd& x0 = VectorXd(), const VectorXd& y0 = VectorXd()); - - /** - * @brief Returns named solution-quality values (obj_val, dual_obj_val, prim_res, dual_res) - * and the dual solution (dual_solution) from the last successful call to - * solve_quadratic_program(). - * @throws std::runtime_error if solve_quadratic_program() has not been called successfully yet. - * @return an Info instance with the values populated. - */ - Info get_info() const; - - VectorXd test_vectorxd(const VectorXd& v); - MatrixXd test_matrixxd(const MatrixXd& m); - -}; - -} diff --git a/include/marinholab/solvers/osqp.h b/include/marinholab/solvers/osqp.h new file mode 100644 index 0000000..14d735f --- /dev/null +++ b/include/marinholab/solvers/osqp.h @@ -0,0 +1,483 @@ +#pragma once + +#include +#include + +#include + +namespace marinholab +{ + +namespace solvers +{ + +namespace osqp +{ +// Keep the using-directive scoped to this namespace rather than global +// scope: a global `using namespace Eigen;` would also be active while +// is parsed, which is unnecessary and can surprise code that +// includes this header. +using namespace Eigen; + +/** + * @brief Holds all user-configurable solver options. + * + * Every `OSQPSettings` field is exposed here. Defaults match OSQP's own + * defaults for a standard double-precision, direct-solver build (see + * `osqp_set_default_settings()`), except `verbose`, which defaults to off so + * the solver is quiet by default. + * + * @note OSQP validates its settings at setup/update time and will reject a + * build with settings outside their allowed range (e.g. non-positive + * tolerances). + */ +struct Configuration +{ + // ------------------------------------------------------------------ + // Wrapper-specific option (not part of OSQP's `OSQPSettings`). + // ------------------------------------------------------------------ + + /** + * @brief Whether subsequent solves reuse the existing OSQP solver and + * update its data in place (`osqp_update_data_vec` / + * `osqp_update_data_mat`) instead of calling `osqp_setup()` again. + * + * This is analogous to qpOASES' `use_hotstart`. It only applies when the + * problem dimensions and sparsity pattern are unchanged since the last + * call; a change in shape always triggers a fresh `osqp_setup()`. + */ + bool use_hotstart = true; + + // ------------------------------------------------------------------ + // OSQP `OSQPSettings` fields (1:1 mapping, native snake_case names). + // ------------------------------------------------------------------ + + // Linear algebra settings + + /** + * @brief Device identifier; currently used for CUDA devices. + * @see `OSQPSettings::device` + */ + OSQPInt device = 0; + + /** + * @brief Linear system solver to use. + * @see `OSQPSettings::linsys_solver` + */ + ::osqp_linsys_solver_type linsys_solver = ::OSQP_DIRECT_SOLVER; + + // Control settings + + /** + * @brief Whether the solution is allocated in the solver during + * `osqp_setup()`. + * @see `OSQPSettings::allocate_solution` + */ + OSQPInt allocate_solution = 1; + + /** + * @brief Whether solver progress is written out. + * + * Defaults to off so the solver is quiet by default; note this + * intentionally differs from OSQP's own default (`OSQP_VERBOSE = 1`). + * @see `OSQPSettings::verbose` + */ + OSQPInt verbose = 0; + + /** + * @brief Level of detail for profiler annotations. + * @see `OSQPSettings::profiler_level` + */ + OSQPInt profiler_level = 0; + + /** + * @brief Whether OSQP warm-starts from the previous solution between + * consecutive `osqp_solve()` calls. + * @see `OSQPSettings::warm_starting` + */ + OSQPInt warm_starting = OSQP_WARM_STARTING; + + /** + * @brief Number of heuristic data-scaling iterations; `0` disables + * scaling. + * @see `OSQPSettings::scaling` + */ + OSQPInt scaling = OSQP_SCALING; + + /** + * @brief Whether the ADMM solution is polished to improve accuracy. + * @see `OSQPSettings::polishing` + */ + OSQPInt polishing = OSQP_POLISHING; + + // ADMM parameters + + /** + * @brief ADMM penalty parameter (scalar). + * @see `OSQPSettings::rho` + */ + OSQPFloat rho = OSQP_RHO; + + /** + * @brief Whether `rho` is a scalar or a vector. + * @see `OSQPSettings::rho_is_vec` + */ + OSQPInt rho_is_vec = OSQP_RHO_IS_VEC; + + /** + * @brief ADMM regularization parameter (improves conditioning). + * @see `OSQPSettings::sigma` + */ + OSQPFloat sigma = OSQP_SIGMA; + + /** + * @brief ADMM relaxation parameter. + * @see `OSQPSettings::alpha` + */ + OSQPFloat alpha = OSQP_ALPHA; + + // CG settings + + /** + * @brief Maximum number of CG iterations per solve. + * @see `OSQPSettings::cg_max_iter` + */ + OSQPInt cg_max_iter = OSQP_CG_MAX_ITER; + + /** + * @brief Number of consecutive zero CG iterations before the tolerance + * is halved. + * @see `OSQPSettings::cg_tol_reduction` + */ + OSQPInt cg_tol_reduction = OSQP_CG_TOL_REDUCTION; + + /** + * @brief CG tolerance, as a fraction of the ADMM residuals. + * @see `OSQPSettings::cg_tol_fraction` + */ + OSQPFloat cg_tol_fraction = OSQP_CG_TOL_FRACTION; + + /** + * @brief Preconditioner used by the CG method. + * @see `OSQPSettings::cg_precond` + */ + ::osqp_precond_type cg_precond = ::OSQP_DIAGONAL_PRECONDITIONER; + + // Adaptive rho logic + + /** + * @brief ADMM `rho` stepsize adaptation method. + * @see `OSQPSettings::adaptive_rho` + */ + OSQPInt adaptive_rho = OSQP_ADAPTIVE_RHO_UPDATE_DEFAULT; + + /** + * @brief Interval between `rho` adaptations (used when + * `adaptive_rho == OSQP_ADAPTIVE_RHO_UPDATE_ITERATIONS`). + * @see `OSQPSettings::adaptive_rho_interval` + */ + OSQPInt adaptive_rho_interval = OSQP_ADAPTIVE_RHO_INTERVAL; + + /** + * @brief Adaptation parameter controlling when non-fixed `rho` + * adaptations occur (fraction of setup time, or of the previous + * KKT error, depending on `adaptive_rho`). + * @see `OSQPSettings::adaptive_rho_fraction` + */ + OSQPFloat adaptive_rho_fraction = OSQP_ADAPTIVE_RHO_FRACTION; + + /** + * @brief Tolerance applied when adapting `rho`: the new `rho` must be + * this many times larger or smaller than the current one. + * @see `OSQPSettings::adaptive_rho_tolerance` + */ + OSQPFloat adaptive_rho_tolerance = OSQP_ADAPTIVE_RHO_TOLERANCE; + + // Termination parameters + + /** + * @brief Maximum number of ADMM iterations. + * @see `OSQPSettings::max_iter` + */ + OSQPInt max_iter = OSQP_MAX_ITER; + + /** + * @brief Absolute solution tolerance. + * @see `OSQPSettings::eps_abs` + */ + OSQPFloat eps_abs = OSQP_EPS_ABS; + + /** + * @brief Relative solution tolerance. + * @see `OSQPSettings::eps_rel` + */ + OSQPFloat eps_rel = OSQP_EPS_REL; + + /** + * @brief Primal infeasibility detection tolerance. + * @see `OSQPSettings::eps_prim_inf` + */ + OSQPFloat eps_prim_inf = OSQP_EPS_PRIM_INF; + + /** + * @brief Dual infeasibility detection tolerance. + * @see `OSQPSettings::eps_dual_inf` + */ + OSQPFloat eps_dual_inf = OSQP_EPS_DUAL_INF; + + /** + * @brief Whether the scaled termination criteria are used. + * @see `OSQPSettings::scaled_termination` + */ + OSQPInt scaled_termination = OSQP_SCALED_TERMINATION; + + /** + * @brief Interval at which termination is checked; `0` disables the + * periodic check. + * @see `OSQPSettings::check_termination` + */ + OSQPInt check_termination = OSQP_CHECK_TERMINATION; + + /** + * @brief Whether the duality-gap termination criteria are used. + * @see `OSQPSettings::check_dualgap` + */ + OSQPInt check_dualgap = OSQP_CHECK_DUALGAP; + + /** + * @brief Maximum time to solve the problem, in seconds. + * @see `OSQPSettings::time_limit` + */ + OSQPFloat time_limit = OSQP_TIME_LIMIT; + + // Polishing parameters + + /** + * @brief Regularization parameter used by polishing. + * @see `OSQPSettings::delta` + */ + OSQPFloat delta = OSQP_DELTA; + + /** + * @brief Number of iterative refinement steps performed during + * polishing. + * @see `OSQPSettings::polish_refine_iter` + */ + OSQPInt polish_refine_iter = OSQP_POLISH_REFINE_ITER; + + /** + * @brief Default constructor. + * + * Initialises every option to the defaults documented above. + * + * @note Declared here and defined out of line so that the in-class + * member initializers are used as expected. See + * https://stackoverflow.com/questions/53408962 + */ + Configuration(); +}; + +/** + * @brief High-level, reusable solver for quadratic programs (QPs) based on + * OSQP. + * + * `Solver` exposes OSQP's first-order ADMM solver through a + * MATLAB/`quadprog`-like, matrix-based interface. Internally it keeps an + * OSQP `OSQPSolver` object so that, once initialised, subsequent calls are + * warm-started (see `use_hotstart` and `warm_starting`). + * + * The solver is configured through a `Configuration` structure whose members + * map directly onto OSQP's `OSQPSettings` struct. In addition to the standard + * OSQP settings, the configuration carries the wrapper-specific `use_hotstart` + * setting. + * + * @note The class is not thread-safe: a single instance owns one underlying + * OSQP solver and its state changes across calls. + */ +class Solver +{ + protected: + /** @brief True until the first solve has set up the solver. */ + bool osqp_solve_first_time_; + /** @brief The underlying OSQP solver. */ + ::OSQPSolver* osqp_solver_; + /** @brief Active configuration, copied at construction. */ + Configuration configuration_; + + /** + * @brief Dimensions used in the last successful `osqp_setup()` call. + * Used to detect whether a new call is compatible with hotstarting or + * requires a fresh `osqp_setup()`. + */ + OSQPInt problem_size_; + OSQPInt constraint_size_; + + /** + * @brief Holds the CSC (compressed-sparse-column) arrays of a + * converted Eigen matrix. + * + * The vectors own the storage so that it outlives the temporary + * `OSQPCscMatrix` wrapper used when calling `osqp_setup()` / + * `osqp_update_data_mat()`. + */ + struct CSCMatrixData + { + std::vector x; + std::vector i; + std::vector p; + OSQPInt rows{0}; + OSQPInt cols{0}; + }; + + /** + * @brief Copies an Eigen vector into a std::vector. + * @param vectorxd Source vector. + * @return A copy of the vector's data. + * + * Copied from SmartArmStack's sas_conversions + * (https://github.com/SmartArmStack/sas_conversions/blob/master/src/eigen3_std_conversions.cpp). + */ + std::vector _vectorxd_to_std_vector_double(const VectorXd& vectorxd); + + /** + * @brief Maps a std::vector into an Eigen vector. + * @param std_vector_double Source vector. + * @return An Eigen vector wrapping the source data. + */ + VectorXd _std_vector_double_to_vectorxd(std::vector std_vector_double) const; + + /** + * @brief Converts a dense n x n matrix into the upper-triangular CSC + * representation required by OSQP for P. + * @param M Source matrix. + * @return The CSC arrays of the (upper triangle of the) matrix. + */ + static CSCMatrixData _dense_to_csc_upper_triangular(const MatrixXd& M); + + /** + * @brief Converts a dense m x n matrix into a structurally-dense CSC + * representation (i.e. every entry, including zeros, is stored + * explicitly). + * + * Keeping the sparsity pattern stable across calls is what allows + * `osqp_update_data_mat()` to be used when hotstarting. + * @param M Source matrix. + * @return The CSC arrays of the matrix. + */ + static CSCMatrixData _dense_to_csc(const MatrixXd& M); + + /** + * @brief Translates the user configuration into an `OSQPSettings` + * object. + * @return An `OSQPSettings` with all fields filled from + * `configuration_`. + */ + OSQPSettings _to_osqp_settings() const; + + /** + * @brief Releases the current `osqp_solver_` instance, if any. + */ + void _cleanup(); + + public: + /** + * @brief Named solution-quality values obtained from the last + * successful call to solve_quadratic_program(). + * + * See `OSQPInfo` in osqp_api_types.h for further details. + */ + struct Info + { + //Primal objective value. See OSQPInfo::obj_val. + OSQPFloat obj_val = 0.0; + //Dual objective value. See OSQPInfo::dual_obj_val. + OSQPFloat dual_obj_val = 0.0; + //Norm of the primal residual. See OSQPInfo::prim_res. + OSQPFloat prim_res = 0.0; + //Norm of the dual residual. See OSQPInfo::dual_res. + OSQPFloat dual_res = 0.0; + //Dual solution, i.e. the Lagrange multiplier associated with l <= Ax <= u. + //See OSQPSolution::y. + VectorXd dual_solution; + Info(); //https://stackoverflow.com/questions/53408962/try-to-understand-compiler-error-message-default-member-initializer-required-be + }; + + /** + * @brief Constructs a solver. + * @param configuration Options to use; defaults to the default + * configuration. + */ + Solver(const Configuration& configuration = Configuration()); + + /** @brief Destructor; releases the underlying OSQP solver. */ + ~Solver(); + + //Not copyable, as this class owns a raw ::OSQPSolver* that is not reference counted. + Solver(const Solver&) = delete; + Solver& operator=(const Solver&) = delete; + + /** + * @brief Solves the following quadratic program: + * + * min(x) 0.5*x'Hx + f'x + * s.t. Ax <= b + * Aeq*x = beq. + * + * Method signature is compatible with MATLAB's `quadprog`. + * + * @param H the n x n matrix of the quadratic coefficients of the + * decision variables. + * @param f the n x 1 vector of the linear coefficients of the + * decision variables. + * @param A the m x n matrix of inequality constraints. + * @param b the m x 1 value for the inequality constraints. + * @param Aeq the k x n matrix of equality constraints. + * @param beq the k x 1 value for the equality constraints. + * @param x0 optional n x 1 warm-start for the primal variable x, e.g. a + * known feasible solution. Passed to OSQP via + * `osqp_warm_start()`. Pass an empty vector (the default) to + * skip warm-starting. Must be compatible with H.rows() when + * provided. + * @param y0 optional warm-start for the dual variable y, e.g. a dual + * solution obtained from get_info().dual_solution in a previous + * call. Passed to OSQP via `osqp_warm_start()`. Pass an empty + * vector (the default) to skip dual warm-starting. Must have + * b.size()+beq.size() entries when provided. + * @return the optimal x. + * @throws std::runtime_error if any matrix is size-incompatible or + * OSQP fails to solve the problem. + */ + VectorXd solve_quadratic_program(const MatrixXd& H, const VectorXd& f, const MatrixXd& A, const VectorXd& b, const MatrixXd& Aeq, const VectorXd& beq, const VectorXd& x0 = VectorXd(), const VectorXd& y0 = VectorXd()); + + /** + * @brief Returns named solution-quality values (obj_val, dual_obj_val, + * prim_res, dual_res) and the dual solution (dual_solution) from + * the last successful call to solve_quadratic_program(). + * @throws std::runtime_error if solve_quadratic_program() has not been + * called successfully yet. + * @return an Info instance with the values populated. + */ + Info get_info() const; + + /** + * @brief Round-trips a vector to help evaluate the Eigen <-> std + * conversions used across the wrapper. + * @param v The vector to test. + * @return The same vector. + */ + VectorXd test_vectorxd(const VectorXd& v); + + /** + * @brief Round-trips a matrix to help evaluate the Eigen <-> std + * conversions used across the wrapper. + * @param m The matrix to test. + * @return The same matrix. + */ + MatrixXd test_matrixxd(const MatrixXd& m); +}; + +} // namespace osqp + +} // namespace solvers + +} // namespace marinholab diff --git a/marinholab/solvers/osqp/__init__.py b/marinholab/solvers/osqp/__init__.py index 1ac50c4..5f0fe34 100644 --- a/marinholab/solvers/osqp/__init__.py +++ b/marinholab/solvers/osqp/__init__.py @@ -1,10 +1,32 @@ """ Copyright (C) 2025 Murilo Marques Marinho (www.murilomarinho.info) LGPLv2.1 License + +Public API of the `marinholab.solvers.osqp` package. + +`Solver` is a thin, numpy-friendly Python wrapper around the compiled OSQP +solver (`OSQP_Solver`). The configuration and enum types are re-exported for +convenience. """ from .solver import Solver # TODO change this mess into inheritance via trampoline class # Interface won't change, so this will do for now from marinholab.solvers.osqp._core import OSQP_Solver + +# Re-exported for convenience so users can write e.g. `osqp.Configuration` +# and `osqp.OSQP_Solver.Status.OSQP_SOLVED`. Configuration = OSQP_Solver.Configuration Info = OSQP_Solver.Info +LinsysSolverType = OSQP_Solver.LinsysSolverType +PreconditionerType = OSQP_Solver.PreconditionerType +Status = OSQP_Solver.Status + +__all__ = [ + "Solver", + "OSQP_Solver", + "Configuration", + "Info", + "LinsysSolverType", + "PreconditionerType", + "Status", +] diff --git a/marinholab/solvers/osqp/_core.pyi b/marinholab/solvers/osqp/_core.pyi new file mode 100644 index 0000000..f4b0d4a --- /dev/null +++ b/marinholab/solvers/osqp/_core.pyi @@ -0,0 +1,190 @@ +"""Type stubs for the compiled `_core` pybind11 extension module. + +The real module is built from ``src/core.cpp``; this file only exists so that +type checkers (e.g. Pyright) can understand the public surface of the +extension without having to parse C++. +""" + +from enum import IntEnum + +import numpy as np + + +class LinsysSolverType(IntEnum): + """OSQP linear system solvers.""" + + OSQP_UNKNOWN_SOLVER: LinsysSolverType + OSQP_DIRECT_SOLVER: LinsysSolverType + OSQP_INDIRECT_SOLVER: LinsysSolverType + + +class PreconditionerType(IntEnum): + """Preconditioners for the conjugate-gradient method.""" + + OSQP_NO_PRECONDITIONER: PreconditionerType + OSQP_DIAGONAL_PRECONDITIONER: PreconditionerType + + +class Status(IntEnum): + """OSQP solver status codes returned for the last solve.""" + + OSQP_SOLVED: Status + OSQP_SOLVED_INACCURATE: Status + OSQP_PRIMAL_INFEASIBLE: Status + OSQP_PRIMAL_INFEASIBLE_INACCURATE: Status + OSQP_DUAL_INFEASIBLE: Status + OSQP_DUAL_INFEASIBLE_INACCURATE: Status + OSQP_MAX_ITER_REACHED: Status + OSQP_TIME_LIMIT_REACHED: Status + OSQP_NON_CVX: Status + OSQP_SIGINT: Status + OSQP_UNSOLVED: Status + + +class OSQP_Solver: + """High-level, reusable solver for quadratic programs (QPs) based on OSQP. + + Solves ``min(x) 0.5*x'Hx + f'x`` subject to ``Ax <= b`` and + ``Aeq*x = beq`` (MATLAB `quadprog`-like signature). Once the problem has + been solved once, subsequent solves are warm-started by default (see + ``Configuration.use_hotstart``). + """ + + # Nested aliases so the enums are also reachable as + # ``OSQP_Solver.LinsysSolverType`` etc. (matching the runtime layout, where + # ``export_values()`` binds them onto the class). + LinsysSolverType: type[LinsysSolverType] = LinsysSolverType + PreconditionerType: type[PreconditionerType] = PreconditionerType + Status: type[Status] = Status + + class Configuration: + """All user-configurable solver options. + + Members are a 1:1 mapping of OSQP's ``OSQPSettings`` fields (plus the + wrapper-specific ``use_hotstart``). Defaults match OSQP's own defaults + for a standard double-precision, direct-solver build (see + ``osqp_set_default_settings()``); see the C++ header + (``include/marinholab/solvers/osqp.h``) and the OSQP documentation + (https://osqp.org/docs/) for the meaning of each option. + """ + + #: Reuse the existing OSQP solver and update its data in place instead of re-running osqp_setup() when the problem shape is unchanged. + use_hotstart: bool + #: Device identifier; currently used for CUDA devices. + device: int + #: Linear system solver to use. + linsys_solver: LinsysSolverType + #: Whether the solution is allocated during osqp_setup(). + allocate_solution: int + #: Whether solver progress is written out (0 = quiet, the default). + verbose: int + #: Level of detail for profiler annotations. + profiler_level: int + #: Whether OSQP warm-starts from the previous solution between consecutive osqp_solve() calls. + warm_starting: int + #: Number of heuristic data-scaling iterations; 0 disables scaling. + scaling: int + #: Whether the ADMM solution is polished to improve accuracy. + polishing: int + #: ADMM penalty parameter (scalar). + rho: float + #: Whether rho is a scalar or a vector. + rho_is_vec: int + #: ADMM regularization parameter (improves conditioning). + sigma: float + #: ADMM relaxation parameter. + alpha: float + #: Maximum number of CG iterations per solve. + cg_max_iter: int + #: Number of consecutive zero CG iterations before the tolerance is halved. + cg_tol_reduction: int + #: CG tolerance, as a fraction of the ADMM residuals. + cg_tol_fraction: float + #: Preconditioner used by the CG method. + cg_precond: PreconditionerType + #: ADMM rho stepsize adaptation method. + adaptive_rho: int + #: Interval between rho adaptations (used with the iterations-based method). + adaptive_rho_interval: int + #: Adaptation parameter controlling when non-fixed rho adaptations occur. + adaptive_rho_fraction: float + #: Tolerance applied when adapting rho (min ratio between new and current rho). + adaptive_rho_tolerance: float + #: Maximum number of ADMM iterations. + max_iter: int + #: Absolute solution tolerance. + eps_abs: float + #: Relative solution tolerance. + eps_rel: float + #: Primal infeasibility detection tolerance. + eps_prim_inf: float + #: Dual infeasibility detection tolerance. + eps_dual_inf: float + #: Whether the scaled termination criteria are used. + scaled_termination: int + #: Interval at which termination is checked; 0 disables the periodic check. + check_termination: int + #: Whether the duality-gap termination criteria are used. + check_dualgap: int + #: Maximum time to solve the problem, in seconds. + time_limit: float + #: Regularization parameter used by polishing. + delta: float + #: Number of iterative refinement steps performed during polishing. + polish_refine_iter: int + + def __init__(self) -> None: ... + + class Info: + """Solution-quality values obtained from the last successful call to ``solve_quadratic_program()``.""" + + #: Primal objective value. + obj_val: float + #: Dual objective value. + dual_obj_val: float + #: Norm of the primal residual. + prim_res: float + #: Norm of the dual residual. + dual_res: float + #: Dual solution, i.e. the Lagrange multiplier associated with l <= Ax <= u. + dual_solution: np.ndarray + + def __init__(self) -> None: ... + + def __init__(self, configuration: Configuration | None = None) -> None: + """Constructs a solver with the given configuration (defaults to the default configuration).""" + ... + + def solve_quadratic_program( + self, + H: np.ndarray, + f: np.ndarray, + A: np.ndarray, + b: np.ndarray, + Aeq: np.ndarray, + beq: np.ndarray, + x0: np.ndarray = ..., + y0: np.ndarray = ..., + ) -> np.ndarray: + """Solves ``min(x) 0.5*x'Hx + f'x s.t. Ax <= b, Aeq*x = beq``. + + Method signature is compatible with MATLAB's ``quadprog``. Returns the + optimal ``x``. ``x0`` and ``y0`` are optional warm-starts for the primal + and dual variables (pass an empty array to skip warm-starting). + """ + ... + + def get_info(self) -> Info: + """Returns the solution-quality values and dual solution from the last successful solve.""" + ... + + def test_vectorxd(self, v: np.ndarray) -> np.ndarray: + """Round-trips a vector to help evaluate the Eigen <-> std conversions.""" + ... + + def test_matrixxd(self, m: np.ndarray) -> np.ndarray: + """Round-trips a matrix to help evaluate the Eigen <-> std conversions.""" + ... + + +__version__: str diff --git a/marinholab/solvers/osqp/example.py b/marinholab/solvers/osqp/example.py index 1dcf8f7..b0af051 100644 --- a/marinholab/solvers/osqp/example.py +++ b/marinholab/solvers/osqp/example.py @@ -62,9 +62,9 @@ def positivedefinite(): def configuration_example(): config = osqp.Configuration() - config.eps_absolute = 1e-5 - config.eps_relative = 1e-5 - config.maximum_iterations = 10000 + config.eps_abs = 1e-5 + config.eps_rel = 1e-5 + config.max_iter = 10000 config.verbose = 0 solver = osqp.Solver(config) @@ -212,9 +212,9 @@ def hierarchical_example(): # value (Aeq = J1, beq = J1@u1) is enforced much more precisely, i.e. # J1@u2 ends up much closer to J1@u1 than with the default tolerances. config = osqp.Configuration() - config.eps_absolute = 1e-9 - config.eps_relative = 1e-9 - config.maximum_iterations = 20000 + config.eps_abs = 1e-9 + config.eps_rel = 1e-9 + config.max_iter = 20000 solver_1 = osqp.Solver(config) solver_2 = osqp.Solver(config) diff --git a/marinholab/solvers/osqp/example_kinematics.py b/marinholab/solvers/osqp/example_kinematics.py index c29f13c..f534776 100644 --- a/marinholab/solvers/osqp/example_kinematics.py +++ b/marinholab/solvers/osqp/example_kinematics.py @@ -14,11 +14,11 @@ pip install --pre dqrobotics dqrobotics-pyplot """ try: - import matplotlib.pyplot as plt + import matplotlib.pyplot as plt # type: ignore[reportMissingImports] import numpy as np - from dqrobotics import i_, translation, vec4 - from dqrobotics.robots import KukaLw4Robot - import dqrobotics_extensions.pyplot as dqp + from dqrobotics import i_, translation, vec4 # type: ignore[reportAttributeAccessIssue] + from dqrobotics.robots import KukaLw4Robot # type: ignore[reportAttributeAccessIssue] + import dqrobotics_extensions.pyplot as dqp # type: ignore[reportAttributeAccessIssue] except ImportError as e: raise ImportError( "This example requires the optional dependencies `dqrobotics` and " @@ -62,9 +62,9 @@ def main(): # that the level-2 equality constraint that reproduces the level-1 task # (Aeq = Jt, beq = Jt @ u1) is enforced much more precisely. config = osqp.Configuration() - config.eps_absolute = 1e-9 - config.eps_relative = 1e-9 - config.maximum_iterations = 20000 + config.eps_abs = 1e-9 + config.eps_rel = 1e-9 + config.max_iter = 20000 solver_1 = osqp.Solver(config) # Level 1: end-effector position control solver_2 = osqp.Solver(config) # Level 2: redundancy resolution diff --git a/marinholab/solvers/osqp/py.typed b/marinholab/solvers/osqp/py.typed new file mode 100644 index 0000000..e69de29 diff --git a/marinholab/solvers/osqp/solver.py b/marinholab/solvers/osqp/solver.py index 95ebf91..63942ba 100644 --- a/marinholab/solvers/osqp/solver.py +++ b/marinholab/solvers/osqp/solver.py @@ -1,41 +1,70 @@ +from __future__ import annotations + import numpy as np from marinholab.solvers.osqp._core import OSQP_Solver + class Solver: - def __init__(self, configuration=OSQP_Solver.Configuration()): - self.configuration = configuration - self.solver = OSQP_Solver(configuration) + """Thin, numpy-friendly wrapper around the compiled OSQP solver. + + Accepts ``None`` for the constraint matrices ``A``/``b``/``Aeq``/``beq`` + and converts them into suitably sized zero matrices before calling the + underlying solver. All matrices are given as dense, row-major numpy + arrays (or anything numpy can treat as one). + """ - def solve_quadratic_program(self, H, f, A, b, Aeq, beq, x0=None, y0=None): + def __init__(self, configuration: OSQP_Solver.Configuration | None = None) -> None: + self.configuration: OSQP_Solver.Configuration = ( + configuration if configuration is not None else OSQP_Solver.Configuration() + ) + self.solver: OSQP_Solver = OSQP_Solver(self.configuration) + + def solve_quadratic_program( + self, + H: np.ndarray, + f: np.ndarray, + A: np.ndarray | None, + b: np.ndarray | None, + Aeq: np.ndarray | None, + beq: np.ndarray | None, + x0: np.ndarray | None = None, + y0: np.ndarray | None = None, + ) -> np.ndarray: + """Solves ``min(x) 0.5*x'Hx + f'x`` subject to ``Ax <= b`` and ``Aeq*x = beq``. + + Any of ``A``/``b``/``Aeq``/``beq`` may be ``None`` (meaning "no such + constraint"), but the matrix and its right-hand side must be provided + together. ``x0`` and ``y0`` are optional warm-starts for the primal and + dual variables; pass ``None`` (the default) to skip warm-starting. + Returns the optimal ``x`` as a 1-D numpy array. + """ - if (A is None and b is not None) or (b is None and A is not None): + if (A is None) != (b is None): raise ValueError(f"A={A} and b={b} must both be None or both not None.") - if (Aeq is None and beq is not None) or (beq is None and Aeq is not None): + if (Aeq is None) != (beq is None): raise ValueError(f"Aeq={Aeq} and beq={beq} must both be None or both not None.") - if A is None: - A = np.zeros((1,H.shape[0])) - b = np.zeros((1,)) - if Aeq is None: - Aeq = np.zeros((1,H.shape[0])) - beq = np.zeros((1,)) + # The solver requires all six matrices; replace the ``None`` + # constraints with a single trivially satisfied zero row. + A_full = np.zeros((1, H.shape[0])) if A is None else A + b_full = np.zeros((1,)) if b is None else b + Aeq_full = np.zeros((1, H.shape[0])) if Aeq is None else Aeq + beq_full = np.zeros((1,)) if beq is None else beq # x0 is an optional warm-start (e.g. a known feasible solution) for the primal # variable. When omitted, an empty array is forwarded and OSQP solves without # warm-starting. - if x0 is None: - x0 = np.zeros((0,)) + x0_full = np.zeros((0,)) if x0 is None else x0 # y0 is an optional warm-start (e.g. a dual solution obtained from # get_info().dual_solution in a previous call) for the dual variable. When omitted, # an empty array is forwarded and OSQP solves without dual warm-starting. Its size # must match b.size()+beq.size() as actually sent to the solver above. - if y0 is None: - y0 = np.zeros((0,)) + y0_full = np.zeros((0,)) if y0 is None else y0 - return self.solver.solve_quadratic_program(H, f, A, b, Aeq, beq, x0, y0) + return self.solver.solve_quadratic_program(H, f, A_full, b_full, Aeq_full, beq_full, x0_full, y0_full) - def get_info(self): + def get_info(self) -> OSQP_Solver.Info: """ Returns named solution-quality values (obj_val, dual_obj_val, prim_res, dual_res) and the dual solution (dual_solution) from the last successful call to diff --git a/pyrightconfig.json b/pyrightconfig.json new file mode 100644 index 0000000..9dce96d --- /dev/null +++ b/pyrightconfig.json @@ -0,0 +1,7 @@ +{ + "include": ["marinholab"], + "pythonVersion": "3.9", + "pythonPlatform": "Linux", + "useLibraryCodeForTypes": false, + "typeCheckingMode": "basic" +} diff --git a/setup.py b/setup.py index 59c6eae..36c688e 100644 --- a/setup.py +++ b/setup.py @@ -137,6 +137,9 @@ def build_extension(self, ext: CMakeExtension) -> None: packages=[ "marinholab.solvers.osqp", ], + package_data={ + "marinholab.solvers.osqp": ["_core.pyi", "py.typed"], + }, ext_modules=[CMakeExtension('marinholab.solvers.osqp._core')], cmdclass={"build_ext": CMakeBuild}, zip_safe=False, diff --git a/src/core.cpp b/src/core.cpp index 6df6450..1ea1e24 100644 --- a/src/core.cpp +++ b/src/core.cpp @@ -1,5 +1,7 @@ /** (C) Copyright 2025-26 Murilo Marinho (murilomarinho@ieee.org) + +pybind11 bindings for marinholab::solvers::osqp::Solver. */ #include @@ -8,51 +10,136 @@ #include #include -#include +#include #define STRINGIFY(x) #x #define MACRO_STRINGIFY(x) STRINGIFY(x) namespace py = pybind11; -using namespace M3; +using namespace marinholab::solvers::osqp; PYBIND11_MODULE(_core, m) { - py::class_ osqp_solver(m, "OSQP_Solver"); + m.doc() = "Python bindings for an OSQP-based quadratic program solver."; + + py::class_ osqp_solver(m, "OSQP_Solver", + "High-level, reusable solver for quadratic programs (QPs) based on OSQP.\n\n" + "Solves the following problem:\n\n" + " min(x) 0.5*x'Hx + f'x\n" + " s.t. Ax <= b\n" + " Aeq*x = beq\n\n" + "Method signature is compatible with MATLAB's `quadprog`. Once the\n" + "problem has been solved once, subsequent solves are warm-started by\n" + "default (see `Configuration.use_hotstart`)."); + + py::enum_<::osqp_linsys_solver_type>(osqp_solver, "LinsysSolverType", + "OSQP linear system solvers.") + .value("OSQP_UNKNOWN_SOLVER", ::OSQP_UNKNOWN_SOLVER) + .value("OSQP_DIRECT_SOLVER", ::OSQP_DIRECT_SOLVER) + .value("OSQP_INDIRECT_SOLVER", ::OSQP_INDIRECT_SOLVER) + .export_values(); + + py::enum_<::osqp_precond_type>(osqp_solver, "PreconditionerType", + "Preconditioners for the conjugate-gradient method.") + .value("OSQP_NO_PRECONDITIONER", ::OSQP_NO_PRECONDITIONER) + .value("OSQP_DIAGONAL_PRECONDITIONER", ::OSQP_DIAGONAL_PRECONDITIONER) + .export_values(); + + py::enum_<::osqp_status_type>(osqp_solver, "Status", + "OSQP solver status codes returned for the last solve.") + .value("OSQP_SOLVED", ::OSQP_SOLVED) + .value("OSQP_SOLVED_INACCURATE", ::OSQP_SOLVED_INACCURATE) + .value("OSQP_PRIMAL_INFEASIBLE", ::OSQP_PRIMAL_INFEASIBLE) + .value("OSQP_PRIMAL_INFEASIBLE_INACCURATE", ::OSQP_PRIMAL_INFEASIBLE_INACCURATE) + .value("OSQP_DUAL_INFEASIBLE", ::OSQP_DUAL_INFEASIBLE) + .value("OSQP_DUAL_INFEASIBLE_INACCURATE", ::OSQP_DUAL_INFEASIBLE_INACCURATE) + .value("OSQP_MAX_ITER_REACHED", ::OSQP_MAX_ITER_REACHED) + .value("OSQP_TIME_LIMIT_REACHED", ::OSQP_TIME_LIMIT_REACHED) + .value("OSQP_NON_CVX", ::OSQP_NON_CVX) + .value("OSQP_SIGINT", ::OSQP_SIGINT) + .value("OSQP_UNSOLVED", ::OSQP_UNSOLVED) + .export_values(); + + py::class_ osqp_configuration(osqp_solver, "Configuration", + "All user-configurable solver options.\n\n" + "Members are a 1:1 mapping of OSQP's `OSQPSettings` fields (plus the\n" + "wrapper-specific `use_hotstart`). See the OSQP documentation for a\n" + "full description of each option: https://osqp.org/docs/"); - py::class_ osqp_configuration(osqp_solver, "Configuration"); osqp_configuration.def(py::init<>()); - osqp_configuration.def_readwrite("maximum_iterations", &OSQP_Solver::Configuration::maximum_iterations); - osqp_configuration.def_readwrite("eps_absolute", &OSQP_Solver::Configuration::eps_absolute); - osqp_configuration.def_readwrite("eps_relative", &OSQP_Solver::Configuration::eps_relative); - osqp_configuration.def_readwrite("eps_primal_infeasibility", &OSQP_Solver::Configuration::eps_primal_infeasibility); - osqp_configuration.def_readwrite("eps_dual_infeasibility", &OSQP_Solver::Configuration::eps_dual_infeasibility); - osqp_configuration.def_readwrite("verbose", &OSQP_Solver::Configuration::verbose); - osqp_configuration.def_readwrite("polishing", &OSQP_Solver::Configuration::polishing); - osqp_configuration.def_readwrite("warm_starting", &OSQP_Solver::Configuration::warm_starting); - osqp_configuration.def_readwrite("use_hotstart", &OSQP_Solver::Configuration::use_hotstart); - - py::class_ osqp_info(osqp_solver, "Info"); + + // Wrapper-specific option + osqp_configuration.def_readwrite("use_hotstart", &Configuration::use_hotstart, + "Reuse the existing OSQP solver and update its data in place instead of re-running osqp_setup() when the problem shape is unchanged."); + + // Linear algebra settings + osqp_configuration.def_readwrite("device", &Configuration::device, "Device identifier; currently used for CUDA devices."); + osqp_configuration.def_readwrite("linsys_solver", &Configuration::linsys_solver, "Linear system solver to use."); + // Control settings + osqp_configuration.def_readwrite("allocate_solution", &Configuration::allocate_solution, "Whether the solution is allocated during osqp_setup()."); + osqp_configuration.def_readwrite("verbose", &Configuration::verbose, "Whether solver progress is written out (0 = quiet, the default)."); + osqp_configuration.def_readwrite("profiler_level", &Configuration::profiler_level, "Level of detail for profiler annotations."); + osqp_configuration.def_readwrite("warm_starting", &Configuration::warm_starting, "Whether OSQP warm-starts from the previous solution between consecutive osqp_solve() calls."); + osqp_configuration.def_readwrite("scaling", &Configuration::scaling, "Number of heuristic data-scaling iterations; 0 disables scaling."); + osqp_configuration.def_readwrite("polishing", &Configuration::polishing, "Whether the ADMM solution is polished to improve accuracy."); + // ADMM parameters + osqp_configuration.def_readwrite("rho", &Configuration::rho, "ADMM penalty parameter (scalar)."); + osqp_configuration.def_readwrite("rho_is_vec", &Configuration::rho_is_vec, "Whether rho is a scalar or a vector."); + osqp_configuration.def_readwrite("sigma", &Configuration::sigma, "ADMM regularization parameter (improves conditioning)."); + osqp_configuration.def_readwrite("alpha", &Configuration::alpha, "ADMM relaxation parameter."); + // CG settings + osqp_configuration.def_readwrite("cg_max_iter", &Configuration::cg_max_iter, "Maximum number of CG iterations per solve."); + osqp_configuration.def_readwrite("cg_tol_reduction", &Configuration::cg_tol_reduction, "Number of consecutive zero CG iterations before the tolerance is halved."); + osqp_configuration.def_readwrite("cg_tol_fraction", &Configuration::cg_tol_fraction, "CG tolerance, as a fraction of the ADMM residuals."); + osqp_configuration.def_readwrite("cg_precond", &Configuration::cg_precond, "Preconditioner used by the CG method."); + // Adaptive rho logic + osqp_configuration.def_readwrite("adaptive_rho", &Configuration::adaptive_rho, "ADMM rho stepsize adaptation method."); + osqp_configuration.def_readwrite("adaptive_rho_interval", &Configuration::adaptive_rho_interval, "Interval between rho adaptations (used with the iterations-based method)."); + osqp_configuration.def_readwrite("adaptive_rho_fraction", &Configuration::adaptive_rho_fraction, "Adaptation parameter controlling when non-fixed rho adaptations occur."); + osqp_configuration.def_readwrite("adaptive_rho_tolerance", &Configuration::adaptive_rho_tolerance, "Tolerance applied when adapting rho (min ratio between new and current rho)."); + // Termination parameters + osqp_configuration.def_readwrite("max_iter", &Configuration::max_iter, "Maximum number of ADMM iterations."); + osqp_configuration.def_readwrite("eps_abs", &Configuration::eps_abs, "Absolute solution tolerance."); + osqp_configuration.def_readwrite("eps_rel", &Configuration::eps_rel, "Relative solution tolerance."); + osqp_configuration.def_readwrite("eps_prim_inf", &Configuration::eps_prim_inf, "Primal infeasibility detection tolerance."); + osqp_configuration.def_readwrite("eps_dual_inf", &Configuration::eps_dual_inf, "Dual infeasibility detection tolerance."); + osqp_configuration.def_readwrite("scaled_termination", &Configuration::scaled_termination, "Whether the scaled termination criteria are used."); + osqp_configuration.def_readwrite("check_termination", &Configuration::check_termination, "Interval at which termination is checked; 0 disables the periodic check."); + osqp_configuration.def_readwrite("check_dualgap", &Configuration::check_dualgap, "Whether the duality-gap termination criteria are used."); + osqp_configuration.def_readwrite("time_limit", &Configuration::time_limit, "Maximum time to solve the problem, in seconds."); + // Polishing parameters + osqp_configuration.def_readwrite("delta", &Configuration::delta, "Regularization parameter used by polishing."); + osqp_configuration.def_readwrite("polish_refine_iter", &Configuration::polish_refine_iter, "Number of iterative refinement steps performed during polishing."); + + py::class_ osqp_info(osqp_solver, "Info", + "Solution-quality values obtained from the last successful call to " + "solve_quadratic_program()."); osqp_info.def(py::init<>()); - osqp_info.def_readonly("obj_val", &OSQP_Solver::Info::obj_val); - osqp_info.def_readonly("dual_obj_val", &OSQP_Solver::Info::dual_obj_val); - osqp_info.def_readonly("prim_res", &OSQP_Solver::Info::prim_res); - osqp_info.def_readonly("dual_res", &OSQP_Solver::Info::dual_res); - osqp_info.def_readonly("dual_solution", &OSQP_Solver::Info::dual_solution); - - osqp_solver.def(py::init(), - py::arg("configuration") = OSQP_Solver::Configuration()); - osqp_solver.def("solve_quadratic_program",&OSQP_Solver::solve_quadratic_program,".", + osqp_info.def_readonly("obj_val", &Solver::Info::obj_val, "Primal objective value."); + osqp_info.def_readonly("dual_obj_val", &Solver::Info::dual_obj_val, "Dual objective value."); + osqp_info.def_readonly("prim_res", &Solver::Info::prim_res, "Norm of the primal residual."); + osqp_info.def_readonly("dual_res", &Solver::Info::dual_res, "Norm of the dual residual."); + osqp_info.def_readonly("dual_solution", &Solver::Info::dual_solution, + "Dual solution, i.e. the Lagrange multiplier associated with l <= Ax <= u."); + + osqp_solver.def(py::init(), + py::arg("configuration") = Configuration(), + "Constructs a solver with the given configuration (defaults to the default configuration)."); + osqp_solver.def("solve_quadratic_program", + &Solver::solve_quadratic_program, py::arg("H"), py::arg("f"), py::arg("A"), py::arg("b"), py::arg("Aeq"), py::arg("beq"), - py::arg("x0") = VectorXd(), py::arg("y0") = VectorXd()); - osqp_solver.def("get_info",&OSQP_Solver::get_info, - "Returns named solution-quality values (obj_val, dual_obj_val, prim_res, dual_res) " - "and the dual solution (dual_solution) from the last successful call to " - "solve_quadratic_program()."); + py::arg("x0") = VectorXd(), py::arg("y0") = VectorXd(), + "Solves min(x) 0.5*x'Hx + f'x s.t. Ax <= b and Aeq*x = beq (MATLAB `quadprog`-like signature).\n\n" + "Returns the optimal x. x0 and y0 are optional warm-starts for the primal and dual variables."); + osqp_solver.def("get_info", + &Solver::get_info, + "Returns the solution-quality values (obj_val, dual_obj_val, prim_res, dual_res) and the dual solution (dual_solution) from the last successful call to solve_quadratic_program()."); // Helps evaluating the wrapper when versions show any issues - osqp_solver.def("test_vectorxd",&OSQP_Solver::test_vectorxd,"."); - osqp_solver.def("test_matrixxd",&OSQP_Solver::test_matrixxd,"."); + osqp_solver.def("test_vectorxd", &Solver::test_vectorxd, py::arg("v"), + "Round-trips a vector to help evaluate the Eigen <-> std conversions used across the wrapper."); + osqp_solver.def("test_matrixxd", &Solver::test_matrixxd, py::arg("m"), + "Round-trips a matrix to help evaluate the Eigen <-> std conversions used across the wrapper."); #ifdef VERSION_INFO m.attr("__version__") = MACRO_STRINGIFY(VERSION_INFO); diff --git a/src/core_function.cpp b/src/core_function.cpp index 5ce0f73..dcc5dbc 100644 --- a/src/core_function.cpp +++ b/src/core_function.cpp @@ -1,21 +1,86 @@ /** -Based on the M3::qpOASES_Solver wrapper in solver-qpoases, adapted to use the OSQP solver. +Based on the marinholab::solvers::qpoases::Solver wrapper in solver-qpoases, +adapted to use the OSQP solver. */ -#include +#include #include #include -namespace M3 +namespace marinholab +{ + +namespace solvers +{ + +namespace osqp { // https://stackoverflow.com/questions/53408962/try-to-understand-compiler-error-message-default-member-initializer-required-be -OSQP_Solver::Configuration::Configuration() = default; +Configuration::Configuration() = default; // https://stackoverflow.com/questions/53408962/try-to-understand-compiler-error-message-default-member-initializer-required-be -OSQP_Solver::Info::Info() = default; +Solver::Info::Info() = default; + +/** + * \brief Builds an `OSQPSettings` object from the user configuration. + * + * Maps every `OSQPSettings` field onto the corresponding `Configuration` + * member. + */ +OSQPSettings Solver::_to_osqp_settings() const +{ + OSQPSettings settings; + + // Linear algebra settings + settings.device = configuration_.device; + settings.linsys_solver = configuration_.linsys_solver; + + // Control settings + settings.allocate_solution = configuration_.allocate_solution; + settings.verbose = configuration_.verbose; + settings.profiler_level = configuration_.profiler_level; + settings.warm_starting = configuration_.warm_starting; + settings.scaling = configuration_.scaling; + settings.polishing = configuration_.polishing; + + // ADMM parameters + settings.rho = configuration_.rho; + settings.rho_is_vec = configuration_.rho_is_vec; + settings.sigma = configuration_.sigma; + settings.alpha = configuration_.alpha; + + // CG settings + settings.cg_max_iter = configuration_.cg_max_iter; + settings.cg_tol_reduction = configuration_.cg_tol_reduction; + settings.cg_tol_fraction = configuration_.cg_tol_fraction; + settings.cg_precond = configuration_.cg_precond; + + // Adaptive rho logic + settings.adaptive_rho = configuration_.adaptive_rho; + settings.adaptive_rho_interval = configuration_.adaptive_rho_interval; + settings.adaptive_rho_fraction = configuration_.adaptive_rho_fraction; + settings.adaptive_rho_tolerance = configuration_.adaptive_rho_tolerance; + + // Termination parameters + settings.max_iter = configuration_.max_iter; + settings.eps_abs = configuration_.eps_abs; + settings.eps_rel = configuration_.eps_rel; + settings.eps_prim_inf = configuration_.eps_prim_inf; + settings.eps_dual_inf = configuration_.eps_dual_inf; + settings.scaled_termination = configuration_.scaled_termination; + settings.check_termination = configuration_.check_termination; + settings.check_dualgap = configuration_.check_dualgap; + settings.time_limit = configuration_.time_limit; + + // Polishing parameters + settings.delta = configuration_.delta; + settings.polish_refine_iter = configuration_.polish_refine_iter; + + return settings; +} -OSQP_Solver::OSQP_Solver(const Configuration& configuration): +Solver::Solver(const Configuration& configuration): osqp_solve_first_time_(true), osqp_solver_(nullptr), configuration_(configuration), @@ -25,12 +90,12 @@ OSQP_Solver::OSQP_Solver(const Configuration& configuration): } -OSQP_Solver::~OSQP_Solver() +Solver::~Solver() { _cleanup(); } -void OSQP_Solver::_cleanup() +void Solver::_cleanup() { if(osqp_solver_ != nullptr) { @@ -39,23 +104,23 @@ void OSQP_Solver::_cleanup() } } -std::vector OSQP_Solver::_vectorxd_to_std_vector_double(const VectorXd& vectorxd) +std::vector Solver::_vectorxd_to_std_vector_double(const VectorXd& vectorxd) { std::vector vec(vectorxd.data(), vectorxd.data() + vectorxd.rows() * vectorxd.cols()); return vec; } -VectorXd OSQP_Solver::_std_vector_double_to_vectorxd(std::vector std_vector_double) const +VectorXd Solver::_std_vector_double_to_vectorxd(std::vector std_vector_double) const { double* ptr = &std_vector_double[0]; Eigen::Map vec(ptr,std_vector_double.size()); return vec; } -OSQP_Solver::CSCMatrixData OSQP_Solver::_dense_to_csc_upper_triangular(const MatrixXd& M) +Solver::CSCMatrixData Solver::_dense_to_csc_upper_triangular(const MatrixXd& M) { if(M.rows()!=M.cols()) - throw std::runtime_error("OSQP_Solver::_dense_to_csc_upper_triangular(): M must be square. M.rows()="+std::to_string(M.rows())+" but M.cols()="+std::to_string(M.cols())+"."); + throw std::runtime_error("Solver::_dense_to_csc_upper_triangular(): M must be square. M.rows()="+std::to_string(M.rows())+" but M.cols()="+std::to_string(M.cols())+"."); const OSQPInt n = static_cast(M.rows()); @@ -80,7 +145,7 @@ OSQP_Solver::CSCMatrixData OSQP_Solver::_dense_to_csc_upper_triangular(const Mat return csc; } -OSQP_Solver::CSCMatrixData OSQP_Solver::_dense_to_csc(const MatrixXd& M) +Solver::CSCMatrixData Solver::_dense_to_csc(const MatrixXd& M) { const OSQPInt rows = static_cast(M.rows()); const OSQPInt cols = static_cast(M.cols()); @@ -112,11 +177,11 @@ void evaluate_osqp_exitflag(OSQPInt exitflag, const std::string& context) { if(exitflag != 0) { - throw std::runtime_error("OSQP_Solver::solve_quadratic_program(): "+context+" failed. OSQP returned error code "+std::to_string(exitflag)+": "+std::string(osqp_error_message(exitflag))); + throw std::runtime_error("Solver::solve_quadratic_program(): "+context+" failed. OSQP returned error code "+std::to_string(exitflag)+": "+std::string(osqp_error_message(exitflag))); } } -VectorXd OSQP_Solver::solve_quadratic_program(const MatrixXd& H, const VectorXd& f, const MatrixXd& A, const VectorXd& b, const MatrixXd& Aeq, const VectorXd& beq, const VectorXd& x0, const VectorXd& y0) +VectorXd Solver::solve_quadratic_program(const MatrixXd& H, const VectorXd& f, const MatrixXd& A, const VectorXd& b, const MatrixXd& Aeq, const VectorXd& beq, const VectorXd& x0, const VectorXd& y0) { const OSQPInt PROBLEM_SIZE = static_cast(H.rows()); const OSQPInt INEQUALITY_CONSTRAINT_SIZE = static_cast(b.size()); @@ -126,30 +191,30 @@ VectorXd OSQP_Solver::solve_quadratic_program(const MatrixXd& H, const VectorXd& ///Check sizes //Objective function if(H.rows()!=H.cols()) - throw std::runtime_error("OSQP_Solver::solve_quadratic_program(): H must be symmetric. H.rows()="+std::to_string(H.rows())+" but H.cols()="+std::to_string(H.cols())+"."); + throw std::runtime_error("Solver::solve_quadratic_program(): H must be symmetric. H.rows()="+std::to_string(H.rows())+" but H.cols()="+std::to_string(H.cols())+"."); if(f.size()!=H.rows()) - throw std::runtime_error("OSQP_Solver::solve_quadratic_program(): f must be compatible with H. H.rows()=H.cols()="+std::to_string(H.rows())+" but f.size()="+std::to_string(f.size())+"."); + throw std::runtime_error("Solver::solve_quadratic_program(): f must be compatible with H. H.rows()=H.cols()="+std::to_string(H.rows())+" but f.size()="+std::to_string(f.size())+"."); //Optional warm-start (e.g. a known feasible solution) for the primal variable x. if(x0.size()!=0 && x0.size()!=PROBLEM_SIZE) - throw std::runtime_error("OSQP_Solver::solve_quadratic_program(): x0 must be compatible with H. H.rows()=H.cols()="+std::to_string(H.rows())+" but x0.size()="+std::to_string(x0.size())+"."); + throw std::runtime_error("Solver::solve_quadratic_program(): x0 must be compatible with H. H.rows()=H.cols()="+std::to_string(H.rows())+" but x0.size()="+std::to_string(x0.size())+"."); //Optional warm-start (e.g. a dual solution obtained from get_info().dual_solution in a //previous call) for the dual variable y. if(y0.size()!=0 && y0.size()!=TOTAL_CONSTRAINT_SIZE) - throw std::runtime_error("OSQP_Solver::solve_quadratic_program(): y0 must be compatible with the total number of constraints. b.size()+beq.size()="+std::to_string(TOTAL_CONSTRAINT_SIZE)+" but y0.size()="+std::to_string(y0.size())+"."); + throw std::runtime_error("Solver::solve_quadratic_program(): y0 must be compatible with the total number of constraints. b.size()+beq.size()="+std::to_string(TOTAL_CONSTRAINT_SIZE)+" but y0.size()="+std::to_string(y0.size())+"."); //Inequality constraints if(b.size()!=A.rows()) - throw std::runtime_error("OSQP_Solver::solve_quadratic_program(): size of b="+std::to_string(b.size())+" should be compatible with rows of A="+std::to_string(A.rows())+"."); + throw std::runtime_error("Solver::solve_quadratic_program(): size of b="+std::to_string(b.size())+" should be compatible with rows of A="+std::to_string(A.rows())+"."); if(INEQUALITY_CONSTRAINT_SIZE!=0 && A.cols()!=PROBLEM_SIZE) - throw std::runtime_error("OSQP_Solver::solve_quadratic_program(): A.cols()="+std::to_string(A.cols())+" should be compatible with H.rows()="+std::to_string(PROBLEM_SIZE)+"."); + throw std::runtime_error("Solver::solve_quadratic_program(): A.cols()="+std::to_string(A.cols())+" should be compatible with H.rows()="+std::to_string(PROBLEM_SIZE)+"."); //Equality constraints if(beq.size()!=Aeq.rows()) - throw std::runtime_error("OSQP_Solver::solve_quadratic_program(): size of beq="+std::to_string(beq.size())+" should be compatible with rows of Aeq="+std::to_string(Aeq.rows())+"."); + throw std::runtime_error("Solver::solve_quadratic_program(): size of beq="+std::to_string(beq.size())+" should be compatible with rows of Aeq="+std::to_string(Aeq.rows())+"."); if(EQUALITY_CONSTRAINT_SIZE!=0 && Aeq.cols()!=PROBLEM_SIZE) - throw std::runtime_error("OSQP_Solver::solve_quadratic_program(): Aeq.cols()="+std::to_string(Aeq.cols())+" should be compatible with H.rows()="+std::to_string(PROBLEM_SIZE)+"."); + throw std::runtime_error("Solver::solve_quadratic_program(): Aeq.cols()="+std::to_string(Aeq.cols())+" should be compatible with H.rows()="+std::to_string(PROBLEM_SIZE)+"."); //Stack the inequality and equality constraints into OSQP's single l <= Ax <= u form. //Equality rows get l==u==beq. Inequality rows get l=-infinity, u=b. @@ -189,16 +254,11 @@ VectorXd OSQP_Solver::solve_quadratic_program(const MatrixXd& H, const VectorXd& OSQPSettings* settings = OSQPSettings_new(); if(settings == nullptr) - throw std::runtime_error("OSQP_Solver::solve_quadratic_program(): unable to allocate OSQPSettings."); + throw std::runtime_error("Solver::solve_quadratic_program(): unable to allocate OSQPSettings."); - settings->max_iter = configuration_.maximum_iterations; - settings->eps_abs = configuration_.eps_absolute; - settings->eps_rel = configuration_.eps_relative; - settings->eps_prim_inf = configuration_.eps_primal_infeasibility; - settings->eps_dual_inf = configuration_.eps_dual_infeasibility; - settings->verbose = configuration_.verbose; - settings->polishing = configuration_.polishing; - settings->warm_starting = configuration_.warm_starting; + //Overwrite OSQP's defaults with the user's configuration. + const OSQPSettings user_settings = _to_osqp_settings(); + *settings = user_settings; OSQPCscMatrix* P = OSQPCscMatrix_new(PROBLEM_SIZE, PROBLEM_SIZE, static_cast(P_csc.x.size()), P_csc.x.data(), P_csc.i.data(), P_csc.p.data()); OSQPCscMatrix* A_mat = OSQPCscMatrix_new(TOTAL_CONSTRAINT_SIZE, PROBLEM_SIZE, static_cast(A_csc.x.size()), A_csc.x.data(), A_csc.i.data(), A_csc.p.data()); @@ -256,17 +316,17 @@ VectorXd OSQP_Solver::solve_quadratic_program(const MatrixXd& H, const VectorXd& const OSQPInt status = osqp_solver_->info->status_val; if(status != OSQP_SOLVED && status != OSQP_SOLVED_INACCURATE) - throw std::runtime_error("OSQP_Solver::solve_quadratic_program(): unable to solve quadratic program. OSQP status: "+std::string(osqp_solver_->info->status)); + throw std::runtime_error("Solver::solve_quadratic_program(): unable to solve quadratic program. OSQP status: "+std::string(osqp_solver_->info->status)); std::vector return_value_std(osqp_solver_->solution->x, osqp_solver_->solution->x + PROBLEM_SIZE); return _std_vector_double_to_vectorxd(return_value_std); } -OSQP_Solver::Info OSQP_Solver::get_info() const +Solver::Info Solver::get_info() const { if(osqp_solver_ == nullptr || osqp_solver_->info == nullptr) - throw std::runtime_error("OSQP_Solver::get_info(): no solution information available. solve_quadratic_program() must be called successfully first."); + throw std::runtime_error("Solver::get_info(): no solution information available. solve_quadratic_program() must be called successfully first."); Info info; info.obj_val = osqp_solver_->info->obj_val; @@ -287,14 +347,18 @@ OSQP_Solver::Info OSQP_Solver::get_info() const } // Helper functions to help evaluate the wrapper when needed. -VectorXd OSQP_Solver::test_vectorxd(const VectorXd& v) +VectorXd Solver::test_vectorxd(const VectorXd& v) { return v; } -MatrixXd OSQP_Solver::test_matrixxd(const MatrixXd& m) +MatrixXd Solver::test_matrixxd(const MatrixXd& m) { return m; } -} // namespace M3 +} // namespace osqp + +} // namespace solvers + +} // namespace marinholab