ruiz.hpp

Go to the documentation of this file.

//
// Copyright (c) 2022 INRIA
//
#ifndef PROXSUITE_PROXQP_DENSE_PRECOND_RUIZ_HPP
#define PROXSUITE_PROXQP_DENSE_PRECOND_RUIZ_HPP
 
#include "proxsuite/proxqp/dense/views.hpp"
#include "proxsuite/proxqp/dense/fwd.hpp"
#include <proxsuite/linalg/dense/core.hpp>
#include <proxsuite/proxqp/settings.hpp>
#include <ostream>
#include <iostream>
#include <Eigen/Core>
 
namespace proxsuite {
namespace proxqp {
enum struct Symmetry
{
  general,
  lower,
  upper,
};
namespace dense {
namespace detail {
 
template<typename T>
auto
ruiz_scale_qp_in_place( //
  VectorViewMut<T> delta_,
  std::ostream* logger_ptr,
  QpViewBoxMut<T> qp,
  T epsilon,
  isize max_iter,
  bool preconditioning_for_infeasible_problems,
  Symmetry sym,
  HessianType HessianType,
  const bool box_constraints,
  proxsuite::linalg::veg::dynstack::DynStackMut stack) -> T
{
  T c(1);
  auto S = delta_.to_eigen();
  auto H = qp.H.to_eigen();
  auto g = qp.g.to_eigen();
  auto A = qp.A.to_eigen();
  auto b = qp.b.to_eigen();
  auto C = qp.C.to_eigen();
  auto u = qp.u.to_eigen();
  auto l = qp.l.to_eigen();
 
  auto l_box = qp.l_box.to_eigen();
  auto u_box = qp.u_box.to_eigen();
  auto i_scaled = qp.I.to_eigen();
  i_scaled.setOnes();
 
  static constexpr T machine_eps = std::numeric_limits<T>::epsilon();
  /*
   * compute equilibration parameters and scale in place the qp following
   * algorithm
   *
   * modified: removed g in gamma computation
   */
 
  isize n = qp.H.rows;
  isize n_eq = qp.A.rows;
  isize n_in = qp.C.rows;
  isize n_constraints(n_in);
  if (box_constraints) {
    n_constraints += n;
  }
 
  T gamma = T(1);
  LDLT_TEMP_VEC(T, delta, n + n_eq + n_constraints, stack);
  i64 iter = 1;
  while (infty_norm((1 - delta.array()).matrix()) > epsilon) {
 
    if (logger_ptr != nullptr) {
      *logger_ptr                                   //
        << "j : "                                   //
        << iter                                     //
        << " ; error : "                            //
        << infty_norm((1 - delta.array()).matrix()) //
        << "\n\n";
    }
    if (iter == max_iter) {
      break;
    } else {
      ++iter;
    }
    // normalization vector
    {
      switch (HessianType) {
        case HessianType::Zero:
          for (isize k = 0; k < n; ++k) {
            T aux = sqrt(std::max({ n_eq > 0 ? infty_norm(A.col(k)) : T(0),
                                    n_in > 0 ? infty_norm(C.col(k)) : T(0),
                                    box_constraints ? i_scaled[k] : T(0) }));
            if (aux == T(0)) {
              delta(k) = T(1);
            } else {
              delta(k) = T(1) / (aux + machine_eps);
            }
          }
          break;
        case HessianType::Dense:
          for (isize k = 0; k < n; ++k) {
            switch (sym) {
              case Symmetry::upper: { // upper triangular part
                T aux =
                  sqrt(std::max({ infty_norm(H.col(k).head(k)),
                                  infty_norm(H.row(k).tail(n - k)),
                                  n_eq > 0 ? infty_norm(A.col(k)) : T(0),
                                  n_in > 0 ? infty_norm(C.col(k)) : T(0),
                                  box_constraints ? i_scaled[k] : T(0) }));
                if (aux == T(0)) {
                  delta(k) = T(1);
                } else {
                  delta(k) = T(1) / (aux + machine_eps);
                }
                break;
              }
              case Symmetry::lower: { // lower triangular part
 
                T aux =
                  sqrt(std::max({ infty_norm(H.col(k).head(k)),
                                  infty_norm(H.col(k).tail(n - k)),
                                  n_eq > 0 ? infty_norm(A.col(k)) : T(0),
                                  n_in > 0 ? infty_norm(C.col(k)) : T(0),
                                  box_constraints ? i_scaled[k] : T(0) }));
                if (aux == T(0)) {
                  delta(k) = T(1);
                } else {
                  delta(k) = T(1) / (aux + machine_eps);
                }
                break;
              }
              case Symmetry::general: {
 
                T aux =
                  sqrt(std::max({ infty_norm(H.col(k)),
                                  n_eq > 0 ? infty_norm(A.col(k)) : T(0),
                                  n_in > 0 ? infty_norm(C.col(k)) : T(0),
                                  box_constraints ? i_scaled[k] : T(0) }));
                if (aux == T(0)) {
                  delta(k) = T(1);
                } else {
                  delta(k) = T(1) / (aux + machine_eps);
                }
                break;
              }
            }
          }
          break;
        case HessianType::Diagonal:
          for (isize k = 0; k < n; ++k) {
            T aux = sqrt(std::max({ std::abs(H(k, k)),
                                    n_eq > 0 ? infty_norm(A.col(k)) : T(0),
                                    n_in > 0 ? infty_norm(C.col(k)) : T(0),
                                    box_constraints ? i_scaled[k] : T(0) }));
            if (aux == T(0)) {
              delta(k) = T(1);
            } else {
              delta(k) = T(1) / (aux + machine_eps);
            }
          }
          break;
      }
      if (preconditioning_for_infeasible_problems) {
        delta.tail(n_eq + n_constraints).setOnes();
      } else {
        for (isize k = 0; k < n_eq; ++k) {
          T aux = sqrt(infty_norm(A.row(k)));
          if (aux == T(0)) {
            delta(n + k) = T(1);
          } else {
            delta(n + k) = T(1) / (aux + machine_eps);
          }
        }
        for (isize k = 0; k < n_in; ++k) {
          T aux = sqrt(infty_norm(C.row(k)));
          if (aux == T(0)) {
            delta(k + n + n_eq) = T(1);
          } else {
            delta(k + n + n_eq) = T(1) / (aux + machine_eps);
          }
        }
        if (box_constraints) {
          for (isize k = 0; k < n; ++k) {
            delta(k + n + n_eq + n_in) = T(1) / sqrt(i_scaled[k] + machine_eps);
          }
        }
      }
      // removed as non deterministic when using avx
      // https://gitlab.com/libeigen/eigen/-/issues/1728
      // if (preconditioning_for_infeasible_problems) {
      //   T mean = delta.segment(n, n_eq_in).mean();
      //   delta.segment(n,n_eq_in).setConstant(mean);
      // }
    }
    {
 
      // normalize A and C
      A = delta.segment(n, n_eq).asDiagonal() * A * delta.head(n).asDiagonal();
      C = delta.segment(n + n_eq, n_in).asDiagonal() * C *
          delta.head(n).asDiagonal();
      if (box_constraints) {
        i_scaled.array() *= delta.head(n).array();
        i_scaled.array() *= delta.tail(n).array();
        u_box.array() *= delta.tail(n).array();
        l_box.array() *= delta.tail(n).array();
      }
      // normalize vectors
      g.array() *= delta.head(n).array();
      b.array() *= delta.segment(n, n_eq).array();
      u.array() *= delta.segment(n + n_eq, n_in).array();
      l.array() *= delta.segment(n + n_eq, n_in).array();
 
      // normalize H
      switch (HessianType) {
        case HessianType::Zero:
          break;
        case HessianType::Dense:
          switch (sym) {
            case Symmetry::upper: {
              // upper triangular part
              for (isize j = 0; j < n; ++j) {
                H.col(j).head(j + 1) *= delta(j);
              }
              // normalisation des lignes
              for (isize i = 0; i < n; ++i) {
                H.row(i).tail(n - i) *= delta(i);
              }
              break;
            }
            case Symmetry::lower: {
              // lower triangular part
              for (isize j = 0; j < n; ++j) {
                H.col(j).tail(n - j) *= delta(j);
              }
              // normalisation des lignes
              for (isize i = 0; i < n; ++i) {
                H.row(i).head(i + 1) *= delta(i);
              }
              break;
            }
            case Symmetry::general: {
              // all matrix
              H = delta.head(n).asDiagonal() * H * delta.head(n).asDiagonal();
              break;
            }
            default:
              break;
          }
          // additional normalization for the cost function
          switch (sym) {
            case Symmetry::upper: {
              // upper triangular part
              T tmp = T(0);
              for (isize j = 0; j < n; ++j) {
                tmp += proxqp::dense::infty_norm(H.row(j).tail(n - j));
              }
              gamma = 1 / std::max(tmp / T(n), T(1));
              break;
            }
            case Symmetry::lower: {
              // lower triangular part
              T tmp = T(0);
              for (isize j = 0; j < n; ++j) {
                tmp += proxqp::dense::infty_norm(H.col(j).tail(n - j));
              }
              gamma = 1 / std::max(tmp / T(n), T(1));
              break;
            }
            case Symmetry::general: {
              // all matrix
              gamma =
                1 / std::max(
                      T(1),
                      (H.colwise().template lpNorm<Eigen::Infinity>()).mean());
              break;
            }
            default:
              break;
          }
          break;
        case HessianType::Diagonal:
          // H = delta.head(n).asDiagonal() * H.asDiagonal() *
          // delta.head(n).asDiagonal();
          H.diagonal().array() *=
            delta.head(n)
              .array(); //* H.asDiagonal() * delta.head(n).asDiagonal();
          H.diagonal().array() *=
            delta.head(n)
              .array(); //* H.asDiagonal() * delta.head(n).asDiagonal();
          gamma =
            1 /
            std::max(T(1),
                     (H.diagonal().template lpNorm<Eigen::Infinity>()) / T(n));
          H *= gamma;
          break;
      }
      g *= gamma;
 
      S.array() *= delta.array(); // coefficientwise product
      c *= gamma;
    }
  }
  return c;
}
} // namespace detail
 
namespace preconditioner {
 
template<typename T>
struct RuizEquilibration
{
  Vec<T> delta;
  T c;
  isize dim;
  isize n_eq;
  isize n_in;
  T epsilon;
  i64 max_iter;
  Symmetry sym;
 
