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

casadi/include/alpaqa/implementation/outer/alm.tpp

@@ -0,0 +1,228 @@
+#pragma once
+
+#include <alpaqa/outer/alm.hpp>
+
+#include <algorithm>
+#include <iomanip>
+#include <iostream>
+#include <utility>
+
+#include <alpaqa/config/config.hpp>
+#include <alpaqa/implementation/outer/internal/alm-helpers.tpp>
+#include <alpaqa/implementation/util/print.tpp>
+#include <alpaqa/inner/inner-solve-options.hpp>
+#include <alpaqa/inner/internal/solverstatus.hpp>
+#include <alpaqa/util/quadmath/quadmath-print.hpp>
+
+namespace alpaqa {
+
+template <class InnerSolverT>
+typename ALMSolver<InnerSolverT>::Stats
+ALMSolver<InnerSolverT>::operator()(const Problem &p, rvec x, rvec y) {
+    using std::chrono::duration_cast;
+    using std::chrono::nanoseconds;
+    auto start_time = std::chrono::steady_clock::now();
+
+    // Check the problem dimensions etc.
+    p.check();
+
+    auto m = p.get_m();
+    if (m == 0) { // No general constraints, only box constraints
+        Stats s;
+        vec Σ(0), error(0);
+        InnerSolveOptions<config_t> opts{
+            .always_overwrite_results = true,
+            .max_time                 = params.max_time,
+            .tolerance                = params.tolerance,
+            .os                       = os,
+            .check                    = false,
+        };
+        auto ps              = inner_solver(p, opts, x, y, Σ, error);
+        bool inner_converged = ps.status == SolverStatus::Converged;
+        auto time_elapsed    = std::chrono::steady_clock::now() - start_time;
+        s.inner_convergence_failures = not inner_converged;
+        s.inner += ps;
+        s.ε                = ps.ε;
+        s.δ                = 0;
+        s.norm_penalty     = 0;
+        s.outer_iterations = 1;
+        s.elapsed_time     = duration_cast<nanoseconds>(time_elapsed);
+        s.status           = ps.status;
+        return s;
+    }
+
+    constexpr auto NaN               = alpaqa::NaN<config_t>;
+    vec Σ                            = vec::Constant(m, NaN);
+    vec Σ_old                        = vec::Constant(m, NaN);
+    vec error_1                      = vec::Constant(m, NaN);
+    vec error_2                      = vec::Constant(m, NaN);
+    [[maybe_unused]] real_t norm_e_1 = NaN;
+    [[maybe_unused]] real_t norm_e_2 = NaN;
+
+    std::array<char, 64> printbuf;
+    auto print_real = [&](real_t x) {
+        return float_to_str_vw(printbuf, x, params.print_precision);
+    };
+
+    Stats s;
+
+    // Initialize the penalty weights
+    if (params.initial_penalty > 0) {
+        Σ.fill(params.initial_penalty);
+    }
+    // Initial penalty weights from problem
+    else {
+        Helpers::initialize_penalty(p, params, x, Σ);
+    }
+
+    real_t ε                      = params.initial_tolerance;
+    [[maybe_unused]] real_t ε_old = NaN;
+    real_t Δ                      = params.penalty_update_factor;
+    real_t ρ                      = params.tolerance_update_factor;
+
+    unsigned num_successful_iters = 0;
+
+    for (unsigned i = 0; i < params.max_iter; ++i) {
+        // TODO: this is unnecessary when the previous iteration lowered the
+        // penalty update factor.
+        p.eval_proj_multipliers(y, params.max_multiplier);
+        // Check if we're allowed to lower the penalty factor even further.
+        bool out_of_penalty_factor_updates =
+            (num_successful_iters == 0
+                 ? s.initial_penalty_reduced == params.max_num_initial_retries
+                 : s.penalty_reduced == params.max_num_retries) ||
+            (s.initial_penalty_reduced + s.penalty_reduced ==
+             params.max_total_num_retries);
+        bool out_of_iter = i + 1 == params.max_iter;
+        // If this is the final iteration, or the final chance to reduce the
+        // penalty update factor, the inner solver can just return its results,
+        // even if it doesn't converge.
+        bool overwrite_results = out_of_iter || out_of_penalty_factor_updates;
+
+        // Inner solver
+        // ------------
+
+        auto time_elapsed = std::chrono::steady_clock::now() - start_time;
+        InnerSolveOptions<config_t> opts{
+            .always_overwrite_results = overwrite_results,
+            .max_time                 = params.max_time - time_elapsed,
+            .tolerance                = ε,
+            .os                       = os,
+            .outer_iter               = i,
+            .check                    = false,
+        };
+        // Call the inner solver to minimize the augmented lagrangian for fixed
+        // Lagrange multipliers y.
+        auto ps = inner_solver(p, opts, x, y, Σ, error_2);
+        // Check if the inner solver converged
+        bool inner_converged = ps.status == SolverStatus::Converged;
+        // Accumulate the inner solver statistics
+        s.inner_convergence_failures += not inner_converged;
+        s.inner += ps;
+
+        time_elapsed     = std::chrono::steady_clock::now() - start_time;
+        bool out_of_time = time_elapsed > params.max_time;
+        bool backtrack =
+            not inner_converged && not overwrite_results && not out_of_time;
+
+        // Print statistics of current iteration
+        if (params.print_interval != 0 && i % params.print_interval == 0) {
+            real_t δ          = backtrack ? NaN : vec_util::norm_inf(error_2);
+            const char *color = inner_converged ? "\x1b[0;32m" : "\x1b[0;31m";
+            const char *color_end = "\x1b[0m";
+            *os << "[\x1b[0;34mALM\x1b[0m]   " << std::setw(5) << i
+                << ": ‖Σ‖ = " << print_real(Σ.norm())
+                << ", ‖y‖ = " << print_real(y.norm())
+                << ", δ = " << print_real(δ) << ", ε = " << print_real(ps.ε)
+                << ", Δ = " << print_real(Δ) << ", status = " << color
+                << std::setw(13) << ps.status << color_end
+                << ", iter = " << std::setw(13) << ps.iterations
+                << std::endl; // Flush for Python buffering
+        }
+
+        // TODO: check penalty size?
+        if (ps.status == SolverStatus::Interrupted) {
+            s.ε                = ps.ε;
+            s.δ                = vec_util::norm_inf(error_2);
+            s.norm_penalty     = Σ.norm();
+            s.outer_iterations = i + 1;
+            s.elapsed_time     = duration_cast<nanoseconds>(time_elapsed);
+            s.status           = ps.status;
+            return s;
+        }
+
+        // Backtrack and lower penalty if inner solver did not converge
+        if (backtrack) {
+            // This means the inner solver didn't produce a solution that
+            // satisfies the required tolerance.
+            // The best thing we can do now is to restore the penalty to its
+            // previous value (when the inner solver did converge), then lower
+            // the penalty update factor Δ, and update the penalty with this
+            // smaller factor.
+            // On convergence failure, error_2 is not overwritten by the inner
+            // solver, so it still contains the error from the iteration before
+            // the previous successful iteration. error_1 contains the error of
+            // the last successful iteration. (Unless, of course, there hasn't
+            // been a successful iteration yet, which is covered by the second
+            // branch of the following if statement.)
+            if (num_successful_iters > 0) {
+                // We have a previous Σ and error
+                // Recompute penalty with smaller Δ
+                Δ = std::fmax(params.min_penalty_update_factor,
+                              Δ * params.penalty_update_factor_lower);
+                Helpers::update_penalty_weights(params, Δ, false, error_1,
+                                                error_2, norm_e_1, norm_e_2,
+                                                Σ_old, Σ, true);
+                // Recompute the primal tolerance with larger ρ
+                ρ = std::fmin(params.ρ_max, ρ * params.ρ_increase);
+                ε = std::fmax(ρ * ε_old, params.tolerance);
+                ++s.penalty_reduced;
+            } else {
+                // We don't have a previous Σ, simply lower the current Σ and
+                // increase ε
+                Σ *= params.initial_penalty_lower;
+                ε *= params.initial_tolerance_increase;
+                ++s.initial_penalty_reduced;
+            }
+        }
+
+        // If the inner solver did converge, increase penalty
+        else {
+            // After this line, error_1 contains the error of the current
+            // (successful) iteration, and error_2 contains the error of the
+            // previous successful iteration.
+            error_2.swap(error_1);
+            norm_e_2 = std::exchange(norm_e_1, vec_util::norm_inf(error_1));
+
+            // Check the termination criteria
+            bool alm_converged = ps.ε <= params.tolerance && inner_converged &&
+                                 norm_e_1 <= params.dual_tolerance;
+            bool exit = alm_converged || out_of_iter || out_of_time;
+            if (exit) {
+                s.ε                = ps.ε;
+                s.δ                = norm_e_1;
+                s.norm_penalty     = Σ.norm();
+                s.outer_iterations = i + 1;
+                s.elapsed_time     = duration_cast<nanoseconds>(time_elapsed);
+                s.status           = alm_converged ? SolverStatus::Converged
+                                     : out_of_time ? SolverStatus::MaxTime
+                                     : out_of_iter ? SolverStatus::MaxIter
+                                                   : SolverStatus::Busy;
+                return s;
+            }
+            // After this line, Σ_old contains the penalty used in the current
+            // (successful) iteration.
+            Σ_old.swap(Σ);
+            // Update Σ to contain the penalty to use on the next iteration.
+            Helpers::update_penalty_weights(
+                params, Δ, num_successful_iters == 0, error_1, error_2,
+                norm_e_1, norm_e_2, Σ_old, Σ, true);
+            // Lower the primal tolerance for the inner solver.
+            ε_old = std::exchange(ε, std::fmax(ρ * ε, params.tolerance));
+            ++num_successful_iters;
+        }
+    }
+    throw std::logic_error("[ALM]   loop error");
+}
+
+} // namespace alpaqa

