casadi 3.6.5__cp39-none-macosx_11_0_arm64.whl → 3.6.7__cp39-none-macosx_11_0_arm64.whl

casadi/include/alpaqa/inner/directions/panoc/structured-lbfgs.hpp

@@ -1,146 +0,0 @@
-#pragma once
-
-#include <alpaqa/accelerators/lbfgs.hpp>
-#include <alpaqa/config/config.hpp>
-#include <alpaqa/inner/directions/panoc-direction-update.hpp>
-#include <alpaqa/inner/internal/panoc-helpers.hpp>
-#include <alpaqa/problem/type-erased-problem.hpp>
-#include <optional>
-
-namespace alpaqa {
-
-/// Parameters for the @ref StructuredLBFGSDirection class.
-template <Config Conf>
-struct StructuredLBFGSDirectionParams {
-    USING_ALPAQA_CONFIG(Conf);
-    /// Set this option to a nonzero value to include the Hessian-vector product
-    /// @f$ \nabla^2_{x_\mathcal{J}x_\mathcal{K}}\psi(x) q_\mathcal{K} @f$ from
-    /// equation 12b in @cite pas2022alpaqa, scaled by this parameter.
-    /// Set it to zero to leave out that term.
-    real_t hessian_vec_factor = 0;
-    /// If @ref hessian_vec_factor is nonzero, set this option to true to
-    /// approximate that term using finite differences instead of using AD.
-    bool hessian_vec_finite_differences = true;
-    /// If @ref hessian_vec_factor is nonzero and
-    /// @ref hessian_vec_finite_differences is true, set this option to true to
-    /// compute the exact Hessian of the augmented Lagrangian, false to
-    /// approximate it using the Hessian of the Lagrangian.
-    bool full_augmented_hessian = true;
-    enum FailurePolicy {
-        /// If L-BFGS fails, propagate the failure and tell PANOC that no
-        /// accelerated step is available, causing it to accept the projected
-        /// gradient step instead.
-        FallbackToProjectedGradient,
-        /// If L-BFGS fails, return @f$ q_\mathcal{J} =
-        /// -\gamma\nabla_{x_\mathcal{J}}\psi(x^k)
-        /// -\gamma\nabla^2_{x_\mathcal{J}x_\mathcal{K}}\psi(x)
-        /// q_\mathcal{K} @f$ as the accelerated step (effectively approximating
-        /// @f$ \nabla_{x_\mathcal{J}x_\mathcal{J}} \approx \gamma I @f$).
-        UseScaledLBFGSInput,
-    }
-    /// What to do when L-BFGS failed (e.g. if there were no pairs (s, y) with
-    /// positive curvature).
-    failure_policy = FallbackToProjectedGradient;
-};
-
-/// @ingroup grp_DirectionProviders
-template <Config Conf = DefaultConfig>
-struct StructuredLBFGSDirection {
-    USING_ALPAQA_CONFIG(Conf);
-    using Problem           = TypeErasedProblem<config_t>;
-    using LBFGS             = alpaqa::LBFGS<config_t>;
-    using AcceleratorParams = typename LBFGS::Params;
-    using DirectionParams   = StructuredLBFGSDirectionParams<config_t>;
-
-    struct Params {
-        AcceleratorParams accelerator = {};
-        DirectionParams direction     = {};
-    };
-
-    StructuredLBFGSDirection() = default;
-    StructuredLBFGSDirection(const Params &params)
-        : lbfgs(params.accelerator), direction_params(params.direction) {}
-    StructuredLBFGSDirection(const typename LBFGS::Params &params,
-                             const DirectionParams &directionparams = {})
-        : lbfgs(params), direction_params(directionparams) {}
-    StructuredLBFGSDirection(const LBFGS &lbfgs,
-                             const DirectionParams &directionparams = {})
-        : lbfgs(lbfgs), direction_params(directionparams) {}
-    StructuredLBFGSDirection(LBFGS &&lbfgs,
-                             const DirectionParams &directionparams = {})
-        : lbfgs(std::move(lbfgs)), direction_params(directionparams) {}
-
-    /// @see @ref PANOCDirection::initialize
-    void initialize(const Problem &problem, crvec y, crvec Σ, real_t γ_0,
-                    crvec x_0, crvec x̂_0, crvec p_0, crvec grad_ψx_0);
-
-    /// @see @ref PANOCDirection::has_initial_direction
-    bool has_initial_direction() const { return false; }
-
-    /// @see @ref PANOCDirection::update
-    bool update([[maybe_unused]] real_t γₖ, [[maybe_unused]] real_t γₙₑₓₜ,
-                crvec xₖ, crvec xₙₑₓₜ, [[maybe_unused]] crvec pₖ,
-                [[maybe_unused]] crvec pₙₑₓₜ, crvec grad_ψxₖ,
-                crvec grad_ψxₙₑₓₜ) {
-        const bool force = true;
-        return lbfgs.update(xₖ, xₙₑₓₜ, grad_ψxₖ, grad_ψxₙₑₓₜ,
-                            LBFGS::Sign::Positive, force);
-    }
-
-    /// @see @ref PANOCDirection::apply
-    bool apply(real_t γₖ, crvec xₖ, crvec x̂ₖ, crvec pₖ, crvec grad_ψxₖ,
-               rvec qₖ) const;
-
-    /// @see @ref PANOCDirection::changed_γ
-    void changed_γ([[maybe_unused]] real_t γₖ, [[maybe_unused]] real_t old_γₖ) {
-        // Nothing, Hessian approximation is independent of step size
-    }
-
-    /// @see @ref PANOCDirection::reset
-    void reset() { lbfgs.reset(); }
-
-    /// @see @ref PANOCDirection::get_name
-    std::string get_name() const {
-        return "StructuredLBFGSDirection<" + std::string(config_t::get_name()) +
-               '>';
-    }
-
-    auto get_params() const {
-        return std::tie(lbfgs.get_params(), direction_params);
-    }
-
-  private:
-    using Helpers = detail::PANOCHelpers<config_t>;
-
-    const Problem *problem = nullptr;
-#ifndef _WIN32
-    std::optional<crvec> y = std::nullopt;
-    std::optional<crvec> Σ = std::nullopt;
-#else
-    std::optional<vec> y = std::nullopt;
-    std::optional<vec> Σ = std::nullopt;
-#endif
-
-    LBFGS lbfgs;
-    mutable indexvec J_sto;
-    mutable vec HqK;
-    mutable vec work_n;
-    mutable vec work_n2;
-    mutable vec work_m;
-
-    void approximate_hessian_vec_term(crvec xₖ, crvec grad_ψxₖ, rvec qₖ,
-                                      crindexvec J) const;
-
-  public:
-    DirectionParams direction_params;
-};
-
-ALPAQA_EXPORT_EXTERN_TEMPLATE(struct, StructuredLBFGSDirection, DefaultConfig);
-ALPAQA_EXPORT_EXTERN_TEMPLATE(struct, StructuredLBFGSDirection, EigenConfigf);
-ALPAQA_EXPORT_EXTERN_TEMPLATE(struct, StructuredLBFGSDirection, EigenConfigd);
-ALPAQA_EXPORT_EXTERN_TEMPLATE(struct, StructuredLBFGSDirection, EigenConfigl);
-#ifdef ALPAQA_WITH_QUAD_PRECISION
-ALPAQA_EXPORT_EXTERN_TEMPLATE(struct, StructuredLBFGSDirection, EigenConfigq);
-#endif
-
-} // namespace alpaqa