  std::ostream* logger_ptr = nullptr;
  explicit RuizEquilibration(isize dim_,
                             isize n_eq_,
                             isize n_in_,
                             bool box_constraints,
                             T epsilon_ = T(1e-3),
                             i64 max_iter_ = 10,
                             Symmetry sym_ = Symmetry::general,
                             std::ostream* logger = nullptr)
    : delta(Vec<T>::Ones(dim_ + n_eq_ + n_in_ + (box_constraints ? dim_ : 0)))
    , c(1)
    , dim(dim_)
    , n_eq(n_eq_)
    , n_in(n_in_)
    , epsilon(epsilon_)
    , max_iter(max_iter_)
    , sym(sym_)
    , logger_ptr(logger)
  {
  }
  void print() const
  {
    // CHANGE: endl to newline
    *logger_ptr << " delta : " << delta << "\n\n";
    *logger_ptr << " c : " << c << "\n\n";
  }
  static auto scale_qp_in_place_req(proxsuite::linalg::veg::Tag<T> tag,
                                    isize n,
                                    isize n_eq,
                                    isize n_in,
                                    bool box_constraints)
    -> proxsuite::linalg::veg::dynstack::StackReq
  {
    if (box_constraints) {
      return proxsuite::linalg::dense::temp_vec_req(tag, 2 * n + n_eq + n_in);
    } else {
      return proxsuite::linalg::dense::temp_vec_req(tag, n + n_eq + n_in);
    }
  }
 
