admm-solver.hpp

//
// Copyright (c) 2022-2025 INRIA
//
 
#pragma once
 
// IWYU pragma: begin_keep
#include <cassert>
#include <cmath>
#include <cstddef>
#include <vector>
#include <optional>
#include <limits>
 
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
 
#include "pinocchio/eigen-common.hpp"
#include "pinocchio/macros.hpp"
 
#include "pinocchio/container/eigen-storage.hpp"
 
#include "pinocchio/math.hpp"
 
#include "pinocchio/tracy.hpp"
 
#include "pinocchio/utils/check.hpp"
 
#include "pinocchio/constraints.hpp"
 
#include "pinocchio/algorithm/solvers/fwd.hpp"
#include "pinocchio/algorithm/solvers/constraint-solver-base.hpp"
#include "pinocchio/algorithm/solvers/constraint-solver-utils.hpp"
#include "pinocchio/algorithm/solvers/anderson-acceleration.hpp"
// IWYU pragma: end_keep
 
namespace pinocchio
{
  // fwd declarations for ADMM-internal structs.
  // user-api structs are fwd delclared in solvers/fwd.hpp.
  // see bottom of file for definitions
  namespace internal
  {
    template<typename Scalar, int Options>
    struct ADMMSolverWorkspaceTpl;
 
    template<typename Scalar>
    struct ADMMSpectralUpdateRuleTpl;
 
    template<typename Scalar>
    struct ADMMLinearUpdateRuleTpl;
 
    template<typename Scalar>
    struct ADMMOSQPUpdateRuleTpl;
 
    template<typename Scalar>
    union ADMMUpdateRuleContainerTpl {
      ADMMUpdateRuleContainerTpl()
      : dummy() {};
      ADMMSpectralUpdateRuleTpl<Scalar> spectral_rule;
      ADMMOSQPUpdateRuleTpl<Scalar> osqp_rule;
      ADMMLinearUpdateRuleTpl<Scalar> linear_rule;
 
    protected:
      struct Dummy
      {
        Dummy() {};
      };
 
      Dummy dummy{};
    }; // struct ADMMUpdateRuleContainerTpl
  } // namespace internal
 
  enum class ADMMUpdateRule : char
  {
    SPECTRAL = 'S',
    OSQP = 'O',
    LINEAR = 'L',
    CONSTANT = 'C',
  };
 
  enum class ADMMProximalRule : char
  {
    MANUAL = 'M',
    AUTOMATIC = 'A',
  };
 
  template<typename _Scalar, int _Options>
  struct traits<ADMMConstraintSolverTpl<_Scalar, _Options>>
  {
    typedef _Scalar Scalar;
    static constexpr int Options = _Options;
 
    typedef ADMMSolverSettingsTpl<Scalar> SolverSettings;
    typedef ADMMSolverResultTpl<Scalar, Options> SolverResult;
  };
 
  template<typename _Scalar, int _Options>
  struct ADMMConstraintSolverTpl : ConstraintSolverBase<ADMMConstraintSolverTpl<_Scalar, _Options>>
  {
    typedef _Scalar Scalar;
    static constexpr int Options = _Options;
 
    typedef ADMMConstraintSolverTpl Self;
    typedef ConstraintSolverBase<Self> Base;
 
    typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> VectorXs;
    typedef Eigen::Ref<VectorXs> RefVectorXs;
    typedef Eigen::Ref<const VectorXs> RefConstVectorXs;
    typedef const Eigen::Ref<const VectorXs> ConstRefVectorXs;
    typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixXs;
 
    typedef ADMMSolverResultTpl<Scalar, Options> ADMMSolverResult;
    typedef internal::ADMMSolverWorkspaceTpl<Scalar, Options> ADMMSolverWorkspace;
    typedef ADMMSolverSettingsTpl<Scalar> ADMMSolverSettings;
    typedef ADMMSolverStatsTpl<Scalar> ADMMSolverStats;
 
    typedef internal::ADMMSpectralUpdateRuleTpl<Scalar> ADMMSpectralUpdateRule;
    typedef internal::ADMMOSQPUpdateRuleTpl<Scalar> ADMMOSQPUpdateRule;
    typedef internal::ADMMLinearUpdateRuleTpl<Scalar> ADMMLinearUpdateRule;
    typedef internal::ADMMUpdateRuleContainerTpl<Scalar> ADMMUpdateRuleContainer;
 
    using Base::reset;
    using Base::timerStart;
    using Base::timerStop;
 
    explicit ADMMConstraintSolverTpl(std::size_t max_problem_size = 0)
    : Base()
    , stats()
    , m_workspace(max_problem_size)
    , m_is_valid(false)
    {
      // we need to call reset - the solver needs to look as if it never ran
      reset();
    }
 