casadi/include/alpaqa/inner/directions/panoc/structured-newton.hpp

@@ -1,264 +0,0 @@
-#pragma once
-
-#include <alpaqa/config/config.hpp>
-#include <alpaqa/export.hpp>
-#include <alpaqa/inner/directions/panoc-direction-update.hpp>
-#include <alpaqa/inner/internal/panoc-helpers.hpp>
-#include <alpaqa/problem/type-erased-problem.hpp>
-#include <alpaqa/util/alloc-check.hpp>
-#include <alpaqa/util/index-set.hpp>
-#include <alpaqa/util/print.hpp>
-#include <iostream>
-#include <limits>
-#include <optional>
-#include <stdexcept>
-
-#include <Eigen/Cholesky>
-#include <Eigen/Eigenvalues>
-
-namespace alpaqa {
-
-/// Parameters for the @ref StructuredNewtonDirection class.
-template <Config Conf>
-struct StructuredNewtonRegularizationParams {
-    USING_ALPAQA_CONFIG(Conf);
-    /// Minimum eigenvalue of the Hessian, scaled by
-    /// @f$ 1 + |\lambda_\mathrm{max}| @f$, enforced by regularization using
-    /// a multiple of identity.
-    real_t min_eig = std::cbrt(std::numeric_limits<real_t>::epsilon());
-    /// Print the minimum and maximum eigenvalue of the Hessian.
-    bool print_eig = false;
-};
-
-/// Parameters for the @ref StructuredNewtonDirection class.
-template <Config Conf>
-struct StructuredNewtonDirectionParams {
-    USING_ALPAQA_CONFIG(Conf);
-    /// Set this option to a nonzero value to include the Hessian-vector product
-    /// @f$ \nabla^2_{x_\mathcal{J}x_\mathcal{K}}\psi(x) q_\mathcal{K} @f$ from
-    /// equation 12b in @cite pas2022alpaqa, scaled by this parameter.
-    /// Set it to zero to leave out that term.
-    real_t hessian_vec_factor = 0;
-};
-
-/// @ingroup grp_DirectionProviders
-template <Config Conf = DefaultConfig>
-struct StructuredNewtonDirection {
-    USING_ALPAQA_CONFIG(Conf);
-    using Problem           = TypeErasedProblem<config_t>;
-    using DirectionParams   = StructuredNewtonDirectionParams<config_t>;
-    using AcceleratorParams = StructuredNewtonRegularizationParams<config_t>;
-
-    struct Params {
-        AcceleratorParams accelerator = {};
-        DirectionParams direction     = {};
-    };
-
-    StructuredNewtonDirection() = default;
-    StructuredNewtonDirection(const Params &params)
-        : reg_params(params.accelerator), direction_params(params.direction) {}
-    StructuredNewtonDirection(const AcceleratorParams &params,
-                              const DirectionParams &directionparams = {})
-        : reg_params(params), direction_params(directionparams) {}
-
-    /// @see @ref PANOCDirection::initialize
-    void initialize(const Problem &problem, crvec y, crvec Σ,
-                    [[maybe_unused]] real_t γ_0, [[maybe_unused]] crvec x_0,
-                    [[maybe_unused]] crvec x̂_0, [[maybe_unused]] crvec p_0,
-                    [[maybe_unused]] crvec grad_ψx_0) {
-        if (!(problem.provides_get_box_C() && problem.provides_get_box_D()))
-            throw std::invalid_argument(
-                "Structured Newton only supports box-constrained problems");
-        // TODO: support eval_inactive_indices_res_lna
-        if (!problem.provides_eval_hess_ψ())
-            throw std::invalid_argument("Structured Newton requires hess_ψ");
-        // Store references to problem and ALM variables
-        this->problem = &problem;
-        this->y.emplace(y);
-        this->Σ.emplace(Σ);
-        // Allocate workspaces
-        const auto n = problem.get_n();
-        JK.resize(n);
-        H_storage.resize(n * n);
-        HJ_storage.resize(n * n);
-        // Store sparsity of H
-        length_t nnz_H = problem.get_hess_ψ_num_nonzeros();
-        if (nnz_H > 0) {
-            inner_idx_H.resize(nnz_H);
-            outer_ptr_H.resize(n + 1);
-            mvec null{nullptr, 0};
-            problem.eval_hess_ψ(x_0, y, Σ, 1, inner_idx_H, outer_ptr_H, null);
-            throw std::logic_error("Sparse hessians not yet implemented");
-        }
-    }
-
-    /// @see @ref PANOCDirection::has_initial_direction
-    bool has_initial_direction() const { return true; }
-
-    /// @see @ref PANOCDirection::update
-    bool update([[maybe_unused]] real_t γₖ, [[maybe_unused]] real_t γₙₑₓₜ,
-                [[maybe_unused]] crvec xₖ, [[maybe_unused]] crvec xₙₑₓₜ,
-                [[maybe_unused]] crvec pₖ, [[maybe_unused]] crvec pₙₑₓₜ,
-                [[maybe_unused]] crvec grad_ψxₖ,
-                [[maybe_unused]] crvec grad_ψxₙₑₓₜ) {
-        return true;
-    }
-
-    /// @see @ref PANOCDirection::apply
-    bool apply(real_t γₖ, crvec xₖ, [[maybe_unused]] crvec x̂ₖ, crvec pₖ,
-               crvec grad_ψxₖ, rvec qₖ) const {
-
-        const auto n  = problem->get_n();
-        const auto &C = problem->get_box_C();
-
-        // Find inactive indices J
-        auto nJ = 0;
-        for (index_t i = 0; i < n; ++i) {
-            real_t gd = xₖ(i) - γₖ * grad_ψxₖ(i);
-            if (gd <= C.lowerbound(i)) {        // i ∊ J̲ ⊆ K
-                qₖ(i) = pₖ(i);                  //
-            } else if (C.upperbound(i) <= gd) { // i ∊ J̅ ⊆ K
-                qₖ(i) = pₖ(i);                  //
-            } else {                            // i ∊ J
-                JK(nJ++) = i;
-                qₖ(i)    = -grad_ψxₖ(i);
-            }
-        }
-
-        // There are no inactive indices J
-        if (nJ == 0) {
-            // No free variables, no Newton step possible
-            return false; // Simply use the projection step
-        }
-
-        // Compute the Hessian
-        mmat H{H_storage.data(), n, n};
-        problem->eval_hess_ψ(xₖ, *y, *Σ, 1, inner_idx_H, outer_ptr_H,
-                             H_storage);
-
-        // There are no active indices K
-        if (nJ == n) {
-            // If all indices are free, we can factor the entire matrix.
-            // Find the minimum eigenvalue to regularize the Hessian matrix and
-            // make it positive definite.
-            ScopedMallocAllower ma;
-            // Find the minimum eigenvalue to regularize the Hessian matrix and
-            // make it positive definite.
-            Eigen::SelfAdjointEigenSolver<mat> eig{H,
-                                                   Eigen::ComputeEigenvectors};
-
-            auto λ_min = eig.eigenvalues().minCoeff(),
-                 λ_max = eig.eigenvalues().maxCoeff();
-
-            if (reg_params.print_eig)
-                std::cout << "λ(H):    " << float_to_str(λ_min, 3) << ", "
-                          << float_to_str(λ_max, 3) << std::endl;
-            // Regularization
-            real_t ε = reg_params.min_eig * (1 + std::abs(λ_max)); // TODO
-            // Solve the system
-            qₖ = eig.eigenvectors().transpose() * qₖ;
-            qₖ = eig.eigenvalues().cwiseMax(ε).asDiagonal().inverse() * qₖ;
-            qₖ = eig.eigenvectors() * qₖ;
-            return true;
-        }
-
-        // There are active indices K
-        crindexvec J = JK.topRows(nJ);
-        rindexvec K  = JK.bottomRows(n - nJ);
-        detail::IndexSet<config_t>::compute_complement(J, K, n);
-
-        // Compute right-hand side of 6.1c
-        if (direction_params.hessian_vec_factor != 0)
-            qₖ(J).noalias() -=
-                direction_params.hessian_vec_factor * (H(J, K) * qₖ(K));
-
-        // If there are active indices, we need to solve the Newton system with
-        // just the inactive indices.
-        mmat HJ{HJ_storage.data(), nJ, nJ};
-        // We copy the inactive block of the Hessian to a temporary dense matrix.
-        // Since it's symmetric, only the lower part is copied.
-        HJ.template triangularView<Eigen::Lower>() =
-            H(J, J).template triangularView<Eigen::Lower>();
-
-        ScopedMallocAllower ma;
-        // Find the minimum eigenvalue to regularize the Hessian matrix and
-        // make it positive definite.
-        Eigen::SelfAdjointEigenSolver<mat> eig{HJ, Eigen::ComputeEigenvectors};
-
-        auto λ_min = eig.eigenvalues().minCoeff(),
-             λ_max = eig.eigenvalues().maxCoeff();
-
-        if (reg_params.print_eig)
-            std::cout << "λ(H_JJ): " << float_to_str(λ_min, 3) << ", "
-                      << float_to_str(λ_max, 3) << std::endl;
-        // Regularization
-        real_t ε = reg_params.min_eig * (1 + std::abs(λ_max)); // TODO
-        // Solve the system
-        auto qJ = H.col(0).topRows(nJ);
-        qJ      = qₖ(J);
-        qJ      = eig.eigenvectors().transpose() * qJ;
-        qJ      = eig.eigenvalues().cwiseMax(ε).asDiagonal().inverse() * qJ;
-        qₖ(J)   = eig.eigenvectors() * qJ;
-        return true;
-    }
-
-    /// @see @ref PANOCDirection::changed_γ
-    void changed_γ([[maybe_unused]] real_t γₖ, [[maybe_unused]] real_t old_γₖ) {
-    }
-
-    /// @see @ref PANOCDirection::reset
-    void reset() {}
-
-    /// @see @ref PANOCDirection::get_name
-    std::string get_name() const {
-        return "StructuredNewtonDirection<" +
-               std::string(config_t::get_name()) + '>';
-    }
-
-    const auto &get_params() const { return direction_params; }
-
-  private:
-    const Problem *problem = nullptr;
-#ifndef _WIN32
-    std::optional<crvec> y = std::nullopt;
-    std::optional<crvec> Σ = std::nullopt;
-#else
-    std::optional<vec> y = std::nullopt;
-    std::optional<vec> Σ = std::nullopt;
-#endif
-
-    mutable indexvec JK;
-    mutable vec H_storage;
-    mutable vec HJ_storage;
-    mutable indexvec inner_idx_H, outer_ptr_H;
-
-  public:
-    AcceleratorParams reg_params;
-    DirectionParams direction_params;
-};
-
-ALPAQA_EXPORT_EXTERN_TEMPLATE(struct, StructuredNewtonDirection, DefaultConfig);
-ALPAQA_EXPORT_EXTERN_TEMPLATE(struct, StructuredNewtonDirection, EigenConfigf);
-ALPAQA_EXPORT_EXTERN_TEMPLATE(struct, StructuredNewtonDirection, EigenConfigd);
-ALPAQA_EXPORT_EXTERN_TEMPLATE(struct, StructuredNewtonDirection, EigenConfigl);
-#ifdef ALPAQA_WITH_QUAD_PRECISION
-ALPAQA_EXPORT_EXTERN_TEMPLATE(struct, StructuredNewtonDirection, EigenConfigq);
-#endif
-
-} // namespace alpaqa
-
-#include <alpaqa/inner/panoc.hpp>
-
-namespace alpaqa {
-
-// clang-format off
-ALPAQA_EXPORT_EXTERN_TEMPLATE(class, PANOCSolver, StructuredNewtonDirection<DefaultConfig>);
-ALPAQA_EXPORT_EXTERN_TEMPLATE(class, PANOCSolver, StructuredNewtonDirection<EigenConfigf>);
-ALPAQA_EXPORT_EXTERN_TEMPLATE(class, PANOCSolver, StructuredNewtonDirection<EigenConfigd>);
-ALPAQA_EXPORT_EXTERN_TEMPLATE(class, PANOCSolver, StructuredNewtonDirection<EigenConfigl>);
-#ifdef ALPAQA_WITH_QUAD_PRECISION
-ALPAQA_EXPORT_EXTERN_TEMPLATE(class, PANOCSolver, StructuredNewtonDirection<EigenConfigq>);
-#endif
-// clang-format on
-
-} // namespace alpaqa

