casadi 3.6.3__cp35-none-win_amd64.whl → 3.6.5__cp35-none-win_amd64.whl

casadi/include/alpaqa/accelerators/lbfgs.hpp

@@ -0,0 +1,244 @@
+#pragma once
+
+#include <alpaqa/config/config.hpp>
+#include <alpaqa/export.hpp>
+
+#include <cmath>
+#include <limits>
+#include <string>
+#include <utility>
+#include <vector>
+
+namespace alpaqa {
+
+/// Cautious BFGS update.
+/// @see @ref LBFGSParams::cbfgs
+template <Config Conf = DefaultConfig>
+struct CBFGSParams {
+    USING_ALPAQA_CONFIG(Conf);
+    real_t α = 1;
+    real_t ϵ = 0; ///< Set to zero to disable CBFGS check.
+    explicit operator bool() const { return ϵ > 0; }
+};
+
+/// Which method to use to select the L-BFGS step size.
+enum class LBFGSStepSize {
+    /// Initial inverse Hessian approximation is set to
+    /// @f$ H_0 = \gamma I @f$.
+    BasedOnExternalStepSize = 0,
+    /// Initial inverse Hessian approximation is set to
+    /// @f$ H_0 = \frac{s^\top y}{y^\top y} I @f$.
+    BasedOnCurvature = 1,
+    BasedOnGradientStepSize
+    [[deprecated("use BasedOnExternalStepSize instead")]] =
+        BasedOnExternalStepSize,
+};
+
+/// Parameters for the @ref LBFGS class.
+template <Config Conf = DefaultConfig>
+struct LBFGSParams {
+    USING_ALPAQA_CONFIG(Conf);
+
+    /// Length of the history to keep.
+    length_t memory = 10;
+    /// Reject update if @f$ y^\top s \le \text{min_div_fac} \cdot s^\top s @f$.
+    real_t min_div_fac = std::numeric_limits<real_t>::epsilon();
+    /// Reject update if @f$ s^\top s \le \text{min_abs_s} @f$.
+    real_t min_abs_s =
+        std::pow(std::numeric_limits<real_t>::epsilon(), real_t(2));
+    /// Parameters in the cautious BFGS update condition
+    /// @f[ \frac{y^\top s}{s^\top s} \ge \epsilon \| g \|^\alpha @f]
+    /// @see https://epubs.siam.org/doi/10.1137/S1052623499354242
+    CBFGSParams<config_t> cbfgs = {};
+    /// If set to true, the inverse Hessian estimate should remain definite,
+    /// i.e. a check is performed that rejects the update if
+    /// @f$ y^\top s \le \text{min_div_fac} \cdot s^\top s @f$.
+    /// If set to false, just try to prevent a singular Hessian by rejecting the
+    /// update if
+    /// @f$ \left| y^\top s \right| \le \text{min_div_fac} \cdot s^\top s @f$.
+    bool force_pos_def = true;
+    /// @see LBFGSStepSize
+    LBFGSStepSize stepsize = LBFGSStepSize::BasedOnCurvature;
+};
+
+/// Layout:
+/// ~~~
+///       ┌───── 2 m ─────┐
+///     ┌ ┌───┬───┬───┬───┐
+///     │ │   │   │   │   │
+///     │ │ s │ y │ s │ y │
+/// n+1 │ │   │   │   │   │
+///     │ ├───┼───┼───┼───┤
+///     │ │ ρ │ α │ ρ │ α │
+///     └ └───┴───┴───┴───┘
+/// ~~~
+template <Config Conf = DefaultConfig>
+struct LBFGSStorage {
+    USING_ALPAQA_CONFIG(Conf);
+
+    /// Re-allocate storage for a problem with a different size.
+    void resize(length_t n, length_t history);
+
+    /// Get the size of the s and y vectors in the buffer.
+    length_t n() const { return sto.rows() - 1; }
+    /// Get the number of previous vectors s and y stored in the buffer.
+    length_t history() const { return sto.cols() / 2; }
+
+    auto s(index_t i) { return sto.col(2 * i).topRows(n()); }
+    auto s(index_t i) const {
+        return std::as_const(sto).col(2 * i).topRows(n());
+    }
+    auto y(index_t i) { return sto.col(2 * i + 1).topRows(n()); }
+    auto y(index_t i) const {
+        return std::as_const(sto).col(2 * i + 1).topRows(n());
+    }
+    real_t &ρ(index_t i) { return sto.coeffRef(n(), 2 * i); }
+    real_t &ρ(index_t i) const { return sto.coeffRef(n(), 2 * i); }
+    real_t &α(index_t i) { return sto.coeffRef(n(), 2 * i + 1); }
+    real_t &α(index_t i) const { return sto.coeffRef(n(), 2 * i + 1); }
+
+    using storage_t = mat;
+    static_assert(!storage_t::IsRowMajor);
+    mutable storage_t sto;
+};
+
+/// Limited memory Broyden–Fletcher–Goldfarb–Shanno (L-BFGS) algorithm
+/// @ingroup grp_Accelerators
+template <Config Conf = DefaultConfig>
+class ALPAQA_EXPORT LBFGS {
+  public:
+    USING_ALPAQA_CONFIG(Conf);
+
+    using Params = LBFGSParams<config_t>;
+
+    /// The sign of the vectors @f$ p @f$ passed to the @ref update method.
+    enum class Sign {
+        Positive, ///< @f$ p \sim \nabla \psi(x) @f$
+        Negative, ///< @f$ p \sim -\nabla \psi(x) @f$
+    };
+
+    LBFGS() = default;
+    LBFGS(Params params) : params(params) {}
+    LBFGS(Params params, length_t n) : params(params) { resize(n); }
+
+    /// Check if the new vectors s and y allow for a valid BFGS update that
+    /// preserves the positive definiteness of the Hessian approximation.
+    static bool update_valid(const Params &params, real_t yᵀs, real_t sᵀs,
+                             real_t pᵀp);
+
+    /// Update the inverse Hessian approximation using the new vectors
+    /// sₖ = xₙₑₓₜ - xₖ and yₖ = pₙₑₓₜ - pₖ.
+    bool update_sy(crvec s, crvec y, real_t pₙₑₓₜᵀpₙₑₓₜ, bool forced = false);
+    /// @see @ref update_sy
+    bool update_sy_impl(const auto &s, const auto &y, real_t pₙₑₓₜᵀpₙₑₓₜ,
+                        bool forced = false);
+
+    /// Update the inverse Hessian approximation using the new vectors xₙₑₓₜ
+    /// and pₙₑₓₜ.
+    bool update(crvec xₖ, crvec xₙₑₓₜ, crvec pₖ, crvec pₙₑₓₜ,
+                Sign sign = Sign::Positive, bool forced = false);
+
+    /// Apply the inverse Hessian approximation to the given vector q.
+    /// Initial inverse Hessian approximation is set to @f$ H_0 = \gamma I @f$.
+    /// If @p γ is negative, @f$ H_0 = \frac{s^\top y}{y^\top y} I @f$.
+    bool apply(rvec q, real_t γ = -1) const;
+
+    /// Apply the inverse Hessian approximation to the given vector q, applying
+    /// only the columns and rows of the Hessian in the index set J.
+    bool apply_masked(rvec q, real_t γ, crindexvec J) const;
+    /// @copydoc apply_masked(rvec, real_t, crindexvec) const
+    bool apply_masked(rvec q, real_t γ, const std::vector<index_t> &J) const;
+    /// @copydoc apply_masked(rvec, real_t, crindexvec) const
+    bool apply_masked_impl(rvec q, real_t γ, const auto &J) const;
+
+    /// Throw away the approximation and all previous vectors s and y.
+    void reset();
+    /// Re-allocate storage for a problem with a different size. Causes
+    /// a @ref reset.
+    void resize(length_t n);
+
+    /// Scale the stored y vectors by the given factor.
+    void scale_y(real_t factor);
+
+    /// Get a string identifier for this accelerator.
+    std::string get_name() const {
+        return "LBFGS<" + std::string(config_t::get_name()) + '>';
+    }
+    /// Get the parameters.
+    const Params &get_params() const { return params; }
+
+    /// Get the size of the s and y vectors in the buffer.
+    length_t n() const { return sto.n(); }
+    /// Get the number of previous vectors s and y stored in the buffer.
+    length_t history() const { return sto.history(); }
+    /// Get the next index in the circular buffer of previous s and y vectors.
+    index_t succ(index_t i) const { return i + 1 < history() ? i + 1 : 0; }
+    /// Get the previous index in the circular buffer of s and y vectors.
+    index_t pred(index_t i) const { return i > 0 ? i - 1 : history() - 1; }
+    /// Get the number of previous s and y vectors currently stored in the
+    /// buffer.
+    length_t current_history() const { return full ? history() : idx; }
+
+    auto s(index_t i) { return sto.s(i); }
+    auto s(index_t i) const { return sto.s(i); }
+    auto y(index_t i) { return sto.y(i); }
+    auto y(index_t i) const { return sto.y(i); }
+    real_t &ρ(index_t i) { return sto.ρ(i); }
+    real_t &ρ(index_t i) const { return sto.ρ(i); }
+    real_t &α(index_t i) { return sto.α(i); }
+    real_t &α(index_t i) const { return sto.α(i); }
+
+    /// Iterate over the indices in the history buffer, oldest first.
+    template <class F>
+    void foreach_fwd(const F &fun) const {
+        if (full)
+            for (index_t i = idx; i < history(); ++i)
+                fun(i);
+        if (idx)
+            for (index_t i = 0; i < idx; ++i)
+                fun(i);
+    }
+
+    /// Iterate over the indices in the history buffer, newest first.
+    template <class F>
+    void foreach_rev(const F &fun) const {
+        if (idx)
+            for (index_t i = idx; i-- > 0;)
+                fun(i);
+        if (full)
+            for (index_t i = history(); i-- > idx;)
+                fun(i);
+    }
+
+  private:
+    LBFGSStorage<config_t> sto;
+    index_t idx = 0;
+    bool full   = false;
+    Params params;
+};
+
+ALPAQA_EXPORT_EXTERN_TEMPLATE(struct, CBFGSParams, DefaultConfig);
+ALPAQA_EXPORT_EXTERN_TEMPLATE(struct, CBFGSParams, EigenConfigf);
+ALPAQA_EXPORT_EXTERN_TEMPLATE(struct, CBFGSParams, EigenConfigd);
+ALPAQA_EXPORT_EXTERN_TEMPLATE(struct, CBFGSParams, EigenConfigl);
+#ifdef ALPAQA_WITH_QUAD_PRECISION
+ALPAQA_EXPORT_EXTERN_TEMPLATE(struct, CBFGSParams, EigenConfigq);
+#endif
+
+ALPAQA_EXPORT_EXTERN_TEMPLATE(struct, LBFGSParams, DefaultConfig);
+ALPAQA_EXPORT_EXTERN_TEMPLATE(struct, LBFGSParams, EigenConfigf);
+ALPAQA_EXPORT_EXTERN_TEMPLATE(struct, LBFGSParams, EigenConfigd);
+ALPAQA_EXPORT_EXTERN_TEMPLATE(struct, LBFGSParams, EigenConfigl);
+#ifdef ALPAQA_WITH_QUAD_PRECISION
+ALPAQA_EXPORT_EXTERN_TEMPLATE(struct, LBFGSParams, EigenConfigq);
+#endif
+
+ALPAQA_EXPORT_EXTERN_TEMPLATE(class, LBFGS, DefaultConfig);
+ALPAQA_EXPORT_EXTERN_TEMPLATE(class, LBFGS, EigenConfigf);
+ALPAQA_EXPORT_EXTERN_TEMPLATE(class, LBFGS, EigenConfigd);
+ALPAQA_EXPORT_EXTERN_TEMPLATE(class, LBFGS, EigenConfigl);
+#ifdef ALPAQA_WITH_QUAD_PRECISION
+ALPAQA_EXPORT_EXTERN_TEMPLATE(class, LBFGS, EigenConfigq);
+#endif
+
+} // namespace alpaqa