    bool isValid() const
    {
      return m_is_valid;
    }
 
    template<
      typename DelassusDerived,
      typename VectorLike,
      typename ConstraintModel,
      typename ConstraintModelAllocator,
      typename ConstraintData,
      typename ConstraintDataAllocator>
    bool solveImpl(
      DelassusOperatorBase<DelassusDerived> & delassus,
      const Eigen::MatrixBase<VectorLike> & g,
      const std::vector<ConstraintModel, ConstraintModelAllocator> & constraint_models,
      const std::vector<ConstraintData, ConstraintDataAllocator> & constraint_datas,
      const ADMMSolverSettings & settings,
      ADMMSolverResult & result);
 
    void resetImpl()
    {
      stats.reset();
      m_workspace.reset();
      m_is_valid = false;
    }
 
    ADMMSolverStats stats;
 
  protected:
    ADMMSolverWorkspace m_workspace;
 
    bool m_is_valid;
 
    template<typename DelassusDerived>
    static Scalar computeDelassusLargestEigenvalue(
      const DelassusOperatorBase<DelassusDerived> & delassus, ADMMSolverWorkspace & workspace);
 
    template<
      typename DelassusDerived,
      typename VectorLike,
      typename ConstraintModel,
      typename ConstraintModelAllocator,
      typename ConstraintData,
      typename ConstraintDataAllocator>
    static void retrievePrimalDualGuess(
      DelassusOperatorBase<DelassusDerived> & delassus,
      const Eigen::MatrixBase<VectorLike> & g,
      const std::vector<ConstraintModel, ConstraintModelAllocator> & constraint_models,
      const std::vector<ConstraintData, ConstraintDataAllocator> & constraint_datas,
      const ADMMSolverSettings & settings,
      const ADMMSolverResult & result,
      ADMMSolverWorkspace & workspace);
 
    template<typename DelassusDerived>
    static void retrieveRhoGuess(
      const DelassusOperatorBase<DelassusDerived> & delassus,
      const ADMMSolverSettings & settings,
      const ADMMSolverResult & result,
      ADMMSolverWorkspace & workspace);
  }; // struct ADMMConstraintSolverTpl
 
  template<typename _Scalar>
  struct traits<ADMMSolverSettingsTpl<_Scalar>>
  {
    typedef _Scalar Scalar;
  };
 
  template<typename _Scalar>
  struct ADMMSolverSettingsTpl : ConstraintSolverSettingsBase<ADMMSolverSettingsTpl<_Scalar>>
  {
    typedef _Scalar Scalar;
    typedef ADMMSolverSettingsTpl Self;
    typedef ConstraintSolverSettingsBase<Self> Base;
 
    ADMMSolverSettingsTpl(
      std::size_t max_iterations = 1000,
      Scalar absolute_feasibility_tol = Scalar(1e-6),
      Scalar relative_feasibility_tol = Scalar(1e-6),
      Scalar absolute_complementarity_tol = Scalar(1e-6),
      Scalar relative_complementarity_tol = Scalar(1e-6),
      bool solve_ncp = true,
      bool measure_timings = false,
      bool stat_record = false,
      std::optional<Scalar> rho_init = std::nullopt,
      bool warmstart_rho_with_previous_result = false,
      ADMMUpdateRule admm_update_rule = ADMMUpdateRule::OSQP,
      ADMMProximalRule admm_proximal_rule = ADMMProximalRule::MANUAL,
      Scalar mu_prox = Scalar(1e-6),
      Scalar tau_prox = Scalar(1),
      Scalar tau = Scalar(0.5),
      Scalar ratio_primal_dual = Scalar(10),
      Scalar dual_momentum = Scalar(0),
      Scalar rho_update_ratio = Scalar(0),
      std::size_t rho_min_update_frequency = 1,
      Scalar rho_momentum = Scalar(0),
      Scalar rho_min = Scalar(1e-6),
      Scalar rho_max = Scalar(1e6),
      Scalar spectral_rho_power_init = Scalar(0.2),
      Scalar spectral_rho_power_factor = Scalar(0.05),
      Scalar linear_update_rule_factor = Scalar(2),
      std::size_t lanczos_size = std::numeric_limits<int>::max(),
      std::size_t max_delassus_decomposition_updates = std::numeric_limits<int>::max(),
      std::size_t anderson_capacity = 0)
    : Base(
        max_iterations,
        absolute_feasibility_tol,
        relative_feasibility_tol,
        absolute_complementarity_tol,
        relative_complementarity_tol,
        solve_ncp,
        measure_timings,
        stat_record)
    , rho_init(rho_init)
    , warmstart_rho_with_previous_result(warmstart_rho_with_previous_result)
    , admm_update_rule(admm_update_rule)
    , admm_proximal_rule(admm_proximal_rule)
    , mu_prox(mu_prox)
    , tau_prox(tau_prox)
    , tau(tau)
    , ratio_primal_dual(ratio_primal_dual)
    , dual_momentum(dual_momentum)
    , rho_update_ratio(rho_update_ratio)
    , rho_min_update_frequency(rho_min_update_frequency)
    , rho_momentum(rho_momentum)
    , rho_min(rho_min)
    , rho_max(rho_max)
    , spectral_rho_power_init(spectral_rho_power_init)
    , spectral_rho_power_factor(spectral_rho_power_factor)
    , linear_update_rule_factor(linear_update_rule_factor)
    , lanczos_size(lanczos_size)
    , max_delassus_decomposition_updates(max_delassus_decomposition_updates)
    , anderson_capacity(anderson_capacity)
    {
    }
 
