mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-29 02:21:44 +00:00
[upstream_utils] Upgrade Sleipnir (#7512)
It now uses SQP for problems without inequality constraints, which is faster. main: ``` [ RUN ] Ellipse2dTest.DistanceToPoint 0.203 ms [ OK ] Ellipse2dTest.DistanceToPoint (0 ms) [ RUN ] Ellipse2dTest.FindNearestPoint 0.019 ms [ OK ] Ellipse2dTest.FindNearestPoint (0 ms) ``` upgrade: ``` [ RUN ] Ellipse2dTest.DistanceToPoint 0.197 ms [ OK ] Ellipse2dTest.DistanceToPoint (0 ms) [ RUN ] Ellipse2dTest.FindNearestPoint 0.015 ms [ OK ] Ellipse2dTest.FindNearestPoint (0 ms) ```
This commit is contained in:
Tyler Veness
@@ -20,6 +20,7 @@
|
||||
#include "sleipnir/optimization/SolverIterationInfo.hpp"
|
||||
#include "sleipnir/optimization/SolverStatus.hpp"
|
||||
#include "sleipnir/optimization/solver/InteriorPoint.hpp"
|
||||
#include "sleipnir/optimization/solver/SQP.hpp"
|
||||
#include "sleipnir/util/Print.hpp"
|
||||
#include "sleipnir/util/SymbolExports.hpp"
|
||||
|
||||
@@ -305,10 +306,15 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem {
|
||||
}
|
||||
|
||||
// Solve the optimization problem
|
||||
Eigen::VectorXd s = Eigen::VectorXd::Ones(m_inequalityConstraints.size());
|
||||
InteriorPoint(m_decisionVariables, m_equalityConstraints,
|
||||
m_inequalityConstraints, m_f.value(), m_callback, config,
|
||||
false, x, s, &status);
|
||||
if (m_inequalityConstraints.empty()) {
|
||||
SQP(m_decisionVariables, m_equalityConstraints, m_f.value(), m_callback,
|
||||
config, x, &status);
|
||||
} else {
|
||||
Eigen::VectorXd s = Eigen::VectorXd::Ones(m_inequalityConstraints.size());
|
||||
InteriorPoint(m_decisionVariables, m_equalityConstraints,
|
||||
m_inequalityConstraints, m_f.value(), m_callback, config,
|
||||
false, x, s, &status);
|
||||
}
|
||||
|
||||
if (config.diagnostics) {
|
||||
sleipnir::println("Exit condition: {}", ToMessage(status.exitCondition));
|
||||
|
||||
46
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/SQP.hpp
vendored
Normal file
46
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/SQP.hpp
vendored
Normal file
@@ -0,0 +1,46 @@
|
||||
// Copyright (c) Sleipnir contributors
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <span>
|
||||
|
||||
#include <Eigen/Core>
|
||||
|
||||
#include "sleipnir/autodiff/Variable.hpp"
|
||||
#include "sleipnir/optimization/SolverConfig.hpp"
|
||||
#include "sleipnir/optimization/SolverIterationInfo.hpp"
|
||||
#include "sleipnir/optimization/SolverStatus.hpp"
|
||||
#include "sleipnir/util/FunctionRef.hpp"
|
||||
#include "sleipnir/util/SymbolExports.hpp"
|
||||
|
||||
namespace sleipnir {
|
||||
|
||||
/**
|
||||
Finds the optimal solution to a nonlinear program using Sequential Quadratic
|
||||
Programming (SQP).
|
||||
|
||||
A nonlinear program has the form:
|
||||
|
||||
@verbatim
|
||||
min_x f(x)
|
||||
subject to cₑ(x) = 0
|
||||
@endverbatim
|
||||
|
||||
where f(x) is the cost function and cₑ(x) are the equality constraints.
|
||||
|
||||
@param[in] decisionVariables The list of decision variables.
|
||||
@param[in] equalityConstraints The list of equality constraints.
|
||||
@param[in] f The cost function.
|
||||
@param[in] callback The user callback.
|
||||
@param[in] config Configuration options for the solver.
|
||||
@param[in,out] x The initial guess and output location for the decision
|
||||
variables.
|
||||
@param[out] status The solver status.
|
||||
*/
|
||||
SLEIPNIR_DLLEXPORT void SQP(
|
||||
std::span<Variable> decisionVariables,
|
||||
std::span<Variable> equalityConstraints, Variable& f,
|
||||
function_ref<bool(const SolverIterationInfo& info)> callback,
|
||||
const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status);
|
||||
|
||||
} // namespace sleipnir
|
||||
@@ -195,7 +195,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
|
||||
// Fraction-to-the-boundary rule scale factor τ
|
||||
double τ = τ_min;
|
||||
|
||||
Filter filter{f, μ};
|
||||
Filter filter{f};
|
||||
|
||||
// This should be run when the error estimate is below a desired threshold for
|
||||
// the current barrier parameter
|
||||
@@ -222,7 +222,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
|
||||
τ = std::max(τ_min, 1.0 - μ);
|
||||
|
||||
// Reset the filter when the barrier parameter is updated
|
||||
filter.Reset(μ);
|
||||
filter.Reset();
|
||||
};
|
||||
|
||||
// Kept outside the loop so its storage can be reused
|
||||
@@ -372,6 +372,9 @@ void InteriorPoint(std::span<Variable> decisionVariables,
|
||||
rhs.segment(x.rows(), y.rows()) = -c_e;
|
||||
|
||||
// Solve the Newton-KKT system
|
||||
//
|
||||
// [H + AᵢᵀΣAᵢ Aₑᵀ][ pₖˣ] = −[∇f − Aₑᵀy + Aᵢᵀ(S⁻¹(Zcᵢ − μe) − z)]
|
||||
// [ Aₑ 0 ][−pₖʸ] [ cₑ ]
|
||||
solver.Compute(lhs, equalityConstraints.size(), μ);
|
||||
Eigen::VectorXd step{x.rows() + y.rows()};
|
||||
if (solver.Info() == Eigen::Success) {
|
||||
@@ -435,7 +438,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
|
||||
}
|
||||
|
||||
// Check whether filter accepts trial iterate
|
||||
auto entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i);
|
||||
auto entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i, μ);
|
||||
if (filter.TryAdd(entry)) {
|
||||
// Accept step
|
||||
break;
|
||||
@@ -499,7 +502,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
|
||||
trial_c_i = c_iAD.Value();
|
||||
|
||||
// Check whether filter accepts trial iterate
|
||||
entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i);
|
||||
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;
|
||||
@@ -531,7 +534,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
|
||||
if (fullStepRejectedCounter >= 4 &&
|
||||
filter.maxConstraintViolation > entry.constraintViolation / 10.0) {
|
||||
filter.maxConstraintViolation *= 0.1;
|
||||
filter.Reset(μ);
|
||||
filter.Reset();
|
||||
continue;
|
||||
}
|
||||
|
||||
@@ -580,7 +583,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
|
||||
return;
|
||||
}
|
||||
|
||||
auto initialEntry = filter.MakeEntry(s, c_e, c_i);
|
||||
auto initialEntry = filter.MakeEntry(s, c_e, c_i, μ);
|
||||
|
||||
// Feasibility restoration phase
|
||||
Eigen::VectorXd fr_x = x;
|
||||
@@ -603,7 +606,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
|
||||
// If current iterate is acceptable to normal filter and
|
||||
// constraint violation has sufficiently reduced, stop
|
||||
// feasibility restoration
|
||||
auto entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i);
|
||||
auto entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i, μ);
|
||||
if (filter.IsAcceptable(entry) &&
|
||||
entry.constraintViolation <
|
||||
0.9 * initialEntry.constraintViolation) {
|
||||
|
||||
559
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/SQP.cpp
vendored
Normal file
559
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/SQP.cpp
vendored
Normal file
@@ -0,0 +1,559 @@
|
||||
// Copyright (c) Sleipnir contributors
|
||||
|
||||
#include "sleipnir/optimization/solver/SQP.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <fstream>
|
||||
#include <limits>
|
||||
|
||||
#include <Eigen/SparseCholesky>
|
||||
#include <wpi/SmallVector.h>
|
||||
|
||||
#include "optimization/RegularizedLDLT.hpp"
|
||||
#include "optimization/solver/util/ErrorEstimate.hpp"
|
||||
#include "optimization/solver/util/FeasibilityRestoration.hpp"
|
||||
#include "optimization/solver/util/Filter.hpp"
|
||||
#include "optimization/solver/util/IsLocallyInfeasible.hpp"
|
||||
#include "optimization/solver/util/KKTError.hpp"
|
||||
#include "sleipnir/autodiff/Gradient.hpp"
|
||||
#include "sleipnir/autodiff/Hessian.hpp"
|
||||
#include "sleipnir/autodiff/Jacobian.hpp"
|
||||
#include "sleipnir/optimization/SolverExitCondition.hpp"
|
||||
#include "sleipnir/util/Print.hpp"
|
||||
#include "sleipnir/util/Spy.hpp"
|
||||
#include "util/ScopeExit.hpp"
|
||||
#include "util/ToMilliseconds.hpp"
|
||||
|
||||
// See docs/algorithms.md#Works_cited for citation definitions.
|
||||
|
||||
namespace sleipnir {
|
||||
|
||||
void SQP(std::span<Variable> decisionVariables,
|
||||
std::span<Variable> equalityConstraints, Variable& f,
|
||||
function_ref<bool(const SolverIterationInfo& info)> callback,
|
||||
const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status) {
|
||||
const auto solveStartTime = std::chrono::system_clock::now();
|
||||
|
||||
// Map decision variables and constraints to VariableMatrices for Lagrangian
|
||||
VariableMatrix xAD{decisionVariables};
|
||||
xAD.SetValue(x);
|
||||
VariableMatrix c_eAD{equalityConstraints};
|
||||
|
||||
// Create autodiff variables for y for Lagrangian
|
||||
VariableMatrix yAD(equalityConstraints.size());
|
||||
for (auto& y : yAD) {
|
||||
y.SetValue(0.0);
|
||||
}
|
||||
|
||||
// Lagrangian L
|
||||
//
|
||||
// L(xₖ, yₖ) = f(xₖ) − yₖᵀcₑ(xₖ)
|
||||
auto L = f - (yAD.T() * c_eAD)(0);
|
||||
|
||||
// Equality constraint Jacobian Aₑ
|
||||
//
|
||||
// [∇ᵀcₑ₁(xₖ)]
|
||||
// Aₑ(x) = [∇ᵀcₑ₂(xₖ)]
|
||||
// [ ⋮ ]
|
||||
// [∇ᵀcₑₘ(xₖ)]
|
||||
Jacobian jacobianCe{c_eAD, xAD};
|
||||
Eigen::SparseMatrix<double> A_e = jacobianCe.Value();
|
||||
|
||||
// Gradient of f ∇f
|
||||
Gradient gradientF{f, xAD};
|
||||
Eigen::SparseVector<double> g = gradientF.Value();
|
||||
|
||||
// Hessian of the Lagrangian H
|
||||
//
|
||||
// Hₖ = ∇²ₓₓL(xₖ, yₖ)
|
||||
Hessian hessianL{L, xAD};
|
||||
Eigen::SparseMatrix<double> H = hessianL.Value();
|
||||
|
||||
Eigen::VectorXd y = yAD.Value();
|
||||
Eigen::VectorXd c_e = c_eAD.Value();
|
||||
|
||||
// Check for overconstrained problem
|
||||
if (equalityConstraints.size() > decisionVariables.size()) {
|
||||
if (config.diagnostics) {
|
||||
sleipnir::println("The problem has too few degrees of freedom.");
|
||||
sleipnir::println(
|
||||
"Violated constraints (cₑ(x) = 0) in order of declaration:");
|
||||
for (int row = 0; row < c_e.rows(); ++row) {
|
||||
if (c_e(row) < 0.0) {
|
||||
sleipnir::println(" {}/{}: {} = 0", row + 1, c_e.rows(), c_e(row));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
status->exitCondition = SolverExitCondition::kTooFewDOFs;
|
||||
return;
|
||||
}
|
||||
|
||||
// Check whether initial guess has finite f(xₖ) and cₑ(xₖ)
|
||||
if (!std::isfinite(f.Value()) || !c_e.allFinite()) {
|
||||
status->exitCondition =
|
||||
SolverExitCondition::kNonfiniteInitialCostOrConstraints;
|
||||
return;
|
||||
}
|
||||
|
||||
// Sparsity pattern files written when spy flag is set in SolverConfig
|
||||
std::ofstream H_spy;
|
||||
std::ofstream A_e_spy;
|
||||
if (config.spy) {
|
||||
A_e_spy.open("A_e.spy");
|
||||
H_spy.open("H.spy");
|
||||
}
|
||||
|
||||
if (config.diagnostics) {
|
||||
sleipnir::println("Error tolerance: {}\n", config.tolerance);
|
||||
}
|
||||
|
||||
std::chrono::system_clock::time_point iterationsStartTime;
|
||||
|
||||
int iterations = 0;
|
||||
|
||||
// Prints final diagnostics when the solver exits
|
||||
scope_exit exit{[&] {
|
||||
status->cost = f.Value();
|
||||
|
||||
if (config.diagnostics) {
|
||||
auto solveEndTime = std::chrono::system_clock::now();
|
||||
|
||||
sleipnir::println("\nSolve time: {:.3f} ms",
|
||||
ToMilliseconds(solveEndTime - solveStartTime));
|
||||
sleipnir::println(" ↳ {:.3f} ms (solver setup)",
|
||||
ToMilliseconds(iterationsStartTime - solveStartTime));
|
||||
if (iterations > 0) {
|
||||
sleipnir::println(
|
||||
" ↳ {:.3f} ms ({} solver iterations; {:.3f} ms average)",
|
||||
ToMilliseconds(solveEndTime - iterationsStartTime), iterations,
|
||||
ToMilliseconds((solveEndTime - iterationsStartTime) / iterations));
|
||||
}
|
||||
sleipnir::println("");
|
||||
|
||||
sleipnir::println("{:^8} {:^10} {:^14} {:^6}", "autodiff",
|
||||
"setup (ms)", "avg solve (ms)", "solves");
|
||||
sleipnir::println("{:=^47}", "");
|
||||
constexpr auto format = "{:^8} {:10.3f} {:14.3f} {:6}";
|
||||
sleipnir::println(format, "∇f(x)",
|
||||
gradientF.GetProfiler().SetupDuration(),
|
||||
gradientF.GetProfiler().AverageSolveDuration(),
|
||||
gradientF.GetProfiler().SolveMeasurements());
|
||||
sleipnir::println(format, "∇²ₓₓL", hessianL.GetProfiler().SetupDuration(),
|
||||
hessianL.GetProfiler().AverageSolveDuration(),
|
||||
hessianL.GetProfiler().SolveMeasurements());
|
||||
sleipnir::println(format, "∂cₑ/∂x",
|
||||
jacobianCe.GetProfiler().SetupDuration(),
|
||||
jacobianCe.GetProfiler().AverageSolveDuration(),
|
||||
jacobianCe.GetProfiler().SolveMeasurements());
|
||||
sleipnir::println("");
|
||||
}
|
||||
}};
|
||||
|
||||
Filter filter{f};
|
||||
|
||||
// Kept outside the loop so its storage can be reused
|
||||
wpi::SmallVector<Eigen::Triplet<double>> triplets;
|
||||
|
||||
RegularizedLDLT solver;
|
||||
|
||||
// Variables for determining when a step is acceptable
|
||||
constexpr double α_red_factor = 0.5;
|
||||
int acceptableIterCounter = 0;
|
||||
|
||||
int fullStepRejectedCounter = 0;
|
||||
|
||||
// Error estimate
|
||||
double E_0 = std::numeric_limits<double>::infinity();
|
||||
|
||||
if (config.diagnostics) {
|
||||
iterationsStartTime = std::chrono::system_clock::now();
|
||||
}
|
||||
|
||||
while (E_0 > config.tolerance &&
|
||||
acceptableIterCounter < config.maxAcceptableIterations) {
|
||||
std::chrono::system_clock::time_point innerIterStartTime;
|
||||
if (config.diagnostics) {
|
||||
innerIterStartTime = std::chrono::system_clock::now();
|
||||
}
|
||||
|
||||
// Check for local equality constraint infeasibility
|
||||
if (IsEqualityLocallyInfeasible(A_e, c_e)) {
|
||||
if (config.diagnostics) {
|
||||
sleipnir::println(
|
||||
"The problem is locally infeasible due to violated equality "
|
||||
"constraints.");
|
||||
sleipnir::println(
|
||||
"Violated constraints (cₑ(x) = 0) in order of declaration:");
|
||||
for (int row = 0; row < c_e.rows(); ++row) {
|
||||
if (c_e(row) < 0.0) {
|
||||
sleipnir::println(" {}/{}: {} = 0", row + 1, c_e.rows(), c_e(row));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
status->exitCondition = SolverExitCondition::kLocallyInfeasible;
|
||||
return;
|
||||
}
|
||||
|
||||
// Check for diverging iterates
|
||||
if (x.lpNorm<Eigen::Infinity>() > 1e20 || !x.allFinite()) {
|
||||
status->exitCondition = SolverExitCondition::kDivergingIterates;
|
||||
return;
|
||||
}
|
||||
|
||||
// Write out spy file contents if that's enabled
|
||||
if (config.spy) {
|
||||
// Gap between sparsity patterns
|
||||
if (iterations > 0) {
|
||||
A_e_spy << "\n";
|
||||
H_spy << "\n";
|
||||
}
|
||||
|
||||
Spy(H_spy, H);
|
||||
Spy(A_e_spy, A_e);
|
||||
}
|
||||
|
||||
// Call user callback
|
||||
if (callback({iterations, x, Eigen::VectorXd::Zero(0), g, H, A_e,
|
||||
Eigen::SparseMatrix<double>{}})) {
|
||||
status->exitCondition = SolverExitCondition::kCallbackRequestedStop;
|
||||
return;
|
||||
}
|
||||
|
||||
// lhs = [H Aₑᵀ]
|
||||
// [Aₑ 0 ]
|
||||
//
|
||||
// Don't assign upper triangle because solver only uses lower triangle.
|
||||
const Eigen::SparseMatrix<double> topLeft =
|
||||
H.triangularView<Eigen::Lower>();
|
||||
triplets.clear();
|
||||
triplets.reserve(topLeft.nonZeros() + A_e.nonZeros());
|
||||
for (int col = 0; col < H.cols(); ++col) {
|
||||
// Append column of H + AᵢᵀΣAᵢ lower triangle in top-left quadrant
|
||||
for (Eigen::SparseMatrix<double>::InnerIterator it{topLeft, col}; it;
|
||||
++it) {
|
||||
triplets.emplace_back(it.row(), it.col(), it.value());
|
||||
}
|
||||
// Append column of Aₑ in bottom-left quadrant
|
||||
for (Eigen::SparseMatrix<double>::InnerIterator it{A_e, col}; it; ++it) {
|
||||
triplets.emplace_back(H.rows() + it.row(), it.col(), it.value());
|
||||
}
|
||||
}
|
||||
Eigen::SparseMatrix<double> lhs(
|
||||
decisionVariables.size() + equalityConstraints.size(),
|
||||
decisionVariables.size() + equalityConstraints.size());
|
||||
lhs.setFromSortedTriplets(triplets.begin(), triplets.end(),
|
||||
[](const auto&, const auto& b) { return b; });
|
||||
|
||||
// rhs = −[∇f − Aₑᵀy]
|
||||
// [ cₑ ]
|
||||
Eigen::VectorXd rhs{x.rows() + y.rows()};
|
||||
rhs.segment(0, x.rows()) = -(g - A_e.transpose() * y);
|
||||
rhs.segment(x.rows(), y.rows()) = -c_e;
|
||||
|
||||
// Solve the Newton-KKT system
|
||||
//
|
||||
// [H Aₑᵀ][ pₖˣ] = −[∇f − Aₑᵀy]
|
||||
// [Aₑ 0 ][−pₖʸ] [ cₑ ]
|
||||
solver.Compute(lhs, equalityConstraints.size(), config.tolerance / 10.0);
|
||||
Eigen::VectorXd step{x.rows() + y.rows()};
|
||||
if (solver.Info() == Eigen::Success) {
|
||||
step = solver.Solve(rhs);
|
||||
} else {
|
||||
// The regularization procedure failed due to a rank-deficient equality
|
||||
// constraint Jacobian with linearly dependent constraints
|
||||
status->exitCondition = SolverExitCondition::kLocallyInfeasible;
|
||||
return;
|
||||
}
|
||||
|
||||
// step = [ pₖˣ]
|
||||
// [−pₖʸ]
|
||||
Eigen::VectorXd p_x = step.segment(0, x.rows());
|
||||
Eigen::VectorXd p_y = -step.segment(x.rows(), y.rows());
|
||||
|
||||
constexpr double α_max = 1.0;
|
||||
double α = α_max;
|
||||
|
||||
// Loop until a step is accepted. If a step becomes acceptable, the loop
|
||||
// will exit early.
|
||||
while (1) {
|
||||
Eigen::VectorXd trial_x = x + α * p_x;
|
||||
Eigen::VectorXd trial_y = y + α * p_y;
|
||||
|
||||
xAD.SetValue(trial_x);
|
||||
|
||||
Eigen::VectorXd trial_c_e = c_eAD.Value();
|
||||
|
||||
// If f(xₖ + αpₖˣ) or cₑ(xₖ + αpₖˣ) aren't finite, reduce step size
|
||||
// immediately
|
||||
if (!std::isfinite(f.Value()) || !trial_c_e.allFinite()) {
|
||||
// Reduce step size
|
||||
α *= α_red_factor;
|
||||
continue;
|
||||
}
|
||||
|
||||
// Check whether filter accepts trial iterate
|
||||
auto entry = filter.MakeEntry(trial_c_e);
|
||||
if (filter.TryAdd(entry)) {
|
||||
// Accept step
|
||||
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) {
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
|
||||
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.
|
||||
if (α == α_max) {
|
||||
++fullStepRejectedCounter;
|
||||
}
|
||||
|
||||
// If the full step was rejected enough times in a row, reset the filter
|
||||
// because it may be impeding progress.
|
||||
//
|
||||
// See section 3.2 case I of [2].
|
||||
if (fullStepRejectedCounter >= 4 &&
|
||||
filter.maxConstraintViolation > entry.constraintViolation / 10.0) {
|
||||
filter.maxConstraintViolation *= 0.1;
|
||||
filter.Reset();
|
||||
continue;
|
||||
}
|
||||
|
||||
// Reduce step size
|
||||
α *= α_red_factor;
|
||||
|
||||
// Safety factor for the minimal step size
|
||||
constexpr double α_min_frac = 0.05;
|
||||
|
||||
// If step size hit a minimum, check if the KKT error was reduced. If it
|
||||
// wasn't, report infeasible.
|
||||
if (α < α_min_frac * Filter::γConstraint) {
|
||||
double currentKKTError = KKTError(g, A_e, c_e, y);
|
||||
|
||||
Eigen::VectorXd trial_x = x + α_max * p_x;
|
||||
Eigen::VectorXd trial_y = y + α_max * p_y;
|
||||
|
||||
// Upate autodiff
|
||||
xAD.SetValue(trial_x);
|
||||
yAD.SetValue(trial_y);
|
||||
|
||||
Eigen::VectorXd trial_c_e = c_eAD.Value();
|
||||
|
||||
double nextKKTError =
|
||||
KKTError(gradientF.Value(), jacobianCe.Value(), trial_c_e, trial_y);
|
||||
|
||||
// If the step using αᵐᵃˣ reduced the KKT error, accept it anyway
|
||||
if (nextKKTError <= 0.999 * currentKKTError) {
|
||||
α = α_max;
|
||||
|
||||
// Accept step
|
||||
break;
|
||||
}
|
||||
|
||||
auto initialEntry = filter.MakeEntry(c_e);
|
||||
|
||||
// Feasibility restoration phase
|
||||
Eigen::VectorXd fr_x = x;
|
||||
SolverStatus fr_status;
|
||||
FeasibilityRestoration(
|
||||
decisionVariables, equalityConstraints,
|
||||
[&](const SolverIterationInfo& info) {
|
||||
trial_x = info.x.segment(0, decisionVariables.size());
|
||||
xAD.SetValue(trial_x);
|
||||
|
||||
trial_c_e = c_eAD.Value();
|
||||
|
||||
// If current iterate is acceptable to normal filter and
|
||||
// constraint violation has sufficiently reduced, stop
|
||||
// feasibility restoration
|
||||
entry = filter.MakeEntry(trial_c_e);
|
||||
if (filter.IsAcceptable(entry) &&
|
||||
entry.constraintViolation <
|
||||
0.9 * initialEntry.constraintViolation) {
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
},
|
||||
config, fr_x, &fr_status);
|
||||
|
||||
if (fr_status.exitCondition ==
|
||||
SolverExitCondition::kCallbackRequestedStop) {
|
||||
p_x = fr_x - x;
|
||||
|
||||
// Lagrange mutliplier estimates
|
||||
//
|
||||
// y = (AₑAₑᵀ)⁻¹Aₑ∇f
|
||||
//
|
||||
// See equation (19.37) of [1].
|
||||
{
|
||||
xAD.SetValue(fr_x);
|
||||
|
||||
A_e = jacobianCe.Value();
|
||||
g = gradientF.Value();
|
||||
|
||||
// lhs = AₑAₑᵀ
|
||||
Eigen::SparseMatrix<double> lhs = A_e * A_e.transpose();
|
||||
|
||||
// rhs = Aₑ∇f
|
||||
Eigen::VectorXd rhs = A_e * g;
|
||||
|
||||
Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> yEstimator{lhs};
|
||||
Eigen::VectorXd sol = yEstimator.solve(rhs);
|
||||
|
||||
p_y = y - sol.block(0, 0, y.rows(), 1);
|
||||
}
|
||||
|
||||
α = 1.0;
|
||||
|
||||
// Accept step
|
||||
break;
|
||||
} else if (fr_status.exitCondition == SolverExitCondition::kSuccess) {
|
||||
status->exitCondition = SolverExitCondition::kLocallyInfeasible;
|
||||
x = fr_x;
|
||||
return;
|
||||
} else {
|
||||
status->exitCondition =
|
||||
SolverExitCondition::kFeasibilityRestorationFailed;
|
||||
x = fr_x;
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// If full step was accepted, reset full-step rejected counter
|
||||
if (α == α_max) {
|
||||
fullStepRejectedCounter = 0;
|
||||
}
|
||||
|
||||
// Handle very small search directions by letting αₖ = αₖᵐᵃˣ when
|
||||
// max(|pₖˣ(i)|/(1 + |xₖ(i)|)) < 10ε_mach.
|
||||
//
|
||||
// See section 3.9 of [2].
|
||||
double maxStepScaled = 0.0;
|
||||
for (int row = 0; row < x.rows(); ++row) {
|
||||
maxStepScaled = std::max(maxStepScaled,
|
||||
std::abs(p_x(row)) / (1.0 + std::abs(x(row))));
|
||||
}
|
||||
if (maxStepScaled < 10.0 * std::numeric_limits<double>::epsilon()) {
|
||||
α = α_max;
|
||||
}
|
||||
|
||||
// xₖ₊₁ = xₖ + αₖpₖˣ
|
||||
// yₖ₊₁ = yₖ + αₖpₖʸ
|
||||
x += α * p_x;
|
||||
y += α * p_y;
|
||||
|
||||
// Update autodiff for Jacobians and Hessian
|
||||
xAD.SetValue(x);
|
||||
yAD.SetValue(y);
|
||||
A_e = jacobianCe.Value();
|
||||
g = gradientF.Value();
|
||||
H = hessianL.Value();
|
||||
|
||||
c_e = c_eAD.Value();
|
||||
|
||||
// Update the error estimate
|
||||
E_0 = ErrorEstimate(g, A_e, c_e, y);
|
||||
if (E_0 < config.acceptableTolerance) {
|
||||
++acceptableIterCounter;
|
||||
} else {
|
||||
acceptableIterCounter = 0;
|
||||
}
|
||||
|
||||
const auto innerIterEndTime = std::chrono::system_clock::now();
|
||||
|
||||
// Diagnostics for current iteration
|
||||
if (config.diagnostics) {
|
||||
if (iterations % 20 == 0) {
|
||||
sleipnir::println("{:^4} {:^9} {:^13} {:^13} {:^13}", "iter",
|
||||
"time (ms)", "error", "cost", "infeasibility");
|
||||
sleipnir::println("{:=^61}", "");
|
||||
}
|
||||
|
||||
sleipnir::println("{:4} {:9.3f} {:13e} {:13e} {:13e}", iterations,
|
||||
ToMilliseconds(innerIterEndTime - innerIterStartTime),
|
||||
E_0, f.Value(), c_e.lpNorm<1>());
|
||||
}
|
||||
|
||||
++iterations;
|
||||
|
||||
// Check for max iterations
|
||||
if (iterations >= config.maxIterations) {
|
||||
status->exitCondition = SolverExitCondition::kMaxIterationsExceeded;
|
||||
return;
|
||||
}
|
||||
|
||||
// Check for max wall clock time
|
||||
if (innerIterEndTime - solveStartTime > config.timeout) {
|
||||
status->exitCondition = SolverExitCondition::kTimeout;
|
||||
return;
|
||||
}
|
||||
|
||||
// Check for solve to acceptable tolerance
|
||||
if (E_0 > config.tolerance &&
|
||||
acceptableIterCounter == config.maxAcceptableIterations) {
|
||||
status->exitCondition = SolverExitCondition::kSolvedToAcceptableTolerance;
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace sleipnir
|
||||
@@ -11,6 +11,42 @@
|
||||
|
||||
namespace sleipnir {
|
||||
|
||||
/**
|
||||
* Returns the error estimate using the KKT conditions for SQP.
|
||||
*
|
||||
* @param g Gradient of the cost function ∇f.
|
||||
* @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
|
||||
* current iterate.
|
||||
* @param c_e The problem's equality constraints cₑ(x) evaluated at the current
|
||||
* iterate.
|
||||
* @param y Equality constraint dual variables.
|
||||
*/
|
||||
inline double ErrorEstimate(const Eigen::VectorXd& g,
|
||||
const Eigen::SparseMatrix<double>& A_e,
|
||||
const Eigen::VectorXd& c_e,
|
||||
const Eigen::VectorXd& y) {
|
||||
int numEqualityConstraints = A_e.rows();
|
||||
|
||||
// Update the error estimate using the KKT conditions from equations (19.5a)
|
||||
// through (19.5d) of [1].
|
||||
//
|
||||
// ∇f − Aₑᵀy = 0
|
||||
// cₑ = 0
|
||||
//
|
||||
// The error tolerance is the max of the following infinity norms scaled by
|
||||
// s_d (see equation (5) of [2]).
|
||||
//
|
||||
// ‖∇f − Aₑᵀy‖_∞ / s_d
|
||||
// ‖cₑ‖_∞
|
||||
|
||||
// s_d = max(sₘₐₓ, ‖y‖₁ / m) / sₘₐₓ
|
||||
constexpr double s_max = 100.0;
|
||||
double s_d = std::max(s_max, y.lpNorm<1>() / numEqualityConstraints) / s_max;
|
||||
|
||||
return std::max({(g - A_e.transpose() * y).lpNorm<Eigen::Infinity>() / s_d,
|
||||
c_e.lpNorm<Eigen::Infinity>()});
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the error estimate using the KKT conditions for the interior-point
|
||||
* method.
|
||||
|
||||
@@ -20,6 +20,168 @@
|
||||
|
||||
namespace sleipnir {
|
||||
|
||||
/**
|
||||
* Finds the iterate that minimizes the constraint violation while not deviating
|
||||
* too far from the starting point. This is a fallback procedure when the normal
|
||||
* Sequential Quadratic Programming method fails to converge to a feasible
|
||||
* point.
|
||||
*
|
||||
* @param[in] decisionVariables The list of decision variables.
|
||||
* @param[in] equalityConstraints The list of equality constraints.
|
||||
* @param[in] callback The user callback.
|
||||
* @param[in] config Configuration options for the solver.
|
||||
* @param[in,out] x The current iterate from the normal solve.
|
||||
* @param[out] status The solver status.
|
||||
*/
|
||||
inline void FeasibilityRestoration(
|
||||
std::span<Variable> decisionVariables,
|
||||
std::span<Variable> equalityConstraints,
|
||||
function_ref<bool(const SolverIterationInfo& info)> callback,
|
||||
const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status) {
|
||||
// Feasibility restoration
|
||||
//
|
||||
// min ρ Σ (pₑ + nₑ) + ζ/2 (x - x_R)ᵀD_R(x - x_R)
|
||||
// x
|
||||
// pₑ,nₑ
|
||||
//
|
||||
// s.t. cₑ(x) - pₑ + nₑ = 0
|
||||
// pₑ ≥ 0
|
||||
// nₑ ≥ 0
|
||||
//
|
||||
// where ρ = 1000, ζ = √μ where μ is the barrier parameter, x_R is original
|
||||
// iterate before feasibility restoration, and D_R is a scaling matrix defined
|
||||
// by
|
||||
//
|
||||
// D_R = diag(min(1, 1/|x_R⁽¹⁾|), …, min(1, 1/|x_R|⁽ⁿ⁾)
|
||||
|
||||
constexpr double ρ = 1000.0;
|
||||
double μ = config.tolerance / 10.0;
|
||||
|
||||
wpi::SmallVector<Variable> fr_decisionVariables;
|
||||
fr_decisionVariables.reserve(decisionVariables.size() +
|
||||
2 * equalityConstraints.size());
|
||||
|
||||
// Assign x
|
||||
fr_decisionVariables.assign(decisionVariables.begin(),
|
||||
decisionVariables.end());
|
||||
|
||||
// Allocate pₑ and nₑ
|
||||
for (size_t row = 0; row < 2 * equalityConstraints.size(); ++row) {
|
||||
fr_decisionVariables.emplace_back();
|
||||
}
|
||||
|
||||
auto it = fr_decisionVariables.begin();
|
||||
|
||||
VariableMatrix xAD{std::span{it, it + decisionVariables.size()}};
|
||||
it += decisionVariables.size();
|
||||
|
||||
VariableMatrix p_e{std::span{it, it + equalityConstraints.size()}};
|
||||
it += equalityConstraints.size();
|
||||
|
||||
VariableMatrix n_e{std::span{it, it + equalityConstraints.size()}};
|
||||
it += equalityConstraints.size();
|
||||
|
||||
// Set initial values for pₑ and nₑ.
|
||||
//
|
||||
//
|
||||
// From equation (33) of [2]:
|
||||
// ______________________
|
||||
// μ − ρ c(x) /(μ − ρ c(x))² μ c(x)
|
||||
// n = −−−−−−−−−− + / (−−−−−−−−−−) + −−−−−− (1)
|
||||
// 2ρ √ ( 2ρ ) 2ρ
|
||||
//
|
||||
// The quadratic formula:
|
||||
// ________
|
||||
// -b + √b² - 4ac
|
||||
// x = −−−−−−−−−−−−−− (2)
|
||||
// 2a
|
||||
//
|
||||
// Rearrange (1) to fit the quadratic formula better:
|
||||
// _________________________
|
||||
// μ - ρ c(x) + √(μ - ρ c(x))² + 2ρ μ c(x)
|
||||
// n = −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
|
||||
// 2ρ
|
||||
//
|
||||
// Solve for coefficients:
|
||||
//
|
||||
// a = ρ (3)
|
||||
// b = ρ c(x) - μ (4)
|
||||
//
|
||||
// -4ac = μ c(x) 2ρ
|
||||
// -4(ρ)c = 2ρ μ c(x)
|
||||
// -4c = 2μ c(x)
|
||||
// c = -μ c(x)/2 (5)
|
||||
//
|
||||
// p = c(x) + n (6)
|
||||
for (int row = 0; row < p_e.Rows(); ++row) {
|
||||
double c_e = equalityConstraints[row].Value();
|
||||
|
||||
constexpr double a = 2 * ρ;
|
||||
double b = ρ * c_e - μ;
|
||||
double c = -μ * c_e / 2.0;
|
||||
|
||||
double n = -b * std::sqrt(b * b - 4.0 * a * c) / (2.0 * a);
|
||||
double p = c_e + n;
|
||||
|
||||
p_e(row).SetValue(p);
|
||||
n_e(row).SetValue(n);
|
||||
}
|
||||
|
||||
// cₑ(x) - pₑ + nₑ = 0
|
||||
wpi::SmallVector<Variable> fr_equalityConstraints;
|
||||
fr_equalityConstraints.assign(equalityConstraints.begin(),
|
||||
equalityConstraints.end());
|
||||
for (size_t row = 0; row < fr_equalityConstraints.size(); ++row) {
|
||||
auto& constraint = fr_equalityConstraints[row];
|
||||
constraint = constraint - p_e(row) + n_e(row);
|
||||
}
|
||||
|
||||
// cᵢ(x) - s - pᵢ + nᵢ = 0
|
||||
wpi::SmallVector<Variable> fr_inequalityConstraints;
|
||||
|
||||
// pₑ ≥ 0
|
||||
std::copy(p_e.begin(), p_e.end(),
|
||||
std::back_inserter(fr_inequalityConstraints));
|
||||
|
||||
// nₑ ≥ 0
|
||||
std::copy(n_e.begin(), n_e.end(),
|
||||
std::back_inserter(fr_inequalityConstraints));
|
||||
|
||||
Variable J = 0.0;
|
||||
|
||||
// J += ρ Σ (pₑ + nₑ)
|
||||
for (auto& elem : p_e) {
|
||||
J += elem;
|
||||
}
|
||||
for (auto& elem : n_e) {
|
||||
J += elem;
|
||||
}
|
||||
J *= ρ;
|
||||
|
||||
// D_R = diag(min(1, 1/|x_R⁽¹⁾|), …, min(1, 1/|x_R|⁽ⁿ⁾)
|
||||
Eigen::VectorXd D_R{x.rows()};
|
||||
for (int row = 0; row < D_R.rows(); ++row) {
|
||||
D_R(row) = std::min(1.0, 1.0 / std::abs(x(row)));
|
||||
}
|
||||
|
||||
// J += ζ/2 (x - x_R)ᵀD_R(x - x_R)
|
||||
for (int row = 0; row < x.rows(); ++row) {
|
||||
J += std::sqrt(μ) / 2.0 * D_R(row) * sleipnir::pow(xAD(row) - x(row), 2);
|
||||
}
|
||||
|
||||
Eigen::VectorXd fr_x = VariableMatrix{fr_decisionVariables}.Value();
|
||||
|
||||
// Set up initial value for inequality constraint slack variables
|
||||
Eigen::VectorXd fr_s{fr_inequalityConstraints.size()};
|
||||
fr_s.setOnes();
|
||||
|
||||
InteriorPoint(fr_decisionVariables, fr_equalityConstraints,
|
||||
fr_inequalityConstraints, J, callback, config, true, fr_x, fr_s,
|
||||
status);
|
||||
|
||||
x = fr_x.segment(0, decisionVariables.size());
|
||||
}
|
||||
|
||||
/**
|
||||
* Finds the iterate that minimizes the constraint violation while not deviating
|
||||
* too far from the starting point. This is a fallback procedure when the normal
|
||||
|
||||
@@ -12,6 +12,8 @@
|
||||
|
||||
#include "sleipnir/autodiff/Variable.hpp"
|
||||
|
||||
// See docs/algorithms.md#Works_cited for citation definitions.
|
||||
|
||||
namespace sleipnir {
|
||||
|
||||
/**
|
||||
@@ -32,26 +34,14 @@ struct FilterEntry {
|
||||
* @param cost The cost function's value.
|
||||
* @param constraintViolation The constraint violation.
|
||||
*/
|
||||
FilterEntry(double cost, double constraintViolation)
|
||||
constexpr FilterEntry(double cost, double constraintViolation)
|
||||
: cost{cost}, constraintViolation{constraintViolation} {}
|
||||
|
||||
/**
|
||||
* Constructs a FilterEntry.
|
||||
*
|
||||
* @param f The cost function.
|
||||
* @param μ The barrier parameter.
|
||||
* @param s The inequality constraint slack variables.
|
||||
* @param c_e The equality constraint values (nonzero means violation).
|
||||
* @param c_i The inequality constraint values (negative means violation).
|
||||
*/
|
||||
FilterEntry(Variable& f, double μ, const Eigen::VectorXd& s,
|
||||
const Eigen::VectorXd& c_e, const Eigen::VectorXd& c_i)
|
||||
: cost{f.Value() - μ * s.array().log().sum()},
|
||||
constraintViolation{c_e.lpNorm<1>() + (c_i - s).lpNorm<1>()} {}
|
||||
};
|
||||
|
||||
/**
|
||||
* Interior-point step filter.
|
||||
* Step filter.
|
||||
*
|
||||
* See the section on filters in chapter 15 of [1].
|
||||
*/
|
||||
class Filter {
|
||||
public:
|
||||
@@ -64,11 +54,9 @@ class Filter {
|
||||
* Construct an empty filter.
|
||||
*
|
||||
* @param f The cost function.
|
||||
* @param μ The barrier parameter.
|
||||
*/
|
||||
explicit Filter(Variable& f, double μ) {
|
||||
explicit Filter(Variable& f) {
|
||||
m_f = &f;
|
||||
m_μ = μ;
|
||||
|
||||
// Initial filter entry rejects constraint violations above max
|
||||
m_filter.emplace_back(std::numeric_limits<double>::infinity(),
|
||||
@@ -77,11 +65,8 @@ class Filter {
|
||||
|
||||
/**
|
||||
* Reset the filter.
|
||||
*
|
||||
* @param μ The new barrier parameter.
|
||||
*/
|
||||
void Reset(double μ) {
|
||||
m_μ = μ;
|
||||
void Reset() {
|
||||
m_filter.clear();
|
||||
|
||||
// Initial filter entry rejects constraint violations above max
|
||||
@@ -90,15 +75,26 @@ class Filter {
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a new filter entry.
|
||||
* Creates a new Sequential Quadratic Programming filter entry.
|
||||
*
|
||||
* @param c_e The equality constraint values (nonzero means violation).
|
||||
*/
|
||||
FilterEntry MakeEntry(const Eigen::VectorXd& c_e) {
|
||||
return FilterEntry{m_f->Value(), c_e.lpNorm<1>()};
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a new interior-point method filter entry.
|
||||
*
|
||||
* @param s The inequality constraint slack variables.
|
||||
* @param c_e The equality constraint values (nonzero means violation).
|
||||
* @param c_i The inequality constraint values (negative means violation).
|
||||
* @param μ The barrier parameter.
|
||||
*/
|
||||
FilterEntry MakeEntry(Eigen::VectorXd& s, const Eigen::VectorXd& c_e,
|
||||
const Eigen::VectorXd& c_i) {
|
||||
return FilterEntry{*m_f, m_μ, s, c_e, c_i};
|
||||
const Eigen::VectorXd& c_i, double μ) {
|
||||
return FilterEntry{m_f->Value() - μ * s.array().log().sum(),
|
||||
c_e.lpNorm<1>() + (c_i - s).lpNorm<1>()};
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -181,7 +177,6 @@ class Filter {
|
||||
|
||||
private:
|
||||
Variable* m_f = nullptr;
|
||||
double m_μ = 0.0;
|
||||
wpi::SmallVector<FilterEntry> m_filter;
|
||||
};
|
||||
|
||||
|
||||
@@ -9,6 +9,28 @@
|
||||
|
||||
namespace sleipnir {
|
||||
|
||||
/**
|
||||
* Returns the KKT error for Sequential Quadratic Programming.
|
||||
*
|
||||
* @param g Gradient of the cost function ∇f.
|
||||
* @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
|
||||
* current iterate.
|
||||
* @param c_e The problem's equality constraints cₑ(x) evaluated at the current
|
||||
* iterate.
|
||||
* @param y Equality constraint dual variables.
|
||||
*/
|
||||
inline double KKTError(const Eigen::VectorXd& g,
|
||||
const Eigen::SparseMatrix<double>& A_e,
|
||||
const Eigen::VectorXd& c_e, const Eigen::VectorXd& y) {
|
||||
// Compute the KKT error as the 1-norm of the KKT conditions from equations
|
||||
// (19.5a) through (19.5d) of [1].
|
||||
//
|
||||
// ∇f − Aₑᵀy = 0
|
||||
// cₑ = 0
|
||||
|
||||
return (g - A_e.transpose() * y).lpNorm<1>() + c_e.lpNorm<1>();
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the KKT error for the interior-point method.
|
||||
*
|
||||
|
||||
Reference in New Issue
Block a user