casadi/include/alpaqa/implementation/outer/internal/alm-helpers.tpp

@@ -0,0 +1,80 @@
+#pragma once
+
+#include <alpaqa/outer/alm.hpp>
+#if ALPAQA_WITH_OCP
+#include <alpaqa/problem/ocproblem.hpp>
+#endif
+
+#include <algorithm>
+
+namespace alpaqa::detail {
+
+template <Config Conf>
+struct ALMHelpers {
+    USING_ALPAQA_CONFIG(Conf);
+
+    static void update_penalty_weights(const ALMParams<config_t> &params,
+                                       real_t Δ, bool first_iter, rvec e,
+                                       rvec old_e, real_t norm_e,
+                                       real_t old_norm_e, crvec Σ_old, rvec Σ,
+                                       bool monotone) {
+        const real_t θ = params.rel_penalty_increase_threshold;
+        if (norm_e <= params.dual_tolerance) {
+            Σ = Σ_old;
+            return;
+        }
+        if (params.single_penalty_factor) {
+            if (first_iter || norm_e > θ * old_norm_e) {
+                real_t new_Σ = std::fmin(params.max_penalty, Δ * Σ_old(0));
+                Σ.setConstant(new_Σ);
+            } else {
+                Σ = Σ_old;
+            }
+        } else {
+            for (index_t i = 0; i < e.rows(); ++i) {
+                if (first_iter || std::abs(e(i)) > θ * std::abs(old_e(i))) {
+                    Σ(i) = std::fmin(params.max_penalty,
+                                     std::fmax(Δ * std::abs(e(i)) / norm_e,
+                                               real_t(1) * monotone) *
+                                         Σ_old(i));
+                } else {
+                    Σ(i) = Σ_old(i);
+                }
+            }
+        }
+    }
+
+    static void initialize_penalty(const TypeErasedProblem<config_t> &p,
+                                   const ALMParams<config_t> &params, crvec x0,
+                                   rvec Σ) {
+        real_t f0 = p.eval_f(x0);
+        vec g0(p.get_m());
+        p.eval_g(x0, g0);
+        // TODO: reuse evaluations of f ang g in PANOC?
+        real_t σ = params.initial_penalty_factor *
+                   std::max(real_t(1), std::abs(f0)) /
+                   std::max(real_t(1), real_t(0.5) * g0.squaredNorm());
+        σ = std::clamp(σ, params.min_penalty, params.max_penalty);
+        Σ.fill(σ);
+    }
+
+#if ALPAQA_WITH_OCP
+    static void initialize_penalty(
+        [[maybe_unused]] const TypeErasedControlProblem<config_t> &p,
+        const ALMParams<config_t> &params, [[maybe_unused]] crvec x0, rvec Σ) {
+        real_t σ = 1;
+        σ        = std::clamp(σ, params.min_penalty, params.max_penalty);
+        Σ.fill(σ);
+    }
+#endif
+};
+
+ALPAQA_EXPORT_EXTERN_TEMPLATE(struct, ALMHelpers, DefaultConfig);
+ALPAQA_EXPORT_EXTERN_TEMPLATE(struct, ALMHelpers, EigenConfigf);
+ALPAQA_EXPORT_EXTERN_TEMPLATE(struct, ALMHelpers, EigenConfigd);
+ALPAQA_EXPORT_EXTERN_TEMPLATE(struct, ALMHelpers, EigenConfigl);
+#ifdef ALPAQA_WITH_QUAD_PRECISION
+ALPAQA_EXPORT_EXTERN_TEMPLATE(struct, ALMHelpers, EigenConfigq);
+#endif
+
+} // namespace alpaqa::detail