    void checkValidityImpl() const
    {
      if (rho_init)
      {
        PINOCCHIO_CHECK_INPUT_ARGUMENT(
          rho_init.value() > Scalar(0), "rho_init should be none or > 0.");
      }
      PINOCCHIO_CHECK_INPUT_ARGUMENT(mu_prox > Scalar(0), "mu_prox should be > 0.");
      PINOCCHIO_CHECK_INPUT_ARGUMENT(
        tau_prox <= Scalar(1) && tau_prox > Scalar(0), "tau_prox should lie in ]0,1].");
      PINOCCHIO_CHECK_INPUT_ARGUMENT(
        tau <= Scalar(1) && tau > Scalar(0), "tau should lie in ]0,1].");
      PINOCCHIO_CHECK_INPUT_ARGUMENT(
        ratio_primal_dual > Scalar(0), "ratio_primal_dual should be > 0.");
      PINOCCHIO_CHECK_INPUT_ARGUMENT(dual_momentum >= Scalar(0), "dual_momentum should be >= 0.");
      PINOCCHIO_CHECK_INPUT_ARGUMENT(
        rho_momentum >= Scalar(0) && rho_momentum <= Scalar(1),
        "rho_momentum should be in [0, 1].");
      PINOCCHIO_CHECK_INPUT_ARGUMENT(
        linear_update_rule_factor >= Scalar(0), "linear_update_rule_factor should be >= 0.");
      PINOCCHIO_CHECK_INPUT_ARGUMENT(
        rho_min >= Scalar(0) && rho_max >= Scalar(0) && rho_min <= rho_max,
        "rho_min and rho_max don't verify 0 <= rho_min <= rho_max");
    }
 
    // ----------------------
    // General settings
 
    using Base::max_iterations;
 
    using Base::absolute_feasibility_tol;
 
    using Base::relative_feasibility_tol;
 
    using Base::absolute_complementarity_tol;
 
    using Base::relative_complementarity_tol;
 
    using Base::solve_ncp;
 
    using Base::measure_timings;
 
    using Base::stat_record;
 
    // ----------------------
    // ADMM specific settings
 
    std::optional<Scalar> rho_init;
 
    bool warmstart_rho_with_previous_result;
 
    ADMMUpdateRule admm_update_rule;
 
    ADMMProximalRule admm_proximal_rule;
 
    Scalar mu_prox;
 
    Scalar tau_prox;
 
    Scalar tau;
 
    Scalar ratio_primal_dual;
 
    Scalar dual_momentum;
 
    Scalar rho_update_ratio;
 
    std::size_t rho_min_update_frequency;
 
    Scalar rho_momentum;
 
    Scalar rho_min;
 
    Scalar rho_max;
 
    Scalar spectral_rho_power_init;
 
    Scalar spectral_rho_power_factor;
 
    Scalar linear_update_rule_factor;
 
    std::size_t lanczos_size;
 
    std::size_t max_delassus_decomposition_updates;
 
    std::size_t anderson_capacity;
 
  }; // struct ADMMSolverSettingsTpl
 
  template<typename _Scalar, int _Options>
  struct traits<ADMMSolverResultTpl<_Scalar, _Options>>
  {
    typedef _Scalar Scalar;
    static constexpr int Options = _Options;
  };
 
  template<typename _Scalar, int _Options>
  struct ADMMSolverResultTpl : ConstraintSolverResultBase<ADMMSolverResultTpl<_Scalar, _Options>>
  {
    typedef _Scalar Scalar;
    static constexpr int Options = _Options;
    typedef ADMMSolverResultTpl Self;
    typedef ConstraintSolverResultBase<Self> Base;
 
    typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> VectorXs;
    typedef internal::EigenStorageTpl<VectorXs> VectorXsStorage;
    typedef Eigen::Ref<const VectorXs> RefConstVectorXs;
 
