feasibility_restoration.hpp

// Copyright (c) Sleipnir contributors
 
#pragma once
 
#include <algorithm>
#include <cmath>
#include <functional>
#include <span>
#include <tuple>
#include <utility>
 
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <gch/small_vector.hpp>
 
#include "sleipnir/optimization/solver/exit_status.hpp"
#include "sleipnir/optimization/solver/ipm_matrix_callbacks.hpp"
#include "sleipnir/optimization/solver/iteration_info.hpp"
#include "sleipnir/optimization/solver/options.hpp"
#include "sleipnir/optimization/solver/sqp_matrix_callbacks.hpp"
#include "sleipnir/optimization/solver/util/append_as_triplets.hpp"
#include "sleipnir/optimization/solver/util/problem_scaling.hpp"
#include "sleipnir/util/print_diagnostics.hpp"
 
namespace slp {
 
template <typename Scalar>
ExitStatus ipm(const IPMMatrixCallbacks<Scalar>& matrix_callbacks,
               std::span<std::function<bool(const IterationInfo<Scalar>& info)>>
                   iteration_callbacks,
               const Options<Scalar>& options, bool in_feasibility_restoration,
#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
               const Eigen::ArrayX<bool>& bound_constraint_mask,
#endif
               Eigen::VectorX<Scalar>& x, Eigen::VectorX<Scalar>& s,
               Eigen::VectorX<Scalar>& y, Eigen::VectorX<Scalar>& z, Scalar& μ,
               int& iterations);
 
template <typename Scalar>
std::tuple<Eigen::VectorX<Scalar>, Eigen::VectorX<Scalar>> compute_p_n(
    const Eigen::VectorX<Scalar>& c, Scalar ρ, Scalar μ) {
  // From equation (33) of [2]:
  //                       ______________________
  //       μ − ρ c(x)     /(μ − ρ c(x))²   μ c(x)
  //   n = −−−−−−−−−− +  / (−−−−−−−−−−)  + −−−−−−     (1)
  //           2ρ       √  (    2ρ    )      2ρ
  //
  // The quadratic formula:
  //             ________
  //       -b + √b² - 4ac
  //   x = −−−−−−−−−−−−−−                             (2)
  //             2a
  //
  // Rearrange (1) to fit the quadratic formula better:
  //                     _________________________
  //       μ - ρ c(x) + √(μ - ρ c(x))² + 2ρ μ c(x)
  //   n = −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
  //                         2ρ
  //
  // Solve for coefficients:
  //
  //   a = ρ                                          (3)
  //   b = ρ c(x) - μ                                 (4)
  //
  //   -4ac = 2ρ μ c(x)
  //   -4(ρ)c = 2ρ μ c(x)
  //   -4c = 2μ c(x)
  //   c = -μ c(x)/2                                  (5)
  //
  //   p = c(x) + n                                   (6)
 
  using DenseVector = Eigen::VectorX<Scalar>;
 
  using std::sqrt;
 
  DenseVector p{c.rows()};
  DenseVector n{c.rows()};
  for (int row = 0; row < p.rows(); ++row) {
    Scalar _a = ρ;
    Scalar _b = ρ * c[row] - μ;
    Scalar _c = -μ * c[row] / Scalar(2);
 
    n[row] = (-_b + sqrt(_b * _b - Scalar(4) * _a * _c)) / (Scalar(2) * _a);
    p[row] = c[row] + n[row];
  }
 
  return {std::move(p), std::move(n)};
}
 
template <typename Scalar>
ExitStatus feasibility_restoration(
    const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
    std::span<std::function<bool(const IterationInfo<Scalar>& info)>>
        iteration_callbacks,
    const Options<Scalar>& options, Eigen::VectorX<Scalar>& x,
    Eigen::VectorX<Scalar>& y, int& iterations) {
  // Feasibility restoration
  //
  //        min  ρ Σ (pₑ + nₑ) + ζ/2 (x - xᵣ)ᵀDᵣ(x - xᵣ)
  //         x
  //       pₑ,nₑ
  //
  //   s.t. cₑ(x) - pₑ + nₑ = 0
  //        pₑ ≥ 0
  //        nₑ ≥ 0
  //
  // where ρ = 1000, ζ = √μ where μ is the barrier parameter, xᵣ is original
  // iterate before feasibility restoration, and Dᵣ is a scaling matrix defined
  // by
  //
  //   Dᵣ = diag(min(1, 1/xᵣ[i]²) for i in x.rows())
 
  using DenseVector = Eigen::VectorX<Scalar>;
  using DiagonalMatrix = Eigen::DiagonalMatrix<Scalar, Eigen::Dynamic>;
  using SparseMatrix = Eigen::SparseMatrix<Scalar>;
  using SparseVector = Eigen::SparseVector<Scalar>;
 
  using std::sqrt;
 
  const auto& matrices = matrix_callbacks;
  const auto& num_vars = matrices.num_decision_variables;
  const auto& num_eq = matrices.num_equality_constraints;
 
  constexpr Scalar ρ(1e3);
  const Scalar μ = options.tolerance / Scalar(10);
 
  const DenseVector c_e = matrices.c_e(x);
 
  Scalar fr_μ = std::max(μ, c_e.template lpNorm<Eigen::Infinity>());
  const Scalar ζ = sqrt(fr_μ);
 
  const auto& x_r = x;
  const auto [p_e_0, n_e_0] = compute_p_n(c_e, ρ, fr_μ);
 
  // Dᵣ = diag(min(1, 1/xᵣ[i]²) for i in x.rows())
  const DiagonalMatrix D_r =
      x.cwiseSquare().cwiseInverse().cwiseMin(Scalar(1)).asDiagonal();
 
  DenseVector fr_x{num_vars + 2 * num_eq};
  fr_x << x, p_e_0, n_e_0;
 
  // Start the constraints pₑ, nₑ ≥ 0 with zero violation
  DenseVector fr_s{2 * num_eq};
  fr_s << p_e_0, n_e_0;
 
  DenseVector fr_y = DenseVector::Zero(num_eq);
 
  // Start the duals with perfect complementarity with respect to the slacks
  DenseVector fr_z{2 * num_eq};
  fr_z << fr_μ * p_e_0.cwiseInverse(), fr_μ * n_e_0.cwiseInverse();
 
  // Inherit the parent problem's scaling for the constraints, and use no
  // scaling for the cost function since it has changed. The new rows introduced
  // are not scaled.
  const ProblemScaling<Scalar> fr_scaling{Scalar(1), matrices.scaling.c_e,
                                          DenseVector::Ones(2 * num_eq)};
 
  IPMMatrixCallbacks<Scalar> fr_matrix_callbacks{
      static_cast<int>(fr_x.rows()),
      static_cast<int>(fr_y.rows()),
      static_cast<int>(fr_z.rows()),
      [&](const DenseVector& x_p) -> Scalar {
        auto x = x_p.segment(0, num_vars);
 
        // Cost function
        //
        //   ρ Σ (pₑ + nₑ) + ζ/2 (x - xᵣ)ᵀDᵣ(x - xᵣ)
 
        auto diff = x - x_r;
        return ρ * x_p.segment(num_vars, 2 * num_eq).array().sum() +
               ζ / Scalar(2) * diff.transpose() * D_r * diff;
      },
      [&](const DenseVector& x_p) -> SparseVector {
        auto x = x_p.segment(0, num_vars);
 
        // Cost function gradient
        //
        //   [ζDᵣ(x − xᵣ)]
        //   [     ρ     ]
        //   [     ρ     ]
        DenseVector g{x_p.rows()};
        g.segment(0, num_vars) = ζ * D_r * (x - x_r);
        g.segment(num_vars, 2 * num_eq).setConstant(ρ);
        return g.sparseView();
      },
      [&](const DenseVector& x_p, const DenseVector& y_p,
          [[maybe_unused]] const DenseVector& z_p) -> SparseMatrix {
        auto x = x_p.segment(0, num_vars);
        const auto& y = y_p;
 
        // Cost function Hessian
        //
        //   [ζDᵣ  0  0]
        //   [ 0   0  0]
        //   [ 0   0  0]
        gch::small_vector<Eigen::Triplet<Scalar>> triplets;
        triplets.reserve(x_p.rows());
        append_as_triplets(triplets, 0, 0, {SparseMatrix{ζ * D_r}});
        SparseMatrix d2f_dx2{x_p.rows(), x_p.rows()};
        d2f_dx2.setFromSortedTriplets(triplets.begin(), triplets.end());
 
        // Constraint part of original problem's Lagrangian Hessian
        //
        //   −∇ₓₓ²yᵀcₑ(x)
        auto H_c = matrices.H_c(x, y);
        H_c.conservativeResize(x_p.rows(), x_p.rows());
 
        // Lagrangian Hessian
        //
        //   [ζDᵣ  0  0]
        //   [ 0   0  0] − ∇ₓₓ²yᵀcₑ(x)
        //   [ 0   0  0]
        return d2f_dx2 + H_c;
      },
      [&](const DenseVector& x_p, [[maybe_unused]] const DenseVector& y_p,
          [[maybe_unused]] const DenseVector& z_p) -> SparseMatrix {
        return SparseMatrix{x_p.rows(), x_p.rows()};
      },
      [&](const DenseVector& x_p) -> DenseVector {
        auto x = x_p.segment(0, num_vars);
        auto p_e = x_p.segment(num_vars, num_eq);
        auto n_e = x_p.segment(num_vars + num_eq, num_eq);
 
        // Equality constraints
        //
        //   cₑ(x) - pₑ + nₑ = 0
        return matrices.c_e(x) - p_e + n_e;
      },
      [&](const DenseVector& x_p) -> SparseMatrix {
        auto x = x_p.segment(0, num_vars);
 
        // Equality constraint Jacobian
        //
        //   [Aₑ  −I  I]
 
        SparseMatrix A_e = matrices.A_e(x);
 
        gch::small_vector<Eigen::Triplet<Scalar>> triplets;
        triplets.reserve(A_e.nonZeros() + 2 * num_eq);
 
        append_as_triplets(triplets, 0, 0, {A_e});
        append_diagonal_as_triplets(
            triplets, 0, num_vars,
            DenseVector::Constant(num_eq, Scalar(-1)).eval());
        append_diagonal_as_triplets(
            triplets, 0, num_vars + num_eq,
            DenseVector::Constant(num_eq, Scalar(1)).eval());
 
        SparseMatrix A_e_p{A_e.rows(), x_p.rows()};
        A_e_p.setFromSortedTriplets(triplets.begin(), triplets.end());
        return A_e_p;
      },
      [&](const DenseVector& x_p) -> DenseVector {
        // Inequality constraints
        //
        //   pₑ ≥ 0
        //   nₑ ≥ 0
        return x_p.segment(num_vars, 2 * num_eq);
      },
      [&](const DenseVector& x_p) -> SparseMatrix {
        // Inequality constraint Jacobian
        //
        //   [0  I  0]
        //   [0  0  I]
 
        gch::small_vector<Eigen::Triplet<Scalar>> triplets;
        triplets.reserve(2 * num_eq);
 
        append_diagonal_as_triplets(
            triplets, 0, num_vars,
            DenseVector::Constant(2 * num_eq, Scalar(1)).eval());
 
        SparseMatrix A_i_p{2 * num_eq, x_p.rows()};
        A_i_p.setFromSortedTriplets(triplets.begin(), triplets.end());
        return A_i_p;
      },
      fr_scaling};
 
  auto status =
      ipm<Scalar>(fr_matrix_callbacks, iteration_callbacks, options, true,
#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
                  Eigen::ArrayX<bool>::Constant(2 * num_eq, true),
#endif
                  fr_x, fr_s, fr_y, fr_z, fr_μ, iterations);
 
  x = fr_x.segment(0, x.rows());
 
  // If feasibility restoration converged, check the constraint violation in
  // the original problem's scaling
  DenseVector c_e_unscaled;
  bool c_e_violated = false;
  if (status == ExitStatus::SUCCESS) {
    c_e_unscaled = matrices.c_e(x);
    if (matrices.scaling.c_e.size() > 0) {
      c_e_unscaled =
          matrices.scaling.c_e.cwiseInverse().cwiseProduct(c_e_unscaled);
    }
 
    c_e_violated =
        c_e_unscaled.template lpNorm<Eigen::Infinity>() > options.tolerance;
  }
 
  // Return to the normal phase if feasibility restoration stopped early, or if
  // it converged to a feasible point (e.g., because it was run to convergence)
  if (status == ExitStatus::CALLBACK_REQUESTED_STOP ||
      (status == ExitStatus::SUCCESS && !c_e_violated)) {
    // Reset the equality constraint duals since feasibility restoration didn't
    // account for the cost function
    y.setZero();
 
    return ExitStatus::SUCCESS;
  } else if (status == ExitStatus::SUCCESS) {
    // Feasibility restoration converged to a minimizer of the constraint
    // violation, and the constraint violation is still above the tolerance, so
    // that minimizer is a certificate of local infeasibility. Declaring local
    // infeasibility anywhere else risks false positives (e.g., a
    // point-in-time test can reject iterates the solver would otherwise
    // escape). See section 3.3, p. 14 of [2].
    if (options.diagnostics) {
      print_c_e_local_infeasibility_error(c_e_unscaled, options.tolerance);
    }
 
    return ExitStatus::LOCALLY_INFEASIBLE;
  } else {
    return ExitStatus::FEASIBILITY_RESTORATION_FAILED;
  }
}
 
template <typename Scalar>
ExitStatus feasibility_restoration(
    const IPMMatrixCallbacks<Scalar>& matrix_callbacks,
    std::span<std::function<bool(const IterationInfo<Scalar>& info)>>
        iteration_callbacks,
    const Options<Scalar>& options,
#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
    const Eigen::ArrayX<bool>& bound_constraint_mask,
#endif
    Eigen::VectorX<Scalar>& x, Eigen::VectorX<Scalar>& s,
    Eigen::VectorX<Scalar>& y, Eigen::VectorX<Scalar>& z, Scalar μ,
    int& iterations) {
  // Feasibility restoration
  //
  //        min  ρ Σ (pₑ + nₑ + pᵢ + nᵢ) + ζ/2 (x - xᵣ)ᵀDᵣ(x - xᵣ)
  //         x
  //       pₑ,nₑ
  //       pᵢ,nᵢ
  //
  //   s.t. cₑ(x) - pₑ + nₑ = 0
  //        cᵢ(x) - pᵢ + nᵢ ≥ 0
  //        pₑ ≥ 0
  //        nₑ ≥ 0
  //        pᵢ ≥ 0
  //        nᵢ ≥ 0
  //
  // where ρ = 1000, ζ = √μ where μ is the barrier parameter, xᵣ is original
  // iterate before feasibility restoration, and Dᵣ is a scaling matrix defined
  // by
  //
  //   Dᵣ = diag(min(1, 1/xᵣ[i]²) for i in x.rows())
 
  using DenseVector = Eigen::VectorX<Scalar>;
  using DiagonalMatrix = Eigen::DiagonalMatrix<Scalar, Eigen::Dynamic>;
  using SparseMatrix = Eigen::SparseMatrix<Scalar>;
  using SparseVector = Eigen::SparseVector<Scalar>;
 
  using std::sqrt;
 
  const auto& matrices = matrix_callbacks;
  const auto& num_vars = matrices.num_decision_variables;
  const auto& num_eq = matrices.num_equality_constraints;
  const auto& num_ineq = matrices.num_inequality_constraints;
 
  constexpr Scalar ρ(1e3);
 
  const DenseVector c_e = matrices.c_e(x);
  const DenseVector c_i = matrices.c_i(x);
 
  Scalar fr_μ = std::max({μ, c_e.template lpNorm<Eigen::Infinity>(),
                          (c_i - s).template lpNorm<Eigen::Infinity>()});
  const Scalar ζ = sqrt(fr_μ);
 
  const auto& x_r = x;
  const auto [p_e_0, n_e_0] = compute_p_n(c_e, ρ, fr_μ);
  const auto [p_i_0, n_i_0] = compute_p_n((c_i - s).eval(), ρ, fr_μ);
 
  // Dᵣ = diag(min(1, 1/xᵣ[i]²) for i in x.rows())
  const DiagonalMatrix D_r =
      x.cwiseSquare().cwiseInverse().cwiseMin(Scalar(1)).asDiagonal();
 
  DenseVector fr_x{num_vars + 2 * num_eq + 2 * num_ineq};
  fr_x << x, p_e_0, n_e_0, p_i_0, n_i_0;
 
  // Use the original problem's slacks for cᵢ(x) − pᵢ + nᵢ ≥ 0 since
  // cᵢ(x) − pᵢ + nᵢ = s by construction.
  //
  // Start the constraints pₑ, nₑ, pᵢ, nᵢ ≥ 0 with zero violation.
  DenseVector fr_s{s.rows() + 2 * num_eq + 2 * num_ineq};
  fr_s.segment(0, s.rows()) = s;
  fr_s.segment(s.rows(), 2 * num_eq + 2 * num_ineq) << p_e_0, n_e_0, p_i_0,
      n_i_0;
 
  DenseVector fr_y = DenseVector::Zero(c_e.rows());
 
  // Start the duals with perfect complementarity with respect to the slacks
  DenseVector fr_z{c_i.rows() + 2 * num_eq + 2 * num_ineq};
  fr_z << fr_μ * s.cwiseInverse(), fr_μ * p_e_0.cwiseInverse(),
      fr_μ * n_e_0.cwiseInverse(), fr_μ * p_i_0.cwiseInverse(),
      fr_μ * n_i_0.cwiseInverse();
 
  // Inherit the parent problem's scaling for the constraints, and use no
  // scaling for the cost function since it has changed. The new rows introduced
  // are not scaled.
  DenseVector fr_d_c_i{c_i.rows() + 2 * num_eq + 2 * num_ineq};
  fr_d_c_i << matrices.scaling.c_i,
      DenseVector::Ones(2 * num_eq + 2 * num_ineq);
  const ProblemScaling<Scalar> fr_scaling{Scalar(1), matrices.scaling.c_e,
                                          fr_d_c_i};
 
  IPMMatrixCallbacks<Scalar> fr_matrix_callbacks{
      static_cast<int>(fr_x.rows()),
      static_cast<int>(fr_y.rows()),
      static_cast<int>(fr_z.rows()),
      [&](const DenseVector& x_p) -> Scalar {
        auto x = x_p.segment(0, num_vars);
 
        // Cost function
        //
        //   ρ Σ (pₑ + nₑ + pᵢ + nᵢ) + ζ/2 (x - xᵣ)ᵀDᵣ(x - xᵣ)
        auto diff = x - x_r;
        return ρ * x_p.segment(num_vars, 2 * num_eq + 2 * num_ineq)
                       .array()
                       .sum() +
               ζ / Scalar(2) * diff.transpose() * D_r * diff;
      },
      [&](const DenseVector& x_p) -> SparseVector {
        auto x = x_p.segment(0, num_vars);
 
        // Cost function gradient
        //
        //   [ζDᵣ(x − xᵣ)]
        //   [     ρ     ]
        //   [     ρ     ]
        //   [     ρ     ]
        //   [     ρ     ]
        DenseVector g{x_p.rows()};
        g.segment(0, num_vars) = ζ * D_r * (x - x_r);
        g.segment(num_vars, 2 * num_eq + 2 * num_ineq).setConstant(ρ);
        return g.sparseView();
      },
      [&](const DenseVector& x_p, const DenseVector& y_p,
          const DenseVector& z_p) -> SparseMatrix {
        auto x = x_p.segment(0, num_vars);
        const auto& y = y_p;
        auto z = z_p.segment(0, num_ineq);
 
        // Cost function Hessian
        //
        //   [ζDᵣ  0  0  0  0]
        //   [ 0   0  0  0  0]
        //   [ 0   0  0  0  0]
        //   [ 0   0  0  0  0]
        //   [ 0   0  0  0  0]
        gch::small_vector<Eigen::Triplet<Scalar>> triplets;
        triplets.reserve(x_p.rows());
        append_as_triplets(triplets, 0, 0, {SparseMatrix{ζ * D_r}});
        SparseMatrix d2f_dx2{x_p.rows(), x_p.rows()};
        d2f_dx2.setFromSortedTriplets(triplets.begin(), triplets.end());
 
        // Constraint part of original problem's Lagrangian Hessian
        //
        //   −∇ₓₓ²yᵀcₑ(x) − ∇ₓₓ²zᵀcᵢ(x)
        auto H_c = matrices.H_c(x, y, z);
        H_c.conservativeResize(x_p.rows(), x_p.rows());
 
        // Lagrangian Hessian
        //
        //   [ζDᵣ  0  0  0  0]
        //   [ 0   0  0  0  0]
        //   [ 0   0  0  0  0] − ∇ₓₓ²yᵀcₑ(x) − ∇ₓₓ²zᵀcᵢ(x)
        //   [ 0   0  0  0  0]
        //   [ 0   0  0  0  0]
        return d2f_dx2 + H_c;
      },
      [&](const DenseVector& x_p, [[maybe_unused]] const DenseVector& y_p,
          [[maybe_unused]] const DenseVector& z_p) -> SparseMatrix {
        return SparseMatrix{x_p.rows(), x_p.rows()};
      },
      [&](const DenseVector& x_p) -> DenseVector {
        auto x = x_p.segment(0, num_vars);
        auto p_e = x_p.segment(num_vars, num_eq);
        auto n_e = x_p.segment(num_vars + num_eq, num_eq);
 
        // Equality constraints
        //
        //   cₑ(x) - pₑ + nₑ = 0
        return matrices.c_e(x) - p_e + n_e;
      },
      [&](const DenseVector& x_p) -> SparseMatrix {
        auto x = x_p.segment(0, num_vars);
 
        // Equality constraint Jacobian
        //
        //   [Aₑ  −I  I  0  0]
 
        SparseMatrix A_e = matrices.A_e(x);
 
        gch::small_vector<Eigen::Triplet<Scalar>> triplets;
        triplets.reserve(A_e.nonZeros() + 2 * num_eq);
 
        append_as_triplets(triplets, 0, 0, {A_e});
        append_diagonal_as_triplets(
            triplets, 0, num_vars,
            DenseVector::Constant(num_eq, Scalar(-1)).eval());
        append_diagonal_as_triplets(
            triplets, 0, num_vars + num_eq,
            DenseVector::Constant(num_eq, Scalar(1)).eval());
 
        SparseMatrix A_e_p{A_e.rows(), x_p.rows()};
        A_e_p.setFromSortedTriplets(triplets.begin(), triplets.end());
        return A_e_p;
      },
      [&](const DenseVector& x_p) -> DenseVector {
        auto x = x_p.segment(0, num_vars);
        auto p_i = x_p.segment(num_vars + 2 * num_eq, num_ineq);
        auto n_i = x_p.segment(num_vars + 2 * num_eq + num_ineq, num_ineq);
 
        // Inequality constraints
        //
        //   cᵢ(x) - pᵢ + nᵢ ≥ 0
        //   pₑ ≥ 0
        //   nₑ ≥ 0
        //   pᵢ ≥ 0
        //   nᵢ ≥ 0
        DenseVector c_i_p{c_i.rows() + 2 * num_eq + 2 * num_ineq};
        c_i_p.segment(0, num_ineq) = matrices.c_i(x) - p_i + n_i;
        c_i_p.segment(p_i.rows(), 2 * num_eq + 2 * num_ineq) =
            x_p.segment(num_vars, 2 * num_eq + 2 * num_ineq);
        return c_i_p;
      },
      [&](const DenseVector& x_p) -> SparseMatrix {
        auto x = x_p.segment(0, num_vars);
 
        // Inequality constraint Jacobian
        //
        //   [Aᵢ  0  0  −I  I]
        //   [0   I  0   0  0]
        //   [0   0  I   0  0]
        //   [0   0  0   I  0]
        //   [0   0  0   0  I]
 
        SparseMatrix A_i = matrices.A_i(x);
 
        gch::small_vector<Eigen::Triplet<Scalar>> triplets;
        triplets.reserve(A_i.nonZeros() + 2 * num_eq + 4 * num_ineq);
 
        // Column 0
        append_as_triplets(triplets, 0, 0, {A_i});
 
        // Columns 1 and 2
        append_diagonal_as_triplets(
            triplets, num_ineq, num_vars,
            DenseVector::Constant(2 * num_eq, Scalar(1)).eval());
 
        SparseMatrix I_ineq{
            DenseVector::Constant(num_ineq, Scalar(1)).asDiagonal()};
 
        // Column 3
        SparseMatrix Z_col3{2 * num_eq, num_ineq};
        append_as_triplets(triplets, 0, num_vars + 2 * num_eq,
                           {(-I_ineq).eval(), Z_col3, I_ineq});
 
        // Column 4
        SparseMatrix Z_col4{2 * num_eq + num_ineq, num_ineq};
        append_as_triplets(triplets, 0, num_vars + 2 * num_eq + num_ineq,
                           {I_ineq, Z_col4, I_ineq});
 
        SparseMatrix A_i_p{2 * num_eq + 3 * num_ineq, x_p.rows()};
        A_i_p.setFromSortedTriplets(triplets.begin(), triplets.end());
        return A_i_p;
      },
      fr_scaling};
 
#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
  Eigen::ArrayX<bool> fr_bound_constraint_mask{2 * num_eq + 3 * num_ineq};
  fr_bound_constraint_mask.segment(0, num_ineq) = bound_constraint_mask;
  fr_bound_constraint_mask.segment(num_ineq, 2 * num_eq + 2 * num_ineq) = true;
#endif
 
  auto status =
      ipm<Scalar>(fr_matrix_callbacks, iteration_callbacks, options, true,
#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
                  fr_bound_constraint_mask,
#endif
                  fr_x, fr_s, fr_y, fr_z, fr_μ, iterations);
 
  x = fr_x.segment(0, x.rows());
 
  // If feasibility restoration converged, check the constraint violation in the
  // original problem's scaling
  DenseVector c_e_unscaled;
  DenseVector c_i_unscaled;
  bool c_e_violated = false;
  bool c_i_violated = false;
  if (status == ExitStatus::SUCCESS) {
    c_e_unscaled = matrices.c_e(x);
    if (matrices.scaling.c_e.size() > 0) {
      c_e_unscaled =
          matrices.scaling.c_e.cwiseInverse().cwiseProduct(c_e_unscaled);
    }
 
    c_i_unscaled = matrices.c_i(x);
    if (matrices.scaling.c_i.size() > 0) {
      c_i_unscaled =
          matrices.scaling.c_i.cwiseInverse().cwiseProduct(c_i_unscaled);
    }
 
    // Inequality constraints cᵢ(x) ≥ 0 are only violated where they're
    // negative
    c_e_violated =
        c_e_unscaled.template lpNorm<Eigen::Infinity>() > options.tolerance;
    c_i_violated =
        (-c_i_unscaled).cwiseMax(Scalar(0)).template lpNorm<Eigen::Infinity>() >
        options.tolerance;
  }
 
  // Return to the normal phase if feasibility restoration stopped early, or if
  // it converged to a feasible point (e.g., because it was run to convergence)
  if (status == ExitStatus::CALLBACK_REQUESTED_STOP ||
      (status == ExitStatus::SUCCESS && !c_e_violated && !c_i_violated)) {
    const DenseVector s_old = s;
 
    // Re-center the slacks on the original constraints cᵢ(x) if they're
    // sufficiently feasible, which zeroes their constraint violation.
    // Otherwise, keep feasibility restoration's slacks.
    //
    // Note that those slacks can make Σ = S⁻¹Z and μS⁻¹e very large because
    // fr_μ drives them toward 0 independently of μ, so they can end up much
    // smaller than μ. We can't fix that by raising the slacks because the added
    // constraint violation would make the filter reject more returned iterates.
    //
    // The normal phase's early-exit callback computes the slacks the same way,
    // so the returned iterate is the one the filter accepted.
    const DenseVector c_i = matrices.c_i(x);
    s = (c_i.array() >= Scalar(1e-2)).select(c_i, fr_s.segment(0, s.rows()));
 
    // Reset the equality constraint duals since feasibility restoration didn't
    // account for the cost function
    y.setZero();
 
    // Update the inequality constraint duals with a Newton step for
    // complementarity, taking the change in slacks during the restoration
    // phase as the primal step.
    //
    //   pᶻ = μS⁻¹e − z − Σpˢ
    //
    // If the result isn't strictly positive or is too large, reset all of them
    // to 1.
    const DenseVector Σ = z.cwiseQuotient(s_old);
    const DenseVector p_s = s - s_old;
    z += μ * s_old.cwiseInverse() - z - Σ.cwiseProduct(p_s);
    if ((z.array() <= Scalar(0)).any() || (z.array() > Scalar(1e3)).any()) {
      z.setOnes();
    }
 
    return ExitStatus::SUCCESS;
  } else if (status == ExitStatus::SUCCESS) {
    // Feasibility restoration converged to a minimizer of the constraint
    // violation, and the constraint violation is still above the tolerance, so
    // that minimizer is a certificate of local infeasibility. Declaring local
    // infeasibility anywhere else risks false positives (e.g., a
    // point-in-time test can reject iterates the solver would otherwise
    // escape). See section 3.3, p. 14 of [2].
 
    s = fr_s.segment(0, s.rows());
 
    if (options.diagnostics) {
      if (c_e_violated) {
        print_c_e_local_infeasibility_error(c_e_unscaled, options.tolerance);
      }
      if (c_i_violated) {
        print_c_i_local_infeasibility_error(c_i_unscaled, options.tolerance);
      }
    }
 
    return ExitStatus::LOCALLY_INFEASIBLE;
  } else {
    s = fr_s.segment(0, s.rows());
    return ExitStatus::FEASIBILITY_RESTORATION_FAILED;
  }
}
 
}  // namespace slp
 
#include "sleipnir/optimization/solver/ipm.hpp"