casadi/include/alpaqa/implementation/params/params.tpp

@@ -0,0 +1,158 @@
+#include <alpaqa/params/params.hpp>
+
+#include <alpaqa/config/config.hpp>
+#include <alpaqa/util/demangled-typename.hpp>
+
+#include <charconv>
+#include <chrono>
+#include <concepts>
+#include <map>
+#include <numeric>
+#include <stdexcept>
+#include <string_view>
+#include <system_error>
+#include <tuple>
+#include <type_traits>
+
+namespace alpaqa::params {
+
+using config_t = DefaultConfig;
+
+/// Throw a meaningful error when `s.key` is not empty, to indicate that
+/// the given type @p T is not of struct type and cannot be indexed into.
+template <class T>
+void assert_key_empty(ParamString s) {
+    if (!s.key.empty())
+        throw invalid_param("Type '" + demangled_typename(typeid(T)) +
+                            "' cannot be indexed in '" +
+                            std::string(s.full_key) + "'");
+}
+
+/// Throw a meaningful error to indicate that parameters of type @p T are not
+/// supported or implemented.
+template <class T>
+void unsupported_type(T &, [[maybe_unused]] ParamString s) {
+    throw invalid_param("Unknown parameter type '" +
+                        demangled_typename(typeid(T)) + "' in '" +
+                        std::string(s.full_key) + "'");
+}
+
+template <>
+void set_param(bool &b, ParamString s);
+
+template <>
+void set_param(std::string_view &v, ParamString s);
+
+template <>
+void set_param(std::string &v, ParamString s);
+
+template <class T>
+    requires((std::floating_point<T> || std::integral<T>) && !std::is_enum_v<T>)
+void set_param(T &f, ParamString s);
+
+template <>
+void set_param(vec<config_t> &v, ParamString s);
+
+template <>
+void set_param(vec_from_file<config_t> &v, ParamString s);
+
+template <class Rep, class Period>
+void set_param(std::chrono::duration<Rep, Period> &t, ParamString s);
+
+/// Return a function that applies @ref set_param to the given attribute of a
+/// value of type @p T.
+template <class T, class A>
+auto param_setter(A T::*attr) {
+    return [attr](T &t, ParamString s) { return set_param(t.*attr, s); };
+}
+
+/// Function wrapper to set attributes of a struct, type-erasing the type of the
+/// attribute.
+template <class T>
+struct param_setter_fun_t {
+    template <class A>
+    param_setter_fun_t(A T::*attr) : set(param_setter(attr)) {}
+    std::function<void(T &, ParamString)> set;
+    void operator()(T &t, ParamString s) const { return set(t, s); }
+};
+
+/// Dictionary that maps struct attribute names to type-erased functions that
+/// set those attributes.
+template <class T>
+using dict_to_struct_table_t =
+    std::map<std::string_view, param_setter_fun_t<T>>;
+
+/// Specialize this type to define the attribute name to attribute setters
+/// dictionaries for a struct type @p T.
+template <class T>
+struct dict_to_struct_table {};
+
+/// Return a string enumerating the possible attribute names for the struct type
+/// @p T.
+template <class T>
+auto possible_keys() {
+    const auto &tbl = dict_to_struct_table<T>::table;
+    if (tbl.empty())
+        return std::string{};
+    auto penult       = std::prev(tbl.end());
+    auto quote_concat = [](std::string &&a, auto b) {
+        return a + "'" + std::string(b.first) + "', ";
+    };
+    return std::accumulate(tbl.begin(), penult, std::string{}, quote_concat) +
+           "'" + std::string(penult->first) + "'";
+}
+
+/// Use @p s to index into the struct type @p T and overwrite the attribute
+/// given by @p s.key.
+template <class T>
+    requires requires { dict_to_struct_table<T>::table; }
+void set_param(T &t, ParamString s) {
+    const auto &m         = dict_to_struct_table<T>::table;
+    auto [key, remainder] = split_key(s.key);
+    auto it               = m.find(key);
+    if (it == m.end())
+        throw invalid_param("Invalid key '" + std::string(key) +
+                            "' for type '" + demangled_typename(typeid(T)) +
+                            "' in '" + std::string(s.full_key) +
+                            "',\n  possible keys are: " + possible_keys<T>());
+    s.key = remainder;
+    it->second.set(t, s);
+}
+
+/// Overwrites @p t based on the @p options that start with @p prefix.
+/// If @p used is not `nullopt`, sets corresponding flag of the options that
+/// were used.
+template <class T>
+void set_params(
+    T &t, std::string_view prefix, std::span<const std::string_view> options,
+    std::optional<std::span<bool>> used) {
+
+    size_t index = 0;
+    for (const auto &kv : options) {
+        auto [key, value]     = split_key(kv, '=');
+        auto [pfx, remainder] = split_key(key);
+        auto curr_index       = index++;
+        if (pfx != prefix)
+            continue;
+        if (used)
+            (*used)[curr_index] = true;
+        set_param(t, {.full_key = kv, .key = remainder, .value = value});
+    }
+}
+
+/// Helper macro to easily specialize @ref alpaqa::params::dict_to_struct_table.
+#define PARAMS_TABLE(type_, ...)                                               \
+    template <>                                                                \
+    struct dict_to_struct_table<type_> {                                       \
+        using type = type_;                                                    \
+        inline static const dict_to_struct_table_t<type> table{__VA_ARGS__};   \
+    }
+
+/// Helper macro to easily initialize a
+/// @ref alpaqa::params::dict_to_struct_table_t.
+#define PARAMS_MEMBER(name)                                                    \
+    {                                                                          \
+#name, &type::name                                                     \
+    }
+
+} // namespace alpaqa::params