    using Base::constraintSize;
    using Base::setConstraintImpulseGuess;
    using Base::setConstraintVelocityGuess;
 
    ADMMSolverResultTpl()
    : Base()
    , problem_size(0)
    , delassus_decomposition_update_count(0)
    , impulse_guess(std::nullopt)
    , velocity_guess(std::nullopt)
    , rho(std::numeric_limits<Scalar>::quiet_NaN())
    , spectral_rho_power(std::numeric_limits<Scalar>::quiet_NaN())
    , mu_prox(std::numeric_limits<Scalar>::quiet_NaN())
    {
    }
 
    ADMMSolverResultTpl(const ADMMSolverResultTpl & other)
    {
      *this = other;
    }
 
    ADMMSolverResultTpl & operator=(const ADMMSolverResultTpl & other)
    {
      if (this != &other)
      {
        Base::operator=(other);
        problem_size = other.problem_size;
        delassus_decomposition_update_count = other.delassus_decomposition_update_count;
        rho = other.rho;
        spectral_rho_power = other.spectral_rho_power;
        mu_prox = other.mu_prox;
 
        // Since some members are maps reference on EigenStorage, we cannot simply copy them.
        // Thus we need to explicitly say we copy the storage, and the maps will automatically point
        // to the new storage.
        x_storage = other.x_storage;
        y_storage = other.y_storage;
        z_storage = other.z_storage;
        desaxce_storage = other.desaxce_storage;
 
        // special care must be taken for the optional guesses
        if (other.impulse_guess)
        {
          setConstraintImpulseGuess(other.impulse_guess.value());
        }
        else
        {
          clearConstraintImpulseGuessImpl();
        }
        if (other.velocity_guess)
        {
          setConstraintVelocityGuess(other.velocity_guess.value());
        }
        else
        {
          clearConstraintVelocityGuessImpl();
        }
      }
      return *this;
    }
 
    int constraintSizeImpl() const
    {
      return static_cast<int>(problem_size);
    }
 
    void resetImpl()
    {
      delassus_decomposition_update_count = 0;
 
      impulse_guess.reset();
      velocity_guess.reset();
 
      rho = std::numeric_limits<Scalar>::quiet_NaN();
      spectral_rho_power = std::numeric_limits<Scalar>::quiet_NaN();
      mu_prox = std::numeric_limits<Scalar>::quiet_NaN();
 
      // set solution to nan - solver has not run
      x.setConstant(std::numeric_limits<Scalar>::quiet_NaN());
      y.setConstant(std::numeric_limits<Scalar>::quiet_NaN());
      z.setConstant(std::numeric_limits<Scalar>::quiet_NaN());
      desaxce.setConstant(std::numeric_limits<Scalar>::quiet_NaN());
    }
 
    void resize(std::size_t problem_size_)
    {
      problem_size = problem_size_;
 
      Eigen::Index np = static_cast<Eigen::Index>(problem_size);
      x_storage.resize(np);
      y_storage.resize(np);
      z_storage.resize(np);
      desaxce_storage.resize(np);
    }
 
    template<typename VectorLike>
    void retrieveNonProjectedPrimalSolution(
      const Eigen::MatrixBase<VectorLike> & non_projected_primal_solution_) const
    {
      auto & non_projected_primal_solution = non_projected_primal_solution_.const_cast_derived();
      non_projected_primal_solution = x;
    }
 
    template<typename VectorLike>
    void retrievePrimalSolution(const Eigen::MatrixBase<VectorLike> & primal_solution_) const
    {
      auto & primal_solution = primal_solution_.const_cast_derived();
      primal_solution = y;
    }
 
    template<typename VectorLike>
    void retrieveDualSolution(const Eigen::MatrixBase<VectorLike> & dual_solution_) const
    {
      auto & dual_solution = dual_solution_.const_cast_derived();
      dual_solution = z;
    }
 
    template<typename VectorLike>
    void retrieveDesaxceTerm(const Eigen::MatrixBase<VectorLike> & desaxce_term_) const
    {
      auto & desaxce_term = desaxce_term_.const_cast_derived();
      desaxce_term = desaxce;
    }
 
    template<typename VectorLike>
    void setConstraintImpulseGuessImpl(const Eigen::MatrixBase<VectorLike> & impulse_guess_in)
    {
      m_impulse_guess_storage.resize(impulse_guess_in.size());
      m_impulse_guess = impulse_guess_in;
      impulse_guess.emplace(m_impulse_guess);
    }
 
    void clearConstraintImpulseGuessImpl()
    {
      m_impulse_guess_storage.resize(0);
      impulse_guess.reset();
    }
 