casadi/include/alpaqa/accelerators/steihaugcg.hpp

@@ -0,0 +1,143 @@
+#pragma once
+
+#include <alpaqa/config/config.hpp>
+#include <alpaqa/util/alloc-check.hpp>
+
+namespace alpaqa {
+
+template <Config Conf>
+struct SteihaugCGParams {
+    USING_ALPAQA_CONFIG(Conf);
+    real_t tol_scale       = 1;
+    real_t tol_scale_root  = real_t(0.5);
+    real_t tol_max         = inf<config_t>;
+    real_t max_iter_factor = 1;
+};
+
+/// Steihaug conjugate gradients procedure based on
+/// https://github.com/scipy/scipy/blob/583e70a50573169fc352b5dc6d94588a97c7389a/scipy/optimize/_trustregion_ncg.py#L44
+template <Config Conf>
+struct SteihaugCG {
+    USING_ALPAQA_CONFIG(Conf);
+
+    using Params = SteihaugCGParams<config_t>;
+    Params params;
+
+    SteihaugCG() = default;
+    SteihaugCG(const Params &params) : params{params} {}
+
+    mutable vec z, r, d, Bd, work_eval;
+
+    void resize(length_t n) {
+        z.resize(n);
+        r.resize(n);
+        d.resize(n);
+        Bd.resize(n);
+        work_eval.resize(n);
+    }
+
+    template <class HessFun>
+    real_t solve(const auto &grad, const HessFun &hess_prod,
+                 real_t trust_radius, rvec step) const {
+        length_t n = grad.size();
+        // get the norm of jacobian and define the origin
+        auto v = [n](auto &v) { return v.topRows(n); };
+        auto z = v(this->z), r = v(this->r), d = v(this->d), Bd = v(this->Bd);
+        auto g = v(grad);
+        auto s = v(step);
+        // init the state for the first iteration
+        z.setZero();
+        r               = g;
+        d               = -r;
+        real_t r_sq     = r.squaredNorm();
+        real_t grad_mag = g.norm();
+
+        // define a default tolerance
+        real_t tolerance =
+            std::fmin(params.tol_max, params.tol_scale * grad_mag *
+                                          std::fmin(params.tol_scale_root,
+                                                    std::sqrt(grad_mag)));
+
+        // Workspaces and function evaluation
+        auto eval = [&](crvec p) {
+            hess_prod(p, work_eval);
+            return p.dot(g) + real_t(0.5) * p.dot(v(work_eval));
+        };
+
+        // Search for the min of the approximation of the objective function.
+        index_t i           = 0;
+        const auto max_iter = static_cast<index_t>(
+            std::round(static_cast<real_t>(n) * params.max_iter_factor));
+        while (true) {
+            // do an iteration
+            hess_prod(d, Bd);
+            real_t dBd = d.dot(Bd);
+            if (dBd <= 0) {
+                // Look at the two boundary points.
+                // Find both values of t to get the boundary points such that
+                // ||z + t d|| == trust_radius
+                // and then choose the one with the predicted min value.
+                auto [ta, tb] =
+                    get_boundaries_intersections(z, d, trust_radius);
+                auto &pa   = r; // Reuse storage
+                auto &pb   = d; // Reuse storage
+                pa         = z + ta * d;
+                pb         = z + tb * d;
+                real_t q_a = eval(pa), q_b = eval(pb);
+                real_t q_min = std::fmin(q_a, q_b);
+                if (q_a == q_min) {
+                    s = pa;
+                    return q_a;
+                } else {
+                    s = pb;
+                    return q_b;
+                }
+            }
+
+            real_t alpha = r_sq / dBd;
+            s            = z + alpha * d;
+            if (s.norm() >= trust_radius) {
+                // Find t >= 0 to get the boundary point such that
+                // ||z + t d|| == trust_radius
+                auto [ta, tb] =
+                    get_boundaries_intersections(z, d, trust_radius);
+                s = z + tb * d;
+                return eval(s);
+            }
+            r += alpha * Bd;
+            real_t r_next_sq = r.squaredNorm();
+            if (std::sqrt(r_next_sq) < tolerance || i > max_iter)
+                return eval(s);
+            real_t beta_next = r_next_sq / r_sq;
+            r_sq             = r_next_sq;
+            d                = beta_next * d - r;
+            z                = s;
+            ++i;
+        }
+    }
+
+    /// Solve the scalar quadratic equation ||z + t d|| == trust_radius.
+    /// This is like a line-sphere intersection.
+    /// Return the two values of t, sorted from low to high.
+    static auto get_boundaries_intersections(crvec z, crvec d,
+                                             real_t trust_radius) {
+        real_t a = d.squaredNorm();
+        real_t b = 2 * z.dot(d);
+        real_t c = z.squaredNorm() - trust_radius * trust_radius;
+        real_t sqrt_discriminant = std::sqrt(b * b - 4 * a * c);
+
+        // The following calculation is mathematically
+        // equivalent to:
+        // ta = (-b - sqrt_discriminant) / (2*a)
+        // tb = (-b + sqrt_discriminant) / (2*a)
+        // but produce smaller round off errors.
+        // Look at Matrix Computation p.97
+        // for a better justification.
+        real_t aux = b + std::copysign(sqrt_discriminant, b);
+        real_t ta  = -aux / (2 * a);
+        real_t tb  = -2 * c / aux;
+        return std::make_tuple(std::fmin(ta, tb), std::fmax(ta, tb));
+    }
+};
+
+} // namespace alpaqa

