filter.hpp

// Copyright (c) Sleipnir contributors
 
#pragma once
 
#include <algorithm>
#include <cmath>
 
#include <Eigen/Core>
#include <gch/small_vector.hpp>
 
// See docs/algorithms.md#Works_cited for citation definitions.
 
namespace slp {
 
template <typename Scalar>
struct FilterEntry {
  using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
 
  Scalar cost{0};
 
  Scalar constraint_violation{0};
 
  constexpr FilterEntry() = default;
 
  explicit constexpr FilterEntry(Scalar cost,
                                 Scalar constraint_violation = Scalar(0))
      : cost{cost}, constraint_violation{constraint_violation} {}
 
  FilterEntry(Scalar f, const DenseVector& c_e)
      : FilterEntry{f, c_e.template lpNorm<1>()} {}
 
  FilterEntry(Scalar f, DenseVector& s, const DenseVector& c_e,
              const DenseVector& c_i, Scalar μ)
      : FilterEntry{f - μ * s.array().log().sum(),
                    c_e.template lpNorm<1>() + (c_i - s).template lpNorm<1>()} {
  }
 
  constexpr bool dominated_by(const FilterEntry<Scalar>& entry) const {
    return entry.cost <= cost &&
           entry.constraint_violation <= constraint_violation;
  }
};
 
template <typename Scalar>
class Filter {
 public:
  Scalar min_constraint_violation;
 
  Scalar max_constraint_violation;
 
  explicit constexpr Filter(Scalar initial_constraint_violation = Scalar(0)) {
    using std::max;
 
    min_constraint_violation =
        Scalar(1e-4) * max(Scalar(1), initial_constraint_violation);
    max_constraint_violation =
        Scalar(1e4) * max(Scalar(1), initial_constraint_violation);
  }
 
  void reset() {
    m_filter.clear();
    m_last_rejection_due_to_filter = false;
  }
 
  bool try_add(const FilterEntry<Scalar>& current_entry,
               const FilterEntry<Scalar>& trial_entry, Scalar D_ϕ, Scalar α) {
    using std::isfinite;
    using std::pow;
 
    // Reject steps with nonfinite cost or constraint violation above maximum
    if (!isfinite(trial_entry.cost) ||
        trial_entry.constraint_violation > max_constraint_violation) {
      return false;
    }
 
    // Switching condition
    constexpr Scalar s_ϕ(2.3);
    constexpr Scalar s_θ(1.1);
    bool switching_condition =
        D_ϕ < Scalar(0) &&
        α * pow(-D_ϕ, s_ϕ) > pow(current_entry.constraint_violation, s_θ);
 
    // Armijo condition
    constexpr Scalar η_ϕ(1e-8);
    bool armijo_condition =
        trial_entry.cost <= current_entry.cost + η_ϕ * α * D_ϕ;
 
    // Sufficient decrease condition
    bool sufficient_decrease =
        trial_entry.cost <=
            current_entry.cost - γ_cost * current_entry.constraint_violation ||
        trial_entry.constraint_violation <=
            (Scalar(1) - γ_constraint) * current_entry.constraint_violation;
 
    // If constraint violation is below threshold and switching condition is
    // true, check Armijo condition for step rejection. Otherwise, check
    // sufficient decrease condition.
    if (current_entry.constraint_violation <= min_constraint_violation &&
        switching_condition) {
      if (!armijo_condition) {
        m_last_rejection_due_to_filter = false;
        return false;
      }
    } else if (!sufficient_decrease) {
      m_last_rejection_due_to_filter = false;
      return false;
    }
 
    // Reject steps in filter (i.e., dominated by any filter entry)
    if (in_filter(trial_entry)) {
      m_last_rejection_due_to_filter = true;
      return false;
    }
 
    // Augment filter with accepted iterate if switching condition or Armijo
    // condition are false
    if (!switching_condition || !armijo_condition) {
      add(FilterEntry{
          current_entry.cost - γ_cost * current_entry.constraint_violation,
          (Scalar(1) - γ_constraint) * current_entry.constraint_violation});
    }
 
    return true;
  }
 
  bool last_rejection_due_to_filter() const {
    return m_last_rejection_due_to_filter;
  }
 
 private:
  static constexpr Scalar γ_cost{1e-8};
  static constexpr Scalar γ_constraint{1e-5};
 
  gch::small_vector<FilterEntry<Scalar>> m_filter;
 
  bool m_last_rejection_due_to_filter = false;
 
  void add(const FilterEntry<Scalar>& entry) {
    // Remove dominated entries
    erase_if(m_filter,
             [&](const auto& elem) { return elem.dominated_by(entry); });
 
    m_filter.push_back(entry);
  }
 
  bool in_filter(const FilterEntry<Scalar>& entry) const {
    // An entry is in the filter if it's dominated by any filter entry
    return std::any_of(m_filter.begin(), m_filter.end(), [&](const auto& elem) {
      return entry.dominated_by(elem);
    });
  }
};
 
template <typename Scalar>
class StallDetector {
 public:
  bool update(const FilterEntry<Scalar>& current_entry,
              const FilterEntry<Scalar>& trial_entry, Scalar α,
              Scalar min_constraint_violation) {
    // If step is tiny, current iterate is infeasible, and accepted trial
    // iterate made insufficient progress toward feasibility, increment stalled
    // step counter
    if (α < Scalar(1e-3) &&
        current_entry.constraint_violation > min_constraint_violation &&
        trial_entry.constraint_violation >
            Scalar(0.99) * current_entry.constraint_violation) {
      ++m_stalled_step_counter;
 
      // If there were too many consecutive tiny steps, report a stall
      if (m_stalled_step_counter >= 5) {
        m_stalled_step_counter = 0;
        return true;
      }
    } else {
      // A break in stalled steps resets the counter
      m_stalled_step_counter = 0;
    }
 
    return false;
  }
 
 private:
  int m_stalled_step_counter = 0;
};
 
}  // namespace slp