    template<typename VectorLike>
    void setConstraintVelocityGuessImpl(const Eigen::MatrixBase<VectorLike> & velocity_guess_in)
    {
      m_velocity_guess_storage.resize(velocity_guess_in.size());
      m_velocity_guess = velocity_guess_in;
      velocity_guess.emplace(m_velocity_guess);
    }
 
    void clearConstraintVelocityGuessImpl()
    {
      m_velocity_guess_storage.resize(0);
      velocity_guess.reset();
    }
 
    template<typename VectorLike>
    void
    retrieveConstraintImpulsesImpl(const Eigen::MatrixBase<VectorLike> & constraint_impulses_) const
    {
      auto & constraint_impulses = constraint_impulses_.const_cast_derived();
      constraint_impulses = y;
    }
 
    template<typename VectorLike>
    void retrieveConstraintVelocitiesImpl(
      const Eigen::MatrixBase<VectorLike> & constraint_velocities_) const
    {
      auto & constraint_velocities = constraint_velocities_.const_cast_derived();
      constraint_velocities = z - desaxce;
    }
 
    using Base::iterations;
 
    using Base::converged;
 
    using Base::primal_feasibility;
 
    using Base::dual_feasibility;
 
    using Base::complementarity;
 
    std::size_t problem_size;
 
    std::size_t delassus_decomposition_update_count;
 
    // ----------------------
    // Solution warmstart
 
    std::optional<RefConstVectorXs> impulse_guess;
 
    std::optional<RefConstVectorXs> velocity_guess;
 
    // ----------------------
    // Solution - output of the solver
 
    Scalar rho;
 
    Scalar spectral_rho_power;
 
    Scalar mu_prox;
 
    VectorXsStorage x_storage;
    typename VectorXsStorage::RefMapType x = x_storage.map();
 
    VectorXsStorage y_storage;
    typename VectorXsStorage::RefMapType y = y_storage.map();
 
    VectorXsStorage z_storage;
    typename VectorXsStorage::RefMapType z = z_storage.map();
 
    VectorXsStorage desaxce_storage;
    typename VectorXsStorage::RefMapType desaxce = desaxce_storage.map();
 
  protected:
    VectorXsStorage m_impulse_guess_storage;
    typename VectorXsStorage::RefMapType m_impulse_guess = m_impulse_guess_storage.map();
 
    VectorXsStorage m_velocity_guess_storage;
    typename VectorXsStorage::RefMapType m_velocity_guess = m_velocity_guess_storage.map();
  }; // struct ADMMSolverResultTpl
 
  template<typename _Scalar>
  struct traits<ADMMSolverStatsTpl<_Scalar>>
  {
    typedef _Scalar Scalar;
  };
 
  template<typename _Scalar>
  struct ADMMSolverStatsTpl : ConstraintSolverStatsBase<ADMMSolverStatsTpl<_Scalar>>
  {
    typedef _Scalar Scalar;
    typedef ADMMSolverStatsTpl Self;
    typedef ConstraintSolverStatsBase<Self> Base;
 
    using Base::reserve;
 
    ADMMSolverStatsTpl()
    : Base()
    , delassus_decomposition_update_count(0)
    {
    }
 
    explicit ADMMSolverStatsTpl(std::size_t max_iterations)
    : Base(max_iterations)
    , delassus_decomposition_update_count(0)
    {
      reserve(max_iterations);
    }
 
    void reserveImpl(std::size_t max_iterations)
    {
      rho.reserve(max_iterations);
      mu_prox.reserve(max_iterations);
      anderson_size.reserve(max_iterations);
      linear_system_residual.reserve(max_iterations);
      linear_system_consistency.reserve(max_iterations);
    }
 
    void resetImpl()
    {
      rho.clear();
      mu_prox.clear();
      anderson_size.clear();
      linear_system_residual.clear();
      linear_system_consistency.clear();
      delassus_decomposition_update_count = 0;
    }
 
    using Base::iterations;
 
    using Base::primal_feasibility;
 
    using Base::dual_feasibility;
 
    using Base::dual_feasibility_ncp;
 
    using Base::complementarity;
 
    std::vector<Scalar> rho;
 
    std::vector<Scalar> mu_prox;
 
    std::vector<std::size_t> anderson_size;
 
    std::vector<Scalar> linear_system_residual;
 
    std::vector<Scalar> linear_system_consistency;
 
    std::size_t delassus_decomposition_update_count;
  }; // struct ADMMSolverStatsTpl
 