casadi/include/alpaqa/alpaqa.hpp

@@ -0,0 +1,3 @@
+#pragma once
+
+#include <alpaqa/panoc-alm.hpp>

casadi/include/alpaqa/casadi/CasADiControlProblem.hpp

@@ -0,0 +1,185 @@
+#pragma once
+
+#include <alpaqa/casadi-ocp-loader-export.hpp>
+#include <alpaqa/config/config.hpp>
+#include <alpaqa/problem/box.hpp>
+#include <alpaqa/util/check-dim.hpp>
+#include <alpaqa/util/copyable_unique_ptr.hpp>
+#include <filesystem>
+
+namespace alpaqa {
+
+namespace casadi_loader {
+template <Config>
+struct CasADiControlFunctionsWithParam;
+} // namespace casadi_loader
+
+template <Config Conf>
+class CasADiControlProblem {
+  public:
+    USING_ALPAQA_CONFIG(Conf);
+    using Box = alpaqa::Box<config_t>;
+    length_t N, nx, nu, nh, nh_N, nc, nc_N;
+    vec x_init;
+    vec param;
+    Box U, D, D_N;
+    mutable vec work;
+
+    /// Components of the constraint function with indices below this number are
+    /// handled using a quadratic penalty method rather than using an
+    /// augmented Lagrangian method. Specifically, the Lagrange multipliers for
+    /// these components (which determine the shifts in ALM) are kept at zero.
+    index_t penalty_alm_split = 0;
+    /// Same as @ref penalty_alm_split, but for the terminal constraint.
+    index_t penalty_alm_split_N = 0;
+
+    CasADiControlProblem(const std::string &so_name, length_t N);
+    ~CasADiControlProblem();
+
+    CasADiControlProblem(const CasADiControlProblem &);
+    CasADiControlProblem &operator=(const CasADiControlProblem &);
+    CasADiControlProblem(CasADiControlProblem &&) noexcept;
+    CasADiControlProblem &operator=(CasADiControlProblem &&) noexcept;
+
+    /// Load the numerical problem data (bounds and parameters) from a CSV file.
+    /// The file should contain 8 rows, with the following contents:
+    ///   1. @ref U lower bound [nu]
+    ///   2. @ref U upper bound [nu]
+    ///   3. @ref D lower bound [nc]
+    ///   4. @ref D upper bound [nc]
+    ///   5. @ref D_N lower bound [nc_N]
+    ///   6. @ref D_N upper bound [nc_N]
+    ///   7. @ref x_init [nx]
+    ///   8. @ref param [p]
+    ///
+    /// Line endings are encoded using a single line feed (`\n`), and the column
+    /// separator can be specified using the @p sep argument.
+    void load_numerical_data(const std::filesystem::path &filepath,
+                             char sep = ',');
+
+    void get_U(Box &U) const { U = this->U; }
+    void get_D(Box &D) const { D = this->D; }
+    void get_D_N(Box &D_N) const { D_N = this->D_N; }
+    void get_x_init(rvec x_init) const { x_init = this->x_init; }
+
+    void eval_f(index_t timestep, crvec x, crvec u, rvec fxu) const;
+    void eval_jac_f(index_t timestep, crvec x, crvec u, rmat J_fxu) const;
+    void eval_grad_f_prod(index_t timestep, crvec x, crvec u, crvec p,
+                          rvec grad_fxu_p) const;
+    void eval_h(index_t timestep, crvec x, crvec u, rvec h) const;
+    void eval_h_N(crvec x, rvec h) const;
+    [[nodiscard]] real_t eval_l(index_t timestep, crvec h) const;
+    [[nodiscard]] real_t eval_l_N(crvec h) const;
+    void eval_qr(index_t timestep, crvec xu, crvec h, rvec qr) const;
+    void eval_q_N(crvec x, crvec h, rvec q) const;
+    void eval_add_Q(index_t timestep, crvec xu, crvec h, rmat Q) const;
+    void eval_add_Q_N(crvec x, crvec h, rmat Q) const;
+    void eval_add_R_masked(index_t timestep, crvec xu, crvec h, crindexvec mask,
+                           rmat R, rvec work) const;
+    void eval_add_S_masked(index_t timestep, crvec xu, crvec h, crindexvec mask,
+                           rmat S, rvec work) const;
+    void eval_add_R_prod_masked(index_t timestep, crvec xu, crvec h,
+                                crindexvec mask_J, crindexvec mask_K, crvec v,
+                                rvec out, rvec work) const;
+    void eval_add_S_prod_masked(index_t timestep, crvec xu, crvec h,
+                                crindexvec mask_K, crvec v, rvec out,
+                                rvec work) const;
+    [[nodiscard]] length_t get_R_work_size() const;
+    [[nodiscard]] length_t get_S_work_size() const;
+    void eval_constr(index_t timestep, crvec x, rvec c) const;
+    void eval_grad_constr_prod(index_t timestep, crvec x, crvec p,
+                               rvec grad_cx_p) const;
+    void eval_add_gn_hess_constr(index_t timestep, crvec x, crvec M,
+                                 rmat out) const;
+    void eval_constr_N(crvec x, rvec c) const;
+    void eval_grad_constr_prod_N(crvec x, crvec p, rvec grad_cx_p) const;
+    void eval_add_gn_hess_constr_N(crvec x, crvec M, rmat out) const;
+
+    void check() const {
+        util::check_dim_msg<config_t>(U.lowerbound, nu,
+                                      "Length of problem.U.lowerbound does not "
+                                      "match problem size problem.nu");
+        util::check_dim_msg<config_t>(U.upperbound, nu,
+                                      "Length of problem.U.upperbound does not "
+                                      "match problem size problem.nu");
+        util::check_dim_msg<config_t>(D.lowerbound, nc,
+                                      "Length of problem.D.lowerbound does not "
+                                      "match problem size problem.nc");
+        util::check_dim_msg<config_t>(D.upperbound, nc,
+                                      "Length of problem.D.upperbound does not "
+                                      "match problem size problem.nc");
+        util::check_dim_msg<config_t>(D_N.lowerbound, nc_N,
+                                      "Length of problem.D_N.lowerbound does "
+                                      "not match problem size problem.nc_N");
+        util::check_dim_msg<config_t>(D_N.upperbound, nc_N,
+                                      "Length of problem.D_N.upperbound does "
+                                      "not match problem size problem.nc_N");
+        if (penalty_alm_split < 0 || penalty_alm_split > nc)
+            throw std::invalid_argument("Invalid penalty_alm_split");
+        if (penalty_alm_split_N < 0 || penalty_alm_split > nc_N)
+            throw std::invalid_argument("Invalid penalty_alm_split_N");
+    }
+
+    [[nodiscard]] length_t get_N() const { return N; }
+    [[nodiscard]] length_t get_nx() const { return nx; }
+    [[nodiscard]] length_t get_nu() const { return nu; }
+    [[nodiscard]] length_t get_nh() const { return nh; }
+    [[nodiscard]] length_t get_nh_N() const { return nh_N; }
+    [[nodiscard]] length_t get_nc() const { return nc; }
+    [[nodiscard]] length_t get_nc_N() const { return nc_N; }
+
+    /// @see @ref TypeErasedControlProblem::eval_proj_diff_g
+    void eval_proj_diff_g(crvec z, rvec e) const {
+        for (index_t t = 0; t < N; ++t)
+            e.segment(t * nc, nc) =
+                alpaqa::projecting_difference(z.segment(t * nc, nc), D);
+        e.segment(N * nc, nc_N) =
+            alpaqa::projecting_difference(z.segment(N * nc, nc_N), D_N);
+    }
+    /// @see @ref TypeErasedControlProblem::eval_proj_multipliers
+    void eval_proj_multipliers(rvec y, real_t M) const {
+        // If there's no lower bound, the multipliers can only be positive
+        auto max_lb = [M](real_t y, real_t z_lb) {
+            real_t y_lb = z_lb == -alpaqa::inf<config_t> ? 0 : -M;
+            return std::max(y, y_lb);
+        };
+        // If there's no upper bound, the multipliers can only be negative
+        auto min_ub = [M](real_t y, real_t z_ub) {
+            real_t y_ub = z_ub == alpaqa::inf<config_t> ? 0 : M;
+            return std::min(y, y_ub);
+        };
+        for (index_t t = 0; t < N; ++t) {
+            auto num_alm    = nc - penalty_alm_split;
+            auto &&yt       = y.segment(t * nc, nc);
+            auto &&y_qpm    = yt.topRows(penalty_alm_split);
+            auto &&y_alm    = yt.bottomRows(num_alm);
+            auto &&z_alm_lb = D.lowerbound.bottomRows(num_alm);
+            auto &&z_alm_ub = D.upperbound.bottomRows(num_alm);
+            y_qpm.setZero();
+            y_alm =
+                y_alm.binaryExpr(z_alm_lb, max_lb).binaryExpr(z_alm_ub, min_ub);
+        }
+        {
+            auto &&yt       = y.segment(N * nc, nc_N);
+            auto num_alm    = nc_N - penalty_alm_split_N;
+            auto &&y_qpm    = yt.topRows(penalty_alm_split_N);
+            auto &&y_alm    = yt.bottomRows(num_alm);
+            auto &&z_alm_lb = D.lowerbound.bottomRows(num_alm);
+            auto &&z_alm_ub = D.upperbound.bottomRows(num_alm);
+            y_qpm.setZero();
+            y_alm =
+                y_alm.binaryExpr(z_alm_lb, max_lb).binaryExpr(z_alm_ub, min_ub);
+        }
+    }
+
+  private:
+    using Functions = casadi_loader::CasADiControlFunctionsWithParam<Conf>;
+    util::copyable_unique_ptr<Functions> impl;
+};
+
+CASADI_OCP_LOADER_EXPORT_EXTERN_TEMPLATE(class, CasADiControlProblem,
+                                         EigenConfigd);
+CASADI_OCP_LOADER_EXPORT_EXTERN_TEMPLATE(class, CasADiControlProblem,
+                                         DefaultConfig);
+
+} // namespace alpaqa

