From e343e91ca908643eb51076939ad918b01e8ee042 Mon Sep 17 00:00:00 2001 From: mmmarinho Date: Fri, 28 Aug 2026 06:03:41 +0000 Subject: [PATCH] Expose all qpOASES Configuration options; document, type, and clean up Extend the C++ `Configuration` struct and pybind11 bindings to expose every qpOASES `Options` field (35 options) plus the wrapper-specific fields, so the full qpOASES API is reachable from Python. Apply them all through a single `_to_qpoases_options()` helper. - `include/qpOASES_solver.h`: full, doxygen-documented `Configuration` with double-precision defaults; scoped `using` directives inside `namespace M3` (preserves the MSVC/`SparseMatrix` fix from #2). - `src/core_function.cpp`: map all options; use `std::vector` instead of a VLA for `xOpt` (preserves the MSVC fix from #2). - `src/core.cpp`: bind all new fields; expose `PrintLevel`, `SubjectToStatus`, and the full `HessianType` enum sets. Documentation & typing: - `README.md`: rewritten with quickstart, configuration reference (defaults + per-option tables), building-from-source, and type-checking notes. - `AGENTS.md`: added with build/test/lint commands and behavioural invariants. - `marinholab/solvers/qpoases/_core.pyi` + `py.typed`: type stub so the package is checkable by Pyright; `setup.py` ships both in the wheel. - `solver.py`, `__init__.py`, `example.py`, `example_kinematics.py`: added type annotations; `pyrightconfig.json` (py3.9, basic) makes `pyright` pass with 0 errors / 0 warnings. Behaviour is preserved: `enableRegularisation`/`enableNZCTests`/ `enableFlippingBounds` keep their fast/MPC defaults and `print_level` now defaults to `PL_LOW` (quiet) while remaining configurable. Co-authored-by: openhands --- .gitignore | 13 + AGENTS.md | 111 +++++ README.md | 219 +++++++++ include/qpOASES_solver.h | 422 ++++++++++++++++-- marinholab/solvers/qpoases/__init__.py | 23 +- marinholab/solvers/qpoases/_core.pyi | 193 ++++++++ marinholab/solvers/qpoases/example.py | 12 +- .../solvers/qpoases/example_kinematics.py | 8 +- marinholab/solvers/qpoases/py.typed | 0 marinholab/solvers/qpoases/solver.py | 55 ++- pyrightconfig.json | 7 + setup.py | 3 + src/core.cpp | 126 +++++- src/core_function.cpp | 54 ++- 14 files changed, 1162 insertions(+), 84 deletions(-) create mode 100644 .gitignore create mode 100644 AGENTS.md create mode 100644 marinholab/solvers/qpoases/_core.pyi create mode 100644 marinholab/solvers/qpoases/py.typed create mode 100644 pyrightconfig.json diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..da62b28 --- /dev/null +++ b/.gitignore @@ -0,0 +1,13 @@ +# Build artifacts +build/ +dist/ +*.egg-info/ + +# Python caches +__pycache__/ +*.py[cod] + +# Editors / OS +.vscode/ +.idea/ +.DS_Store diff --git a/AGENTS.md b/AGENTS.md new file mode 100644 index 0000000..ee7e30a --- /dev/null +++ b/AGENTS.md @@ -0,0 +1,111 @@ +# AGENTS.md + +Repository conventions for the `marinholab-solvers-qpoases` project — a Python +(C++ via pybind11) wrapper around [qpOASES](https://github.com/coin-or/qpOASES), +an online active-set solver for quadratic programs. + +## Project layout + +``` +marinholab/solvers/qpoases/ + __init__.py Public API re-exports (Solver, Configuration, enums) + solver.py numpy-friendly Solver wrapper + example.py runnable example (console script: qpoases_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/qpOASES_solver.h C++ header: qpOASES_Solver + Configuration (doxygen-documented) +src/core.cpp pybind11 module (_core): binds qpOASES_Solver + Configuration + enums +src/core_function.cpp C++ implementation (wraps qpOASES' QPSolver) +qpOASES/ qpOASES (git submodule) +pybind11/ pybind11 (git submodule) +setup.py PEP 517 build (CMake + pybind11) +``` + +## 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_qpoases-*.whl +``` + +The extension name is `marinholab.solvers.qpoases._core`. Builds are slow on +first run; CMake reuses the `build/` cache across builds. + +## Run the example (smoke test) + +```console +qpoases_example +``` + +(or `python -m marinholab.solvers.qpoases.example`). Exits 0 and prints one +line of optimal `x` per sub-example (positive-definite, semi-definite, +`None`-constraints, active set). + +`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. + +## 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 third-party `dqrobotics` + package; its `import` lines and matplotlib 3-D axis calls carry targeted + `# type: ignore[reportAttributeAccessIssue]` comments. Do not remove them + (they are the documented reason those lines are ignored). +- The package ships `py.typed` + `_core.pyi` in the wheel (`package_data` in + `setup.py`) so downstream projects can be checked against it. + +## Conventions + +- **Preserve prior solver behaviour.** The wrapper's historical defaults are + the behavioural contract. In particular: + - `Configuration.enableRegularisation` defaults to `BT_FALSE` + (qpOASES' own default is `BT_TRUE`) — do not "fix" it to `BT_TRUE` without + re-validating the semi-definite / indefinite examples. + - `Configuration.print_level` defaults to `PL_LOW` (qpOASES default is + `PL_MEDIUM`) so the solver is quiet by default. + - `Configuration.enableNZCTests` and `enableFlippingBounds` default to + `BT_FALSE` (qpOASES default is `BT_TRUE`) — the "fast"/MPC preset. + - `Solver` accepts `None` for `A`/`b`/`Aeq`/`beq` and substitutes a single + trivially-satisfied zero row; `Solver.get_active_set()` returns one entry + per combined constraint row (-1 lower / 0 inactive / +1 upper). +- **Style.** Match the existing style: no trailing-whitespace obsession, + docstrings on public API, snake_case for new `Configuration` fields that + map to qpOASES `Options` fields (keep qpOASES-native names where they are + already the established wrapper spelling: `enableFlippingBounds`, + `enableRegularisation`, `enableNZCTests`). +- **Doxygen.** C++ types and members are documented with Doxygen + (`/** ... @brief ... @see ... */` blocks). Keep that when adding fields. +- **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 `0.0.1` in `pyproject.toml`; `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/README.md b/README.md index df024d0..94d96b7 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,224 @@ # solver-qpoases +A [qpOASES](https://github.com/coin-or/qpOASES) wrapper for Python that ships +prebuilt binaries. It exposes qpOASES' online active-set solver through a thin, +numpy-friendly, MATLAB-`quadprog`-like interface. + ```console pip install marinholab-solvers-qpoases ``` + +## 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 +``` + +Once a problem has been solved once, subsequent solves on the same `Solver` +instance are *warm-started* by default (`Configuration.use_hotstart = True`), +which is the main performance benefit of qpOASES for repeated, related QPs. + +## Quickstart + +```python +import numpy as np +from marinholab.solvers import qpoases + +solver = qpoases.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) +``` + +### Active set + +`solver.get_active_set()` reports, for each row of the combined constraint +matrix (rows of `A` followed by rows of `Aeq`), whether it is: + +* `-1` active at its **lower** bound, +* `0` inactive, +* `+1` active at its **upper** bound (equality constraints are always `+1`, + since their bounds coincide). + +```python +solver.solve_quadratic_program(H, f, A, b, Aeq, beq) +solver.get_active_set() # e.g. [ 1.0, 0.0, 0.0] +``` + +## Configuration + +All of qpOASES' `Options` fields are exposed, plus a few wrapper-specific +settings. Create a `Configuration`, tweak the fields you need, and pass it to +the solver: + +```python +config = qpoases.Configuration() +config.hessian_type = qpoases.HessianType.HST_SEMIDEF # H is rank-deficient +config.enableRegularisation = qpoases.BooleanType.BT_FALSE +config.termination_tolerance = 1.0e-9 # tighter convergence +solver = qpoases.Solver(config) +``` + +The enum types are re-exported for convenience: `qpoases.BooleanType`, +`qpoases.HessianType`, `qpoases.PrintLevel`, and `qpoases.SubjectToStatus`. + +### Wrapper-specific options + +| Option | Default | Type | Description | +|---|---|---|---| +| `maximum_working_set_recalculations` | `150` | `int` | Max working-set recalculations during the initial homotopy (`nWSR` passed to `init`/`hotstart`). Increase if solves hit the maximum. | +| `use_hotstart` | `True` | `bool` | Warm-start subsequent solves with `hotstart()` instead of re-initialising with `init()`. | +| `hessian_type` | `HST_POSDEF` | `HessianType` | Definiteness assumed for `H`; given to the underlying `SQProblem`. | + +### Hessian definiteness (`HessianType`) + +| Value | Meaning | +|---|---| +| `HST_ZERO` | Hessian is the zero matrix (LP formulation) | +| `HST_IDENTITY` | Hessian is the identity matrix | +| `HST_POSDEF` | Hessian is (strictly) positive definite | +| `HST_POSDEF_NULLSPACE` | Positive definite on the null space of active bounds/constraints | +| `HST_SEMIDEF` | Positive semi-definite | +| `HST_INDEF` | Indefinite | +| `HST_UNKNOWN` | Unknown | + +### qpOASES options + +These map 1:1 onto qpOASES' `Options` fields. Defaults match qpOASES' own +defaults for a **double-precision** build (see `Options::setToDefault()`), with +two deliberate exceptions noted inline: `enableNZCTests` and +`enableFlippingBounds` default to `BT_FALSE` here (qpOASES' default is +`BT_TRUE`) because those are the recommended "fast"/MPC settings for repeated, +online solves. See the [qpOASES manual](https://www.coin-or.org/qpOASES/doc/3.0/manual.pdf) +for a full description of each option. + +**Booleans** (`BooleanType`: `BT_FALSE` / `BT_TRUE`) + +| Option | Default | Description | +|---|---|---| +| `enable_ramping` | `BT_TRUE` | Enables the ramping strategy. | +| `enable_far_bounds` | `BT_TRUE` | Enables the far bounds strategy. | +| `enableFlippingBounds` | `BT_FALSE` | Allows flipping active bounds between lower and upper values. *(differs from qpOASES default)* | +| `enableRegularisation` | `BT_FALSE` | Regularises `H` when (semi-)definiteness is detected. | +| `enable_full_li_tests` | `BT_FALSE` | Uses the condition-hardened linear-independence (LI) test. | +| `enableNZCTests` | `BT_FALSE` | Enables the nonzero-curvature test. *(differs from qpOASES default)* | +| `enable_equalities` | `BT_FALSE` | Treats equality constraints as always active. | +| `enable_inertia_correction` | `BT_TRUE` | Repairs the working set when negative curvature is found during a hotstart. | +| `enable_drop_infeasibles` | `BT_FALSE` | Whether infeasible constraints may be dropped. | + +**Integers** (`int_t`) + +| Option | Default | Description | +|---|---|---| +| `enable_drift_correction` | `1` | Frequency of drift corrections (`0` = off). | +| `enable_cholesky_refactorisation` | `0` | Frequency of full Cholesky refactorisation of the projected Hessian (`0` = rank updates only). | +| `num_regularisation_steps` | `0` | Max successive regularisation steps. | +| `num_refinement_steps` | `1` | Max iterative-refinement steps. | +| `drop_bound_priority` | `1` | Priority used when dropping bounds. | +| `drop_eq_con_priority` | `1` | Priority used when dropping equality constraints. | +| `drop_ineq_con_priority` | `1` | Priority used when dropping inequality constraints. | + +**Reals** (`real_t`, `double`) + +| Option | Default | Description | +|---|---|---| +| `termination_tolerance` | `5.0e6 * EPS` (~`1.1e-9`) | Relative tolerance that stops the homotopy. Smaller = more accurate, more work. | +| `bound_tolerance` | `1.0e6 * EPS` | Bound tolerance; a constraint whose bounds differ by less is treated as an equality. | +| `bound_relaxation` | `1.0e4` | Offset for relaxing bounds at the start of the initial homotopy (also the initial far-bound value). | +| `eps_num` | `-1.0e3 * EPS` | Numerator tolerance for the ratio test. | +| `eps_den` | `1.0e3 * EPS` | Denominator tolerance for the ratio test. | +| `max_primal_jump` | `1.0e8` | Max allowed primal jump in nonzero-curvature tests. | +| `max_dual_jump` | `1.0e8` | Max allowed dual jump in LI tests. | +| `initial_ramping` | `0.5` | Start value of the ramping strategy. | +| `final_ramping` | `1.0` | Final value of the ramping strategy. | +| `initial_far_bounds` | `1.0e6` | Initial size of the far bounds. | +| `grow_far_bounds` | `1.0e3` | Growth factor applied to the far bounds. | +| `eps_flipping` | `1.0e3 * EPS` | Tolerance of the squared Cholesky diagonal factor that triggers flipping a bound. | +| `eps_regularisation` | `1.0e3 * EPS` | Scaling factor of the identity matrix used for Hessian regularisation. | +| `eps_iter_ref` | `1.0e2 * EPS` | Early-termination tolerance for iterative refinement. | +| `eps_li_tests` | `1.0e5 * EPS` | Tolerance for the linear-independence tests. | +| `eps_nzc_tests` | `3.0e3 * EPS` | Tolerance for the nonzero-curvature tests. | +| `rcond_s_min` | `1.0e-14` | Min reciprocal condition number of the Schur complement before a refactorisation is triggered. | + +**Status / print enums** + +| Option | Default | Type | Description | +|---|---|---|---| +| `print_level` | `PL_LOW` | `PrintLevel` | Verbosity of qpOASES output (`PL_NONE`, `PL_LOW`, `PL_MEDIUM`, `PL_HIGH`, `PL_TABULAR`, `PL_DEBUG_ITER`). Defaults to `PL_LOW` so the solver stays quiet (qpOASES' own default is `PL_MEDIUM`). | +| `initial_status_bounds` | `ST_LOWER` | `SubjectToStatus` | Status assumed for all bounds at the first iteration. | + +### Print levels (`PrintLevel`) + +`PL_DEBUG_ITER`, `PL_TABULAR`, `PL_NONE`, `PL_LOW`, `PL_MEDIUM`, `PL_HIGH`. + +### Bound/constraint statuses (`SubjectToStatus`) + +`ST_LOWER`, `ST_INACTIVE`, `ST_UPPER`, `ST_INFEASIBLE_LOWER`, +`ST_INFEASIBLE_UPPER`, `ST_UNDEFINED`. + +## Examples + +* `marinholab/solvers/qpoases/example.py` — positive-definite and + semi-definite solves, the `None`-constraint path, and the active set. Run it + with `qpoases_example` (installed as a console script). +* `marinholab/solvers/qpoases/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 +[qpOASES](https://github.com/coin-or/qpOASES) 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`. + +## Type checking + +The package ships a type stub (`marinholab/solvers/qpoases/_core.pyi`) and a +`py.typed` marker, so downstream projects can be checked with +[Pyright](https://github.com/microsoft/pyright) (or Pylance) without extra +configuration. Run the project's own check with: + +```console +pyright +``` + +## License + +The qpOASES library is LGPLv2.1; the wrapper is under the terms of the +included `LICENSE` file. diff --git a/include/qpOASES_solver.h b/include/qpOASES_solver.h index a96263c..c07a2ec 100644 --- a/include/qpOASES_solver.h +++ b/include/qpOASES_solver.h @@ -15,74 +15,430 @@ namespace M3 using namespace Eigen; using namespace qpOASES; +/** + * @brief High-level, reusable solver for quadratic programs (QPs) based on + * qpOASES. + * + * qpOASES_Solver exposes qpOASES' online active set solver through a + * MATLAB/`quadprog`-like, matrix-based interface. Internally it keeps an + * SQProblem object so that, once initialised, subsequent calls are warm + * started (see use_hotstart). + * + * The solver is configured through a Configuration structure whose members + * map directly onto qpOASES' `Options` class. In addition to the standard + * qpOASES options, the configuration carries a few wrapper-specific + * settings (hotstart, maximum working-set recalculations). + * + * @note The class is not thread-safe: a single instance owns one + * underlying qpOASES problem and its state changes across calls. + */ class qpOASES_Solver { public: + /** + * @brief Holds all user-configurable solver options. + * + * Every qpOASES `Options` field is exposed here. Defaults are chosen + * to match qpOASES' own defaults for a double-precision build (see + * `Options::setToDefault()`), with three deliberate exceptions: + * + * - enableNZCTests defaults to BT_FALSE (qpOASES default is + * BT_TRUE) and enableFlippingBounds defaults to BT_FALSE + * (qpOASES default is BT_TRUE). These are the settings used by + * qpOASES' "fast"/MPC preset and are the recommended settings + * for online, embedded use. + * - print_level defaults to PL_LOW (qpOASES default is PL_MEDIUM) + * so the wrapper stays quiet by default. + * + * @note qpOASES applies `Options::ensureConsistency()` when it sets + * its options, which will silently adjust any value that falls + * outside its allowed range (e.g. negative tolerances) to a + * valid one. + */ struct Configuration { - //The integer argument nWSR specifies the maximum number of working set recalculations to be performed during the initial homotopy (on output it contains the number - //of working set recalculations actually performed!) - //Page 14 of https://www.coin-or.org/qpOASES/doc/3.0/manual.pdf + // ------------------------------------------------------------------ + // Wrapper-specific options (not part of qpOASES' `Options`). + // ------------------------------------------------------------------ + + /** + * @brief Maximum number of working set recalculations performed + * during the initial homotopy. + * + * This is the nWSR argument passed to `init()`/`hotstart()`. + * On qpOASES' side it is overwritten with the number of + * recalculations actually performed. See page 14 of + * https://www.coin-or.org/qpOASES/doc/3.0/manual.pdf + */ int_t maximum_working_set_recalculations = 150; - bool use_hotstart = true; //Use hotstart for subsequent calls. - HessianType hessian_type = HST_POSDEF; //Hessian definiteness. Page 22. - BooleanType enableRegularisation = BT_TRUE; //Regularisation. Page 26. - BooleanType enableNZCTests = BT_FALSE; //Nonzero curvature test. Page 22. - BooleanType enableFlippingBounds = BT_FALSE; //Flipping bounds. Page 22. - real_t termination_tolerance = 5.0e6 * EPS; //Relative termination tolerance to stop homotopy. - Configuration(); //https://stackoverflow.com/questions/53408962/try-to-understand-compiler-error-message-default-member-initializer-required-be + + /** + * @brief Whether subsequent solves warm-start the problem + * (`hotstart()`) instead of re-initialising it + * (`init()`). + */ + bool use_hotstart = true; + + // ------------------------------------------------------------------ + // qpOASES `Options` fields (1:1 mapping, snake_case names). + // ------------------------------------------------------------------ + + /** + * @brief Verbose-ness of qpOASES output. + * + * Defaults to PL_LOW so the solver stays quiet, matching this + * wrapper's historical behaviour. + * @see `Options::printLevel` + */ + PrintLevel print_level = PL_LOW; + + /** + * @brief Whether the ramping strategy shall be used. + * @see `Options::enableRamping` + */ + BooleanType enable_ramping = BT_TRUE; + + /** + * @brief Whether far bounds shall be used. + * @see `Options::enableFarBounds` + */ + BooleanType enable_far_bounds = BT_TRUE; + + /** + * @brief Whether active bounds may flip between lower and upper + * values. + * @see `Options::enableFlippingBounds`. Page 22 of the manual. + */ + BooleanType enableFlippingBounds = BT_FALSE; + + /** + * @brief Whether the Hessian shall be regularised in case + * (semi-)definiteness is detected. + * + * Defaults to BT_FALSE to match both qpOASES' own default and the + * effective default of this wrapper prior to this option being + * passed through. + * @see `Options::enableRegularisation`. Page 26 of the manual. + */ + BooleanType enableRegularisation = BT_FALSE; + + /** + * @brief Whether the condition-hardened linear independence + * (LI) test shall be used. + * @see `Options::enableFullLITests` + */ + BooleanType enable_full_li_tests = BT_FALSE; + + /** + * @brief Whether nonzero curvature tests shall be used. + * @see `Options::enableNZCTests`. Page 22 of the manual. + */ + BooleanType enableNZCTests = BT_FALSE; + + /** + * @brief Frequency of drift corrections (0 = off). + * @see `Options::enableDriftCorrection` + */ + int_t enable_drift_correction = 1; + + /** + * @brief Frequency of full Cholesky refactorisation of the + * projected Hessian (0 = use rank updates only). + * @see `Options::enableCholeskyRefactorisation` + */ + int_t enable_cholesky_refactorisation = 0; + + /** + * @brief Whether equality constraints shall always be treated + * as active. + * @see `Options::enableEqualities` + */ + BooleanType enable_equalities = BT_FALSE; + + /** + * @brief Relative termination tolerance to stop the homotopy. + * @see `Options::terminationTolerance` + */ + real_t termination_tolerance = 5.0e6 * EPS; + + /** + * @brief Lower/upper (constraints') bound tolerance; a + * constraint whose bounds differ by less is regarded as + * an equality constraint. + * @see `Options::boundTolerance` + */ + real_t bound_tolerance = 1.0e6 * EPS; + + /** + * @brief Offset for relaxing constraint bounds at the start of + * an initial homotopy; also used as the initial far-bound + * value. + * @see `Options::boundRelaxation` + */ + real_t bound_relaxation = 1.0e4; + + /** + * @brief Numerator tolerance for the ratio test. + * @see `Options::epsNum` + */ + real_t eps_num = -1.0e3 * EPS; + + /** + * @brief Denominator tolerance for the ratio test. + * @see `Options::epsDen` + */ + real_t eps_den = 1.0e3 * EPS; + + /** + * @brief Maximum allowed jump in primal variables during nonzero + * curvature tests. + * @see `Options::maxPrimalJump` + */ + real_t max_primal_jump = 1.0e8; + + /** + * @brief Maximum allowed jump in dual variables during linear + * independence tests. + * @see `Options::maxDualJump` + */ + real_t max_dual_jump = 1.0e8; + + /** + * @brief Start value of the ramping strategy. + * @see `Options::initialRamping` + */ + real_t initial_ramping = 0.5; + + /** + * @brief Final value of the ramping strategy. + * @see `Options::finalRamping` + */ + real_t final_ramping = 1.0; + + /** + * @brief Initial size of the far bounds. + * @see `Options::initialFarBounds` + */ + real_t initial_far_bounds = 1.0e6; + + /** + * @brief Growth factor applied to the far bounds. + * @see `Options::growFarBounds` + */ + real_t grow_far_bounds = 1.0e3; + + /** + * @brief Status assumed for all bounds at the first iteration. + * @see `Options::initialStatusBounds` + */ + SubjectToStatus initial_status_bounds = ST_LOWER; + + /** + * @brief Tolerance of the squared Cholesky diagonal factor which + * triggers flipping of a bound. + * @see `Options::epsFlipping` + */ + real_t eps_flipping = 1.0e3 * EPS; + + /** + * @brief Maximum number of successive regularisation steps. + * @see `Options::numRegularisationSteps` + */ + int_t num_regularisation_steps = 0; + + /** + * @brief Scaling factor of the identity matrix used for Hessian + * regularisation. + * @see `Options::epsRegularisation` + */ + real_t eps_regularisation = 1.0e3 * EPS; + + /** + * @brief Maximum number of iterative refinement steps. + * @see `Options::numRefinementSteps` + */ + int_t num_refinement_steps = 1; + + /** + * @brief Early termination tolerance for iterative refinement. + * @see `Options::epsIterRef` + */ + real_t eps_iter_ref = 1.0e2 * EPS; + + /** + * @brief Tolerance used by the linear independence tests. + * @see `Options::epsLITests` + */ + real_t eps_li_tests = 1.0e5 * EPS; + + /** + * @brief Tolerance used by the nonzero curvature tests. + * @see `Options::epsNZCTests` + */ + real_t eps_nzc_tests = 3.0e3 * EPS; + + /** + * @brief Minimum reciprocal condition number of the Schur + * complement matrix S below which a refactorisation is + * triggered. + * @see `Options::rcondSMin` + */ + real_t rcond_s_min = 1.0e-14; + + /** + * @brief Whether the working set shall be repaired when negative + * curvature is discovered during a hotstart. + * @see `Options::enableInertiaCorrection` + */ + BooleanType enable_inertia_correction = BT_TRUE; + + /** + * @brief Whether infeasible constraints may be dropped. + * @see `Options::enableDropInfeasibles` + */ + BooleanType enable_drop_infeasibles = BT_FALSE; + + /** + * @brief Priority used when dropping bounds. + * @see `Options::dropBoundPriority` + */ + int_t drop_bound_priority = 1; + + /** + * @brief Priority used when dropping equality constraints. + * @see `Options::dropEqConPriority` + */ + int_t drop_eq_con_priority = 1; + + /** + * @brief Priority used when dropping inequality constraints. + * @see `Options::dropIneqConPriority` + */ + int_t drop_ineq_con_priority = 1; + + /** + * @brief Definiteness assumed for the Hessian matrix. + * + * Not an `Options` field: this is the HessianType handed to the + * underlying SQProblem. Page 22 of the manual. + */ + HessianType hessian_type = HST_POSDEF; + + /** + * @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(); }; + protected: + /** @brief True until the first solve has initialised the problem. */ bool qpoases_solve_first_time_; + /** @brief The underlying qpOASES problem. */ SQProblem qpoases_problem_; + /** @brief Active configuration, copied at construction. */ Configuration configuration_; - //https://github.com/SmartArmStack/sas_conversions/blob/master/src/eigen3_std_conversions.cpp - //A copy from sas + /** + * @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); - //Another copy from sas + /** + * @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); + /** + * @brief Translates the user configuration into a qpOASES `Options` + * object. + * @return An `Options` with all qpOASES fields filled from + * `configuration_`. + */ + Options _to_qpoases_options() const; public: + /** + * @brief Constructs a solver. + * @param configuration Options to use; defaults to the default + * configuration. + */ qpOASES_Solver(const Configuration& configuration = qpOASES_Solver::Configuration()); + + /** @brief Default destructor. */ ~qpOASES_Solver()=default; /** - * @brief - * Solves the following quadratic program + * @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. + * 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 m x n matrix of equality constraints. - * @param beq the m x 1 value for the inequality constraints. - * @return the optimal x + * @param Aeq the k x n matrix of equality constraints. + * @param beq the k x 1 value for the equality constraints. + * @return the optimal x. + * @throws std::runtime_error if any matrix is size-incompatible or + * qpOASES 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); /** - * @brief - * Returns the active set of constraints obtained in the most recent call to - * solve_quadratic_program(). The returned vector has one entry for each row of - * the combined constraint matrix, i.e. the rows of A followed by the rows of Aeq, - * in that same order, with the following meaning for each entry: - * -1: the constraint is active at its lower bound; - * 0: the constraint is inactive; - * +1: the constraint is active at its upper bound (this is also the value used - * for active equality constraints, as their lower and upper bounds coincide). + * @brief Returns the active set of constraints obtained in the most + * recent call to solve_quadratic_program(). The returned + * vector has one entry for each row of the combined + * constraint matrix, i.e. the rows of A followed by the rows + * of Aeq, in that same order, with the following meaning for + * each entry: + * + * - -1: the constraint is active at its lower bound; + * - 0: the constraint is inactive; + * - +1: the constraint is active at its upper bound (this is + * also the value used for active equality + * constraints, as their lower and upper bounds + * coincide). + * * @return the active set, as described above. + * @throws std::runtime_error if solve_quadratic_program() has not + * been called at least once. */ VectorXd get_active_set(); + /** + * @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); - MatrixXd test_matrixxd(const MatrixXd& m); + /** + * @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); }; -} \ No newline at end of file +} // namespace M3 diff --git a/marinholab/solvers/qpoases/__init__.py b/marinholab/solvers/qpoases/__init__.py index e4a4193..f06243c 100644 --- a/marinholab/solvers/qpoases/__init__.py +++ b/marinholab/solvers/qpoases/__init__.py @@ -1,11 +1,32 @@ """ Copyright (C) 2025 Murilo Marques Marinho (www.murilomarinho.info) LGPLv2.1 License + +Public API of the `marinholab.solvers.qpoases` package. + +`Solver` is a thin, numpy-friendly Python wrapper around the compiled +qpOASES solver (`qpOASES_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.qpoases._core import qpOASES_Solver + +# Re-exported for convenience so users can write e.g. ``qpoases.Configuration`` +# and ``qpoases.HessianType.HST_POSDEF``. Configuration = qpOASES_Solver.Configuration HessianType = qpOASES_Solver.HessianType -BooleanType = qpOASES_Solver.BooleanType \ No newline at end of file +BooleanType = qpOASES_Solver.BooleanType +PrintLevel = qpOASES_Solver.PrintLevel +SubjectToStatus = qpOASES_Solver.SubjectToStatus + +__all__ = [ + "Solver", + "qpOASES_Solver", + "Configuration", + "HessianType", + "BooleanType", + "PrintLevel", + "SubjectToStatus", +] \ No newline at end of file diff --git a/marinholab/solvers/qpoases/_core.pyi b/marinholab/solvers/qpoases/_core.pyi new file mode 100644 index 0000000..75ea716 --- /dev/null +++ b/marinholab/solvers/qpoases/_core.pyi @@ -0,0 +1,193 @@ +"""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 BooleanType(IntEnum): + """qpOASES logical values.""" + + BT_FALSE: BooleanType + BT_TRUE: BooleanType + + +class HessianType(IntEnum): + """qpOASES Hessian definiteness types.""" + + HST_ZERO: HessianType + HST_IDENTITY: HessianType + HST_POSDEF: HessianType + HST_POSDEF_NULLSPACE: HessianType + HST_SEMIDEF: HessianType + HST_INDEF: HessianType + HST_UNKNOWN: HessianType + + +class PrintLevel(IntEnum): + """qpOASES print levels, describing the amount of output at runtime.""" + + PL_DEBUG_ITER: PrintLevel + PL_TABULAR: PrintLevel + PL_NONE: PrintLevel + PL_LOW: PrintLevel + PL_MEDIUM: PrintLevel + PL_HIGH: PrintLevel + + +class SubjectToStatus(IntEnum): + """qpOASES bound/constraint statuses.""" + + ST_LOWER: SubjectToStatus + ST_INACTIVE: SubjectToStatus + ST_UPPER: SubjectToStatus + ST_INFEASIBLE_LOWER: SubjectToStatus + ST_INFEASIBLE_UPPER: SubjectToStatus + ST_UNDEFINED: SubjectToStatus + + +class qpOASES_Solver: + """High-level, reusable solver for quadratic programs (QPs) based on qpOASES.""" + + # Nested aliases so the enums are also reachable as + # ``qpOASES_Solver.BooleanType`` etc. (matching the runtime layout, where + # ``export_values()`` binds them onto the class). + BooleanType: type[BooleanType] = BooleanType + HessianType: type[HessianType] = HessianType + PrintLevel: type[PrintLevel] = PrintLevel + SubjectToStatus: type[SubjectToStatus] = SubjectToStatus + + class Configuration: + """All user-configurable solver options. + + Members are the 1:1 mapping of qpOASES' ``Options`` fields (plus the + wrapper-specific ``maximum_working_set_recalculations``, + ``use_hotstart`` and ``hessian_type``). Defaults match qpOASES' + double-precision defaults; see the C++ header (``include/qpOASES_solver.h``) + and the qpOASES manual for the meaning of each option. + """ + + #: Maximum number of working set recalculations during the initial homotopy. + maximum_working_set_recalculations: int + #: Whether subsequent solves are warm-started instead of re-initialised. + use_hotstart: bool + #: qpOASES print level. + print_level: PrintLevel + #: Enables the ramping strategy. + enable_ramping: BooleanType + #: Enables the far bounds strategy. + enable_far_bounds: BooleanType + #: Enables flipping of active bounds between lower and upper values. + enableFlippingBounds: BooleanType + #: Regularises the Hessian in case (semi-)definiteness is detected. + enableRegularisation: BooleanType + #: Uses the condition-hardened linear independence test. + enable_full_li_tests: BooleanType + #: Enables the nonzero curvature test. + enableNZCTests: BooleanType + #: Frequency of drift corrections (0 = off). + enable_drift_correction: int + #: Frequency of full Cholesky refactorisation of the projected Hessian (0 = updates only). + enable_cholesky_refactorisation: int + #: Treats equality constraints as always active. + enable_equalities: BooleanType + #: Relative termination tolerance to stop the homotopy. + termination_tolerance: float + #: Lower/upper (constraints') bound tolerance. + bound_tolerance: float + #: Offset for relaxing constraint bounds at the start of an initial homotopy. + bound_relaxation: float + #: Numerator tolerance for the ratio test. + eps_num: float + #: Denominator tolerance for the ratio test. + eps_den: float + #: Maximum allowed jump in primal variables during nonzero curvature tests. + max_primal_jump: float + #: Maximum allowed jump in dual variables during linear independence tests. + max_dual_jump: float + #: Start value of the ramping strategy. + initial_ramping: float + #: Final value of the ramping strategy. + final_ramping: float + #: Initial size of the far bounds. + initial_far_bounds: float + #: Growth factor applied to the far bounds. + grow_far_bounds: float + #: Status assumed for all bounds at the first iteration. + initial_status_bounds: SubjectToStatus + #: Tolerance of the squared Cholesky diagonal factor which triggers flipping a bound. + eps_flipping: float + #: Maximum number of successive regularisation steps. + num_regularisation_steps: int + #: Scaling factor of the identity matrix used for Hessian regularisation. + eps_regularisation: float + #: Maximum number of iterative refinement steps. + num_refinement_steps: int + #: Early termination tolerance for iterative refinement. + eps_iter_ref: float + #: Tolerance used by the linear independence tests. + eps_li_tests: float + #: Tolerance used by the nonzero curvature tests. + eps_nzc_tests: float + #: Minimum reciprocal condition number of the Schur complement before refactorisation is triggered. + rcond_s_min: float + #: Repairs the working set when negative curvature is discovered during a hotstart. + enable_inertia_correction: BooleanType + #: Whether infeasible constraints may be dropped. + enable_drop_infeasibles: BooleanType + #: Priority used when dropping bounds. + drop_bound_priority: int + #: Priority used when dropping equality constraints. + drop_eq_con_priority: int + #: Priority used when dropping inequality constraints. + drop_ineq_con_priority: int + #: Definiteness assumed for the Hessian matrix. + hessian_type: HessianType + + 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, + ) -> 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``. + """ + ... + + def get_active_set(self) -> np.ndarray: + """Returns the active set of constraints from the most recent solve. + + One entry per row of the combined constraint matrix (rows of ``A`` + followed by rows of ``Aeq``): -1 = active at its lower bound, 0 = + inactive, +1 = active at its upper bound (equality constraints are + always reported as +1). + """ + ... + + 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/qpoases/example.py b/marinholab/solvers/qpoases/example.py index 87d1406..efd77e3 100644 --- a/marinholab/solvers/qpoases/example.py +++ b/marinholab/solvers/qpoases/example.py @@ -5,7 +5,7 @@ import numpy as np from marinholab.solvers import qpoases -def positivedefinite(): +def positivedefinite() -> None: solver = qpoases.Solver() x = np.array([1.0, 0.0, 0.0, 0.0]) @@ -60,7 +60,7 @@ def positivedefinite(): print(u_ineq) print(u_both) -def semidefinite(): +def semidefinite() -> None: config = qpoases.Configuration() config.hessian_type = qpoases.HessianType.HST_SEMIDEF config.enableRegularisation = qpoases.BooleanType.BT_FALSE @@ -95,7 +95,7 @@ def semidefinite(): ) print(u) -def termination_tolerance(): +def termination_tolerance() -> None: # The termination tolerance is the relative tolerance used by qpOASES to # decide when the homotopy algorithm has converged. Tightening it (i.e. # using a smaller value) can improve solution accuracy at the cost of @@ -126,7 +126,7 @@ def termination_tolerance(): ) print(u) -def nones(): +def nones() -> None: solver = qpoases.Solver() x = np.array([1.0, 0.0, 0.0, 0.0]) @@ -168,7 +168,7 @@ def nones(): None ) -def active_set(): +def active_set() -> None: # get_active_set() reports, for each row of the combined constraint matrix # (rows of A followed by rows of Aeq), whether it is inactive (0), active at # its lower bound (-1), or active at its upper bound (+1). Equality @@ -197,7 +197,7 @@ def active_set(): print(u) print(solver.get_active_set()) # expected [1, 0, 0]: first constraint active at its upper bound -def main(): +def main() -> None: positivedefinite() semidefinite() termination_tolerance() diff --git a/marinholab/solvers/qpoases/example_kinematics.py b/marinholab/solvers/qpoases/example_kinematics.py index acc7d15..afda005 100644 --- a/marinholab/solvers/qpoases/example_kinematics.py +++ b/marinholab/solvers/qpoases/example_kinematics.py @@ -20,8 +20,8 @@ try: import matplotlib.pyplot as plt import numpy as np - from dqrobotics import i_, translation, vec4 - from dqrobotics.robots import KukaLw4Robot + from dqrobotics import i_, translation, vec4 # type: ignore[reportAttributeAccessIssue] + from dqrobotics.robots import KukaLw4Robot # type: ignore[reportAttributeAccessIssue] import dqrobotics_extensions.pyplot as dqp except ImportError as e: raise ImportError( @@ -136,11 +136,11 @@ def main(): ax = plt.axes(projection='3d') ax.set_xlabel('$x$') ax.set_ylabel('$y$') - ax.set_zlabel('$z$') + ax.set_zlabel('$z$') # type: ignore[reportAttributeAccessIssue] plot_size = 1.0 ax.set_xlim((-plot_size, plot_size)) ax.set_ylim((-plot_size, plot_size)) - ax.set_zlim((0.0, plot_size)) + ax.set_zlim((0.0, plot_size)) # type: ignore[reportAttributeAccessIssue] dqp.plot(robot, q=stored_qs[0], line_color='b', cylinder_color='b', cylinder_alpha=0.15) dqp.plot(robot, q=stored_qs[-1], line_color='r') diff --git a/marinholab/solvers/qpoases/py.typed b/marinholab/solvers/qpoases/py.typed new file mode 100644 index 0000000..e69de29 diff --git a/marinholab/solvers/qpoases/solver.py b/marinholab/solvers/qpoases/solver.py index 948c0b1..952f9b1 100644 --- a/marinholab/solvers/qpoases/solver.py +++ b/marinholab/solvers/qpoases/solver.py @@ -1,28 +1,55 @@ +from __future__ import annotations + import numpy as np from marinholab.solvers.qpoases._core import qpOASES_Solver + class Solver: - def __init__(self, configuration=qpOASES_Solver.Configuration()): - self.configuration = configuration - self.solver = qpOASES_Solver(configuration) + """Thin, numpy-friendly wrapper around the compiled qpOASES 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): + def __init__(self, configuration: qpOASES_Solver.Configuration | None = None) -> None: + self.configuration: qpOASES_Solver.Configuration = ( + configuration if configuration is not None else qpOASES_Solver.Configuration() + ) + self.solver: qpOASES_Solver = qpOASES_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, + ) -> 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. 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 - return self.solver.solve_quadratic_program(H, f, A, b, Aeq, beq) + return self.solver.solve_quadratic_program(H, f, A_full, b_full, Aeq_full, beq_full) - def get_active_set(self): + def get_active_set(self) -> np.ndarray: """ Returns the active set of constraints obtained in the most recent call to solve_quadratic_program(). The returned vector has one entry for each row of the 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 25a8229..160cc50 100644 --- a/setup.py +++ b/setup.py @@ -137,6 +137,9 @@ def build_extension(self, ext: CMakeExtension) -> None: packages=[ "marinholab.solvers.qpoases", ], + package_data={ + "marinholab.solvers.qpoases": ["_core.pyi", "py.typed"], + }, ext_modules=[CMakeExtension('marinholab.solvers.qpoases._core')], cmdclass={"build_ext": CMakeBuild}, zip_safe=False, diff --git a/src/core.cpp b/src/core.cpp index 1db4a91..a28fd23 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 M3::qpOASES_Solver. */ #include @@ -18,38 +20,122 @@ using namespace M3; PYBIND11_MODULE(_core, m) { - py::class_ qpoases_solver(m, "qpOASES_Solver"); + m.doc() = "Python bindings for a qpOASES-based quadratic program solver."; + + py::class_ qpoases_solver(m, "qpOASES_Solver", + "High-level, reusable solver for quadratic programs (QPs) based on qpOASES.\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_(qpoases_solver, "BooleanType") + py::enum_(qpoases_solver, "BooleanType", + "qpOASES logical values.") .value("BT_FALSE", BooleanType::BT_FALSE) .value("BT_TRUE", BooleanType::BT_TRUE) .export_values(); - py::enum_(qpoases_solver, "HessianType") + py::enum_(qpoases_solver, "HessianType", + "qpOASES Hessian definiteness types.") + .value("HST_ZERO", HessianType::HST_ZERO) + .value("HST_IDENTITY", HessianType::HST_IDENTITY) .value("HST_POSDEF", HessianType::HST_POSDEF) + .value("HST_POSDEF_NULLSPACE", HessianType::HST_POSDEF_NULLSPACE) .value("HST_SEMIDEF", HessianType::HST_SEMIDEF) - .value("HST_IDENTITY", HessianType::HST_IDENTITY) + .value("HST_INDEF", HessianType::HST_INDEF) + .value("HST_UNKNOWN", HessianType::HST_UNKNOWN) .export_values(); - py::class_ qpoases_configuration(qpoases_solver, "Configuration"); + py::enum_(qpoases_solver, "PrintLevel", + "qpOASES print levels, describing the desired amount of output at runtime.") + .value("PL_DEBUG_ITER", PrintLevel::PL_DEBUG_ITER) + .value("PL_TABULAR", PrintLevel::PL_TABULAR) + .value("PL_NONE", PrintLevel::PL_NONE) + .value("PL_LOW", PrintLevel::PL_LOW) + .value("PL_MEDIUM", PrintLevel::PL_MEDIUM) + .value("PL_HIGH", PrintLevel::PL_HIGH) + .export_values(); + + py::enum_(qpoases_solver, "SubjectToStatus", + "qpOASES bound/constraint statuses.") + .value("ST_LOWER", SubjectToStatus::ST_LOWER) + .value("ST_INACTIVE", SubjectToStatus::ST_INACTIVE) + .value("ST_UPPER", SubjectToStatus::ST_UPPER) + .value("ST_INFEASIBLE_LOWER", SubjectToStatus::ST_INFEASIBLE_LOWER) + .value("ST_INFEASIBLE_UPPER", SubjectToStatus::ST_INFEASIBLE_UPPER) + .value("ST_UNDEFINED", SubjectToStatus::ST_UNDEFINED) + .export_values(); + + py::class_ qpoases_configuration(qpoases_solver, "Configuration", + "All user-configurable solver options.\n\n" + "Members are the 1:1 mapping of qpOASES' `Options` fields (plus the\n" + "wrapper-specific `maximum_working_set_recalculations`, `use_hotstart`\n" + "and `hessian_type`). See the qpOASES manual for a full description\n" + "of each option: https://www.coin-or.org/qpOASES/doc/3.0/manual.pdf"); + qpoases_configuration.def(py::init<>()); - qpoases_configuration.def_readwrite("maximum_working_set_recalculations", &qpOASES_Solver::Configuration::maximum_working_set_recalculations); - qpoases_configuration.def_readwrite("use_hotstart", &qpOASES_Solver::Configuration::use_hotstart); - qpoases_configuration.def_readwrite("hessian_type", &qpOASES_Solver::Configuration::hessian_type); - qpoases_configuration.def_readwrite("enableRegularisation", &qpOASES_Solver::Configuration::enableRegularisation); - qpoases_configuration.def_readwrite("enableNZCTests", &qpOASES_Solver::Configuration::enableNZCTests); - qpoases_configuration.def_readwrite("enableFlippingBounds", &qpOASES_Solver::Configuration::enableFlippingBounds); - qpoases_configuration.def_readwrite("termination_tolerance", &qpOASES_Solver::Configuration::termination_tolerance); + // Wrapper-specific options + qpoases_configuration.def_readwrite("maximum_working_set_recalculations", &qpOASES_Solver::Configuration::maximum_working_set_recalculations, "Maximum number of working set recalculations during the initial homotopy."); + qpoases_configuration.def_readwrite("use_hotstart", &qpOASES_Solver::Configuration::use_hotstart, "Whether subsequent solves are warm-started instead of re-initialised."); + // qpOASES `Options` fields + qpoases_configuration.def_readwrite("print_level", &qpOASES_Solver::Configuration::print_level, "qpOASES print level (default PL_LOW, quiet)."); + qpoases_configuration.def_readwrite("enable_ramping", &qpOASES_Solver::Configuration::enable_ramping, "Enables the ramping strategy."); + qpoases_configuration.def_readwrite("enable_far_bounds", &qpOASES_Solver::Configuration::enable_far_bounds, "Enables the far bounds strategy."); + qpoases_configuration.def_readwrite("enableFlippingBounds", &qpOASES_Solver::Configuration::enableFlippingBounds, "Enables flipping of active bounds between lower and upper values."); + qpoases_configuration.def_readwrite("enableRegularisation", &qpOASES_Solver::Configuration::enableRegularisation, "Regularises the Hessian in case (semi-)definiteness is detected."); + qpoases_configuration.def_readwrite("enable_full_li_tests", &qpOASES_Solver::Configuration::enable_full_li_tests, "Uses the condition-hardened linear independence test."); + qpoases_configuration.def_readwrite("enableNZCTests", &qpOASES_Solver::Configuration::enableNZCTests, "Enables the nonzero curvature test."); + qpoases_configuration.def_readwrite("enable_drift_correction", &qpOASES_Solver::Configuration::enable_drift_correction, "Frequency of drift corrections (0 = off)."); + qpoases_configuration.def_readwrite("enable_cholesky_refactorisation", &qpOASES_Solver::Configuration::enable_cholesky_refactorisation, "Frequency of full Cholesky refactorisation of the projected Hessian (0 = updates only)."); + qpoases_configuration.def_readwrite("enable_equalities", &qpOASES_Solver::Configuration::enable_equalities, "Treats equality constraints as always active."); + qpoases_configuration.def_readwrite("termination_tolerance", &qpOASES_Solver::Configuration::termination_tolerance, "Relative termination tolerance to stop the homotopy."); + qpoases_configuration.def_readwrite("bound_tolerance", &qpOASES_Solver::Configuration::bound_tolerance, "Lower/upper (constraints') bound tolerance."); + qpoases_configuration.def_readwrite("bound_relaxation", &qpOASES_Solver::Configuration::bound_relaxation, "Offset for relaxing constraint bounds at the start of an initial homotopy."); + qpoases_configuration.def_readwrite("eps_num", &qpOASES_Solver::Configuration::eps_num, "Numerator tolerance for the ratio test."); + qpoases_configuration.def_readwrite("eps_den", &qpOASES_Solver::Configuration::eps_den, "Denominator tolerance for the ratio test."); + qpoases_configuration.def_readwrite("max_primal_jump", &qpOASES_Solver::Configuration::max_primal_jump, "Maximum allowed jump in primal variables during nonzero curvature tests."); + qpoases_configuration.def_readwrite("max_dual_jump", &qpOASES_Solver::Configuration::max_dual_jump, "Maximum allowed jump in dual variables during linear independence tests."); + qpoases_configuration.def_readwrite("initial_ramping", &qpOASES_Solver::Configuration::initial_ramping, "Start value of the ramping strategy."); + qpoases_configuration.def_readwrite("final_ramping", &qpOASES_Solver::Configuration::final_ramping, "Final value of the ramping strategy."); + qpoases_configuration.def_readwrite("initial_far_bounds", &qpOASES_Solver::Configuration::initial_far_bounds, "Initial size of the far bounds."); + qpoases_configuration.def_readwrite("grow_far_bounds", &qpOASES_Solver::Configuration::grow_far_bounds, "Growth factor applied to the far bounds."); + qpoases_configuration.def_readwrite("initial_status_bounds", &qpOASES_Solver::Configuration::initial_status_bounds, "Status assumed for all bounds at the first iteration."); + qpoases_configuration.def_readwrite("eps_flipping", &qpOASES_Solver::Configuration::eps_flipping, "Tolerance of the squared Cholesky diagonal factor which triggers flipping a bound."); + qpoases_configuration.def_readwrite("num_regularisation_steps", &qpOASES_Solver::Configuration::num_regularisation_steps, "Maximum number of successive regularisation steps."); + qpoases_configuration.def_readwrite("eps_regularisation", &qpOASES_Solver::Configuration::eps_regularisation, "Scaling factor of the identity matrix used for Hessian regularisation."); + qpoases_configuration.def_readwrite("num_refinement_steps", &qpOASES_Solver::Configuration::num_refinement_steps, "Maximum number of iterative refinement steps."); + qpoases_configuration.def_readwrite("eps_iter_ref", &qpOASES_Solver::Configuration::eps_iter_ref, "Early termination tolerance for iterative refinement."); + qpoases_configuration.def_readwrite("eps_li_tests", &qpOASES_Solver::Configuration::eps_li_tests, "Tolerance used by the linear independence tests."); + qpoases_configuration.def_readwrite("eps_nzc_tests", &qpOASES_Solver::Configuration::eps_nzc_tests, "Tolerance used by the nonzero curvature tests."); + qpoases_configuration.def_readwrite("rcond_s_min", &qpOASES_Solver::Configuration::rcond_s_min, "Minimum reciprocal condition number of the Schur complement before refactorisation is triggered."); + qpoases_configuration.def_readwrite("enable_inertia_correction", &qpOASES_Solver::Configuration::enable_inertia_correction, "Repairs the working set when negative curvature is discovered during a hotstart."); + qpoases_configuration.def_readwrite("enable_drop_infeasibles", &qpOASES_Solver::Configuration::enable_drop_infeasibles, "Whether infeasible constraints may be dropped."); + qpoases_configuration.def_readwrite("drop_bound_priority", &qpOASES_Solver::Configuration::drop_bound_priority, "Priority used when dropping bounds."); + qpoases_configuration.def_readwrite("drop_eq_con_priority", &qpOASES_Solver::Configuration::drop_eq_con_priority, "Priority used when dropping equality constraints."); + qpoases_configuration.def_readwrite("drop_ineq_con_priority", &qpOASES_Solver::Configuration::drop_ineq_con_priority, "Priority used when dropping inequality constraints."); + qpoases_configuration.def_readwrite("hessian_type", &qpOASES_Solver::Configuration::hessian_type, "Definiteness assumed for the Hessian matrix."); qpoases_solver.def(py::init(), - py::arg("configuration") = qpOASES_Solver::Configuration()); - qpoases_solver.def("solve_quadratic_program",&qpOASES_Solver::solve_quadratic_program,"."); - qpoases_solver.def("get_active_set",&qpOASES_Solver::get_active_set, - "Returns the active set of constraints obtained in the most recent call to solve_quadratic_program()."); - - // Helps evaluating the wrapper when versions show any issues - qpoases_solver.def("test_vectorxd",&qpOASES_Solver::test_vectorxd,"."); - qpoases_solver.def("test_matrixxd",&qpOASES_Solver::test_matrixxd,"."); + py::arg("configuration") = qpOASES_Solver::Configuration(), + "Constructs a solver with the given configuration (defaults to the default configuration)."); + qpoases_solver.def("solve_quadratic_program", + &qpOASES_Solver::solve_quadratic_program, + py::arg("H"), py::arg("f"), py::arg("A"), py::arg("b"), py::arg("Aeq"), py::arg("beq"), + "Solves min(x) 0.5*x'Hx + f'x s.t. Ax <= b and Aeq*x = beq (MATLAB `quadprog`-like signature). Returns the optimal x."); + qpoases_solver.def("get_active_set", + &qpOASES_Solver::get_active_set, + "Returns the active set of constraints obtained in the most recent call to solve_quadratic_program(). One entry per row of the combined constraint matrix (rows of A followed by rows of Aeq): -1 = active at its lower bound, 0 = inactive, +1 = active at its upper bound (equality constraints are always reported as +1)."); + qpoases_solver.def("test_vectorxd", + &qpOASES_Solver::test_vectorxd, + py::arg("v"), + "Round-trips a vector to help evaluate the Eigen <-> std conversions used across the wrapper."); + qpoases_solver.def("test_matrixxd", + &qpOASES_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 6082243..3019557 100644 --- a/src/core_function.cpp +++ b/src/core_function.cpp @@ -11,6 +11,53 @@ namespace M3 // https://stackoverflow.com/questions/53408962/try-to-understand-compiler-error-message-default-member-initializer-required-be qpOASES_Solver::Configuration::Configuration() = default; +/** + * \brief Builds a qpOASES `Options` object from the user configuration. + * + * Maps every qpOASES `Options` field onto the corresponding `Configuration` + * member. + */ +Options qpOASES_Solver::_to_qpoases_options() const +{ + Options options; + options.printLevel = configuration_.print_level; + options.enableRamping = configuration_.enable_ramping; + options.enableFarBounds = configuration_.enable_far_bounds; + options.enableFlippingBounds = configuration_.enableFlippingBounds; + options.enableRegularisation = configuration_.enableRegularisation; + options.enableFullLITests = configuration_.enable_full_li_tests; + options.enableNZCTests = configuration_.enableNZCTests; + options.enableDriftCorrection = configuration_.enable_drift_correction; + options.enableCholeskyRefactorisation = configuration_.enable_cholesky_refactorisation; + options.enableEqualities = configuration_.enable_equalities; + options.terminationTolerance = configuration_.termination_tolerance; + options.boundTolerance = configuration_.bound_tolerance; + options.boundRelaxation = configuration_.bound_relaxation; + options.epsNum = configuration_.eps_num; + options.epsDen = configuration_.eps_den; + options.maxPrimalJump = configuration_.max_primal_jump; + options.maxDualJump = configuration_.max_dual_jump; + options.initialRamping = configuration_.initial_ramping; + options.finalRamping = configuration_.final_ramping; + options.initialFarBounds = configuration_.initial_far_bounds; + options.growFarBounds = configuration_.grow_far_bounds; + options.initialStatusBounds = configuration_.initial_status_bounds; + options.epsFlipping = configuration_.eps_flipping; + options.numRegularisationSteps = configuration_.num_regularisation_steps; + options.epsRegularisation = configuration_.eps_regularisation; + options.numRefinementSteps = configuration_.num_refinement_steps; + options.epsIterRef = configuration_.eps_iter_ref; + options.epsLITests = configuration_.eps_li_tests; + options.epsNZCTests = configuration_.eps_nzc_tests; + options.rcondSMin = configuration_.rcond_s_min; + options.enableInertiaCorrection = configuration_.enable_inertia_correction; + options.enableDropInfeasibles = configuration_.enable_drop_infeasibles; + options.dropBoundPriority = configuration_.drop_bound_priority; + options.dropEqConPriority = configuration_.drop_eq_con_priority; + options.dropIneqConPriority = configuration_.drop_ineq_con_priority; + return options; +} + qpOASES_Solver::qpOASES_Solver(const Configuration& configuration): qpoases_solve_first_time_(true), configuration_(configuration) @@ -121,12 +168,7 @@ VectorXd qpOASES_Solver::solve_quadratic_program(const MatrixXd& H, const Vector if(qpoases_solve_first_time_) { qpoases_problem_ = SQProblem(PROBLEM_SIZE, INEQUALITY_CONSTRAINT_SIZE + EQUALITY_CONSTRAINT_SIZE, configuration_.hessian_type); - Options options; - options.printLevel = qpOASES::PrintLevel::PL_LOW; - options.enableNZCTests = configuration_.enableNZCTests; //Nonzero curvature test - options.enableFlippingBounds = configuration_.enableFlippingBounds; //Flipping bounds - options.terminationTolerance = configuration_.termination_tolerance; //Relative termination tolerance to stop homotopy - qpoases_problem_.setOptions( options ); + qpoases_problem_.setOptions(_to_qpoases_options()); auto maximum_working_set_recalculations_local = configuration_.maximum_working_set_recalculations; //qpOASES changes the value, so we make a local copy auto problem_init_return = qpoases_problem_.init(H_vec,g_vec,A_vec,NULL,NULL,lbA_vec,ubA_vec,maximum_working_set_recalculations_local);