casadi/include/alpaqa/inner/directions/panoc-direction-update.hpp

@@ -1,96 +0,0 @@
-#pragma once
-
-#include <alpaqa/config/config.hpp>
-#include <alpaqa/problem/type-erased-problem.hpp>
-
-namespace alpaqa {
-
-/// This class outlines the interface for direction providers used by PANOC-like
-/// algorithms.
-///
-/// @ingroup grp_DirectionProviders
-template <Config Conf>
-struct PANOCDirection {
-    USING_ALPAQA_CONFIG(Conf);
-    using Problem = TypeErasedProblem<config_t>;
-
-    /// Initialize the direction provider.
-    ///
-    /// @param[in]  problem
-    ///             Problem description.
-    /// @param[in]  y
-    ///             Lagrange multipliers.
-    /// @param[in]  Σ
-    ///             Penalty factors.
-    /// @param[in]  γ_0
-    ///             Initial step size.
-    /// @param[in]  x_0
-    ///             Initial iterate.
-    /// @param[in]  x̂_0
-    ///             Result of proximal gradient step in @p x_0.
-    /// @param[in]  p_0
-    ///             Proximal gradient step in @p x_0.
-    /// @param[in]  grad_ψx_0
-    ///             Gradient of the objective in @p x_0.
-    ///
-    /// The references @p problem, @p y and @p Σ are guaranteed to remain valid
-    /// for subsequent calls to @ref update, @ref apply, @ref changed_γ and
-    /// @ref reset.
-    void initialize(const Problem &problem, crvec y, crvec Σ, real_t γ_0,
-                    crvec x_0, crvec x̂_0, crvec p_0, crvec grad_ψx_0) = delete;
-
-    /// Return whether a direction is available on the very first iteration,
-    /// before the first call to @ref update.
-    bool has_initial_direction() const = delete;
-
-    /// Update the direction provider when accepting the next iterate.
-    ///
-    /// @param[in]  γₖ
-    ///             Current step size.
-    /// @param[in]  γₙₑₓₜ
-    ///             Step size for the next iterate.
-    /// @param[in]  xₖ
-    ///             Current iterate.
-    /// @param[in]  xₙₑₓₜ
-    ///             Next iterate.
-    /// @param[in]  pₖ
-    ///             Proximal gradient step in the current iterate.
-    /// @param[in]  pₙₑₓₜ
-    ///             Proximal gradient step in the next iterate.
-    /// @param[in]  grad_ψxₖ
-    ///             Gradient of the objective in the current iterate.
-    /// @param[in]  grad_ψxₙₑₓₜ
-    ///             Gradient of the objective in the next iterate.
-    bool update(real_t γₖ, real_t γₙₑₓₜ, crvec xₖ, crvec xₙₑₓₜ, crvec pₖ,
-                crvec pₙₑₓₜ, crvec grad_ψxₖ, crvec grad_ψxₙₑₓₜ) = delete;
-
-    /// Apply the direction estimation in the current point.
-    ///
-    /// @param[in]  γₖ
-    ///             Current step size.
-    /// @param[in]  xₖ
-    ///             Current iterate.
-    /// @param[in]  x̂ₖ
-    ///             Result of proximal gradient step in @p xₖ.
-    /// @param[in]  pₖ
-    ///             Proximal gradient step in @p xₖ.
-    /// @param[in]  grad_ψxₖ
-    ///             Gradient of the objective at @p xₖ.
-    /// @param[out] qₖ
-    ///             Resulting step.
-    bool apply(real_t γₖ, crvec xₖ, crvec x̂ₖ, crvec pₖ, crvec grad_ψxₖ,
-               rvec qₖ) const = delete;
-
-    /// Called when the PANOC step size changes.
-    void changed_γ(real_t γₖ, real_t old_γₖ) = delete;
-
-    /// Called when using the direction failed. A possible behavior could be to
-    /// flush the buffers, hopefully yielding a better direction on the next
-    /// iteration.
-    void reset() = delete;
-
-    /// Get a human-readable name for this direction provider.
-    std::string get_name() const = delete;
-};
-
-} // namespace alpaqa