  namespace internal
  {
    template<typename _Scalar, int _Options>
    struct ADMMSolverWorkspaceTpl
    {
      typedef _Scalar Scalar;
      static constexpr int Options = _Options;
      typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> VectorXs;
      typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixXs;
      typedef internal::EigenStorageTpl<VectorXs> VectorXsStorage;
      typedef LanczosDecompositionTpl<MatrixXs> LanczosDecomposition;
      typedef AndersonAccelerationTpl<Scalar> AndersonAcceleration;
 
      ADMMSolverWorkspaceTpl(
        std::size_t problem_size = 0, //
        std::size_t lanczos_size = 2, //
        std::size_t anderson_capacity = 0)
      : problem_size(problem_size)
      , lanczos_size(lanczos_size)
      , anderson_capacity(anderson_capacity)
      , delassus_decomposition_update_count(0)
      , delassus_smallest_eigenvalue(std::nullopt)
      , delassus_largest_eigenvalue(std::nullopt)
      , rho(std::numeric_limits<Scalar>::quiet_NaN())
      , spectral_rho_power(std::numeric_limits<Scalar>::quiet_NaN())
      , mu_prox(std::numeric_limits<Scalar>::quiet_NaN())
      , lanczos_decomposition(
          static_cast<Eigen::Index>(math::max(std::size_t(2), problem_size)), //
          static_cast<Eigen::Index>(
            math::max(std::size_t(2), math::min(problem_size, lanczos_size))))
      , anderson_history(problem_size, anderson_capacity)
      {
        resize(problem_size, lanczos_size, anderson_capacity);
        reset();
      }
 
      ADMMSolverWorkspaceTpl(const ADMMSolverWorkspaceTpl & other)
      : ADMMSolverWorkspaceTpl(0, 2, 0)
      {
        *this = other;
      }
 
      ADMMSolverWorkspaceTpl & operator=(const ADMMSolverWorkspaceTpl & other)
      {
        if (this != &other)
        {
          problem_size = other.problem_size;
          lanczos_size = other.lanczos_size;
          anderson_capacity = other.anderson_capacity;
          delassus_decomposition_update_count = other.delassus_decomposition_update_count;
          delassus_smallest_eigenvalue = other.delassus_smallest_eigenvalue;
          delassus_largest_eigenvalue = other.delassus_largest_eigenvalue;
          rho = other.rho;
          spectral_rho_power = other.spectral_rho_power;
          mu_prox = other.mu_prox;
          lanczos_decomposition = other.lanczos_decomposition;
          anderson_history = other.anderson_history;
 
          // Since we some members are maps on EigenStorage, we cannot simply copy them.
          // Thus we need to explicitly say we copy the storage, and the maps will automatically
          // point to the new storage.
          x_storage = other.x_storage;
          x_previous_storage = other.x_previous_storage;
          x_anderson_storage = other.x_anderson_storage;
          y_storage = other.y_storage;
          y_previous_storage = other.y_previous_storage;
          z_storage = other.z_storage;
          z_previous_storage = other.z_previous_storage;
          z_anderson_storage = other.z_anderson_storage;
          desaxce_storage = other.desaxce_storage;
          rhs_storage = other.rhs_storage;
          tmp_storage = other.tmp_storage;
          primal_feasibility_vector_storage = other.primal_feasibility_vector_storage;
          anderson_primal_feasibility_vector_storage =
            other.anderson_primal_feasibility_vector_storage;
          dual_feasibility_vector_storage = other.dual_feasibility_vector_storage;
        }
        return *this;
      }
 
      void reset()
      {
        delassus_decomposition_update_count = 0;
        delassus_smallest_eigenvalue.reset();
        delassus_largest_eigenvalue.reset();
 
        rho = std::numeric_limits<Scalar>::quiet_NaN();
        spectral_rho_power = std::numeric_limits<Scalar>::quiet_NaN();
        mu_prox = std::numeric_limits<Scalar>::quiet_NaN();
 
#ifndef NDEBUG
        // for debugging purposes
        x.setConstant(std::numeric_limits<Scalar>::quiet_NaN());
        x_previous.setConstant(std::numeric_limits<Scalar>::quiet_NaN());
        x_anderson.setConstant(std::numeric_limits<Scalar>::quiet_NaN());
        y.setConstant(std::numeric_limits<Scalar>::quiet_NaN());
        y_previous.setConstant(std::numeric_limits<Scalar>::quiet_NaN());
        z.setConstant(std::numeric_limits<Scalar>::quiet_NaN());
        z_previous.setConstant(std::numeric_limits<Scalar>::quiet_NaN());
        z_anderson.setConstant(std::numeric_limits<Scalar>::quiet_NaN());
        desaxce.setConstant(std::numeric_limits<Scalar>::quiet_NaN());
        rhs.setConstant(std::numeric_limits<Scalar>::quiet_NaN());
        tmp.setConstant(std::numeric_limits<Scalar>::quiet_NaN());
        primal_feasibility_vector.setConstant(std::numeric_limits<Scalar>::quiet_NaN());
        anderson_primal_feasibility_vector.setConstant(std::numeric_limits<Scalar>::quiet_NaN());
        dual_feasibility_vector.setConstant(std::numeric_limits<Scalar>::quiet_NaN());
#endif
      }
 