casadi/include/alpaqa/casadi/CasADiFunctionWrapper.hpp

@@ -0,0 +1,104 @@
+#pragma once
+
+#include <alpaqa/config/config.hpp>
+
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+#include <casadi/core/function.hpp>
+#include <casadi/mem.h>
+
+namespace alpaqa::casadi_loader {
+
+/// Class for evaluating CasADi functions, allocating the necessary workspace
+/// storage in advance for allocation-free evaluations.
+template <Config Conf, size_t N_in, size_t N_out>
+class CasADiFunctionEvaluator {
+  public:
+    USING_ALPAQA_CONFIG(Conf);
+    static_assert(std::is_same_v<real_t, casadi_real>);
+
+    using casadi_dim = std::pair<casadi_int, casadi_int>;
+
+    /// @throws std::invalid_argument
+    CasADiFunctionEvaluator(casadi::Function &&f)
+        : fun(std::move(f)), iwork(fun.sz_iw()), dwork(fun.sz_w()),
+          arg_work(fun.sz_arg()), res_work(fun.sz_res()) {
+        using namespace std::literals::string_literals;
+        if (N_in != fun.n_in())
+            throw std::invalid_argument(
+                "Invalid number of input arguments: got "s +
+                std::to_string(fun.n_in()) + ", should be " +
+                std::to_string(N_in) + ".");
+        if (N_out != fun.n_out())
+            throw std::invalid_argument(
+                "Invalid number of output arguments: got "s +
+                std::to_string(fun.n_out()) + ", should be " +
+                std::to_string(N_out) + ".");
+    }
+
+    /// @throws std::invalid_argument
+    CasADiFunctionEvaluator(casadi::Function &&f,
+                            const std::array<casadi_dim, N_in> &dim_in,
+                            const std::array<casadi_dim, N_out> &dim_out)
+        : CasADiFunctionEvaluator{std::move(f)} {
+        validate_dimensions(dim_in, dim_out);
+    }
+
+    /// @throws std::invalid_argument
+    void
+    validate_dimensions(const std::array<casadi_dim, N_in> &dim_in   = {},
+                        const std::array<casadi_dim, N_out> &dim_out = {}) {
+        using namespace std::literals::string_literals;
+        static constexpr std::array count{"first",   "second", "third",
+                                          "fourth",  "fifth",  "sixth",
+                                          "seventh", "eighth"};
+        static_assert(N_in <= count.size());
+        static_assert(N_out <= count.size());
+        auto to_string = [](casadi_dim d) {
+            return "(" + std::to_string(d.first) + ", " +
+                   std::to_string(d.second) + ")";
+        };
+        for (size_t n = 0; n < N_in; ++n) {
+            auto cs_n = static_cast<casadi_int>(n);
+            if (dim_in[n].first != 0 && dim_in[n] != fun.size_in(cs_n))
+                throw std::invalid_argument(
+                    "Invalid dimension of "s + count[n] +
+                    " input argument: got " + to_string(fun.size_in(cs_n)) +
+                    ", should be " + to_string(dim_in[n]) + ".");
+        }
+        for (size_t n = 0; n < N_out; ++n) {
+            auto cs_n = static_cast<casadi_int>(n);
+            if (dim_out[n].first != 0 && dim_out[n] != fun.size_out(cs_n))
+                throw std::invalid_argument(
+                    "Invalid dimension of "s + count[n] +
+                    " output argument: got " + to_string(fun.size_out(cs_n)) +
+                    ", should be " + to_string(dim_out[n]) + ".");
+        }
+    }
+
+  protected:
+    void operator()(const double *const *in, double *const *out) const {
+        std::copy_n(in, N_in, arg_work.begin());
+        std::copy_n(out, N_out, res_work.begin());
+        fun(arg_work.data(), res_work.data(), iwork.data(), dwork.data(), 0);
+    }
+
+  public:
+    void operator()(const double *const (&in)[N_in],
+                    double *const (&out)[N_out]) const {
+        this->operator()(&in[0], &out[0]);
+    }
+
+  public:
+    casadi::Function fun;
+
+  private:
+    mutable std::vector<casadi_int> iwork;
+    mutable std::vector<double> dwork;
+    mutable std::vector<const double *> arg_work;
+    mutable std::vector<double *> res_work;
+};
+
+} // namespace alpaqa::casadi_loader