Sleipnir C++ API
Loading...
Searching...
No Matches
filter.hpp
1// Copyright (c) Sleipnir contributors
2
3#pragma once
4
5#include <algorithm>
6#include <cmath>
7#include <limits>
8#include <utility>
9
10#include <Eigen/Core>
11#include <gch/small_vector.hpp>
12
13// See docs/algorithms.md#Works_cited for citation definitions.
14
15namespace slp {
16
20template <typename Scalar>
23 using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
24
26 Scalar cost{0};
27
30
31 constexpr FilterEntry() = default;
32
39
43 explicit FilterEntry(Scalar f) : FilterEntry{f, Scalar(0)} {}
44
49 FilterEntry(Scalar f, const DenseVector& c_e)
50 : FilterEntry{f, c_e.template lpNorm<1>()} {}
51
59 FilterEntry(Scalar f, DenseVector& s, const DenseVector& c_e,
60 const DenseVector& c_i, Scalar μ)
61 : FilterEntry{f - μ * s.array().log().sum(),
62 c_e.template lpNorm<1>() + (c_i - s).template lpNorm<1>()} {
63 }
64};
65
71template <typename Scalar>
72class Filter {
73 public:
76
79 // Initial filter entry rejects constraint violations above max
80 m_filter.emplace_back(std::numeric_limits<Scalar>::infinity(),
82 }
83
85 void reset() {
86 m_filter.clear();
87
88 // Initial filter entry rejects constraint violations above max
89 m_filter.emplace_back(std::numeric_limits<Scalar>::infinity(),
91 }
92
97 // Remove dominated entries
98 erase_if(m_filter, [&](const auto& elem) {
99 return entry.cost <= elem.cost &&
100 entry.constraint_violation <= elem.constraint_violation;
101 });
102
103 m_filter.push_back(entry);
104 }
105
110 // Remove dominated entries
111 erase_if(m_filter, [&](const auto& elem) {
112 return entry.cost <= elem.cost &&
113 entry.constraint_violation <= elem.constraint_violation;
114 });
115
116 m_filter.push_back(entry);
117 }
118
124 bool try_add(const FilterEntry<Scalar>& entry, Scalar α) {
125 if (is_acceptable(entry, α)) {
126 add(entry);
127 return true;
128 } else {
129 return false;
130 }
131 }
132
139 if (is_acceptable(entry, α)) {
140 add(std::move(entry));
141 return true;
142 } else {
143 return false;
144 }
145 }
146
153 using std::isfinite;
154 using std::pow;
155
156 if (!isfinite(entry.cost) || !isfinite(entry.constraint_violation)) {
157 return false;
158 }
159
160 Scalar ϕ = pow(α, Scalar(1.5));
161
162 // If current filter entry is better than all prior ones in some respect,
163 // accept it.
164 //
165 // See equation (2.13) of [4].
166 return std::ranges::all_of(m_filter, [&](const auto& elem) {
167 return entry.cost <= elem.cost - ϕ * γ_cost * elem.constraint_violation ||
168 entry.constraint_violation <=
169 (Scalar(1) - ϕ * γ_constraint) * elem.constraint_violation;
170 });
171 }
172
176 const FilterEntry<Scalar>& back() const { return m_filter.back(); }
177
178 private:
179 static constexpr Scalar γ_cost{1e-8};
180 static constexpr Scalar γ_constraint{1e-5};
181
182 gch::small_vector<FilterEntry<Scalar>> m_filter;
183};
184
185} // namespace slp
Definition filter.hpp:72
void add(const FilterEntry< Scalar > &entry)
Definition filter.hpp:96
const FilterEntry< Scalar > & back() const
Definition filter.hpp:176
bool is_acceptable(const FilterEntry< Scalar > &entry, Scalar α)
Definition filter.hpp:152
Filter()
Constructs an empty filter.
Definition filter.hpp:78
bool try_add(const FilterEntry< Scalar > &entry, Scalar α)
Definition filter.hpp:124
bool try_add(FilterEntry< Scalar > &&entry, Scalar α)
Definition filter.hpp:138
Scalar max_constraint_violation
The maximum constraint violation.
Definition filter.hpp:75
void reset()
Resets the filter.
Definition filter.hpp:85
void add(FilterEntry< Scalar > &&entry)
Definition filter.hpp:109
Definition intrusive_shared_ptr.hpp:27
Definition filter.hpp:21
FilterEntry(Scalar f)
Definition filter.hpp:43
Scalar cost
The cost function's value.
Definition filter.hpp:26
FilterEntry(Scalar f, DenseVector &s, const DenseVector &c_e, const DenseVector &c_i, Scalar μ)
Definition filter.hpp:59
Eigen::Vector< Scalar, Eigen::Dynamic > DenseVector
Type alias for dense vector.
Definition filter.hpp:23
Scalar constraint_violation
The constraint violation.
Definition filter.hpp:29
constexpr FilterEntry(Scalar cost, Scalar constraint_violation)
Definition filter.hpp:37
FilterEntry(Scalar f, const DenseVector &c_e)
Definition filter.hpp:49