Skip to content

Commit

Permalink
Implement filter from restoration-free SQP
Browse files Browse the repository at this point in the history
  • Loading branch information
@calcmogul
calcmogul committed Jan 30, 2025
1 parent 79d3938 commit 2580b88
Show file tree
Hide file tree
Showing 5 changed files with 49 additions and 198 deletions.
2 changes: 2 additions & 0 deletions docs/algorithms.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -208,3 +208,5 @@ Section 6 of [^3] describes how to check for local infeasibility.
[^3]: Byrd, R. and Nocedal, J. and Waltz, R. "KNITRO: An Integrated Package for Nonlinear Optimization", 2005. [https://users.iems.northwestern.edu/~nocedal/PDFfiles/integrated.pdf](https://users.iems.northwestern.edu/~nocedal/PDFfiles/integrated.pdf)

[^4]: Gu, C. and Zhu, D. "A Dwindling Filter Algorithm with a Modified Subproblem for Nonlinear Inequality Constrained Optimization", 2014. [https://sci-hub.st/10.1007/s11401-014-0826-z](https://sci-hub.st/10.1007/s11401-014-0826-z)

[^5]: Zhu, X. and Pu, D. "A restoration-free filter SQP algorithm for equality constrained optimization", 2012. [https://sci-hub.st/10.1016/j.amc.2012.12.002](https://sci-hub.st/10.1016/j.amc.2012.12.002)
97 changes: 0 additions & 97 deletions src/optimization/solver/InteriorPoint.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -464,103 +464,6 @@ void InteriorPoint(
break;
}

double prevConstraintViolation =
c_e.lpNorm<1>() + (c_i - s).lpNorm<1>();
double nextConstraintViolation =
trial_c_e.lpNorm<1>() + (trial_c_i - trial_s).lpNorm<1>();

// Second-order corrections
//
// If first trial point was rejected and constraint violation stayed the
// same or went up, apply second-order corrections
if (nextConstraintViolation >= prevConstraintViolation) {
// Apply second-order corrections. See section 2.4 of [2].
Eigen::VectorXd p_x_cor = p_x;
Eigen::VectorXd p_y_soc = p_y;
Eigen::VectorXd p_z_soc = p_z;
Eigen::VectorXd p_s_soc = p_s;

double α_soc = α;
Eigen::VectorXd c_e_soc = c_e;

bool stepAcceptable = false;
for (int soc_iteration = 0; soc_iteration < 5 && !stepAcceptable;
++soc_iteration) {
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
std::chrono::steady_clock::time_point socIterStartTime;
if (config.diagnostics) {
socIterStartTime = std::chrono::steady_clock::now();
}
#endif

// Rebuild Newton-KKT rhs with updated constraint values.
//
// rhs = −[∇f − Aₑᵀy − Aᵢᵀ(−Σcᵢ + μS⁻¹e + z)]
// [ cₑˢᵒᶜ ]
//
// where cₑˢᵒᶜ = αc(xₖ) + c(xₖ + αpₖˣ)
c_e_soc = α_soc * c_e_soc + trial_c_e;
rhs.bottomRows(y.rows()) = -c_e_soc;

// Solve the Newton-KKT system
step = solver.Solve(rhs);

p_x_cor = step.segment(0, x.rows());
p_y_soc = -step.segment(x.rows(), y.rows());

// pₖˢ = cᵢ − s + Aᵢpₖˣ
p_s_soc = c_i - s + A_i * p_x_cor;

// pₖᶻ = −Σcᵢ + μS⁻¹e − ΣAᵢpₖˣ
p_z_soc = -Σ * c_i + μ * Sinv * e - Σ * A_i * p_x_cor;

// αˢᵒᶜ = max(α ∈ (0, 1] : sₖ + αpₖˢ ≥ (1−τⱼ)sₖ)
α_soc = FractionToTheBoundaryRule(s, p_s_soc, τ);
trial_x = x + α_soc * p_x_cor;
trial_s = s + α_soc * p_s_soc;

// αₖᶻ = max(α ∈ (0, 1] : zₖ + αpₖᶻ ≥ (1−τⱼ)zₖ)
double α_z_soc = FractionToTheBoundaryRule(z, p_z_soc, τ);
trial_y = y + α_z_soc * p_y_soc;
trial_z = z + α_z_soc * p_z_soc;

xAD.SetValue(trial_x);

trial_c_e = c_eAD.Value();
trial_c_i = c_iAD.Value();

// Check whether filter accepts trial iterate
entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i, μ);
if (filter.TryAdd(entry, α)) {
p_x = p_x_cor;
p_y = p_y_soc;
p_z = p_z_soc;
p_s = p_s_soc;
α = α_soc;
α_z = α_z_soc;
stepAcceptable = true;
}

#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
if (config.diagnostics) {
const auto socIterEndTime = std::chrono::steady_clock::now();

double E = ErrorEstimate(g, A_e, trial_c_e, trial_y);
PrintIterationDiagnostics(
iterations, IterationMode::kSecondOrderCorrection,
socIterEndTime - socIterStartTime, E, f.Value(),
trial_c_e.lpNorm<1>() + (trial_c_i - trial_s).lpNorm<1>(),
solver.HessianRegularization(), 1.0, 1.0);
}
#endif
}

if (stepAcceptable) {
// Accept step
break;
}
}

// If we got here and α is the full step, the full step was rejected.
// Increment the full-step rejected counter to keep track of how many
// full steps have been rejected in a row.
Expand Down
76 changes: 0 additions & 76 deletions src/optimization/solver/SQP.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -321,82 +321,6 @@ void SQP(
break;
}

double prevConstraintViolation = c_e.lpNorm<1>();
double nextConstraintViolation = trial_c_e.lpNorm<1>();

// Second-order corrections
//
// If first trial point was rejected and constraint violation stayed the
// same or went up, apply second-order corrections
if (nextConstraintViolation >= prevConstraintViolation) {
// Apply second-order corrections. See section 2.4 of [2].
Eigen::VectorXd p_x_cor = p_x;
Eigen::VectorXd p_y_soc = p_y;

double α_soc = α;
Eigen::VectorXd c_e_soc = c_e;

bool stepAcceptable = false;
for (int soc_iteration = 0; soc_iteration < 5 && !stepAcceptable;
++soc_iteration) {
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
std::chrono::steady_clock::time_point socIterStartTime;
if (config.diagnostics) {
socIterStartTime = std::chrono::steady_clock::now();
}
#endif

// Rebuild Newton-KKT rhs with updated constraint values.
//
// rhs = −[∇f − Aₑᵀy]
// [ cₑˢᵒᶜ ]
//
// where cₑˢᵒᶜ = αc(xₖ) + c(xₖ + αpₖˣ)
c_e_soc = α_soc * c_e_soc + trial_c_e;
rhs.bottomRows(y.rows()) = -c_e_soc;

// Solve the Newton-KKT system
step = solver.Solve(rhs);

p_x_cor = step.segment(0, x.rows());
p_y_soc = -step.segment(x.rows(), y.rows());

trial_x = x + α_soc * p_x_cor;
trial_y = y + α_soc * p_y_soc;

xAD.SetValue(trial_x);

trial_c_e = c_eAD.Value();

// Check whether filter accepts trial iterate
entry = filter.MakeEntry(trial_c_e);
if (filter.TryAdd(entry, α)) {
p_x = p_x_cor;
p_y = p_y_soc;
α = α_soc;
stepAcceptable = true;
}

#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
if (config.diagnostics) {
const auto socIterEndTime = std::chrono::steady_clock::now();

double E = ErrorEstimate(g, A_e, trial_c_e, trial_y);
PrintIterationDiagnostics(
iterations, IterationMode::kSecondOrderCorrection,
socIterEndTime - socIterStartTime, E, f.Value(),
trial_c_e.lpNorm<1>(), solver.HessianRegularization(), 1.0,
0.0);
}
#endif
}

if (stepAcceptable) {
// Accept step
break;
}
}

// If we got here and α is the full step, the full step was rejected.
// Increment the full-step rejected counter to keep track of how many
// full steps have been rejected in a row.
Expand Down
68 changes: 46 additions & 22 deletions src/optimization/solver/util/Filter.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,7 @@ class Filter {
public:
static constexpr double γCost = 1e-8;
static constexpr double γConstraint = 1e-5;
static constexpr int maxEntries = 40;

double maxConstraintViolation = 1e4;

Expand Down Expand Up @@ -108,13 +109,16 @@ class Filter {
* @param entry The entry to add to the filter.
*/
void Add(const FilterEntry& entry) {
// Remove dominated entries
erase_if(m_filter, [&](const auto& elem) {
return entry.cost <= elem.cost &&
entry.constraintViolation <= elem.constraintViolation;
});

m_filter.push_back(entry);
m_currentIterate = entry;
m_filter.push_back(m_currentIterate);

if (m_filter.size() > maxEntries) {
m_filter.erase(std::max_element(m_filter.begin(), m_filter.end(),
[](const auto& lhs, const auto& rhs) {
return lhs.constraintViolation <
rhs.constraintViolation;
}));
}
}

/**
Expand All @@ -123,13 +127,16 @@ class Filter {
* @param entry The entry to add to the filter.
*/
void Add(FilterEntry&& entry) {
// Remove dominated entries
erase_if(m_filter, [&](const auto& elem) {
return entry.cost <= elem.cost &&
entry.constraintViolation <= elem.constraintViolation;
});

m_filter.push_back(entry);
m_currentIterate = entry;
m_filter.push_back(m_currentIterate);

if (m_filter.size() > maxEntries) {
m_filter.erase(std::max_element(m_filter.begin(), m_filter.end(),
[](const auto& lhs, const auto& rhs) {
return lhs.constraintViolation <
rhs.constraintViolation;
}));
}
}

/**
Expand Down Expand Up @@ -176,20 +183,37 @@ class Filter {

double ϕ = std::pow(α, 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.constraintViolation ||
entry.constraintViolation <=
// See equation (2.13) of [4] and equations (2.7) to (2.11) of [5].

// If current filter entry isn't acceptable to current iterate, reject it
if (entry.cost >
m_currentIterate.cost - ϕ * γCost * entry.constraintViolation &&
entry.constraintViolation >
(1.0 - ϕ * γConstraint) * m_currentIterate.constraintViolation) {
return false;
}

size_t dominatedEntries =
std::ranges::count_if(m_filter, [&](const auto& elem) {
return entry.constraintViolation <=
(1.0 - ϕ * γConstraint) * elem.constraintViolation;
});
});
// If trial iterate is infeasible, require at least two dominated filter
// entries. Otherwise, only require at least one.
if (m_currentIterate.constraintViolation > 1e-8) {
dominatedEntries +=
entry.constraintViolation <=
(1.0 - ϕ * γConstraint) * m_currentIterate.constraintViolation;
return dominatedEntries >= 2;
} else {
return dominatedEntries >= 1;
}
}

private:
Variable* m_f = nullptr;
small_vector<FilterEntry> m_filter;
FilterEntry m_currentIterate;
};

} // namespace sleipnir
4 changes: 1 addition & 3 deletions src/util/PrintIterationDiagnostics.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,8 +22,6 @@ namespace sleipnir {
enum class IterationMode : uint8_t {
/// Normal iteration.
kNormal,
/// Second-order correction iteration.
kSecondOrderCorrection,
/// Feasibility restoration iteration.
kFeasibilityRestoration
};
Expand Down Expand Up @@ -54,7 +52,7 @@ void PrintIterationDiagnostics(int iterations, IterationMode mode,
sleipnir::println("{:=^96}", "");
}

constexpr const char* kIterationModes[] = {" ", "s", "r"};
constexpr const char* kIterationModes[] = {" ", "r"};
sleipnir::print("{:4}{} {:9.3f} {:13e} {:13e} {:13e} ", iterations,
kIterationModes[std::to_underlying(mode)],
ToMilliseconds(time), error, cost, infeasibility);
Expand Down

0 comments on commit 2580b88

Please sign in to comment.