filter.hpp

// Copyright (c) Sleipnir contributors
 
#pragma once
 
#include <algorithm>
#include <cmath>
#include <limits>
#include <utility>
 
#include <Eigen/Core>
#include <gch/small_vector.hpp>
 
// See docs/algorithms.md#Works_cited for citation definitions.
 
namespace slp {
 
template <typename Scalar>
struct FilterEntry {
  Scalar cost{0};
 
  Scalar constraint_violation{0};
 
  constexpr FilterEntry() = default;
 
  constexpr FilterEntry(Scalar cost, Scalar constraint_violation)
      : cost{cost}, constraint_violation{constraint_violation} {}
 
  explicit FilterEntry(Scalar f) : FilterEntry{f, Scalar(0)} {}
 
  FilterEntry(Scalar f, const Eigen::Vector<Scalar, Eigen::Dynamic>& c_e)
      : FilterEntry{f, c_e.template lpNorm<1>()} {}
 
  FilterEntry(Scalar f, Eigen::Vector<Scalar, Eigen::Dynamic>& s,
              const Eigen::Vector<Scalar, Eigen::Dynamic>& c_e,
              const Eigen::Vector<Scalar, Eigen::Dynamic>& c_i, Scalar μ)
      : FilterEntry{f - μ * s.array().log().sum(),
                    c_e.template lpNorm<1>() + (c_i - s).template lpNorm<1>()} {
  }
};
 
template <typename Scalar>
class Filter {
 public:
  Scalar max_constraint_violation{1e4};
 
  Filter() {
    // Initial filter entry rejects constraint violations above max
    m_filter.emplace_back(std::numeric_limits<Scalar>::infinity(),
                          max_constraint_violation);
  }
 
  void reset() {
    m_filter.clear();
 
    // Initial filter entry rejects constraint violations above max
    m_filter.emplace_back(std::numeric_limits<Scalar>::infinity(),
                          max_constraint_violation);
  }
 
  void add(const FilterEntry<Scalar>& entry) {
    // Remove dominated entries
    erase_if(m_filter, [&](const auto& elem) {
      return entry.cost <= elem.cost &&
             entry.constraint_violation <= elem.constraint_violation;
    });
 
    m_filter.push_back(entry);
  }
 
  void add(FilterEntry<Scalar>&& entry) {
    // Remove dominated entries
    erase_if(m_filter, [&](const auto& elem) {
      return entry.cost <= elem.cost &&
             entry.constraint_violation <= elem.constraint_violation;
    });
 
    m_filter.push_back(entry);
  }
 
  bool try_add(const FilterEntry<Scalar>& entry, Scalar α) {
    if (is_acceptable(entry, α)) {
      add(entry);
      return true;
    } else {
      return false;
    }
  }
 
  bool try_add(FilterEntry<Scalar>&& entry, Scalar α) {
    if (is_acceptable(entry, α)) {
      add(std::move(entry));
      return true;
    } else {
      return false;
    }
  }
 
  bool is_acceptable(const FilterEntry<Scalar>& entry, Scalar α) {
    using std::isfinite;
    using std::pow;
 
    if (!isfinite(entry.cost) || !isfinite(entry.constraint_violation)) {
      return false;
    }
 
    Scalar ϕ = pow(α, Scalar(1.5));
 
    // If current filter entry is better than all prior ones in some respect,
    // accept it.
    //
    // See equation (2.13) of [4].
    return std::ranges::all_of(m_filter, [&](const auto& elem) {
      return entry.cost <= elem.cost - ϕ * γ_cost * elem.constraint_violation ||
             entry.constraint_violation <=
                 (Scalar(1) - ϕ * γ_constraint) * elem.constraint_violation;
    });
  }
 
  const FilterEntry<Scalar>& back() const { return m_filter.back(); }
 
 private:
  static constexpr Scalar γ_cost{1e-8};
  static constexpr Scalar γ_constraint{1e-5};
 
  gch::small_vector<FilterEntry<Scalar>> m_filter;
};
 
}  // namespace slp