kkt_error.hpp

// Copyright (c) Sleipnir contributors
 
#pragma once
 
#include <algorithm>
#include <cmath>
#include <limits>
 
#include <Eigen/Core>
#include <Eigen/SparseCore>
 
#include "sleipnir/optimization/solver/util/problem_scaling.hpp"
 
// See docs/algorithms.md#Works_cited for citation definitions
 
namespace slp {
 
enum class KKTErrorType {
  INF_NORM_SCALED,
  ONE_NORM
};
 
template <typename Scalar, KKTErrorType T>
Scalar kkt_error(const Eigen::VectorX<Scalar>& g) {
  // The KKT conditions from docs/algorithms.md:
  //
  //   ∇f = 0
 
  if constexpr (T == KKTErrorType::INF_NORM_SCALED) {
    return g.template lpNorm<Eigen::Infinity>();
  } else if constexpr (T == KKTErrorType::ONE_NORM) {
    return g.template lpNorm<1>();
  }
}
 
template <typename Scalar, KKTErrorType T>
Scalar kkt_error(const Eigen::VectorX<Scalar>& g,
                 const Eigen::SparseMatrix<Scalar>& A_e,
                 const Eigen::VectorX<Scalar>& c_e,
                 const Eigen::VectorX<Scalar>& y, Scalar tolerance) {
  using std::pow;
 
  // The KKT conditions from docs/algorithms.md:
  //
  //   ∇f − Aₑᵀy = 0
  //   cₑ = 0
 
  if constexpr (T == KKTErrorType::INF_NORM_SCALED) {
    constexpr Scalar ε = std::numeric_limits<Scalar>::epsilon();
 
    // The stationarity residual must not go below its rounding error
    // floor κεω, where κ is how much the problem amplifies rounding
    // error and ω is the size of the largest stationarity terms. If
    // κεω > tol, the solver stalls instead of terminating.
    //
    // κ is unknown, so assume κ ≤ ε^(-3/8) (≈ 7.4e5 for double, 395 for
    // float). Then the floor is at most
    //
    //   κεω ≤ ε^(-3/8) εω = ε^(5/8) ω
    //
    // and dividing the residual by ε^(5/8) ω/tol makes it meet tol at the
    // floor. Dividing by less than 1 would tighten tol instead, so
    //
    //   s_d = max(1, ε^(5/8) ω/tol)
 
    // ω = ‖|∇f| + |Aₑ|ᵀ|y|‖_∞
    const Scalar ω = (g.cwiseAbs() + A_e.cwiseAbs().transpose() * y.cwiseAbs())
                         .template lpNorm<Eigen::Infinity>();
 
    Scalar s_d = std::max(Scalar(1), pow(ε, Scalar(0.625)) * ω / tolerance);
 
    // ‖∇f − Aₑᵀy‖_∞ / s_d
    // ‖cₑ‖_∞
    return std::max(
        {(g - A_e.transpose() * y).template lpNorm<Eigen::Infinity>() / s_d,
         c_e.template lpNorm<Eigen::Infinity>()});
  } else if constexpr (T == KKTErrorType::ONE_NORM) {
    return (g - A_e.transpose() * y).template lpNorm<1>() +
           c_e.template lpNorm<1>();
  }
}
 
template <typename Scalar, KKTErrorType T>
Scalar kkt_error(const Eigen::VectorX<Scalar>& g,
                 const Eigen::SparseMatrix<Scalar>& A_e,
                 const Eigen::VectorX<Scalar>& c_e,
                 const Eigen::SparseMatrix<Scalar>& A_i,
                 const Eigen::VectorX<Scalar>& c_i,
                 const Eigen::VectorX<Scalar>& s,
                 const Eigen::VectorX<Scalar>& y,
                 const Eigen::VectorX<Scalar>& z, Scalar μ, Scalar tolerance) {
  using std::pow;
 
  // The KKT conditions from docs/algorithms.md:
  //
  //   ∇f − Aₑᵀy − Aᵢᵀz = 0
  //   Sz − μe = 0
  //   cₑ = 0
  //   cᵢ − s = 0
 
  if constexpr (T == KKTErrorType::INF_NORM_SCALED) {
    constexpr Scalar ε = std::numeric_limits<Scalar>::epsilon();
 
    // The stationarity residual must not go below its rounding error
    // floor κεω, where κ is how much the problem amplifies rounding
    // error and ω is the size of the largest stationarity terms. If
    // κεω > tol, the solver stalls instead of terminating.
    //
    // κ is unknown, so assume κ ≤ ε^(-3/8) (≈ 7.4e5 for double, 395 for
    // float). Then the floor is at most
    //
    //   κεω ≤ ε^(-3/8) εω = ε^(5/8) ω
    //
    // and dividing the residual by ε^(5/8) ω/tol makes it meet tol at the
    // floor. Dividing by less than 1 would tighten tol instead, so
    //
    //   s_d = max(1, ε^(5/8) ω/tol)
 
    // ω = ‖|∇f| + |Aₑ|ᵀ|y| + |Aᵢ|ᵀ|z|‖_∞
    const Scalar ω = (g.cwiseAbs() + A_e.cwiseAbs().transpose() * y.cwiseAbs() +
                      A_i.cwiseAbs().transpose() * z.cwiseAbs())
                         .template lpNorm<Eigen::Infinity>();
 
    Scalar s_d = std::max(Scalar(1), pow(ε, Scalar(0.625)) * ω / tolerance);
 
    // s_c = max(sₘₐₓ, ‖z‖₁ / n) / sₘₐₓ
    //
    // See equation (5) of [2].
    constexpr Scalar s_max(100);
    Scalar s_c =
        std::max(s_max, z.template lpNorm<1>() / Scalar(z.rows())) / s_max;
 
    const auto S = s.asDiagonal();
    const Eigen::VectorX<Scalar> μe =
        Eigen::VectorX<Scalar>::Constant(s.rows(), μ);
 
    // ‖∇f − Aₑᵀy − Aᵢᵀz‖_∞ / s_d
    // ‖Sz − μe‖_∞ / s_c
    // ‖cₑ‖_∞
    // ‖cᵢ − s‖_∞
    return std::max({(g - A_e.transpose() * y - A_i.transpose() * z)
                             .template lpNorm<Eigen::Infinity>() /
                         s_d,
                     (S * z - μe).template lpNorm<Eigen::Infinity>() / s_c,
                     c_e.template lpNorm<Eigen::Infinity>(),
                     (c_i - s).template lpNorm<Eigen::Infinity>()});
  } else if constexpr (T == KKTErrorType::ONE_NORM) {
    const auto S = s.asDiagonal();
    const Eigen::VectorX<Scalar> μe =
        Eigen::VectorX<Scalar>::Constant(s.rows(), μ);
 
    return (g - A_e.transpose() * y - A_i.transpose() * z)
               .template lpNorm<1>() +
           (S * z - μe).template lpNorm<1>() + c_e.template lpNorm<1>() +
           (c_i - s).template lpNorm<1>();
  }
}
 
template <typename Scalar, KKTErrorType T>
Scalar unscaled_kkt_error(const ProblemScaling<Scalar>& scaling,
                          const Eigen::VectorX<Scalar>& g) {
  using DenseVector = Eigen::VectorX<Scalar>;
 
  if (scaling.is_identity()) {
    return kkt_error<Scalar, T>(g);
  }
 
  const DenseVector g_unscaled = (Scalar(1) / scaling.f) * g;
 
  return kkt_error<Scalar, T>(g_unscaled);
}
 
template <typename Scalar, KKTErrorType T>
Scalar unscaled_kkt_error(const ProblemScaling<Scalar>& scaling,
                          const Eigen::VectorX<Scalar>& g,
                          const Eigen::SparseMatrix<Scalar>& A_e,
                          const Eigen::VectorX<Scalar>& c_e,
                          const Eigen::VectorX<Scalar>& y, Scalar tolerance) {
  using DenseVector = Eigen::VectorX<Scalar>;
  using SparseMatrix = Eigen::SparseMatrix<Scalar>;
 
  if (scaling.is_identity()) {
    return kkt_error<Scalar, T>(g, A_e, c_e, y, tolerance);
  }
 
  const Scalar inv_d_f = Scalar(1) / scaling.f;
  const DenseVector inv_d_c_e = scaling.c_e.cwiseInverse();
 
  const DenseVector g_unscaled = inv_d_f * g;
  const SparseMatrix A_e_unscaled = inv_d_c_e.asDiagonal() * A_e;
  const DenseVector c_e_unscaled = inv_d_c_e.cwiseProduct(c_e);
  const DenseVector y_unscaled = scaling.c_e.cwiseProduct(y) * inv_d_f;
 
  return kkt_error<Scalar, T>(g_unscaled, A_e_unscaled, c_e_unscaled,
                              y_unscaled, tolerance);
}
 
template <typename Scalar, KKTErrorType T>
Scalar unscaled_kkt_error(
    const ProblemScaling<Scalar>& scaling, const Eigen::VectorX<Scalar>& g,
    const Eigen::SparseMatrix<Scalar>& A_e, const Eigen::VectorX<Scalar>& c_e,
    const Eigen::SparseMatrix<Scalar>& A_i, const Eigen::VectorX<Scalar>& c_i,
    const Eigen::VectorX<Scalar>& s, const Eigen::VectorX<Scalar>& y,
    const Eigen::VectorX<Scalar>& z, Scalar μ, Scalar tolerance) {
  using DenseVector = Eigen::VectorX<Scalar>;
  using SparseMatrix = Eigen::SparseMatrix<Scalar>;
 
  if (scaling.is_identity()) {
    return kkt_error<Scalar, T>(g, A_e, c_e, A_i, c_i, s, y, z, μ, tolerance);
  }
 
  const Scalar inv_d_f = Scalar(1) / scaling.f;
  const DenseVector inv_d_c_e = scaling.c_e.cwiseInverse();
  const DenseVector inv_d_c_i = scaling.c_i.cwiseInverse();
 
  const DenseVector g_unscaled = inv_d_f * g;
  const SparseMatrix A_e_unscaled = inv_d_c_e.asDiagonal() * A_e;
  const DenseVector c_e_unscaled = inv_d_c_e.cwiseProduct(c_e);
  const SparseMatrix A_i_unscaled = inv_d_c_i.asDiagonal() * A_i;
  const DenseVector c_i_unscaled = inv_d_c_i.cwiseProduct(c_i);
  const DenseVector s_unscaled = inv_d_c_i.cwiseProduct(s);
  const DenseVector y_unscaled = scaling.c_e.cwiseProduct(y) * inv_d_f;
  const DenseVector z_unscaled = scaling.c_i.cwiseProduct(z) * inv_d_f;
  const Scalar μ_unscaled = inv_d_f * μ;
 
  return kkt_error<Scalar, T>(g_unscaled, A_e_unscaled, c_e_unscaled,
                              A_i_unscaled, c_i_unscaled, s_unscaled,
                              y_unscaled, z_unscaled, μ_unscaled, tolerance);
}
 
}  // namespace slp