casadi/include/alpaqa/implementation/problem/ocproblem.tpp

@@ -0,0 +1,56 @@
+#pragma once
+
+#include <alpaqa/problem/ocproblem.hpp>
+
+namespace alpaqa {
+
+template <Config Conf>
+void ControlProblemVTable<Conf>::default_get_D_N(const void *self, Box &D,
+                                                 const ControlProblemVTable &vtable) {
+    vtable.get_D(self, D, vtable);
+}
+template <Config Conf>
+void ControlProblemVTable<Conf>::default_eval_add_Q_N(const void *self, crvec x, crvec h, rmat Q,
+                                                      const ControlProblemVTable &vtable) {
+    vtable.eval_add_Q(self, vtable.N, x, h, Q);
+}
+template <Config Conf>
+void ControlProblemVTable<Conf>::default_eval_add_R_prod_masked(const void *, index_t, crvec, crvec,
+                                                                crindexvec, crindexvec, crvec, rvec,
+                                                                rvec,
+                                                                const ControlProblemVTable &) {
+    throw not_implemented_error("default_eval_add_R_prod_masked");
+}
+template <Config Conf>
+void ControlProblemVTable<Conf>::default_eval_add_S_prod_masked(const void *, index_t, crvec, crvec,
+                                                                crindexvec, crvec, rvec, rvec,
+                                                                const ControlProblemVTable &) {
+    throw not_implemented_error("default_eval_add_S_prod_masked");
+}
+template <Config Conf>
+auto ControlProblemVTable<Conf>::default_get_R_work_size(const void *, const ControlProblemVTable &)
+    -> length_t {
+    return 0;
+}
+template <Config Conf>
+auto ControlProblemVTable<Conf>::default_get_S_work_size(const void *, const ControlProblemVTable &)
+    -> length_t {
+    return 0;
+}
+template <Config Conf>
+void ControlProblemVTable<Conf>::default_eval_constr_N(const void *self, crvec x, rvec c,
+                                                       const ControlProblemVTable &vtable) {
+    vtable.eval_constr(self, vtable.N, x, c, vtable);
+}
+template <Config Conf>
+void ControlProblemVTable<Conf>::default_eval_grad_constr_prod_N(
+    const void *self, crvec x, crvec p, rvec grad_cx_p, const ControlProblemVTable &vtable) {
+    vtable.eval_grad_constr_prod(self, vtable.N, x, p, grad_cx_p, vtable);
+}
+template <Config Conf>
+void ControlProblemVTable<Conf>::default_eval_add_gn_hess_constr_N(
+    const void *self, crvec x, crvec M, rmat out, const ControlProblemVTable &vtable) {
+    vtable.eval_add_gn_hess_constr(self, vtable.N, x, M, out, vtable);
+}
+
+} // namespace alpaqa