  // H_new = c * head @ H @ head
  // A_new = tail @ A @ head
  // g_new = c * head @ g
  // b_new = tail @ b
  void scale_qp_in_place(QpViewBoxMut<T> qp,
                         bool execute_preconditioner,
                         bool preconditioning_for_infeasible_problems,
                         const isize max_iter,
                         const T epsilon,
                         const HessianType& HessianType,
                         const bool box_constraints,
                         proxsuite::linalg::veg::dynstack::DynStackMut stack)
  {
    if (execute_preconditioner) {
      delta.setOnes();
      c =
        detail::ruiz_scale_qp_in_place({ proxqp::from_eigen, delta },
                                       logger_ptr,
                                       qp,
                                       epsilon,
                                       max_iter,
                                       preconditioning_for_infeasible_problems,
                                       sym,
                                       HessianType,
                                       box_constraints,
                                       stack);
    } else {
 
      auto H = qp.H.to_eigen();
      auto g = qp.g.to_eigen();
      auto A = qp.A.to_eigen();
      auto b = qp.b.to_eigen();
      auto C = qp.C.to_eigen();
      auto u = qp.u.to_eigen();
      auto l = qp.l.to_eigen();
      auto l_box = qp.l_box.to_eigen();
      auto u_box = qp.u_box.to_eigen();
      auto i_scaled = qp.I.to_eigen(); // it is a vector
      isize n = qp.H.rows;
      isize n_eq = qp.A.rows;
      isize n_in = qp.C.rows;
 
      // normalize A and C
      A = delta.segment(n, n_eq).asDiagonal() * A * delta.head(n).asDiagonal();
      C = delta.segment(n + n_eq, n_in).asDiagonal() * C *
          delta.head(n).asDiagonal();
 
      // normalize H
      switch (HessianType) {
        case HessianType::Dense:
          switch (sym) {
            case Symmetry::upper: {
              // upper triangular part
              for (isize j = 0; j < n; ++j) {
                H.col(j).head(j + 1) *= delta(j);
              }
              // normalisation des lignes
              for (isize i = 0; i < n; ++i) {
                H.row(i).tail(n - i) *= delta(i);
              }
              break;
            }
            case Symmetry::lower: {
              // lower triangular part
              for (isize j = 0; j < n; ++j) {
                H.col(j).tail(n - j) *= delta(j);
              }
              // normalisation des lignes
              for (isize i = 0; i < n; ++i) {
                H.row(i).head(i + 1) *= delta(i);
              }
              break;
            }
            case Symmetry::general: {
              // all matrix
              H = delta.head(n).asDiagonal() * H * delta.head(n).asDiagonal();
              break;
            }
            default:
              break;
          }
          break;
 
        case HessianType::Zero:
          break;
        case HessianType::Diagonal:
          // H = delta.head(n).asDiagonal() * H.asDiagonal() *
          // delta.head(n).asDiagonal();
          H.diagonal().array() *=
            delta.head(n)
              .array(); //* H.asDiagonal() * delta.head(n).asDiagonal();
          H.diagonal().array() *=
            delta.head(n)
              .array(); //* H.asDiagonal() * delta.head(n).asDiagonal();
          break;
      }
 
      // normalize vectors
      g.array() *= delta.head(n).array();
      b.array() *= delta.segment(n, n_eq).array();
      l.array() *= delta.segment(n + n_eq, n_in).array();
      u.array() *= delta.segment(n + n_eq, n_in).array();
 
      if (box_constraints) {
        u_box.array() *= delta.tail(n).array();
        l_box.array() *= delta.tail(n).array();
        i_scaled.array() *= delta.tail(n).array();
        i_scaled.array() *= delta.head(n).array();
      }
 
      g *= c;
      H *= c;
    }
  }
  // modifies variables in place
  void scale_primal_in_place(VectorViewMut<T> primal) const
  {
    primal.to_eigen().array() /= delta.array().head(dim);
  }
  void scale_dual_in_place(VectorViewMut<T> dual) const
  {
    dual.to_eigen().array() = dual.as_const().to_eigen().array() /
                              delta.tail(delta.size() - dim).array() * c;
  }
  void scale_dual_in_place_eq(VectorViewMut<T> dual) const
  {
    dual.to_eigen().array() = dual.as_const().to_eigen().array() /
                              delta.middleRows(dim, n_eq).array() * c;
  }
  void scale_dual_in_place_in(VectorViewMut<T> dual) const
  {
    dual.to_eigen().array() = dual.as_const().to_eigen().array() /
                              delta.segment(dim + n_eq, n_in).array() * c;
  }
  void unscale_primal_in_place(VectorViewMut<T> primal) const
  {
    primal.to_eigen().array() *= delta.array().head(dim);
  }
  void unscale_dual_in_place(VectorViewMut<T> dual) const
  {
    dual.to_eigen().array() = dual.as_const().to_eigen().array() *
                              delta.tail(delta.size() - dim).array() / c;
  }
  void unscale_box_dual_in_place_in(VectorViewMut<T> dual) const
  {
    dual.to_eigen().array() =
      delta.tail(dim).array() * dual.as_const().to_eigen().array() / c;
  }
  void scale_box_dual_in_place_in(VectorViewMut<T> dual) const
  {
    dual.to_eigen().array() =
      dual.as_const().to_eigen().array() / delta.tail(dim).array() * c;
  }
  void unscale_dual_in_place_eq(VectorViewMut<T> dual) const
  {
    dual.to_eigen().array() = dual.as_const().to_eigen().array() *
                              delta.middleRows(dim, n_eq).array() / c;
  }
  void unscale_dual_in_place_in(VectorViewMut<T> dual) const
  {
    dual.to_eigen().array() = dual.as_const().to_eigen().array() *
                              delta.segment(dim + n_eq, n_in).array() / c;
  }
  // modifies residuals in place
  void scale_primal_residual_in_place(VectorViewMut<T> primal) const
  {
    primal.to_eigen().array() *= delta.tail(delta.size() - dim).array();
  }
 