      void resize(
        std::size_t problem_size_, //
        std::size_t lanczos_size_, //
        std::size_t anderson_capacity_)
      {
        problem_size = problem_size_;
        lanczos_size = math::max(std::size_t(2), math::min(problem_size, lanczos_size_));
        anderson_capacity = anderson_capacity_;
 
        const Eigen::Index np = static_cast<Eigen::Index>(problem_size);
        x_storage.resize(np);
        x_anderson_storage.resize(np);
        y_storage.resize(np);
        x_previous_storage.resize(np);
        y_previous_storage.resize(np);
        z_storage.resize(np);
        z_anderson_storage.resize(np);
        z_previous_storage.resize(np);
        desaxce_storage.resize(np);
        rhs_storage.resize(np);
        tmp_storage.resize(np);
        primal_feasibility_vector_storage.resize(np);
        anderson_primal_feasibility_vector_storage.resize(np);
        dual_feasibility_vector_storage.resize(np);
 
        // resize lanczos
        const std::size_t lanczos_problem_size = math::max(std::size_t(2), problem_size);
        if (
          lanczos_decomposition.size() != static_cast<Eigen::Index>(lanczos_problem_size)
          || lanczos_decomposition.decompositionSize() != static_cast<Eigen::Index>(lanczos_size))
        {
          lanczos_decomposition = LanczosDecomposition(
            static_cast<Eigen::Index>(lanczos_problem_size),
            static_cast<Eigen::Index>(lanczos_size));
        }
 
        // resize anderson
        anderson_history.reserve(problem_size, anderson_capacity);
      }
 
      std::size_t problem_size;
 
      std::size_t lanczos_size;
 
      std::size_t anderson_capacity;
 
      std::size_t delassus_decomposition_update_count;
 
      std::optional<Scalar> delassus_smallest_eigenvalue;
 
      std::optional<Scalar> delassus_largest_eigenvalue;
 
      Scalar rho;
 
      Scalar spectral_rho_power;
 
      Scalar mu_prox;
 
      LanczosDecomposition lanczos_decomposition;
 
      AndersonAcceleration anderson_history;
 
      VectorXsStorage x_storage;
      typename VectorXsStorage::RefMapType x = x_storage.map();
 
      VectorXsStorage x_previous_storage;
      typename VectorXsStorage::RefMapType x_previous = x_previous_storage.map();
 
      VectorXsStorage x_anderson_storage;
      typename VectorXsStorage::RefMapType x_anderson = x_anderson_storage.map();
 
      VectorXsStorage y_storage;
      typename VectorXsStorage::RefMapType y = y_storage.map();
 
      VectorXsStorage y_previous_storage;
      typename VectorXsStorage::RefMapType y_previous = y_previous_storage.map();
 
      VectorXsStorage z_storage;
      typename VectorXsStorage::RefMapType z = z_storage.map();
 
      VectorXsStorage z_previous_storage;
      typename VectorXsStorage::RefMapType z_previous = z_previous_storage.map();
 
      VectorXsStorage z_anderson_storage;
      typename VectorXsStorage::RefMapType z_anderson = z_anderson_storage.map();
 
      VectorXsStorage desaxce_storage;
      typename VectorXsStorage::RefMapType desaxce = desaxce_storage.map();
 
      VectorXsStorage rhs_storage;
      typename VectorXsStorage::RefMapType rhs = rhs_storage.map();
 
      VectorXsStorage tmp_storage;
      typename VectorXsStorage::RefMapType tmp = tmp_storage.map();
 
      VectorXsStorage primal_feasibility_vector_storage;
      typename VectorXsStorage::RefMapType primal_feasibility_vector =
        primal_feasibility_vector_storage.map();
 
      VectorXsStorage anderson_primal_feasibility_vector_storage;
      typename VectorXsStorage::RefMapType anderson_primal_feasibility_vector =
        anderson_primal_feasibility_vector_storage.map();
 
      VectorXsStorage dual_feasibility_vector_storage;
      typename VectorXsStorage::RefMapType dual_feasibility_vector =
        dual_feasibility_vector_storage.map();
    }; // struct ADMMSolverWorkspaceTpl
 
    template<typename _Scalar>
    struct ADMMSpectralUpdateRuleTpl
    {
      typedef _Scalar Scalar;
 
