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