casadi/include/alpaqa/inner/directions/panoc-ocp/lqr.hpp

@@ -1,181 +0,0 @@
-#pragma once
-
-#include <alpaqa/config/config.hpp>
-#include <Eigen/Cholesky>
-#include <Eigen/LU>
-#include <cassert>
-
-namespace alpaqa {
-
-template <Config Conf>
-struct Dim {
-    USING_ALPAQA_CONFIG(Conf);
-    length_t N, nx, nu;
-    struct Horizon {
-        length_t N;
-        struct Iter {
-            index_t i;
-            Iter &operator++() {
-                ++i;
-                return *this;
-            }
-            Iter operator++(int) const {
-                Iter t = *this;
-                ++i;
-                return t;
-            }
-            index_t &operator*() { return i; }
-            const index_t &operator*() const { return i; }
-            friend auto operator<=>(const Iter &, const Iter &) = default;
-        };
-        static Iter begin() { return {0}; }
-        Iter end() const { return {N}; }
-    };
-    Horizon horizon() const { return {N}; }
-};
-
-template <Config Conf>
-struct StatefulLQRFactor {
-    USING_ALPAQA_CONFIG(Conf);
-
-    using Dim = alpaqa::Dim<config_t>;
-
-    StatefulLQRFactor(Dim d) : dim{d} {}
-    Dim dim;
-    mat P{dim.nx, dim.nx};
-    mat gain_K{dim.nu * dim.nx, dim.N};
-    mat e{dim.nu, dim.N};
-    vec s{dim.nx};
-    vec c{dim.nx};
-    vec y{dim.nx};
-    vec t{dim.nu};
-    vec R̅_sto{dim.nu * dim.nu};
-    vec S̅_sto{dim.nu * dim.nx};
-    vec BiJ_sto{dim.nx * dim.nu};
-    vec PBiJ_sto{dim.nx * dim.nu};
-    mat PA{dim.nx, dim.nx};
-    real_t min_rcond = 1;
-
-    void factor_masked(auto &&AB,        ///< System matrix A & input matrix B
-                       auto &&Q,         ///< State cost matrix Q
-                       auto &&R,         ///< Input cost matrix R
-                       auto &&S,         ///< Cross cost matrix S
-                       auto &&R_prod,    ///< Product with input cost matrix R
-                       auto &&S_prod,    ///< Product with cross cost matrix S
-                       auto &&q,         ///< Linear state factor q
-                       auto &&r,         ///< Linear input factor r
-                       auto &&u,         ///< Fixed inputs u
-                       auto &&J,         ///< Index set of inactive constraints
-                       auto &&K,         ///< Index set of active constraints
-                       bool use_cholesky ///< Use Cholesky instead of LU solver
-    ) {
-        using mmat = Eigen::Map<mat>;
-        using Eigen::indexing::all;
-        auto [N, nx, nu] = dim;
-
-        min_rcond = 1;
-        P.setZero();
-        Q(N)(P);
-        s = q(N);
-        for (index_t i = N; i-- > 0;) {
-            auto &&ABi  = AB(i);
-            auto &&Ai   = ABi.leftCols(nx);
-            auto &&Bi   = ABi.rightCols(nu);
-            auto &&ui   = u(i);
-            auto &&Ji   = J(i);
-            auto &&Ki   = K(i);
-            length_t nJ = Ji.size(); // number of inactive constraints
-            mmat R̅{R̅_sto.data(), nJ, nJ};
-            mmat S̅{S̅_sto.data(), nJ, nx};
-            mmat BiJ{BiJ_sto.data(), nx, nJ};
-            mmat PBiJ{PBiJ_sto.data(), nx, nJ};
-            auto &&ti = t.topRows(nJ);
-            mmat gain_Ki{gain_K.col(i).data(), nJ, nx};
-            auto &&ei = e.col(i).topRows(nJ);
-            // R̅ ← R + Bᵀ P B
-            BiJ.noalias()  = Bi(all, Ji);
-            PBiJ.noalias() = P * BiJ;
-            R̅.noalias()    = BiJ.transpose() * PBiJ;
-            R(i)(Ji, R̅);
-            // S̅ ← S + Bᵀ P A
-            PA.noalias() = P * Ai;
-            S̅.noalias()  = BiJ.transpose() * PA;
-            S(i)(Ji, S̅);
-            // c = B(·,K) u(K), y ← P c + s
-            c.noalias() = Bi(all, Ki) * ui(Ki);
-            y.noalias() = P * c;
-            y += s;
-            // t ← Bᵀy + r + R(J,K) u(K)
-            ti.noalias() = BiJ.transpose() * y;
-            ti += r(i)(Ji);
-            R_prod(i)(Ji, Ki, ui, ti);
-            // Factor R̅
-            if (use_cholesky) {
-#ifdef EIGEN_RUNTIME_NO_MALLOC
-                bool prev = Eigen::internal::is_malloc_allowed();
-                Eigen::internal::set_is_malloc_allowed(true); // TODO
-#endif
-                Eigen::LDLT<rmat> R̅LU{R̅};
-                min_rcond = std::min(R̅LU.rcond(), min_rcond);
-#ifdef EIGEN_RUNTIME_NO_MALLOC
-                Eigen::internal::set_is_malloc_allowed(prev);
-#endif
-                // K ← -R̅⁻¹S̅
-                gain_Ki.noalias() = R̅LU.solve(S̅);
-                // e ← -R̅⁻¹(Bᵀy + r)
-                ei.noalias() = R̅LU.solve(ti);
-            } else {
-#ifdef EIGEN_RUNTIME_NO_MALLOC
-                bool prev = Eigen::internal::is_malloc_allowed();
-                Eigen::internal::set_is_malloc_allowed(true); // TODO
-#endif
-                Eigen::PartialPivLU<rmat> R̅LU{R̅};
-                min_rcond = std::min(R̅LU.rcond(), min_rcond);
-#ifdef EIGEN_RUNTIME_NO_MALLOC
-                Eigen::internal::set_is_malloc_allowed(prev);
-#endif
-                // K ← -R̅⁻¹S̅
-                gain_Ki.noalias() = R̅LU.solve(S̅);
-                // e ← -R̅⁻¹(Bᵀy + r)
-                ei.noalias() = R̅LU.solve(ti);
-            }
-            gain_Ki = -gain_Ki;
-            ei      = -ei;
-            if (i > 0) {
-                // P ← Q + Aᵀ P A + S̅ᵀ K
-                P.noalias() = Ai.transpose() * PA;
-                P.noalias() += S̅.transpose() * gain_Ki;
-                // s ← S̅ᵀ e + Aᵀ y + q + Sᵀ(·,K) u(K)
-                s.noalias() = S̅.transpose() * ei;
-                s.noalias() += Ai.transpose() * y;
-                s += q(i);
-                S_prod(i)(Ki, ui, s);
-                Q(i)(P);
-            }
-        }
-    }
-
-    void solve_masked(auto &&AB, auto &&J, rvec Δu_eq, rvec Δx) {
-        auto [N, nx, nu] = dim;
-        assert(Δx.size() == 2 * nx);
-        Δx.topRows(nx).setZero();
-        for (index_t i = 0; i < N; ++i) {
-            auto &&ABi     = AB(i);
-            auto &&Ai      = ABi.leftCols(nx);
-            auto &&Bi      = ABi.rightCols(nu);
-            auto &&Ji      = J(i);
-            auto &&Δxi     = Δx.segment((i % 2) * nx, nx);
-            auto &&Δx_next = Δx.segment(((i + 1) % 2) * nx, nx);
-            length_t nJ    = Ji.size();
-            mmat Ki{gain_K.col(i).data(), nJ, nx};
-            auto &&ei  = e.col(i).topRows(nJ);
-            auto &&Δui = Δu_eq.segment(i * nu, nu);
-            ei.noalias() += Ki * Δxi;
-            Δui(Ji).noalias() = ei;
-            Δx_next.noalias() = Ai * Δxi;
-            Δx_next.noalias() += Bi * Δui;
-        }
-    }
-};
-
-} // namespace alpaqa