From 8f451ecec74bcdf2582b2271804564af5687b68d Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Mon, 18 Dec 2023 12:32:04 -0800 Subject: [PATCH] Refactor interior-point method into separate file --- .../optimization/OptimizationProblem.hpp | 109 +-- src/optimization/OptimizationProblem.cpp | 816 +--------------- src/optimization/solver/InteriorPoint.cpp | 878 ++++++++++++++++++ src/optimization/solver/InteriorPoint.hpp | 52 ++ 4 files changed, 942 insertions(+), 913 deletions(-) create mode 100644 src/optimization/solver/InteriorPoint.cpp create mode 100644 src/optimization/solver/InteriorPoint.hpp diff --git a/include/sleipnir/optimization/OptimizationProblem.hpp b/include/sleipnir/optimization/OptimizationProblem.hpp index 20897ad64..2570f9fe3 100644 --- a/include/sleipnir/optimization/OptimizationProblem.hpp +++ b/include/sleipnir/optimization/OptimizationProblem.hpp @@ -3,7 +3,6 @@ #pragma once #include -#include #include #include #include @@ -11,7 +10,6 @@ #include #include -#include #include "sleipnir/autodiff/Variable.hpp" #include "sleipnir/autodiff/VariableMatrix.hpp" @@ -372,124 +370,29 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem { // https://stackoverflow.com/a/50639754 static constexpr SolverConfig kDefaultConfig{}; - // Decision variables, which are the root of the problem's expression tree + // The list of decision variables, which are the root of the problem's + // expression tree std::vector m_decisionVariables; - // Cost function: f(x) + // The cost function: f(x) std::optional m_f; - // Equality constraints: cₑ(x) = 0 + // The list of equality constraints: cₑ(x) = 0 std::vector m_equalityConstraints; - // Inequality constraints: cᵢ(x) ≥ 0 + // The list of inequality constraints: cᵢ(x) ≥ 0 std::vector m_inequalityConstraints; - SolverConfig m_config; - - // Sparsity pattern files written when spy flag is set in SolverConfig - std::ofstream m_H_spy; - std::ofstream m_A_e_spy; - std::ofstream m_A_i_spy; - + // The user callback std::function m_callback = [](const SolverIterationInfo&) { return false; }; - /** - Find the optimal solution to the nonlinear program using an interior-point - solver. - - A nonlinear program has the form: - - @verbatim - min_x f(x) - subject to cₑ(x) = 0 - cᵢ(x) ≥ 0 - @endverbatim - - where f(x) is the cost function, cₑ(x) are the equality constraints, and cᵢ(x) - are the inequality constraints. - - @param[in] initialGuess The initial guess. - @param[out] status The solver status. - */ - Eigen::VectorXd InteriorPoint( - const Eigen::Ref& initialGuess, - SolverStatus* status); - /** * Prints exit condition as a user-readable message. * * @param exitCondition The solver exit condition. */ static void PrintExitCondition(const SolverExitCondition& exitCondition); - - /** - * Returns true if the problem is locally feasible. - * - * @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at - * the current iterate. - * @param c_e The problem's equality constraints cₑ(x) evaluated at the - * current iterate. - * @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at - * the current iterate. - * @param c_i The problem's inequality constraints cᵢ(x) evaluated at the - * current iterate. - */ - bool IsLocallyFeasible(const Eigen::SparseMatrix& A_e, - const Eigen::VectorXd& c_e, - const Eigen::SparseMatrix& A_i, - const Eigen::VectorXd& c_i) const; - - /** - * Returns the error estimate using the KKT conditions for the interior-point - * method. - * - * @param g Gradient of the cost function ∇f. - * @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at - * the current iterate. - * @param c_e The problem's equality constraints cₑ(x) evaluated at the - * current iterate. - * @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at - * the current iterate. - * @param c_i The problem's inequality constraints cᵢ(x) evaluated at the - * current iterate. - * @param s Inequality constraint slack variables. - * @param y Equality constraint dual variables. - * @param z Inequality constraint dual variables. - * @param μ Barrier parameter. - */ - double ErrorEstimate(const Eigen::VectorXd& g, - const Eigen::SparseMatrix& A_e, - const Eigen::VectorXd& c_e, - const Eigen::SparseMatrix& A_i, - const Eigen::VectorXd& c_i, const Eigen::VectorXd& s, - const Eigen::VectorXd& y, const Eigen::VectorXd& z, - double μ) const; - - /** - * Returns the KKT error for the interior-point method. - * - * @param g Gradient of the cost function ∇f. - * @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at - * the current iterate. - * @param c_e The problem's equality constraints cₑ(x) evaluated at the - * current iterate. - * @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at - * the current iterate. - * @param c_i The problem's inequality constraints cᵢ(x) evaluated at the - * current iterate. - * @param s Inequality constraint slack variables. - * @param y Equality constraint dual variables. - * @param z Inequality constraint dual variables. - * @param μ Barrier parameter. - */ - double KKTError(const Eigen::VectorXd& g, - const Eigen::SparseMatrix& A_e, - const Eigen::VectorXd& c_e, - const Eigen::SparseMatrix& A_i, - const Eigen::VectorXd& c_i, const Eigen::VectorXd& s, - const Eigen::VectorXd& y, const Eigen::VectorXd& z, - double μ) const; }; } // namespace sleipnir diff --git a/src/optimization/OptimizationProblem.cpp b/src/optimization/OptimizationProblem.cpp index 49efb63c0..c7aecdbfd 100644 --- a/src/optimization/OptimizationProblem.cpp +++ b/src/optimization/OptimizationProblem.cpp @@ -2,91 +2,15 @@ #include "sleipnir/optimization/OptimizationProblem.hpp" -#include #include -#include -#include -#include -#include #include -#include "optimization/Filter.hpp" -#include "optimization/RegularizedLDLT.hpp" -#include "sleipnir/autodiff/Expression.hpp" -#include "sleipnir/autodiff/Gradient.hpp" -#include "sleipnir/autodiff/Hessian.hpp" -#include "sleipnir/autodiff/Jacobian.hpp" -#include "sleipnir/autodiff/Variable.hpp" -#include "sleipnir/optimization/SolverExitCondition.hpp" -#include "sleipnir/util/Spy.hpp" +#include "optimization/solver/InteriorPoint.hpp" #include "util/AutodiffUtil.hpp" -#include "util/ScopeExit.hpp" -#include "util/SparseMatrixBuilder.hpp" -#include "util/SparseUtil.hpp" using namespace sleipnir; -// Works cited: -// -// [1] Nocedal, J. and Wright, S. "Numerical Optimization", 2nd. ed., Ch. 19. -// Springer, 2006. -// [2] Wächter, A. and Biegler, L. "On the implementation of an interior-point -// filter line-search algorithm for large-scale nonlinear programming", -// 2005. http://cepac.cheme.cmu.edu/pasilectures/biegler/ipopt.pdf -// [3] Byrd, R. and Nocedal J. and Waltz R. "KNITRO: An Integrated Package for -// Nonlinear Optimization", 2005. -// https://users.iems.northwestern.edu/~nocedal/PDFfiles/integrated.pdf - -namespace { - -/** - * Applies fraction-to-the-boundary rule to a variable and its iterate, then - * returns a fraction of the iterate step size within (0, 1]. - * - * @param x The variable. - * @param p The iterate on the variable. - * @param τ Fraction-to-the-boundary rule scaling factor within (0, 1]. - * @return Fraction of the iterate step size within (0, 1]. - */ -double FractionToTheBoundaryRule(const Eigen::Ref& x, - const Eigen::Ref& p, - double τ) { - // α = max(α ∈ (0, 1] : x + αp ≥ (1 − τ)x) - // - // where x and τ are positive. - // - // x + αp ≥ (1 − τ)x - // x + αp ≥ x − τx - // αp ≥ −τx - // - // If the inequality is false, p < 0 and α is too big. Find the largest value - // of α that makes the inequality true. - // - // α = −τ/p x - double α = 1.0; - for (int i = 0; i < x.rows(); ++i) { - if (α * p(i) < -τ * x(i)) { - α = -τ / p(i) * x(i); - } - } - - return α; -} - -/** - * Converts std::chrono::duration to a number of milliseconds rounded to three - * decimals. - */ -template > -double ToMilliseconds(const std::chrono::duration& duration) { - using std::chrono::duration_cast; - using std::chrono::microseconds; - return duration_cast(duration).count() / 1000.0; -} - -} // namespace - OptimizationProblem::OptimizationProblem() noexcept { m_decisionVariables.reserve(1024); m_equalityConstraints.reserve(1024); @@ -196,8 +120,6 @@ void OptimizationProblem::SubjectTo(InequalityConstraints&& constraint) { } SolverStatus OptimizationProblem::Solve(const SolverConfig& config) { - m_config = config; - // Create the initial value column vector Eigen::VectorXd x{m_decisionVariables.size()}; for (size_t i = 0; i < m_decisionVariables.size(); ++i) { @@ -231,7 +153,7 @@ SolverStatus OptimizationProblem::Solve(const SolverConfig& config) { } } - if (m_config.diagnostics) { + if (config.diagnostics) { constexpr std::array kExprTypeToName = { "empty", "constant", "linear", "quadratic", "nonlinear"}; @@ -262,16 +184,12 @@ SolverStatus OptimizationProblem::Solve(const SolverConfig& config) { return status; } - if (config.spy) { - m_A_e_spy.open("A_e.spy"); - m_A_i_spy.open("A_i.spy"); - m_H_spy.open("H.spy"); - } - // Solve the optimization problem - Eigen::VectorXd solution = InteriorPoint(x, &status); + Eigen::VectorXd solution = + InteriorPoint(m_decisionVariables, m_f, m_equalityConstraints, + m_inequalityConstraints, m_callback, config, x, &status); - if (m_config.diagnostics) { + if (config.diagnostics) { PrintExitCondition(status.exitCondition); } @@ -281,609 +199,6 @@ SolverStatus OptimizationProblem::Solve(const SolverConfig& config) { return status; } -Eigen::VectorXd OptimizationProblem::InteriorPoint( - const Eigen::Ref& initialGuess, - SolverStatus* status) { - // Read docs/algorithms.md#Interior-point_method for a derivation of the - // interior-point method formulation being used. - - auto solveStartTime = std::chrono::system_clock::now(); - - // Map decision variables and constraints to Eigen vectors for Lagrangian - MapVectorXvar xAD(m_decisionVariables.data(), m_decisionVariables.size()); - SetAD(xAD, initialGuess); - MapVectorXvar c_eAD(m_equalityConstraints.data(), - m_equalityConstraints.size()); - MapVectorXvar c_iAD(m_inequalityConstraints.data(), - m_inequalityConstraints.size()); - - // Create autodiff variables for s, y, and z for Lagrangian - VectorXvar sAD = VectorXvar::Ones(m_inequalityConstraints.size()); - VectorXvar yAD = VectorXvar::Zero(m_equalityConstraints.size()); - VectorXvar zAD = VectorXvar::Ones(m_inequalityConstraints.size()); - - // Lagrangian L - // - // L(x, s, y, z)ₖ = f(x)ₖ − yₖᵀcₑ(x)ₖ − zₖᵀ(cᵢ(x)ₖ − sₖ) - Variable L = - m_f.value() - yAD.transpose() * c_eAD - zAD.transpose() * (c_iAD - sAD); - - // Equality constraint Jacobian Aₑ - // - // [∇ᵀcₑ₁(x)ₖ] - // Aₑ(x) = [∇ᵀcₑ₂(x)ₖ] - // [ ⋮ ] - // [∇ᵀcₑₘ(x)ₖ] - Jacobian jacobianCe{c_eAD, xAD}; - Eigen::SparseMatrix A_e = jacobianCe.Calculate(); - - // Inequality constraint Jacobian Aᵢ - // - // [∇ᵀcᵢ₁(x)ₖ] - // Aᵢ(x) = [∇ᵀcᵢ₂(x)ₖ] - // [ ⋮ ] - // [∇ᵀcᵢₘ(x)ₖ] - Jacobian jacobianCi{c_iAD, xAD}; - Eigen::SparseMatrix A_i = jacobianCi.Calculate(); - - // Gradient of f ∇f - Gradient gradientF{m_f.value(), xAD}; - Eigen::SparseVector g = gradientF.Calculate(); - - // Hessian of the Lagrangian H - // - // Hₖ = ∇²ₓₓL(x, s, y, z)ₖ - Hessian hessianL{L, xAD}; - Eigen::SparseMatrix H = hessianL.Calculate(); - - Eigen::VectorXd x = initialGuess; - Eigen::VectorXd s = GetAD(sAD); - Eigen::VectorXd y = GetAD(yAD); - Eigen::VectorXd z = GetAD(zAD); - Eigen::VectorXd c_e = GetAD(m_equalityConstraints); - Eigen::VectorXd c_i = GetAD(m_inequalityConstraints); - - // Check for overconstrained problem - if (m_equalityConstraints.size() > m_decisionVariables.size()) { - if (m_config.diagnostics) { - fmt::print("The problem has too few degrees of freedom.\n"); - fmt::print("Violated constraints (cₑ(x) = 0) in order of declaration:\n"); - for (int row = 0; row < c_e.rows(); ++row) { - if (c_e(row) < 0.0) { - fmt::print(" {}/{}: {} = 0\n", row + 1, c_e.rows(), c_e(row)); - } - } - } - - status->exitCondition = SolverExitCondition::kTooFewDOFs; - return x; - } - - if (m_config.diagnostics) { - fmt::print("Error tolerance: {}\n\n", m_config.tolerance); - } - - std::chrono::system_clock::time_point iterationsStartTime; - - int iterations = 0; - - scope_exit exit{[&] { - if (m_config.diagnostics) { - auto solveEndTime = std::chrono::system_clock::now(); - - fmt::print("\nSolve time: {} ms\n", - ToMilliseconds(solveEndTime - solveStartTime)); - fmt::print(" ↳ {} ms (IPM setup)\n", - ToMilliseconds(iterationsStartTime - solveStartTime)); - if (iterations > 0) { - fmt::print( - " ↳ {} ms ({} IPM iterations; {} ms average)\n", - ToMilliseconds(solveEndTime - iterationsStartTime), iterations, - ToMilliseconds((solveEndTime - iterationsStartTime) / iterations)); - } - fmt::print("\n"); - - constexpr auto format = "{:>8} {:>10} {:>14} {:>6}\n"; - fmt::print(format, "autodiff", "setup (ms)", "avg solve (ms)", "solves"); - fmt::print("{:=^44}\n", ""); - fmt::print(format, "∇f(x)", gradientF.GetProfiler().SetupDuration(), - gradientF.GetProfiler().AverageSolveDuration(), - gradientF.GetProfiler().SolveMeasurements()); - fmt::print(format, "∇²ₓₓL", hessianL.GetProfiler().SetupDuration(), - hessianL.GetProfiler().AverageSolveDuration(), - hessianL.GetProfiler().SolveMeasurements()); - fmt::print(format, "∂cₑ/∂x", jacobianCe.GetProfiler().SetupDuration(), - jacobianCe.GetProfiler().AverageSolveDuration(), - jacobianCe.GetProfiler().SolveMeasurements()); - fmt::print(format, "∂cᵢ/∂x", jacobianCi.GetProfiler().SetupDuration(), - jacobianCi.GetProfiler().AverageSolveDuration(), - jacobianCi.GetProfiler().SolveMeasurements()); - fmt::print("\n"); - } - }}; - - // Barrier parameter minimum - double μ_min = m_config.tolerance / 10.0; - - // Barrier parameter μ - double μ = 0.1; - - // Fraction-to-the-boundary rule scale factor minimum - constexpr double τ_min = 0.99; - - // Fraction-to-the-boundary rule scale factor τ - double τ = τ_min; - - Filter filter; - - // This should be run when the error estimate is below a desired threshold for - // the current barrier parameter - auto UpdateBarrierParameterAndResetFilter = [&] { - // Barrier parameter linear decrease power in "κ_μ μ". Range of (0, 1). - constexpr double κ_μ = 0.2; - - // Barrier parameter superlinear decrease power in "μ^(θ_μ)". Range of (1, - // 2). - constexpr double θ_μ = 1.5; - - // Update the barrier parameter. - // - // μⱼ₊₁ = max(εₜₒₗ/10, min(κ_μ μⱼ, μⱼ^θ_μ)) - // - // See equation (7) of [2]. - μ = std::max(μ_min, std::min(κ_μ * μ, std::pow(μ, θ_μ))); - - // Update the fraction-to-the-boundary rule scaling factor. - // - // τⱼ = max(τₘᵢₙ, 1 − μⱼ) - // - // See equation (8) of [2]. - τ = std::max(τ_min, 1.0 - μ); - - // Reset the filter when the barrier parameter is updated - filter.Reset(); - }; - - // Kept outside the loop so its storage can be reused - SparseMatrixBuilder lhsBuilder( - m_decisionVariables.size() + m_equalityConstraints.size(), - m_decisionVariables.size() + m_equalityConstraints.size()); - - RegularizedLDLT solver; - - int acceptableIterCounter = 0; - constexpr int maxAcceptableIterations = 15; - const double acceptableTolerance = m_config.tolerance * 100; - - int fullStepRejectedCounter = 0; - int stepTooSmallCounter = 0; - - // Error estimate - double E_0 = std::numeric_limits::infinity(); - - iterationsStartTime = std::chrono::system_clock::now(); - - while (E_0 > m_config.tolerance && - acceptableIterCounter < maxAcceptableIterations) { - auto innerIterStartTime = std::chrono::system_clock::now(); - - // Check for local infeasibility - if (!IsLocallyFeasible(A_e, c_e, A_i, c_i)) { - status->exitCondition = SolverExitCondition::kLocallyInfeasible; - return x; - } - - if (m_config.spy) { - // Gap between sparsity patterns - if (iterations > 0) { - m_A_e_spy << "\n"; - m_A_i_spy << "\n"; - m_H_spy << "\n"; - } - - Spy(m_H_spy, H); - Spy(m_A_e_spy, A_e); - Spy(m_A_i_spy, A_i); - } - - // Call user callback - if (m_callback({iterations, x, g, H, A_e, A_i})) { - status->exitCondition = SolverExitCondition::kCallbackRequestedStop; - return x; - } - - // [s₁ 0 ⋯ 0 ] - // S = [0 ⋱ ⋮ ] - // [⋮ ⋱ 0 ] - // [0 ⋯ 0 sₘ] - Eigen::SparseMatrix S = SparseDiagonal(s); - - // [z₁ 0 ⋯ 0 ] - // Z = [0 ⋱ ⋮ ] - // [⋮ ⋱ 0 ] - // [0 ⋯ 0 zₘ] - Eigen::SparseMatrix Z = SparseDiagonal(z); - - // Σ = S⁻¹Z - Eigen::SparseMatrix Σ = S.cwiseInverse() * Z; - - // lhs = [H + AᵢᵀΣAᵢ Aₑᵀ] - // [ Aₑ 0 ] - lhsBuilder.Clear(); - // Assign top-left quadrant - lhsBuilder.Block(0, 0, H.rows(), H.cols()) = H + A_i.transpose() * Σ * A_i; - // Assign bottom-left quadrant - lhsBuilder.Block(H.rows(), 0, A_e.rows(), A_e.cols()) = A_e; - // Assign top-right quadrant - lhsBuilder.Block(0, H.rows(), A_e.cols(), A_e.rows()) = A_e.transpose(); - Eigen::SparseMatrix lhs = lhsBuilder.Build(); - - const Eigen::VectorXd e = Eigen::VectorXd::Ones(s.rows()); - - // rhs = −[∇f − Aₑᵀy + Aᵢᵀ(S⁻¹(Zcᵢ − μe) − z)] - // [ cₑ ] - Eigen::VectorXd rhs{x.rows() + y.rows()}; - rhs.segment(0, x.rows()) = - -(g - A_e.transpose() * y + - A_i.transpose() * (S.cwiseInverse() * (Z * c_i - μ * e) - z)); - rhs.segment(x.rows(), y.rows()) = -c_e; - - // Solve the Newton-KKT system - solver.Compute(lhs, m_equalityConstraints.size(), μ); - Eigen::VectorXd step{x.rows() + y.rows(), 1}; - if (solver.Info() == Eigen::Success) { - step = solver.Solve(rhs); - } else { - // The regularization procedure failed due to a rank-deficient equality - // constraint Jacobian with linearly dependent constraints. Set the step - // length to zero and let second-order corrections attempt to restore - // feasibility. - step.setZero(); - } - - // step = [ pₖˣ] - // [−pₖʸ] - Eigen::VectorXd p_x = step.segment(0, x.rows()); - Eigen::VectorXd p_y = -step.segment(x.rows(), y.rows()); - - // pₖᶻ = −Σcᵢ + μS⁻¹e − ΣAᵢpₖˣ - Eigen::VectorXd p_z = -Σ * c_i + μ * S.cwiseInverse() * e - Σ * A_i * p_x; - - // pₖˢ = μZ⁻¹e − s − Z⁻¹Spₖᶻ - Eigen::VectorXd p_s = - μ * Z.cwiseInverse() * e - s - Z.cwiseInverse() * S * p_z; - - bool stepAcceptable = false; - - // αᵐᵃˣ = max(α ∈ (0, 1] : sₖ + αpₖˢ ≥ (1−τⱼ)sₖ) - double α_max = FractionToTheBoundaryRule(s, p_s, τ); - double α = α_max; - - // αₖᶻ = max(α ∈ (0, 1] : zₖ + αpₖᶻ ≥ (1−τⱼ)zₖ) - double α_z = FractionToTheBoundaryRule(z, p_z, τ); - - while (!stepAcceptable) { - Eigen::VectorXd trial_x = x + α * p_x; - Eigen::VectorXd trial_s = s + α * p_s; - - Eigen::VectorXd trial_y = y + α_z * p_y; - Eigen::VectorXd trial_z = z + α_z * p_z; - - SetAD(xAD, trial_x); - - for (int row = 0; row < c_e.rows(); ++row) { - c_eAD(row).Update(); - } - Eigen::VectorXd trial_c_e = GetAD(m_equalityConstraints); - - for (int row = 0; row < c_i.rows(); ++row) { - c_iAD(row).Update(); - } - Eigen::VectorXd trial_c_i = GetAD(m_inequalityConstraints); - - m_f.value().Update(); - FilterEntry entry{m_f.value(), μ, trial_s, trial_c_e, trial_c_i}; - if (filter.IsAcceptable(entry)) { - stepAcceptable = true; - filter.Add(std::move(entry)); - continue; - } - - double prevConstraintViolation = c_e.lpNorm<1>() + (c_i - s).lpNorm<1>(); - double nextConstraintViolation = - trial_c_e.lpNorm<1>() + (trial_c_i - trial_s).lpNorm<1>(); - - // If first trial point was rejected and constraint violation stayed the - // same or went up, apply second-order corrections - if (nextConstraintViolation >= prevConstraintViolation) { - // Apply second-order corrections. See section 2.4 of [2]. - Eigen::VectorXd p_x_cor = p_x; - Eigen::VectorXd p_y_soc = p_y; - Eigen::VectorXd p_z_soc = p_z; - Eigen::VectorXd p_s_soc = p_s; - - double α_soc = α; - Eigen::VectorXd c_e_soc = c_e; - - for (int soc_iteration = 0; soc_iteration < 5 && !stepAcceptable; - ++soc_iteration) { - // Rebuild Newton-KKT rhs with updated constraint values. - // - // rhs = −[∇f − Aₑᵀy + Aᵢᵀ(S⁻¹(Zcᵢ − μe) − z)] - // [ cₑˢᵒᶜ ] - // - // where cₑˢᵒᶜ = αc(xₖ) + c(xₖ + αp_x) - c_e_soc = α_soc * c_e_soc + trial_c_e; - rhs.bottomRows(y.rows()) = -c_e_soc; - - // Solve the Newton-KKT system - step = solver.Solve(rhs); - - p_x_cor = step.segment(0, x.rows()); - p_y_soc = -step.segment(x.rows(), y.rows()); - - // pₖᶻ = −Σcᵢ + μS⁻¹e − ΣAᵢpₖˣ - p_z_soc = -Σ * c_i + μ * S.cwiseInverse() * e - Σ * A_i * p_x_cor; - - // pₖˢ = μZ⁻¹e − s − Z⁻¹Spₖᶻ - p_s_soc = - μ * Z.cwiseInverse() * e - s - Z.cwiseInverse() * S * p_z_soc; - - // αˢᵒᶜ = max(α ∈ (0, 1] : sₖ + αpₖˢ ≥ (1−τⱼ)sₖ) - α_soc = FractionToTheBoundaryRule(s, p_s_soc, τ); - trial_x = x + α_soc * p_x_cor; - trial_s = s + α_soc * p_s_soc; - - // αₖᶻ = max(α ∈ (0, 1] : zₖ + αpₖᶻ ≥ (1−τⱼ)zₖ) - double α_z_soc = FractionToTheBoundaryRule(z, p_z_soc, τ); - trial_y = y + α_z_soc * p_y_soc; - trial_z = z + α_z_soc * p_z_soc; - - SetAD(xAD, trial_x); - - for (int row = 0; row < c_e.rows(); ++row) { - c_eAD(row).Update(); - } - trial_c_e = GetAD(m_equalityConstraints); - - for (int row = 0; row < c_i.rows(); ++row) { - c_iAD(row).Update(); - } - trial_c_i = GetAD(m_inequalityConstraints); - - m_f.value().Update(); - entry = FilterEntry{m_f.value(), μ, trial_s, trial_c_e, trial_c_i}; - if (filter.IsAcceptable(entry)) { - p_x = p_x_cor; - p_y = p_y_soc; - p_z = p_z_soc; - p_s = p_s_soc; - α = α_soc; - α_z = α_z_soc; - stepAcceptable = true; - filter.Add(std::move(entry)); - } - } - } - - // Count number of times full step is rejected in a row - if (α == 1.0) { - if (!stepAcceptable) { - ++fullStepRejectedCounter; - } else { - fullStepRejectedCounter = 0; - } - } - - if (!stepAcceptable) { - // Reset filter if it's repeatedly impeding progress - // - // See section 3.2 case I of [2]. - if (fullStepRejectedCounter == 4 && - filter.maxConstraintViolation > entry.constraintViolation / 10.0) { - filter.maxConstraintViolation *= 0.1; - filter.Reset(); - continue; - } - - constexpr double α_red_factor = 0.5; - α *= α_red_factor; - - // Safety factor for the minimal step size - constexpr double α_min_frac = 0.05; - - if (α < α_min_frac * Filter::γConstraint) { - // If the step using αᵐᵃˣ reduced the KKT error, accept it anyway - - double currentKKTError = KKTError(g, A_e, c_e, A_i, c_i, s, y, z, μ); - - Eigen::VectorXd trial_x = x + α_max * p_x; - Eigen::VectorXd trial_s = s + α_max * p_s; - - Eigen::VectorXd trial_y = y + α_z * p_y; - Eigen::VectorXd trial_z = z + α_z * p_z; - - // Upate autodiff - SetAD(xAD, trial_x); - SetAD(sAD, trial_s); - SetAD(yAD, trial_y); - SetAD(zAD, trial_z); - - for (int row = 0; row < c_e.rows(); ++row) { - c_eAD(row).Update(); - } - Eigen::VectorXd trial_c_e = GetAD(m_equalityConstraints); - - for (int row = 0; row < c_i.rows(); ++row) { - c_iAD(row).Update(); - } - Eigen::VectorXd trial_c_i = GetAD(m_inequalityConstraints); - - double nextKKTError = KKTError( - gradientF.Calculate(), jacobianCe.Calculate(), trial_c_e, - jacobianCi.Calculate(), trial_c_i, trial_s, trial_y, trial_z, μ); - - if (nextKKTError <= 0.999 * currentKKTError) { - α = α_max; - stepAcceptable = true; - continue; - } - - // TODO: Feasibility restoration phase - status->exitCondition = SolverExitCondition::kBadSearchDirection; - return x; - } - } - } - - if (p_x.lpNorm() > 1e20 || - p_s.lpNorm() > 1e20) { - status->exitCondition = SolverExitCondition::kDivergingIterates; - return x; - } - - if (stepAcceptable) { - // Handle very small search directions by letting αₖ = αₖᵐᵃˣ when - // max(|pₖˣ(i)|/(1 + |xₖ(i)|)) < 10ε_mach. - // - // See section 3.9 of [2]. - double maxStepScaled = 0.0; - for (int row = 0; row < x.rows(); ++row) { - maxStepScaled = std::max(maxStepScaled, - std::abs(p_x(row)) / (1.0 + std::abs(x(row)))); - } - if (maxStepScaled < 10.0 * std::numeric_limits::epsilon()) { - α = α_max; - ++stepTooSmallCounter; - } else { - stepTooSmallCounter = 0; - } - - // xₖ₊₁ = xₖ + αₖpₖˣ - // sₖ₊₁ = xₖ + αₖpₖˢ - // yₖ₊₁ = xₖ + αₖᶻpₖʸ - // zₖ₊₁ = xₖ + αₖᶻpₖᶻ - x += α * p_x; - s += α * p_s; - y += α_z * p_y; - z += α_z * p_z; - - // A requirement for the convergence proof is that the "primal-dual - // barrier term Hessian" Σₖ does not deviate arbitrarily much from the - // "primal Hessian" μⱼSₖ⁻². We ensure this by resetting - // - // zₖ₊₁⁽ⁱ⁾ = max(min(zₖ₊₁⁽ⁱ⁾, κ_Σ μⱼ/sₖ₊₁⁽ⁱ⁾), μⱼ/(κ_Σ sₖ₊₁⁽ⁱ⁾)) - // - // for some fixed κ_Σ ≥ 1 after each step. See equation (16) of [2]. - { - // Barrier parameter scale factor for inequality constraint Lagrange - // multiplier safeguard - constexpr double κ_Σ = 1e10; - - for (int row = 0; row < z.rows(); ++row) { - z(row) = - std::max(std::min(z(row), κ_Σ * μ / s(row)), μ / (κ_Σ * s(row))); - } - } - } - - // Update autodiff for Jacobians and Hessian - SetAD(xAD, x); - SetAD(sAD, s); - SetAD(yAD, y); - SetAD(zAD, z); - - A_e = jacobianCe.Calculate(); - A_i = jacobianCi.Calculate(); - g = gradientF.Calculate(); - H = hessianL.Calculate(); - - // Update cₑ - for (int row = 0; row < c_e.rows(); ++row) { - c_eAD(row).Update(); - } - c_e = GetAD(m_equalityConstraints); - - // Update cᵢ - for (int row = 0; row < c_i.rows(); ++row) { - c_iAD(row).Update(); - } - c_i = GetAD(m_inequalityConstraints); - - // Update the error estimate - E_0 = ErrorEstimate(g, A_e, c_e, A_i, c_i, s, y, z, 0.0); - if (E_0 < acceptableTolerance) { - ++acceptableIterCounter; - } else { - acceptableIterCounter = 0; - } - - // Update the barrier parameter if necessary - if (E_0 > m_config.tolerance) { - // Barrier parameter scale factor for tolerance checks - constexpr double κ_ε = 10.0; - - // While the error estimate is below the desired threshold for this - // barrier parameter value, decrease the barrier parameter further - double E_μ = ErrorEstimate(g, A_e, c_e, A_i, c_i, s, y, z, μ); - while (μ > μ_min && E_μ <= κ_ε * μ) { - UpdateBarrierParameterAndResetFilter(); - E_μ = ErrorEstimate(g, A_e, c_e, A_i, c_i, s, y, z, μ); - } - } - - auto innerIterEndTime = std::chrono::system_clock::now(); - - if (m_config.diagnostics) { - if (iterations % 20 == 0) { - fmt::print("{:>4} {:>10} {:>10} {:>16} {:>19}\n", "iter", - "time (ms)", "error", "cost", "infeasibility"); - fmt::print("{:=^70}\n", ""); - } - - FilterEntry entry{m_f.value(), μ, s, c_e, c_i}; - fmt::print("{:>4} {:>9} {:>15e} {:>16e} {:>16e}\n", iterations, - ToMilliseconds(innerIterEndTime - innerIterStartTime), E_0, - entry.cost, entry.constraintViolation); - } - - ++iterations; - if (iterations >= m_config.maxIterations) { - status->exitCondition = SolverExitCondition::kMaxIterationsExceeded; - return x; - } - - if (innerIterEndTime - solveStartTime > m_config.timeout) { - status->exitCondition = SolverExitCondition::kMaxWallClockTimeExceeded; - return x; - } - - if (E_0 > m_config.tolerance && - acceptableIterCounter == maxAcceptableIterations) { - status->exitCondition = SolverExitCondition::kSolvedToAcceptableTolerance; - return x; - } - - // The search direction has been very small twice, so assume the problem has - // been solved as well as possible given finite precision and reduce the - // barrier parameter. - // - // See section 3.9 of [2]. - if (stepTooSmallCounter == 2) { - if (μ > μ_min) { - UpdateBarrierParameterAndResetFilter(); - continue; - } else { - status->exitCondition = - SolverExitCondition::kMaxSearchDirectionTooSmall; - return x; - } - } - } - - return x; -} - void OptimizationProblem::PrintExitCondition( const SolverExitCondition& exitCondition) { fmt::print("Exit condition: "); @@ -927,122 +242,3 @@ void OptimizationProblem::PrintExitCondition( } fmt::print("\n"); } - -bool OptimizationProblem::IsLocallyFeasible( - const Eigen::SparseMatrix& A_e, const Eigen::VectorXd& c_e, - const Eigen::SparseMatrix& A_i, const Eigen::VectorXd& c_i) const { - // Check for problem local infeasibility. The problem is locally infeasible if - // - // Aₑᵀcₑ → 0 - // Aᵢᵀcᵢ⁺ → 0 - // ‖(cₑ, cᵢ⁺)‖ > ε - // - // where cᵢ⁺ = min(cᵢ, 0). - // - // See "Infeasibility detection" in section 6 of [3]. - // - // cᵢ⁺ is used instead of cᵢ⁻ from the paper to follow the convention that - // feasible inequality constraints are ≥ 0. - - if (m_equalityConstraints.size() > 0 && - (A_e.transpose() * c_e).norm() < 1e-6 && c_e.norm() > 1e-2) { - if (m_config.diagnostics) { - fmt::print( - "The problem is locally infeasible due to violated equality " - "constraints.\n"); - fmt::print("Violated constraints (cₑ(x) = 0) in order of declaration:\n"); - for (int row = 0; row < c_e.rows(); ++row) { - if (c_e(row) < 0.0) { - fmt::print(" {}/{}: {} = 0\n", row + 1, c_e.rows(), c_e(row)); - } - } - } - - return false; - } - - if (m_inequalityConstraints.size() > 0) { - Eigen::VectorXd c_i_plus = c_i.cwiseMin(0.0); - if ((A_i.transpose() * c_i_plus).norm() < 1e-6 && c_i_plus.norm() > 1e-6) { - if (m_config.diagnostics) { - fmt::print( - "The problem is infeasible due to violated inequality " - "constraints.\n"); - fmt::print( - "Violated constraints (cᵢ(x) ≥ 0) in order of declaration:\n"); - for (int row = 0; row < c_i.rows(); ++row) { - if (c_i(row) < 0.0) { - fmt::print(" {}/{}: {} ≥ 0\n", row + 1, c_i.rows(), c_i(row)); - } - } - } - - return false; - } - } - - return true; -} - -double OptimizationProblem::ErrorEstimate( - const Eigen::VectorXd& g, const Eigen::SparseMatrix& A_e, - const Eigen::VectorXd& c_e, const Eigen::SparseMatrix& A_i, - const Eigen::VectorXd& c_i, const Eigen::VectorXd& s, - const Eigen::VectorXd& y, const Eigen::VectorXd& z, double μ) const { - // Update the error estimate using the KKT conditions from equations (19.5a) - // through (19.5d) of [1]. - // - // ∇f − Aₑᵀy − Aᵢᵀz = 0 - // Sz − μe = 0 - // cₑ = 0 - // cᵢ − s = 0 - // - // The error tolerance is the max of the following infinity norms scaled by - // s_d and s_c (see equation (5) of [2]). - // - // ‖∇f − Aₑᵀy − Aᵢᵀz‖_∞ / s_d - // ‖Sz − μe‖_∞ / s_c - // ‖cₑ‖_∞ - // ‖cᵢ − s‖_∞ - - // s_d = max(sₘₐₓ, (‖y‖₁ + ‖z‖₁) / (m + n)) / sₘₐₓ - constexpr double s_max = 100.0; - double s_d = std::max(s_max, (y.lpNorm<1>() + z.lpNorm<1>()) / - (m_equalityConstraints.size() + - m_inequalityConstraints.size())) / - s_max; - - // s_c = max(sₘₐₓ, ‖z‖₁ / n) / sₘₐₓ - double s_c = - std::max(s_max, z.lpNorm<1>() / m_inequalityConstraints.size()) / s_max; - - const Eigen::SparseMatrix S = SparseDiagonal(s); - const Eigen::VectorXd e = Eigen::VectorXd::Ones(s.rows()); - - return std::max({(g - A_e.transpose() * y - A_i.transpose() * z) - .lpNorm() / - s_d, - (S * z - μ * e).lpNorm() / s_c, - c_e.lpNorm(), - (c_i - s).lpNorm()}); -} - -double OptimizationProblem::KKTError( - const Eigen::VectorXd& g, const Eigen::SparseMatrix& A_e, - const Eigen::VectorXd& c_e, const Eigen::SparseMatrix& A_i, - const Eigen::VectorXd& c_i, const Eigen::VectorXd& s, - const Eigen::VectorXd& y, const Eigen::VectorXd& z, double μ) const { - // Compute the KKT error as the 1-norm of the KKT conditions from equations - // (19.5a) through (19.5d) of [1]. - // - // ∇f − Aₑᵀy − Aᵢᵀz = 0 - // Sz − μe = 0 - // cₑ = 0 - // cᵢ − s = 0 - - const Eigen::SparseMatrix S = SparseDiagonal(s); - const Eigen::VectorXd e = Eigen::VectorXd::Ones(s.rows()); - - return (g - A_e.transpose() * y - A_i.transpose() * z).lpNorm<1>() + - (S * z - μ * e).lpNorm<1>() + c_e.lpNorm<1>() + (c_i - s).lpNorm<1>(); -} diff --git a/src/optimization/solver/InteriorPoint.cpp b/src/optimization/solver/InteriorPoint.cpp new file mode 100644 index 000000000..45e0bea09 --- /dev/null +++ b/src/optimization/solver/InteriorPoint.cpp @@ -0,0 +1,878 @@ +// Copyright (c) Sleipnir contributors + +#include "InteriorPoint.hpp" + +#include +#include +#include +#include +#include + +#include + +#include "optimization/Filter.hpp" +#include "optimization/RegularizedLDLT.hpp" +#include "sleipnir/autodiff/Gradient.hpp" +#include "sleipnir/autodiff/Hessian.hpp" +#include "sleipnir/autodiff/Jacobian.hpp" +#include "sleipnir/optimization/SolverExitCondition.hpp" +#include "sleipnir/util/Spy.hpp" +#include "util/AutodiffUtil.hpp" +#include "util/ScopeExit.hpp" +#include "util/SparseMatrixBuilder.hpp" +#include "util/SparseUtil.hpp" + +namespace sleipnir { + +// Works cited: +// +// [1] Nocedal, J. and Wright, S. "Numerical Optimization", 2nd. ed., Ch. 19. +// Springer, 2006. +// [2] Wächter, A. and Biegler, L. "On the implementation of an interior-point +// filter line-search algorithm for large-scale nonlinear programming", +// 2005. http://cepac.cheme.cmu.edu/pasilectures/biegler/ipopt.pdf +// [3] Byrd, R. and Nocedal J. and Waltz R. "KNITRO: An Integrated Package for +// Nonlinear Optimization", 2005. +// https://users.iems.northwestern.edu/~nocedal/PDFfiles/integrated.pdf + +namespace { + +/** + * Returns true if the problem is locally feasible. + * + * @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the + * current iterate. + * @param c_e The problem's equality constraints cₑ(x) evaluated at the current + * iterate. + * @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at + * the current iterate. + * @param c_i The problem's inequality constraints cᵢ(x) evaluated at the + * current iterate. + * @param diagnostics Whether to enable diagnostics. + */ +bool IsLocallyFeasible(const Eigen::SparseMatrix& A_e, + const Eigen::VectorXd& c_e, + const Eigen::SparseMatrix& A_i, + const Eigen::VectorXd& c_i, bool diagnostics) { + // Check for problem local infeasibility. The problem is locally infeasible if + // + // Aₑᵀcₑ → 0 + // Aᵢᵀcᵢ⁺ → 0 + // ‖(cₑ, cᵢ⁺)‖ > ε + // + // where cᵢ⁺ = min(cᵢ, 0). + // + // See "Infeasibility detection" in section 6 of [3]. + // + // cᵢ⁺ is used instead of cᵢ⁻ from the paper to follow the convention that + // feasible inequality constraints are ≥ 0. + + if (A_e.rows() > 0 && (A_e.transpose() * c_e).norm() < 1e-6 && + c_e.norm() > 1e-2) { + if (diagnostics) { + fmt::print( + "The problem is locally infeasible due to violated equality " + "constraints.\n"); + fmt::print("Violated constraints (cₑ(x) = 0) in order of declaration:\n"); + for (int row = 0; row < c_e.rows(); ++row) { + if (c_e(row) < 0.0) { + fmt::print(" {}/{}: {} = 0\n", row + 1, c_e.rows(), c_e(row)); + } + } + } + + return false; + } + + if (A_i.rows() > 0) { + Eigen::VectorXd c_i_plus = c_i.cwiseMin(0.0); + if ((A_i.transpose() * c_i_plus).norm() < 1e-6 && c_i_plus.norm() > 1e-6) { + if (diagnostics) { + fmt::print( + "The problem is infeasible due to violated inequality " + "constraints.\n"); + fmt::print( + "Violated constraints (cᵢ(x) ≥ 0) in order of declaration:\n"); + for (int row = 0; row < c_i.rows(); ++row) { + if (c_i(row) < 0.0) { + fmt::print(" {}/{}: {} ≥ 0\n", row + 1, c_i.rows(), c_i(row)); + } + } + } + + return false; + } + } + + return true; +} + +/** + * Returns the error estimate using the KKT conditions for the interior-point + * method. + * + * @param g Gradient of the cost function ∇f. + * @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the + * current iterate. + * @param c_e The problem's equality constraints cₑ(x) evaluated at the current + * iterate. + * @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at + * the current iterate. + * @param c_i The problem's inequality constraints cᵢ(x) evaluated at the + * current iterate. + * @param s Inequality constraint slack variables. + * @param y Equality constraint dual variables. + * @param z Inequality constraint dual variables. + * @param μ Barrier parameter. + */ +double ErrorEstimate(const Eigen::VectorXd& g, + const Eigen::SparseMatrix& A_e, + const Eigen::VectorXd& c_e, + const Eigen::SparseMatrix& A_i, + const Eigen::VectorXd& c_i, const Eigen::VectorXd& s, + const Eigen::VectorXd& y, const Eigen::VectorXd& z, + double μ) { + int numEqualityConstraints = A_e.rows(); + int numInequalityConstraints = A_i.rows(); + + // Update the error estimate using the KKT conditions from equations (19.5a) + // through (19.5d) of [1]. + // + // ∇f − Aₑᵀy − Aᵢᵀz = 0 + // Sz − μe = 0 + // cₑ = 0 + // cᵢ − s = 0 + // + // The error tolerance is the max of the following infinity norms scaled by + // s_d and s_c (see equation (5) of [2]). + // + // ‖∇f − Aₑᵀy − Aᵢᵀz‖_∞ / s_d + // ‖Sz − μe‖_∞ / s_c + // ‖cₑ‖_∞ + // ‖cᵢ − s‖_∞ + + // s_d = max(sₘₐₓ, (‖y‖₁ + ‖z‖₁) / (m + n)) / sₘₐₓ + constexpr double s_max = 100.0; + double s_d = + std::max(s_max, (y.lpNorm<1>() + z.lpNorm<1>()) / + (numEqualityConstraints + numInequalityConstraints)) / + s_max; + + // s_c = max(sₘₐₓ, ‖z‖₁ / n) / sₘₐₓ + double s_c = + std::max(s_max, z.lpNorm<1>() / numInequalityConstraints) / s_max; + + const Eigen::SparseMatrix S = SparseDiagonal(s); + const Eigen::VectorXd e = Eigen::VectorXd::Ones(s.rows()); + + return std::max({(g - A_e.transpose() * y - A_i.transpose() * z) + .lpNorm() / + s_d, + (S * z - μ * e).lpNorm() / s_c, + c_e.lpNorm(), + (c_i - s).lpNorm()}); +} + +/** + * Returns the KKT error for the interior-point method. + * + * @param g Gradient of the cost function ∇f. + * @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the + * current iterate. + * @param c_e The problem's equality constraints cₑ(x) evaluated at the current + * iterate. + * @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at + * the current iterate. + * @param c_i The problem's inequality constraints cᵢ(x) evaluated at the + * current iterate. + * @param s Inequality constraint slack variables. + * @param y Equality constraint dual variables. + * @param z Inequality constraint dual variables. + * @param μ Barrier parameter. + */ +double KKTError(const Eigen::VectorXd& g, + const Eigen::SparseMatrix& A_e, + const Eigen::VectorXd& c_e, + const Eigen::SparseMatrix& A_i, + const Eigen::VectorXd& c_i, const Eigen::VectorXd& s, + const Eigen::VectorXd& y, const Eigen::VectorXd& z, double μ) { + // Compute the KKT error as the 1-norm of the KKT conditions from equations + // (19.5a) through (19.5d) of [1]. + // + // ∇f − Aₑᵀy − Aᵢᵀz = 0 + // Sz − μe = 0 + // cₑ = 0 + // cᵢ − s = 0 + + const Eigen::SparseMatrix S = SparseDiagonal(s); + const Eigen::VectorXd e = Eigen::VectorXd::Ones(s.rows()); + + return (g - A_e.transpose() * y - A_i.transpose() * z).lpNorm<1>() + + (S * z - μ * e).lpNorm<1>() + c_e.lpNorm<1>() + (c_i - s).lpNorm<1>(); +} + +/** + * Applies fraction-to-the-boundary rule to a variable and its iterate, then + * returns a fraction of the iterate step size within (0, 1]. + * + * @param x The variable. + * @param p The iterate on the variable. + * @param τ Fraction-to-the-boundary rule scaling factor within (0, 1]. + * @return Fraction of the iterate step size within (0, 1]. + */ +double FractionToTheBoundaryRule(const Eigen::Ref& x, + const Eigen::Ref& p, + double τ) { + // α = max(α ∈ (0, 1] : x + αp ≥ (1 − τ)x) + // + // where x and τ are positive. + // + // x + αp ≥ (1 − τ)x + // x + αp ≥ x − τx + // αp ≥ −τx + // + // If the inequality is false, p < 0 and α is too big. Find the largest value + // of α that makes the inequality true. + // + // α = −τ/p x + double α = 1.0; + for (int i = 0; i < x.rows(); ++i) { + if (α * p(i) < -τ * x(i)) { + α = -τ / p(i) * x(i); + } + } + + return α; +} + +/** + * Converts std::chrono::duration to a number of milliseconds rounded to three + * decimals. + */ +template > +double ToMilliseconds(const std::chrono::duration& duration) { + using std::chrono::duration_cast; + using std::chrono::microseconds; + return duration_cast(duration).count() / 1000.0; +} + +} // namespace + +Eigen::VectorXd InteriorPoint( + std::vector& decisionVariables, std::optional& f, + std::vector& equalityConstraints, + std::vector& inequalityConstraints, + const std::function& callback, + const SolverConfig& config, + const Eigen::Ref& initialGuess, + SolverStatus* status) { + // Read docs/algorithms.md#Interior-point_method for a derivation of the + // interior-point method formulation being used. + + auto solveStartTime = std::chrono::system_clock::now(); + + // Map decision variables and constraints to Eigen vectors for Lagrangian + MapVectorXvar xAD(decisionVariables.data(), decisionVariables.size()); + SetAD(xAD, initialGuess); + MapVectorXvar c_eAD(equalityConstraints.data(), equalityConstraints.size()); + MapVectorXvar c_iAD(inequalityConstraints.data(), + inequalityConstraints.size()); + + // Create autodiff variables for s, y, and z for Lagrangian + VectorXvar sAD = VectorXvar::Ones(inequalityConstraints.size()); + VectorXvar yAD = VectorXvar::Zero(equalityConstraints.size()); + VectorXvar zAD = VectorXvar::Ones(inequalityConstraints.size()); + + // Lagrangian L + // + // L(x, s, y, z)ₖ = f(x)ₖ − yₖᵀcₑ(x)ₖ − zₖᵀ(cᵢ(x)ₖ − sₖ) + Variable L = + f.value() - yAD.transpose() * c_eAD - zAD.transpose() * (c_iAD - sAD); + + // Equality constraint Jacobian Aₑ + // + // [∇ᵀcₑ₁(x)ₖ] + // Aₑ(x) = [∇ᵀcₑ₂(x)ₖ] + // [ ⋮ ] + // [∇ᵀcₑₘ(x)ₖ] + Jacobian jacobianCe{c_eAD, xAD}; + Eigen::SparseMatrix A_e = jacobianCe.Calculate(); + + // Inequality constraint Jacobian Aᵢ + // + // [∇ᵀcᵢ₁(x)ₖ] + // Aᵢ(x) = [∇ᵀcᵢ₂(x)ₖ] + // [ ⋮ ] + // [∇ᵀcᵢₘ(x)ₖ] + Jacobian jacobianCi{c_iAD, xAD}; + Eigen::SparseMatrix A_i = jacobianCi.Calculate(); + + // Gradient of f ∇f + Gradient gradientF{f.value(), xAD}; + Eigen::SparseVector g = gradientF.Calculate(); + + // Hessian of the Lagrangian H + // + // Hₖ = ∇²ₓₓL(x, s, y, z)ₖ + Hessian hessianL{L, xAD}; + Eigen::SparseMatrix H = hessianL.Calculate(); + + Eigen::VectorXd x = initialGuess; + Eigen::VectorXd s = GetAD(sAD); + Eigen::VectorXd y = GetAD(yAD); + Eigen::VectorXd z = GetAD(zAD); + Eigen::VectorXd c_e = GetAD(equalityConstraints); + Eigen::VectorXd c_i = GetAD(inequalityConstraints); + + // Check for overconstrained problem + if (equalityConstraints.size() > decisionVariables.size()) { + if (config.diagnostics) { + fmt::print("The problem has too few degrees of freedom.\n"); + fmt::print("Violated constraints (cₑ(x) = 0) in order of declaration:\n"); + for (int row = 0; row < c_e.rows(); ++row) { + if (c_e(row) < 0.0) { + fmt::print(" {}/{}: {} = 0\n", row + 1, c_e.rows(), c_e(row)); + } + } + } + + status->exitCondition = SolverExitCondition::kTooFewDOFs; + return x; + } + + // Sparsity pattern files written when spy flag is set in SolverConfig + std::ofstream H_spy; + std::ofstream A_e_spy; + std::ofstream A_i_spy; + if (config.spy) { + A_e_spy.open("A_e.spy"); + A_i_spy.open("A_i.spy"); + H_spy.open("H.spy"); + } + + if (config.diagnostics) { + fmt::print("Error tolerance: {}\n\n", config.tolerance); + } + + std::chrono::system_clock::time_point iterationsStartTime; + + int iterations = 0; + + scope_exit exit{[&] { + if (config.diagnostics) { + auto solveEndTime = std::chrono::system_clock::now(); + + fmt::print("\nSolve time: {} ms\n", + ToMilliseconds(solveEndTime - solveStartTime)); + fmt::print(" ↳ {} ms (IPM setup)\n", + ToMilliseconds(iterationsStartTime - solveStartTime)); + if (iterations > 0) { + fmt::print( + " ↳ {} ms ({} IPM iterations; {} ms average)\n", + ToMilliseconds(solveEndTime - iterationsStartTime), iterations, + ToMilliseconds((solveEndTime - iterationsStartTime) / iterations)); + } + fmt::print("\n"); + + constexpr auto format = "{:>8} {:>10} {:>14} {:>6}\n"; + fmt::print(format, "autodiff", "setup (ms)", "avg solve (ms)", "solves"); + fmt::print("{:=^44}\n", ""); + fmt::print(format, "∇f(x)", gradientF.GetProfiler().SetupDuration(), + gradientF.GetProfiler().AverageSolveDuration(), + gradientF.GetProfiler().SolveMeasurements()); + fmt::print(format, "∇²ₓₓL", hessianL.GetProfiler().SetupDuration(), + hessianL.GetProfiler().AverageSolveDuration(), + hessianL.GetProfiler().SolveMeasurements()); + fmt::print(format, "∂cₑ/∂x", jacobianCe.GetProfiler().SetupDuration(), + jacobianCe.GetProfiler().AverageSolveDuration(), + jacobianCe.GetProfiler().SolveMeasurements()); + fmt::print(format, "∂cᵢ/∂x", jacobianCi.GetProfiler().SetupDuration(), + jacobianCi.GetProfiler().AverageSolveDuration(), + jacobianCi.GetProfiler().SolveMeasurements()); + fmt::print("\n"); + } + }}; + + // Barrier parameter minimum + double μ_min = config.tolerance / 10.0; + + // Barrier parameter μ + double μ = 0.1; + + // Fraction-to-the-boundary rule scale factor minimum + constexpr double τ_min = 0.99; + + // Fraction-to-the-boundary rule scale factor τ + double τ = τ_min; + + Filter filter; + + // This should be run when the error estimate is below a desired threshold for + // the current barrier parameter + auto UpdateBarrierParameterAndResetFilter = [&] { + // Barrier parameter linear decrease power in "κ_μ μ". Range of (0, 1). + constexpr double κ_μ = 0.2; + + // Barrier parameter superlinear decrease power in "μ^(θ_μ)". Range of (1, + // 2). + constexpr double θ_μ = 1.5; + + // Update the barrier parameter. + // + // μⱼ₊₁ = max(εₜₒₗ/10, min(κ_μ μⱼ, μⱼ^θ_μ)) + // + // See equation (7) of [2]. + μ = std::max(μ_min, std::min(κ_μ * μ, std::pow(μ, θ_μ))); + + // Update the fraction-to-the-boundary rule scaling factor. + // + // τⱼ = max(τₘᵢₙ, 1 − μⱼ) + // + // See equation (8) of [2]. + τ = std::max(τ_min, 1.0 - μ); + + // Reset the filter when the barrier parameter is updated + filter.Reset(); + }; + + // Kept outside the loop so its storage can be reused + SparseMatrixBuilder lhsBuilder( + decisionVariables.size() + equalityConstraints.size(), + decisionVariables.size() + equalityConstraints.size()); + + RegularizedLDLT solver; + + int acceptableIterCounter = 0; + constexpr int maxAcceptableIterations = 15; + const double acceptableTolerance = config.tolerance * 100; + + int fullStepRejectedCounter = 0; + int stepTooSmallCounter = 0; + + // Error estimate + double E_0 = std::numeric_limits::infinity(); + + iterationsStartTime = std::chrono::system_clock::now(); + + while (E_0 > config.tolerance && + acceptableIterCounter < maxAcceptableIterations) { + auto innerIterStartTime = std::chrono::system_clock::now(); + + // Check for local infeasibility + if (!IsLocallyFeasible(A_e, c_e, A_i, c_i, config.diagnostics)) { + status->exitCondition = SolverExitCondition::kLocallyInfeasible; + return x; + } + + if (config.spy) { + // Gap between sparsity patterns + if (iterations > 0) { + A_e_spy << "\n"; + A_i_spy << "\n"; + H_spy << "\n"; + } + + Spy(H_spy, H); + Spy(A_e_spy, A_e); + Spy(A_i_spy, A_i); + } + + // Call user callback + if (callback({iterations, x, g, H, A_e, A_i})) { + status->exitCondition = SolverExitCondition::kCallbackRequestedStop; + return x; + } + + // [s₁ 0 ⋯ 0 ] + // S = [0 ⋱ ⋮ ] + // [⋮ ⋱ 0 ] + // [0 ⋯ 0 sₘ] + Eigen::SparseMatrix S = SparseDiagonal(s); + + // [z₁ 0 ⋯ 0 ] + // Z = [0 ⋱ ⋮ ] + // [⋮ ⋱ 0 ] + // [0 ⋯ 0 zₘ] + Eigen::SparseMatrix Z = SparseDiagonal(z); + + // Σ = S⁻¹Z + Eigen::SparseMatrix Σ = S.cwiseInverse() * Z; + + // lhs = [H + AᵢᵀΣAᵢ Aₑᵀ] + // [ Aₑ 0 ] + lhsBuilder.Clear(); + // Assign top-left quadrant + lhsBuilder.Block(0, 0, H.rows(), H.cols()) = H + A_i.transpose() * Σ * A_i; + // Assign bottom-left quadrant + lhsBuilder.Block(H.rows(), 0, A_e.rows(), A_e.cols()) = A_e; + // Assign top-right quadrant + lhsBuilder.Block(0, H.rows(), A_e.cols(), A_e.rows()) = A_e.transpose(); + Eigen::SparseMatrix lhs = lhsBuilder.Build(); + + const Eigen::VectorXd e = Eigen::VectorXd::Ones(s.rows()); + + // rhs = −[∇f − Aₑᵀy + Aᵢᵀ(S⁻¹(Zcᵢ − μe) − z)] + // [ cₑ ] + Eigen::VectorXd rhs{x.rows() + y.rows()}; + rhs.segment(0, x.rows()) = + -(g - A_e.transpose() * y + + A_i.transpose() * (S.cwiseInverse() * (Z * c_i - μ * e) - z)); + rhs.segment(x.rows(), y.rows()) = -c_e; + + // Solve the Newton-KKT system + solver.Compute(lhs, equalityConstraints.size(), μ); + Eigen::VectorXd step{x.rows() + y.rows(), 1}; + if (solver.Info() == Eigen::Success) { + step = solver.Solve(rhs); + } else { + // The regularization procedure failed due to a rank-deficient equality + // constraint Jacobian with linearly dependent constraints. Set the step + // length to zero and let second-order corrections attempt to restore + // feasibility. + step.setZero(); + } + + // step = [ pₖˣ] + // [−pₖʸ] + Eigen::VectorXd p_x = step.segment(0, x.rows()); + Eigen::VectorXd p_y = -step.segment(x.rows(), y.rows()); + + // pₖᶻ = −Σcᵢ + μS⁻¹e − ΣAᵢpₖˣ + Eigen::VectorXd p_z = -Σ * c_i + μ * S.cwiseInverse() * e - Σ * A_i * p_x; + + // pₖˢ = μZ⁻¹e − s − Z⁻¹Spₖᶻ + Eigen::VectorXd p_s = + μ * Z.cwiseInverse() * e - s - Z.cwiseInverse() * S * p_z; + + bool stepAcceptable = false; + + // αᵐᵃˣ = max(α ∈ (0, 1] : sₖ + αpₖˢ ≥ (1−τⱼ)sₖ) + double α_max = FractionToTheBoundaryRule(s, p_s, τ); + double α = α_max; + + // αₖᶻ = max(α ∈ (0, 1] : zₖ + αpₖᶻ ≥ (1−τⱼ)zₖ) + double α_z = FractionToTheBoundaryRule(z, p_z, τ); + + while (!stepAcceptable) { + Eigen::VectorXd trial_x = x + α * p_x; + Eigen::VectorXd trial_s = s + α * p_s; + + Eigen::VectorXd trial_y = y + α_z * p_y; + Eigen::VectorXd trial_z = z + α_z * p_z; + + SetAD(xAD, trial_x); + + for (int row = 0; row < c_e.rows(); ++row) { + c_eAD(row).Update(); + } + Eigen::VectorXd trial_c_e = GetAD(equalityConstraints); + + for (int row = 0; row < c_i.rows(); ++row) { + c_iAD(row).Update(); + } + Eigen::VectorXd trial_c_i = GetAD(inequalityConstraints); + + f.value().Update(); + FilterEntry entry{f.value(), μ, trial_s, trial_c_e, trial_c_i}; + if (filter.IsAcceptable(entry)) { + stepAcceptable = true; + filter.Add(std::move(entry)); + continue; + } + + double prevConstraintViolation = c_e.lpNorm<1>() + (c_i - s).lpNorm<1>(); + double nextConstraintViolation = + trial_c_e.lpNorm<1>() + (trial_c_i - trial_s).lpNorm<1>(); + + // If first trial point was rejected and constraint violation stayed the + // same or went up, apply second-order corrections + if (nextConstraintViolation >= prevConstraintViolation) { + // Apply second-order corrections. See section 2.4 of [2]. + Eigen::VectorXd p_x_cor = p_x; + Eigen::VectorXd p_y_soc = p_y; + Eigen::VectorXd p_z_soc = p_z; + Eigen::VectorXd p_s_soc = p_s; + + double α_soc = α; + Eigen::VectorXd c_e_soc = c_e; + + for (int soc_iteration = 0; soc_iteration < 5 && !stepAcceptable; + ++soc_iteration) { + // Rebuild Newton-KKT rhs with updated constraint values. + // + // rhs = −[∇f − Aₑᵀy + Aᵢᵀ(S⁻¹(Zcᵢ − μe) − z)] + // [ cₑˢᵒᶜ ] + // + // where cₑˢᵒᶜ = αc(xₖ) + c(xₖ + αp_x) + c_e_soc = α_soc * c_e_soc + trial_c_e; + rhs.bottomRows(y.rows()) = -c_e_soc; + + // Solve the Newton-KKT system + step = solver.Solve(rhs); + + p_x_cor = step.segment(0, x.rows()); + p_y_soc = -step.segment(x.rows(), y.rows()); + + // pₖᶻ = −Σcᵢ + μS⁻¹e − ΣAᵢpₖˣ + p_z_soc = -Σ * c_i + μ * S.cwiseInverse() * e - Σ * A_i * p_x_cor; + + // pₖˢ = μZ⁻¹e − s − Z⁻¹Spₖᶻ + p_s_soc = + μ * Z.cwiseInverse() * e - s - Z.cwiseInverse() * S * p_z_soc; + + // αˢᵒᶜ = max(α ∈ (0, 1] : sₖ + αpₖˢ ≥ (1−τⱼ)sₖ) + α_soc = FractionToTheBoundaryRule(s, p_s_soc, τ); + trial_x = x + α_soc * p_x_cor; + trial_s = s + α_soc * p_s_soc; + + // αₖᶻ = max(α ∈ (0, 1] : zₖ + αpₖᶻ ≥ (1−τⱼ)zₖ) + double α_z_soc = FractionToTheBoundaryRule(z, p_z_soc, τ); + trial_y = y + α_z_soc * p_y_soc; + trial_z = z + α_z_soc * p_z_soc; + + SetAD(xAD, trial_x); + + for (int row = 0; row < c_e.rows(); ++row) { + c_eAD(row).Update(); + } + trial_c_e = GetAD(equalityConstraints); + + for (int row = 0; row < c_i.rows(); ++row) { + c_iAD(row).Update(); + } + trial_c_i = GetAD(inequalityConstraints); + + f.value().Update(); + entry = FilterEntry{f.value(), μ, trial_s, trial_c_e, trial_c_i}; + if (filter.IsAcceptable(entry)) { + p_x = p_x_cor; + p_y = p_y_soc; + p_z = p_z_soc; + p_s = p_s_soc; + α = α_soc; + α_z = α_z_soc; + stepAcceptable = true; + filter.Add(std::move(entry)); + } + } + } + + // Count number of times full step is rejected in a row + if (α == 1.0) { + if (!stepAcceptable) { + ++fullStepRejectedCounter; + } else { + fullStepRejectedCounter = 0; + } + } + + if (!stepAcceptable) { + // Reset filter if it's repeatedly impeding progress + // + // See section 3.2 case I of [2]. + if (fullStepRejectedCounter == 4 && + filter.maxConstraintViolation > entry.constraintViolation / 10.0) { + filter.maxConstraintViolation *= 0.1; + filter.Reset(); + continue; + } + + constexpr double α_red_factor = 0.5; + α *= α_red_factor; + + // Safety factor for the minimal step size + constexpr double α_min_frac = 0.05; + + if (α < α_min_frac * Filter::γConstraint) { + // If the step using αᵐᵃˣ reduced the KKT error, accept it anyway + + double currentKKTError = KKTError(g, A_e, c_e, A_i, c_i, s, y, z, μ); + + Eigen::VectorXd trial_x = x + α_max * p_x; + Eigen::VectorXd trial_s = s + α_max * p_s; + + Eigen::VectorXd trial_y = y + α_z * p_y; + Eigen::VectorXd trial_z = z + α_z * p_z; + + // Upate autodiff + SetAD(xAD, trial_x); + SetAD(sAD, trial_s); + SetAD(yAD, trial_y); + SetAD(zAD, trial_z); + + for (int row = 0; row < c_e.rows(); ++row) { + c_eAD(row).Update(); + } + Eigen::VectorXd trial_c_e = GetAD(equalityConstraints); + + for (int row = 0; row < c_i.rows(); ++row) { + c_iAD(row).Update(); + } + Eigen::VectorXd trial_c_i = GetAD(inequalityConstraints); + + double nextKKTError = KKTError( + gradientF.Calculate(), jacobianCe.Calculate(), trial_c_e, + jacobianCi.Calculate(), trial_c_i, trial_s, trial_y, trial_z, μ); + + if (nextKKTError <= 0.999 * currentKKTError) { + α = α_max; + stepAcceptable = true; + continue; + } + + // TODO: Feasibility restoration phase + status->exitCondition = SolverExitCondition::kBadSearchDirection; + return x; + } + } + } + + if (p_x.lpNorm() > 1e20 || + p_s.lpNorm() > 1e20) { + status->exitCondition = SolverExitCondition::kDivergingIterates; + return x; + } + + if (stepAcceptable) { + // Handle very small search directions by letting αₖ = αₖᵐᵃˣ when + // max(|pₖˣ(i)|/(1 + |xₖ(i)|)) < 10ε_mach. + // + // See section 3.9 of [2]. + double maxStepScaled = 0.0; + for (int row = 0; row < x.rows(); ++row) { + maxStepScaled = std::max(maxStepScaled, + std::abs(p_x(row)) / (1.0 + std::abs(x(row)))); + } + if (maxStepScaled < 10.0 * std::numeric_limits::epsilon()) { + α = α_max; + ++stepTooSmallCounter; + } else { + stepTooSmallCounter = 0; + } + + // xₖ₊₁ = xₖ + αₖpₖˣ + // sₖ₊₁ = xₖ + αₖpₖˢ + // yₖ₊₁ = xₖ + αₖᶻpₖʸ + // zₖ₊₁ = xₖ + αₖᶻpₖᶻ + x += α * p_x; + s += α * p_s; + y += α_z * p_y; + z += α_z * p_z; + + // A requirement for the convergence proof is that the "primal-dual + // barrier term Hessian" Σₖ does not deviate arbitrarily much from the + // "primal Hessian" μⱼSₖ⁻². We ensure this by resetting + // + // zₖ₊₁⁽ⁱ⁾ = max(min(zₖ₊₁⁽ⁱ⁾, κ_Σ μⱼ/sₖ₊₁⁽ⁱ⁾), μⱼ/(κ_Σ sₖ₊₁⁽ⁱ⁾)) + // + // for some fixed κ_Σ ≥ 1 after each step. See equation (16) of [2]. + { + // Barrier parameter scale factor for inequality constraint Lagrange + // multiplier safeguard + constexpr double κ_Σ = 1e10; + + for (int row = 0; row < z.rows(); ++row) { + z(row) = + std::max(std::min(z(row), κ_Σ * μ / s(row)), μ / (κ_Σ * s(row))); + } + } + } + + // Update autodiff for Jacobians and Hessian + SetAD(xAD, x); + SetAD(sAD, s); + SetAD(yAD, y); + SetAD(zAD, z); + + A_e = jacobianCe.Calculate(); + A_i = jacobianCi.Calculate(); + g = gradientF.Calculate(); + H = hessianL.Calculate(); + + // Update cₑ + for (int row = 0; row < c_e.rows(); ++row) { + c_eAD(row).Update(); + } + c_e = GetAD(equalityConstraints); + + // Update cᵢ + for (int row = 0; row < c_i.rows(); ++row) { + c_iAD(row).Update(); + } + c_i = GetAD(inequalityConstraints); + + // Update the error estimate + E_0 = ErrorEstimate(g, A_e, c_e, A_i, c_i, s, y, z, 0.0); + if (E_0 < acceptableTolerance) { + ++acceptableIterCounter; + } else { + acceptableIterCounter = 0; + } + + // Update the barrier parameter if necessary + if (E_0 > config.tolerance) { + // Barrier parameter scale factor for tolerance checks + constexpr double κ_ε = 10.0; + + // While the error estimate is below the desired threshold for this + // barrier parameter value, decrease the barrier parameter further + double E_μ = ErrorEstimate(g, A_e, c_e, A_i, c_i, s, y, z, μ); + while (μ > μ_min && E_μ <= κ_ε * μ) { + UpdateBarrierParameterAndResetFilter(); + E_μ = ErrorEstimate(g, A_e, c_e, A_i, c_i, s, y, z, μ); + } + } + + auto innerIterEndTime = std::chrono::system_clock::now(); + + if (config.diagnostics) { + if (iterations % 20 == 0) { + fmt::print("{:>4} {:>10} {:>10} {:>16} {:>19}\n", "iter", + "time (ms)", "error", "cost", "infeasibility"); + fmt::print("{:=^70}\n", ""); + } + + FilterEntry entry{f.value(), μ, s, c_e, c_i}; + fmt::print("{:>4} {:>9} {:>15e} {:>16e} {:>16e}\n", iterations, + ToMilliseconds(innerIterEndTime - innerIterStartTime), E_0, + entry.cost, entry.constraintViolation); + } + + ++iterations; + if (iterations >= config.maxIterations) { + status->exitCondition = SolverExitCondition::kMaxIterationsExceeded; + return x; + } + + if (innerIterEndTime - solveStartTime > config.timeout) { + status->exitCondition = SolverExitCondition::kMaxWallClockTimeExceeded; + return x; + } + + if (E_0 > config.tolerance && + acceptableIterCounter == maxAcceptableIterations) { + status->exitCondition = SolverExitCondition::kSolvedToAcceptableTolerance; + return x; + } + + // The search direction has been very small twice, so assume the problem has + // been solved as well as possible given finite precision and reduce the + // barrier parameter. + // + // See section 3.9 of [2]. + if (stepTooSmallCounter == 2) { + if (μ > μ_min) { + UpdateBarrierParameterAndResetFilter(); + continue; + } else { + status->exitCondition = + SolverExitCondition::kMaxSearchDirectionTooSmall; + return x; + } + } + } + + return x; +} + +} // namespace sleipnir diff --git a/src/optimization/solver/InteriorPoint.hpp b/src/optimization/solver/InteriorPoint.hpp new file mode 100644 index 000000000..d997e058b --- /dev/null +++ b/src/optimization/solver/InteriorPoint.hpp @@ -0,0 +1,52 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include + +#include + +#include "sleipnir/autodiff/Variable.hpp" +#include "sleipnir/optimization/Constraints.hpp" +#include "sleipnir/optimization/SolverConfig.hpp" +#include "sleipnir/optimization/SolverIterationInfo.hpp" +#include "sleipnir/optimization/SolverStatus.hpp" + +namespace sleipnir { + +/** +Find the optimal solution to the nonlinear program using an interior-point +solver. + +A nonlinear program has the form: + +@verbatim + min_x f(x) +subject to cₑ(x) = 0 + cᵢ(x) ≥ 0 +@endverbatim + +where f(x) is the cost function, cₑ(x) are the equality constraints, and cᵢ(x) +are the inequality constraints. + +@param[in] decisionVariables The list of decision variables. +@param[in] f The cost function. +@param[in] equalityConstraints The list of equality constraints. +@param[in] inequalityConstraints The list of inequality constraints. +@param[in] callback The user callback. +@param[in] config Configuration options for the solver. +@param[in] initialGuess The initial guess. +@param[out] status The solver status. +@return The optimal state. +*/ +Eigen::VectorXd InteriorPoint( + std::vector& decisionVariables, std::optional& f, + std::vector& equalityConstraints, + std::vector& inequalityConstraints, + const std::function& callback, + const SolverConfig& config, + const Eigen::Ref& initialGuess, + SolverStatus* status); + +} // namespace sleipnir