  void scale_primal_residual_in_place_eq(VectorViewMut<T> primal_eq) const
  {
    primal_eq.to_eigen().array() *= delta.middleRows(dim, n_eq).array();
  }
  void scale_primal_residual_in_place_in(VectorViewMut<T> primal_in) const
  {
    primal_in.to_eigen().array() *= delta.segment(dim + n_eq, n_in).array();
  }
  void scale_box_primal_residual_in_place_in(VectorViewMut<T> primal_in) const
  {
    primal_in.to_eigen().array() *= delta.tail(dim).array();
  }
  void scale_dual_residual_in_place(VectorViewMut<T> dual) const
  {
    dual.to_eigen().array() *= delta.head(dim).array() * c;
  }
  void unscale_primal_residual_in_place(VectorViewMut<T> primal) const
  {
    primal.to_eigen().array() /= delta.tail(delta.size() - dim).array();
  }
  void unscale_box_primal_residual_in_place(VectorViewMut<T> primal) const
  {
    primal.to_eigen().array() /= delta.tail(dim).array();
  }
  void unscale_primal_residual_in_place_eq(VectorViewMut<T> primal_eq) const
  {
    primal_eq.to_eigen().array() /= delta.middleRows(dim, n_eq).array();
  }
  void unscale_primal_residual_in_place_in(VectorViewMut<T> primal_in) const
  {
    primal_in.to_eigen().array() /= delta.middleRows(dim + n_eq, n_in).array();
  }
  void unscale_box_primal_residual_in_place_in(VectorViewMut<T> primal_in) const
  {
    primal_in.to_eigen().array() /= delta.tail(dim).array();
  }
  void unscale_dual_residual_in_place(VectorViewMut<T> dual) const
  {
    dual.to_eigen().array() /= delta.head(dim).array() * c;
  }
};
 
template<typename T>
bool
operator==(const RuizEquilibration<T>& ruiz1, const RuizEquilibration<T>& ruiz2)
{
  bool value =
    // ruiz1.delta == ruiz2.delta &&
    ruiz1.c == ruiz2.c
    // ruiz1.dim == ruiz2.dim
    // ruiz1.epsilon == ruiz2.epsilon &&
    // ruiz1.max_iter == ruiz2.max_iter &&
    // ruiz1.sym == ruiz2.sym &&
    // ruiz1.logger_ptr == ruiz2.logger_ptr
    ;
  return value;
}
 
template<typename T>
bool
operator!=(const RuizEquilibration<T>& ruiz1, const RuizEquilibration<T>& ruiz2)
{
  return !(ruiz1 == ruiz2);
}
 
} // namespace preconditioner
} // namespace dense
} // namespace proxqp
} // namespace proxsuite
 
#endif /* end of include guard PROXSUITE_PROXQP_DENSE_PRECOND_RUIZ_HPP */