      ADMMSpectralUpdateRuleTpl(
        const Scalar ratio_primal_dual,
        const Scalar L,
        const Scalar m,
        const Scalar rho_power_factor)
      : ratio_primal_dual(ratio_primal_dual)
      , rho_increment(std::pow(L / m, rho_power_factor))
      {
        PINOCCHIO_CHECK_INPUT_ARGUMENT(m > Scalar(0), "m should be positive.");
        PINOCCHIO_CHECK_INPUT_ARGUMENT(L > m, "L should be > m");
      }
 
      void eval(const Scalar primal_feasibility, const Scalar dual_feasibility, Scalar & rho) const
      {
        if (primal_feasibility > ratio_primal_dual * dual_feasibility)
        {
          rho *= rho_increment;
          //        rho *= math::pow(cond,rho_power_factor);
          //        rho_power += rho_power_factor;
        }
        else if (dual_feasibility > ratio_primal_dual * primal_feasibility)
        {
          rho /= rho_increment;
          //        rho *= math::pow(cond,-rho_power_factor);
          //        rho_power -= rho_power_factor;
        }
      }
 
      static inline Scalar computeRho(const Scalar L, const Scalar m, const Scalar rho_power)
      {
        const Scalar cond = L / m;
        const Scalar rho = math::sqrt(L * m) * math::pow(cond, rho_power);
        return rho;
      }
 
      static inline Scalar computeRhoPower(const Scalar L, const Scalar m, const Scalar rho)
      {
        const Scalar cond = L / m;
        const Scalar sqrt_L_m = math::sqrt(L * m);
        const Scalar rho_power = math::log(rho / sqrt_L_m) / math::log(cond);
        return rho_power;
      }
 
      Scalar ratio_primal_dual;
 
      Scalar rho_increment;
    };
 
    template<typename _Scalar>
    struct ADMMLinearUpdateRuleTpl
    {
      typedef _Scalar Scalar;
 
      ADMMLinearUpdateRuleTpl(
        const Scalar ratio_primal_dual, const Scalar increase_factor, const Scalar decrease_factor)
      : ratio_primal_dual(ratio_primal_dual)
      , increase_factor(increase_factor)
      , decrease_factor(decrease_factor)
      {
        PINOCCHIO_CHECK_INPUT_ARGUMENT(
          increase_factor > Scalar(1), "increase_factor should be greater than one.");
        PINOCCHIO_CHECK_INPUT_ARGUMENT(
          decrease_factor > Scalar(1), "decrease_factor should be greater than one.");
      }
 
      ADMMLinearUpdateRuleTpl(const Scalar ratio_primal_dual, const Scalar factor)
      : ratio_primal_dual(ratio_primal_dual)
      , increase_factor(factor)
      , decrease_factor(factor)
      {
        PINOCCHIO_CHECK_INPUT_ARGUMENT(factor > Scalar(1), "factor should be greater than one.");
      }
 
      void eval(const Scalar primal_feasibility, const Scalar dual_feasibility, Scalar & rho) const
      {
        if (primal_feasibility > ratio_primal_dual * dual_feasibility)
        {
          rho *= increase_factor;
        }
        else if (dual_feasibility > ratio_primal_dual * primal_feasibility)
        {
          rho /= decrease_factor;
        }
      }
 
      Scalar ratio_primal_dual;
 
      Scalar increase_factor;
 
      Scalar decrease_factor;
    };
 
    template<typename _Scalar>
    struct ADMMOSQPUpdateRuleTpl
    {
      typedef _Scalar Scalar;
 
      ADMMOSQPUpdateRuleTpl(const Scalar ratio_primal_dual, const Scalar eps_reg = 1e-8)
      : ratio_primal_dual(ratio_primal_dual)
      , eps_reg(eps_reg)
      {
        PINOCCHIO_CHECK_INPUT_ARGUMENT(
          ratio_primal_dual > Scalar(0), "ratio_primal_dual should be positive.");
        PINOCCHIO_CHECK_INPUT_ARGUMENT(eps_reg > Scalar(0), "eps_reg should be positive.");
      }
 
      void eval(const Scalar primal_feasibility, const Scalar dual_feasibility, Scalar & rho) const
      {
        if (
          primal_feasibility > ratio_primal_dual * dual_feasibility //
          || dual_feasibility > ratio_primal_dual * primal_feasibility)
        {
          rho *= std::sqrt(primal_feasibility / (dual_feasibility + eps_reg));
        }
      }
 
      Scalar ratio_primal_dual;
 
      Scalar eps_reg;
    };
  } // namespace internal
 
} // namespace pinocchio
 
// IWYU pragma: begin_exports
#include "pinocchio/src/algorithm/solvers/admm-solver.hxx"
// IWYU pragma: end_exports