Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[upstream_utils] Upgrade to Sleipnir 0.6.0 (#8923)

Browse Source
This commit is contained in:
Tyler Veness
2026-05-27 21:26:42 -07:00
committed by GitHub
parent 7ff32d7756
commit f1d95ee5f8
37 changed files with 1356 additions and 554 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
wpimath/src/main/java/org/wpilib/math/autodiff/VariableMatrix.java
View File

@@ -904,11 +904,11 @@ public class VariableMatrix implements AutoCloseable, Iterable<Variable> {
}
/**
* Returns a variable matrix filled with zeroes.
* Returns a variable matrix filled with zeros.
*
* @param rows The number of matrix rows.
* @param cols The number of matrix columns.
* @return A variable matrix filled with zeroes.
* @return A variable matrix filled with zeros.
*/
public static VariableMatrix zero(int rows, int cols) {
return new VariableMatrix(new SimpleMatrix(rows, cols));

47
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression.hpp vendored
View File

@@ -153,14 +153,16 @@ struct Expression {
// Prune expression
if (lhs->is_constant(Scalar(0))) {
// Return zero
// Return zero, which lhs currently is
return lhs;
} else if (rhs->is_constant(Scalar(0))) {
// Return zero
// Return zero, which rhs currently is
return rhs;
} else if (lhs->is_constant(Scalar(1))) {
// Return rhs unmodified
return rhs;
} else if (rhs->is_constant(Scalar(1))) {
// Return lhs unmodified
return lhs;
}
@@ -203,9 +205,10 @@ struct Expression {
// Prune expression
if (lhs->is_constant(Scalar(0))) {
// Return zero
// Return zero, which lhs currently is
return lhs;
} else if (rhs->is_constant(Scalar(1))) {
// Return lhs unmodified
return lhs;
}
@@ -236,10 +239,13 @@ struct Expression {
const ExpressionPtr<Scalar>& rhs) {
using enum ExpressionType;
// Prune expression
// Prune expression. We check for nullptr because operator+ is used in
// adjoint accumulation, and child nodes can be null.
if (lhs == nullptr || lhs->is_constant(Scalar(0))) {
// Return rhs unmodified
return rhs;
} else if (rhs == nullptr || rhs->is_constant(Scalar(0))) {
// Return lhs unmodified
return lhs;
}
@@ -281,12 +287,14 @@ struct Expression {
// Prune expression
if (lhs->is_constant(Scalar(0))) {
if (rhs->is_constant(Scalar(0))) {
// Return zero
// Return zero, which rhs currently is
return rhs;
} else {
// Return rhs negated
return -rhs;
}
} else if (rhs->is_constant(Scalar(0))) {
// Return lhs unmodified
return lhs;
}
@@ -316,7 +324,7 @@ struct Expression {
// Prune expression
if (lhs->is_constant(Scalar(0))) {
// Return zero
// Return zero, which lhs currently is
return lhs;
}
@@ -825,7 +833,7 @@ ExpressionPtr<Scalar> abs(const ExpressionPtr<Scalar>& x) {
// Prune expression
if (x->is_constant(Scalar(0))) {
// Return zero
// Return zero, which x currently is
return x;
}
@@ -934,7 +942,7 @@ ExpressionPtr<Scalar> asin(const ExpressionPtr<Scalar>& x) {
// Prune expression
if (x->is_constant(Scalar(0))) {
// Return zero
// Return zero, which x currently is
return x;
}
@@ -988,7 +996,7 @@ ExpressionPtr<Scalar> atan(const ExpressionPtr<Scalar>& x) {
// Prune expression
if (x->is_constant(Scalar(0))) {
// Return zero
// Return zero, which x currently is
return x;
}
@@ -1056,7 +1064,7 @@ ExpressionPtr<Scalar> atan2(const ExpressionPtr<Scalar>& y,
// Prune expression
if (y->is_constant(Scalar(0))) {
// Return zero
// Return zero, which y currently is
return y;
} else if (x->is_constant(Scalar(0))) {
return constant_ptr(Scalar(std::numbers::pi) / Scalar(2));
@@ -1223,7 +1231,7 @@ ExpressionPtr<Scalar> erf(const ExpressionPtr<Scalar>& x) {
// Prune expression
if (x->is_constant(Scalar(0))) {
// Return zero
// Return zero, which x currently is
return x;
}
@@ -1406,7 +1414,7 @@ ExpressionPtr<Scalar> log(const ExpressionPtr<Scalar>& x) {
// Prune expression
if (x->is_constant(Scalar(0))) {
// Return zero
// Return zero, which x currently is
return x;
}
@@ -1460,7 +1468,7 @@ ExpressionPtr<Scalar> log10(const ExpressionPtr<Scalar>& x) {
// Prune expression
if (x->is_constant(Scalar(0))) {
// Return zero
// Return zero, which x currently is
return x;
}
@@ -1708,15 +1716,16 @@ ExpressionPtr<Scalar> pow(const ExpressionPtr<Scalar>& base,
// Prune expression
if (base->is_constant(Scalar(0))) {
// Return zero
// Return zero, which base currently is
return base;
} else if (base->is_constant(Scalar(1))) {
// Return one
// Return one, which base currently is
return base;
}
if (power->is_constant(Scalar(0))) {
return constant_ptr(Scalar(1));
} else if (power->is_constant(Scalar(1))) {
// Return base unmodified
return base;
}
@@ -1828,7 +1837,7 @@ ExpressionPtr<Scalar> sin(const ExpressionPtr<Scalar>& x) {
// Prune expression
if (x->is_constant(Scalar(0))) {
// Return zero
// Return zero, which x currently is
return x;
}
@@ -1883,7 +1892,7 @@ ExpressionPtr<Scalar> sinh(const ExpressionPtr<Scalar>& x) {
// Prune expression
if (x->is_constant(Scalar(0))) {
// Return zero
// Return zero, which x currently is
return x;
}
@@ -1997,7 +2006,7 @@ ExpressionPtr<Scalar> tan(const ExpressionPtr<Scalar>& x) {
// Prune expression
if (x->is_constant(Scalar(0))) {
// Return zero
// Return zero, which x currently is
return x;
}
@@ -2055,7 +2064,7 @@ ExpressionPtr<Scalar> tanh(const ExpressionPtr<Scalar>& x) {
// Prune expression
if (x->is_constant(Scalar(0))) {
// Return zero
// Return zero, which x currently is
return x;
}

37
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient_expression_graph.hpp vendored
View File

@@ -102,22 +102,11 @@ class GradientExpressionGraph {
///
/// @param triplets The sparse matrix triplets.
/// @param row The row of wrt.
/// @param wrt Vector of variables with respect to which to compute the
/// Jacobian.
void append_triplets(gch::small_vector<Eigen::Triplet<Scalar>>& triplets,
int row, const VariableMatrix<Scalar>& wrt) const {
slp_assert(wrt.cols() == 1);
int row) const {
// Read docs/algorithms.md#Reverse_accumulation_automatic_differentiation
// for background on reverse accumulation automatic differentiation.
// If wrt has fewer nodes than graph, zero wrt's adjoints
if (static_cast<size_t>(wrt.rows()) < m_top_list.size()) {
for (const auto& elem : wrt) {
elem.expr->adjoint = Scalar(0);
}
}
if (m_top_list.empty()) {
return;
}
@@ -150,25 +139,13 @@ class GradientExpressionGraph {
}
}
// If wrt has fewer nodes than graph, iterate over wrt
if (static_cast<size_t>(wrt.rows()) < m_top_list.size()) {
for (int col = 0; col < wrt.rows(); ++col) {
const auto& node = wrt[col].expr;
for (size_t i = 0; i < m_top_list.size(); ++i) {
const auto& col = m_col_list[i];
const auto& node = m_top_list[i];
// Append adjoints of wrt to sparse matrix triplets
if (node->adjoint != Scalar(0)) {
triplets.emplace_back(row, col, node->adjoint);
}
}
} else {
for (size_t i = 0; i < m_top_list.size(); ++i) {
const auto& col = m_col_list[i];
const auto& node = m_top_list[i];
// Append adjoints of wrt to sparse matrix triplets
if (col != -1 && node->adjoint != Scalar(0)) {
triplets.emplace_back(row, col, node->adjoint);
}
// Append adjoints of wrt to sparse matrix triplets
if (col != -1) {
triplets.emplace_back(row, col, node->adjoint);
}
}
}

5
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/hessian.hpp vendored
View File

@@ -23,6 +23,7 @@ namespace slp {
///
/// @tparam Scalar Scalar type.
/// @tparam UpLo Which part of the Hessian to compute (Lower or Lower | Upper).
/// Default is Lower | Upper.
template <typename Scalar, int UpLo>
requires(UpLo == Eigen::Lower) || (UpLo == (Eigen::Lower | Eigen::Upper))
class Hessian {
@@ -69,7 +70,7 @@ class Hessian {
// If the row is linear, compute its gradient once here and cache its
// triplets. Constant rows are ignored because their gradients have no
// nonzero triplets.
m_graphs[row].append_triplets(m_cached_triplets, row, m_wrt);
m_graphs[row].append_triplets(m_cached_triplets, row);
} else if (m_variables[row].type() > ExpressionType::LINEAR) {
// If the row is quadratic or nonlinear, add it to the list of nonlinear
// rows to be recomputed in value().
@@ -127,7 +128,7 @@ class Hessian {
// Compute each nonlinear row of the Hessian
for (int row : m_nonlinear_rows) {
m_graphs[row].append_triplets(triplets, row, m_wrt);
m_graphs[row].append_triplets(triplets, row);
}
m_H.setFromTriplets(triplets.begin(), triplets.end());

4
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/jacobian.hpp vendored
View File

@@ -77,7 +77,7 @@ class Jacobian {
// If the row is linear, compute its gradient once here and cache its
// triplets. Constant rows are ignored because their gradients have no
// nonzero triplets.
m_graphs[row].append_triplets(m_cached_triplets, row, m_wrt);
m_graphs[row].append_triplets(m_cached_triplets, row);
} else if (m_variables[row].type() > ExpressionType::LINEAR) {
// If the row is quadratic or nonlinear, add it to the list of nonlinear
// rows to be recomputed in value().
@@ -132,7 +132,7 @@ class Jacobian {
// Compute each nonlinear row of the Jacobian
for (int row : m_nonlinear_rows) {
m_graphs[row].append_triplets(triplets, row, m_wrt);
m_graphs[row].append_triplets(triplets, row);
}
m_J.setFromTriplets(triplets.begin(), triplets.end());

8
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_matrix.hpp vendored
View File

@@ -1141,12 +1141,12 @@ class VariableMatrix : public SleipnirBase {
/// Returns const begin iterator.
///
/// @return Const begin iterator.
const_iterator cbegin() const { return const_iterator{m_storage.begin()}; }
const_iterator cbegin() const { return const_iterator{m_storage.cbegin()}; }
/// Returns const end iterator.
///
/// @return Const end iterator.
const_iterator cend() const { return const_iterator{m_storage.end()}; }
const_iterator cend() const { return const_iterator{m_storage.cend()}; }
/// Returns reverse begin iterator.
///
@@ -1191,11 +1191,11 @@ class VariableMatrix : public SleipnirBase {
/// @return Number of elements in matrix.
size_t size() const { return m_storage.size(); }
/// Returns a variable matrix filled with zeroes.
/// Returns a variable matrix filled with zeros.
///
/// @param rows The number of matrix rows.
/// @param cols The number of matrix columns.
/// @return A variable matrix filled with zeroes.
/// @return A variable matrix filled with zeros.
static VariableMatrix<Scalar> zero(int rows, int cols) {
VariableMatrix<Scalar> result{detail::empty, rows, cols};

71
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp vendored
View File

@@ -31,6 +31,7 @@
#include "sleipnir/optimization/solver/options.hpp"
#include "sleipnir/optimization/solver/sqp.hpp"
#include "sleipnir/optimization/solver/util/bounds.hpp"
#include "sleipnir/optimization/solver/util/problem_scaling.hpp"
#include "sleipnir/util/empty.hpp"
#include "sleipnir/util/print.hpp"
#include "sleipnir/util/print_diagnostics.hpp"
@@ -113,7 +114,7 @@ class Problem {
/// Decision variables have an initial value of zero.
///
/// @param rows Number of matrix rows.
/// @return A symmetric matrix of decision varaibles in the optimization
/// @return A symmetric matrix of decision variables in the optimization
/// problem.
[[nodiscard]]
VariableMatrix<Scalar> symmetric_decision_variable(int rows) {
@@ -377,20 +378,26 @@ class Problem {
}
#endif
// Automatically scale the cost. The problem scaling procedure is
// described in more detail in docs/algorithms.md#problem-scaling.
x_ad.set_value(x);
const ProblemScaling<Scalar> scaling{g.value()};
NewtonMatrixCallbacks<Scalar> matrix_callbacks{
num_decision_variables,
[&](const DenseVector& x) -> Scalar {
x_ad.set_value(x);
return f.value();
return scaling.f * f.value();
},
[&](const DenseVector& x) -> SparseVector {
x_ad.set_value(x);
return g.value();
return scaling.f * g.value();
},
[&](const DenseVector& x) -> SparseMatrix {
x_ad.set_value(x);
return H.value();
}};
return scaling.f * H.value();
},
scaling};
// Invoke Newton solver
status =
@@ -465,35 +472,42 @@ class Problem {
}
#endif
// Automatically scale the cost and constraints. The problem scaling
// procedure is described in more detail in
// docs/algorithms.md#problem-scaling.
x_ad.set_value(x);
const ProblemScaling<Scalar> scaling{g.value(), A_e.value()};
SQPMatrixCallbacks<Scalar> matrix_callbacks{
num_decision_variables,
num_equality_constraints,
[&](const DenseVector& x) -> Scalar {
x_ad.set_value(x);
return f.value();
return scaling.f * f.value();
},
[&](const DenseVector& x) -> SparseVector {
x_ad.set_value(x);
return g.value();
return scaling.f * g.value();
},
[&](const DenseVector& x, const DenseVector& y) -> SparseMatrix {
x_ad.set_value(x);
y_ad.set_value(y);
return H_f.value() + H_c.value();
y_ad.set_value(scaling.c_e.cwiseProduct(y));
return scaling.f * H_f.value() + H_c.value();
},
[&](const DenseVector& x, const DenseVector& y) -> SparseMatrix {
x_ad.set_value(x);
y_ad.set_value(y);
y_ad.set_value(scaling.c_e.cwiseProduct(y));
return H_c.value();
},
[&](const DenseVector& x) -> DenseVector {
x_ad.set_value(x);
return c_e_ad.value();
return scaling.c_e.cwiseProduct(c_e_ad.value());
},
[&](const DenseVector& x) -> SparseMatrix {
x_ad.set_value(x);
return A_e.value();
}};
return scaling.c_e.asDiagonal() * A_e.value();
},
scaling};
// Invoke SQP solver
status = sqp<Scalar>(matrix_callbacks, iteration_callbacks, options, x);
@@ -597,48 +611,55 @@ class Problem {
project_onto_bounds(x, bounds);
#endif
// Automatically scale the cost and constraints. The problem scaling
// procedure is described in more detail in
// docs/algorithms.md#problem-scaling.
x_ad.set_value(x);
const ProblemScaling<Scalar> scaling{g.value(), A_e.value(), A_i.value()};
InteriorPointMatrixCallbacks<Scalar> matrix_callbacks{
num_decision_variables,
num_equality_constraints,
num_inequality_constraints,
[&](const DenseVector& x) -> Scalar {
x_ad.set_value(x);
return f.value();
return scaling.f * f.value();
},
[&](const DenseVector& x) -> SparseVector {
x_ad.set_value(x);
return g.value();
return scaling.f * g.value();
},
[&](const DenseVector& x, const DenseVector& y,
const DenseVector& z) -> SparseMatrix {
x_ad.set_value(x);
y_ad.set_value(y);
z_ad.set_value(z);
return H_f.value() + H_c.value();
y_ad.set_value(scaling.c_e.cwiseProduct(y));
z_ad.set_value(scaling.c_i.cwiseProduct(z));
return scaling.f * H_f.value() + H_c.value();
},
[&](const DenseVector& x, const DenseVector& y,
const DenseVector& z) -> SparseMatrix {
x_ad.set_value(x);
y_ad.set_value(y);
z_ad.set_value(z);
y_ad.set_value(scaling.c_e.cwiseProduct(y));
z_ad.set_value(scaling.c_i.cwiseProduct(z));
return H_c.value();
},
[&](const DenseVector& x) -> DenseVector {
x_ad.set_value(x);
return c_e_ad.value();
return scaling.c_e.cwiseProduct(c_e_ad.value());
},
[&](const DenseVector& x) -> SparseMatrix {
x_ad.set_value(x);
return A_e.value();
return scaling.c_e.asDiagonal() * A_e.value();
},
[&](const DenseVector& x) -> DenseVector {
x_ad.set_value(x);
return c_i_ad.value();
return scaling.c_i.cwiseProduct(c_i_ad.value());
},
[&](const DenseVector& x) -> SparseMatrix {
x_ad.set_value(x);
return A_i.value();
}};
return scaling.c_i.asDiagonal() * A_i.value();
},
scaling};
// Invoke interior-point method solver
status =

183
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point.hpp vendored
View File

@@ -6,7 +6,6 @@
#include <chrono>
#include <cmath>
#include <functional>
#include <limits>
#include <span>
#include <Eigen/Core>
@@ -77,13 +76,14 @@ ExitStatus interior_point(
DenseVector y = DenseVector::Zero(matrix_callbacks.num_equality_constraints);
DenseVector z =
DenseVector::Ones(matrix_callbacks.num_inequality_constraints);
Scalar μ(0.1);
Scalar μ = Scalar(0.1) * matrix_callbacks.scaling.f;
int iterations = 0;
return interior_point(matrix_callbacks, iteration_callbacks, options, false,
#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
bound_constraint_mask,
#endif
x, s, y, z, μ);
x, s, y, z, μ, iterations);
}
/// Finds the optimal solution to a nonlinear program using the interior-point
@@ -117,6 +117,7 @@ ExitStatus interior_point(
/// constraint dual variables.
/// @param[in,out] μ The initial guess and output location for the barrier
/// parameter.
/// @param[in,out] iterations The iteration counter.
/// @return The exit status.
template <typename Scalar>
ExitStatus interior_point(
@@ -130,7 +131,7 @@ ExitStatus interior_point(
Eigen::Vector<Scalar, Eigen::Dynamic>& x,
Eigen::Vector<Scalar, Eigen::Dynamic>& s,
Eigen::Vector<Scalar, Eigen::Dynamic>& y,
Eigen::Vector<Scalar, Eigen::Dynamic>& z, Scalar& μ) {
Eigen::Vector<Scalar, Eigen::Dynamic>& z, Scalar& μ, int& iterations) {
using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
using SparseMatrix = Eigen::SparseMatrix<Scalar>;
using SparseVector = Eigen::SparseVector<Scalar>;
@@ -162,7 +163,7 @@ ExitStatus interior_point(
solve_profilers.emplace_back(" ↳ KKT system solve");
solve_profilers.emplace_back(" ↳ line search");
solve_profilers.emplace_back(" ↳ SOC");
solve_profilers.emplace_back(" ↳ next iter prep");
solve_profilers.emplace_back(" ↳ feas. restoration");
solve_profilers.emplace_back(" ↳ f(x)");
solve_profilers.emplace_back(" ↳ ∇f(x)");
solve_profilers.emplace_back(" ↳ ∇²ₓₓL");
@@ -182,7 +183,7 @@ ExitStatus interior_point(
auto& kkt_system_solve_prof = solve_profilers[7];
auto& line_search_prof = solve_profilers[8];
auto& soc_prof = solve_profilers[9];
auto& next_iter_prep_prof = solve_profilers[10];
auto& feasibility_restoration_prof = solve_profilers[10];
// Set up profiled matrix callbacks
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
@@ -232,7 +233,8 @@ ExitStatus interior_point(
[&](const DenseVector& x) -> SparseMatrix {
ScopedProfiler prof{A_i_prof};
return matrix_callbacks.A_i(x);
}};
},
matrix_callbacks.scaling};
#else
const auto& matrices = matrix_callbacks;
#endif
@@ -259,6 +261,15 @@ ExitStatus interior_point(
slp_assert(A_i.rows() == matrices.num_inequality_constraints);
slp_assert(A_i.cols() == matrices.num_decision_variables);
DenseVector trial_x;
DenseVector trial_s;
DenseVector trial_y;
DenseVector trial_z;
Scalar trial_f;
DenseVector trial_c_e;
DenseVector trial_c_i;
// Check for overconstrained problem
if (matrices.num_equality_constraints > matrices.num_decision_variables) {
if (options.diagnostics) {
@@ -279,10 +290,9 @@ ExitStatus interior_point(
s = bound_constraint_mask.select(c_i, s);
#endif
int iterations = 0;
// Barrier parameter minimum
const Scalar μ_min = Scalar(options.tolerance) / Scalar(10);
const Scalar μ_min =
matrices.scaling.f * Scalar(options.tolerance) / Scalar(10);
// Fraction-to-the-boundary rule scale factor minimum
constexpr Scalar τ_min(0.99);
@@ -325,10 +335,20 @@ ExitStatus interior_point(
// Kept outside the loop so its storage can be reused
gch::small_vector<Eigen::Triplet<Scalar>> triplets;
// Constraint regularization is forced to zero in feasibility restoration
// because the equality constraint Jacobian cannot be rank-deficient
const int lhs_rows =
matrices.num_decision_variables + matrices.num_equality_constraints;
RegularizedLDLT<Scalar> solver{
// Use sparse solver if lower triangle fills < 25% of system
H.nonZeros() +
(A_i.transpose() * A_i)
.template triangularView<Eigen::Lower>()
.eval()
.nonZeros() +
A_e.nonZeros() <
0.25 * lhs_rows * lhs_rows,
matrices.num_decision_variables, matrices.num_equality_constraints,
// Constraint regularization is forced to zero in feasibility restoration
// because the equality constraint Jacobian cannot be rank-deficient
in_feasibility_restoration ? Scalar(0) : Scalar(1e-10)};
// Variables for determining when a step is acceptable
@@ -338,7 +358,8 @@ ExitStatus interior_point(
int full_step_rejected_counter = 0;
// Error
Scalar E_0 = std::numeric_limits<Scalar>::infinity();
Scalar E_0 = unscaled_kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
matrices.scaling, g, A_e, c_e, A_i, c_i, s, y, z, Scalar(0));
setup_prof.stop();
@@ -479,13 +500,22 @@ ExitStatus interior_point(
// Loop until a step is accepted
while (1) {
DenseVector trial_x = x + α * step.p_x;
DenseVector trial_y = y + α_z * step.p_y;
DenseVector trial_z = z + α_z * step.p_z;
trial_x = x + α * step.p_x;
if (options.feasible_ipm && c_i.cwiseGreater(Scalar(0)).all()) {
// If the inequality constraints are all feasible, prevent them from
// becoming infeasible again.
//
// See equation (19.30) in [1].
trial_s = trial_c_i;
} else {
trial_s = s + α * step.p_s;
}
trial_y = y + α_z * step.p_y;
trial_z = z + α_z * step.p_z;
Scalar trial_f = matrices.f(trial_x);
DenseVector trial_c_e = matrices.c_e(trial_x);
DenseVector trial_c_i = matrices.c_i(trial_x);
trial_f = matrices.f(trial_x);
trial_c_e = matrices.c_e(trial_x);
trial_c_i = matrices.c_i(trial_x);
// If f(xₖ + αpₖˣ), cₑ(xₖ + αpₖˣ), or cᵢ(xₖ + αpₖˣ) aren't finite, reduce
// step size immediately
@@ -501,17 +531,6 @@ ExitStatus interior_point(
continue;
}
DenseVector trial_s;
if (options.feasible_ipm && c_i.cwiseGreater(Scalar(0)).all()) {
// If the inequality constraints are all feasible, prevent them from
// becoming infeasible again.
//
// See equation (19.30) in [1].
trial_s = trial_c_i;
} else {
trial_s = s + α * step.p_s;
}
// Check whether filter accepts trial iterate
FilterEntry trial_entry{trial_f, trial_s, trial_c_e, trial_c_i, μ};
if (filter.try_add(current_entry, trial_entry, step.p_x, g, α)) {
@@ -538,6 +557,8 @@ ExitStatus interior_point(
Scalar α_z_soc = α_z;
DenseVector c_e_soc = c_e;
Scalar soc_constraint_violation = next_constraint_violation;
bool step_acceptable = false;
for (int soc_iteration = 0; soc_iteration < 5 && !step_acceptable;
++soc_iteration) {
@@ -546,19 +567,22 @@ ExitStatus interior_point(
scope_exit soc_exit{[&] {
soc_profiler.stop();
if (options.diagnostics) {
if (options.diagnostics && step_acceptable) {
print_iteration_diagnostics(
iterations,
step_acceptable ? IterationType::ACCEPTED_SOC
: IterationType::REJECTED_SOC,
iterations, IterationType::SECOND_ORDER_CORRECTION,
soc_profiler.current_duration(),
kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
g, A_e, trial_c_e, A_i, trial_c_i, trial_s, trial_y,
trial_z, Scalar(0)),
unscaled_kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
matrices.scaling, g, A_e, trial_c_e, A_i, trial_c_i,
trial_s, trial_y, trial_z, Scalar(0)),
trial_f,
trial_c_e.template lpNorm<1>() +
(trial_c_i - trial_s).template lpNorm<1>(),
trial_s.dot(trial_z), μ, solver.hessian_regularization(),
solver.constraint_jacobian_regularization(),
std::max(soc_step.p_x.template lpNorm<Eigen::Infinity>(),
soc_step.p_s.template lpNorm<Eigen::Infinity>()),
std::max(soc_step.p_y.template lpNorm<Eigen::Infinity>(),
soc_step.p_z.template lpNorm<Eigen::Infinity>()),
α_soc, Scalar(1), α_reduction_factor, α_z_soc);
}
}};
@@ -576,12 +600,12 @@ ExitStatus interior_point(
compute_step(soc_step);
// αˢᵒᶜ = max(α ∈ (0, 1] : sₖ + αpₖˢ ≥ (1−τⱼ)sₖ)
// αₖᶻˢᵒᶜ = max(α ∈ (0, 1] : zₖ + αpₖᶻ ≥ (1−τⱼ)zₖ)
α_soc = fraction_to_the_boundary_rule<Scalar>(s, soc_step.p_s, τ);
α_z_soc = fraction_to_the_boundary_rule<Scalar>(z, soc_step.p_z, τ);
trial_x = x + α_soc * soc_step.p_x;
trial_s = s + α_soc * soc_step.p_s;
// αₖᶻ = max(α ∈ (0, 1] : zₖ + αpₖᶻ ≥ (1−τⱼ)zₖ)
α_z_soc = fraction_to_the_boundary_rule<Scalar>(z, soc_step.p_z, τ);
trial_y = y + α_z_soc * soc_step.p_y;
trial_z = z + α_z_soc * soc_step.p_z;
@@ -589,6 +613,16 @@ ExitStatus interior_point(
trial_c_e = matrices.c_e(trial_x);
trial_c_i = matrices.c_i(trial_x);
// Check whether filter accepts trial iterate
FilterEntry trial_entry{trial_f, trial_s, trial_c_e, trial_c_i, μ};
if (filter.try_add(current_entry, trial_entry, step.p_x, g, α)) {
step = soc_step;
α = α_soc;
α_z = α_z_soc;
step_acceptable = true;
break;
}
// Constraint violation scale factor for second-order corrections
constexpr Scalar κ_soc(0.99);
@@ -597,18 +631,11 @@ ExitStatus interior_point(
next_constraint_violation =
trial_c_e.template lpNorm<1>() +
(trial_c_i - trial_s).template lpNorm<1>();
if (next_constraint_violation > κ_soc * prev_constraint_violation) {
if (next_constraint_violation > κ_soc * soc_constraint_violation) {
break;
}
// Check whether filter accepts trial iterate
FilterEntry trial_entry{trial_f, trial_s, trial_c_e, trial_c_i, μ};
if (filter.try_add(current_entry, trial_entry, step.p_x, g, α)) {
step = soc_step;
α = α_soc;
α_z = α_z_soc;
step_acceptable = true;
}
soc_constraint_violation = next_constraint_violation;
}
if (step_acceptable) {
@@ -648,21 +675,19 @@ ExitStatus interior_point(
trial_x = x + α_max * step.p_x;
trial_s = s + α_max * step.p_s;
trial_y = y + α_z * step.p_y;
trial_z = z + α_z * step.p_z;
trial_f = matrices.f(trial_x);
trial_c_e = matrices.c_e(trial_x);
trial_c_i = matrices.c_i(trial_x);
Scalar next_kkt_error = kkt_error<Scalar, KKTErrorType::ONE_NORM>(
matrices.g(trial_x), matrices.A_e(trial_x), matrices.c_e(trial_x),
matrices.g(trial_x), matrices.A_e(trial_x), trial_c_e,
matrices.A_i(trial_x), trial_c_i, trial_s, trial_y, trial_z, μ);
// If the step using αᵐᵃˣ reduced the KKT error, accept it anyway
if (next_kkt_error <= Scalar(0.999) * current_kkt_error) {
α = α_max;
// Accept step
break;
}
@@ -675,6 +700,9 @@ ExitStatus interior_point(
line_search_profiler.stop();
if (call_feasibility_restoration) {
ScopedProfiler feasibility_restoration_profiler{
feasibility_restoration_prof};
// If already in feasibility restoration mode, running it again won't help
if (in_feasibility_restoration) {
return ExitStatus::FEASIBILITY_RESTORATION_FAILED;
@@ -705,27 +733,32 @@ ExitStatus interior_point(
Scalar(0.9) * initial_entry.constraint_violation &&
filter.try_add(initial_entry, trial_entry, trial_x - x, g, α);
});
auto status = feasibility_restoration<Scalar>(matrices, callbacks,
options, x, s, y, z, μ);
auto status =
feasibility_restoration<Scalar>(matrices, callbacks, options,
#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
bound_constraint_mask,
#endif
x, s, y, z, μ, iterations);
if (status != ExitStatus::SUCCESS) {
// Report failure
return status;
}
f = matrices.f(x);
c_e = matrices.c_e(x);
c_i = matrices.c_i(x);
} else {
// If full step was accepted, reset full-step rejected counter
if (α == α_max) {
full_step_rejected_counter = 0;
}
// xₖ₊₁ = xₖ + αₖpₖˣ
// sₖ₊₁ = sₖ + αₖpₖˢ
// yₖ₊₁ = yₖ + αₖᶻpₖʸ
// zₖ₊₁ = zₖ + αₖᶻpₖᶻ
x += α * step.p_x;
s += α * step.p_s;
y += α_z * step.p_y;
z += α_z * step.p_z;
// Update iterates
x = trial_x;
s = trial_s;
y = trial_y;
z = trial_z;
// A requirement for the convergence proof is that the primal-dual barrier
// term Hessian Σₖ₊₁ does not deviate arbitrarily much from the primal
@@ -745,23 +778,21 @@ ExitStatus interior_point(
z[row] =
std::clamp(z[row], Scalar(1) / κ_Σ * μ / s[row], κ_Σ * μ / s[row]);
}
f = trial_f;
c_e = trial_c_e;
c_i = trial_c_i;
}
// Update autodiff for Jacobians and Hessian
f = matrices.f(x);
A_e = matrices.A_e(x);
A_i = matrices.A_i(x);
g = matrices.g(x);
H = matrices.H(x, y, z);
ScopedProfiler next_iter_prep_profiler{next_iter_prep_prof};
c_e = matrices.c_e(x);
c_i = matrices.c_i(x);
// Update the error
E_0 = kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
g, A_e, c_e, A_i, c_i, s, y, z, Scalar(0));
E_0 = unscaled_kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
matrices.scaling, g, A_e, c_e, A_i, c_i, s, y, z, Scalar(0));
// Update the barrier parameter if necessary
if (E_0 > Scalar(options.tolerance)) {
@@ -779,7 +810,6 @@ ExitStatus interior_point(
}
}
next_iter_prep_profiler.stop();
inner_iter_profiler.stop();
if (options.diagnostics) {
@@ -789,8 +819,13 @@ ExitStatus interior_point(
: IterationType::NORMAL,
inner_iter_profiler.current_duration(), E_0, f,
c_e.template lpNorm<1>() + (c_i - s).template lpNorm<1>(), s.dot(z),
μ, solver.hessian_regularization(), α, α_max, α_reduction_factor,
α_z);
μ, solver.hessian_regularization(),
solver.constraint_jacobian_regularization(),
std::max(step.p_x.template lpNorm<Eigen::Infinity>(),
step.p_s.template lpNorm<Eigen::Infinity>()),
std::max(step.p_y.template lpNorm<Eigen::Infinity>(),
step.p_z.template lpNorm<Eigen::Infinity>()),
α, α_max, α_reduction_factor, α_z);
}
++iterations;

10
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point_matrix_callbacks.hpp vendored
View File

@@ -7,6 +7,8 @@
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include "sleipnir/optimization/solver/util/problem_scaling.hpp"
namespace slp {
/// Matrix callbacks for the interior-point method solver.
@@ -76,6 +78,8 @@ struct InteriorPointMatrixCallbacks {
///
/// L(x, y, z) = f(x) − yᵀcₑ(x) − zᵀcᵢ(x)
///
/// Only the lower triangle is used.
///
/// <table>
/// <tr>
/// <th>Variable</th>
@@ -109,6 +113,8 @@ struct InteriorPointMatrixCallbacks {
/// Constraint part of Lagrangian Hessian ∇ₓₓ²(−yᵀcₑ(x) − zᵀcᵢ(x)) getter.
///
/// Only the lower triangle is used.
///
/// <table>
/// <tr>
/// <th>Variable</th>
@@ -237,6 +243,10 @@ struct InteriorPointMatrixCallbacks {
/// </tr>
/// </table>
std::function<SparseMatrix(const DenseVector& x)> A_i;
/// Automatic problem scaling factors. Used to scale the cost, constraints,
/// and tolerance inside the interior-point solver.
ProblemScaling<Scalar> scaling;
};
} // namespace slp

56
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton.hpp vendored
View File

@@ -5,7 +5,6 @@
#include <chrono>
#include <cmath>
#include <functional>
#include <limits>
#include <span>
#include <Eigen/Core>
@@ -71,7 +70,6 @@ ExitStatus newton(
solve_profilers.emplace_back(" ↳ KKT matrix decomp");
solve_profilers.emplace_back(" ↳ KKT system solve");
solve_profilers.emplace_back(" ↳ line search");
solve_profilers.emplace_back(" ↳ next iter prep");
solve_profilers.emplace_back(" ↳ f(x)");
solve_profilers.emplace_back(" ↳ ∇f(x)");
solve_profilers.emplace_back(" ↳ ∇²ₓₓL");
@@ -84,13 +82,12 @@ ExitStatus newton(
auto& kkt_matrix_decomp_prof = solve_profilers[5];
auto& kkt_system_solve_prof = solve_profilers[6];
auto& line_search_prof = solve_profilers[7];
auto& next_iter_prep_prof = solve_profilers[8];
// Set up profiled matrix callbacks
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
auto& f_prof = solve_profilers[9];
auto& g_prof = solve_profilers[10];
auto& H_prof = solve_profilers[11];
auto& f_prof = solve_profilers[8];
auto& g_prof = solve_profilers[9];
auto& H_prof = solve_profilers[10];
NewtonMatrixCallbacks<Scalar> matrices{
matrix_callbacks.num_decision_variables,
@@ -105,7 +102,8 @@ ExitStatus newton(
[&](const DenseVector& x) -> SparseMatrix {
ScopedProfiler prof{H_prof};
return matrix_callbacks.H(x);
}};
},
matrix_callbacks.scaling};
#else
const auto& matrices = matrix_callbacks;
#endif
@@ -122,6 +120,10 @@ ExitStatus newton(
slp_assert(H.rows() == matrices.num_decision_variables);
slp_assert(H.cols() == matrices.num_decision_variables);
DenseVector trial_x;
Scalar trial_f;
// Check whether initial guess has finite cost and derivatives
if (!isfinite(f) || !all_finite(g) || !all_finite(H)) {
return ExitStatus::NONFINITE_INITIAL_GUESS;
@@ -131,14 +133,17 @@ ExitStatus newton(
Filter<Scalar> filter;
RegularizedLDLT<Scalar> solver{matrices.num_decision_variables, 0};
RegularizedLDLT<Scalar> solver{
// Use sparse solver if lower triangle fills < 25% of system
H.nonZeros() < 0.25 * H.size(), matrices.num_decision_variables, 0};
// Variables for determining when a step is acceptable
constexpr Scalar α_reduction_factor(0.5);
constexpr Scalar α_min(1e-20);
// Error
Scalar E_0 = std::numeric_limits<Scalar>::infinity();
Scalar E_0 = unscaled_kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
matrices.scaling, g);
setup_prof.stop();
@@ -194,9 +199,9 @@ ExitStatus newton(
// Loop until a step is accepted. If a step becomes acceptable, the loop
// will exit early.
while (1) {
DenseVector trial_x = x + α * p_x;
trial_x = x + α * p_x;
Scalar trial_f = matrices.f(trial_x);
trial_f = matrices.f(trial_x);
// If f(xₖ + αpₖˣ) isn't finite, reduce step size immediately
if (!isfinite(trial_f)) {
@@ -223,14 +228,14 @@ ExitStatus newton(
if (α < α_min) {
Scalar current_kkt_error = kkt_error<Scalar, KKTErrorType::ONE_NORM>(g);
DenseVector trial_x = x + α_max * p_x;
trial_x = x + α_max * p_x;
Scalar next_kkt_error =
kkt_error<Scalar, KKTErrorType::ONE_NORM>(matrices.g(trial_x));
// If the step using αᵐᵃˣ reduced the KKT error, accept it anyway
if (next_kkt_error <= Scalar(0.999) * current_kkt_error) {
α = α_max;
trial_f = matrices.f(trial_x);
// Accept step
break;
@@ -242,28 +247,29 @@ ExitStatus newton(
line_search_profiler.stop();
// xₖ₊₁ = xₖ + αₖpₖˣ
x += α * p_x;
// Update iterates
x = trial_x;
f = trial_f;
// Update autodiff for Hessian
f = matrices.f(x);
g = matrices.g(x);
H = matrices.H(x);
ScopedProfiler next_iter_prep_profiler{next_iter_prep_prof};
// Update the error
E_0 = kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(g);
E_0 = unscaled_kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
matrices.scaling, g);
next_iter_prep_profiler.stop();
inner_iter_profiler.stop();
if (options.diagnostics) {
print_iteration_diagnostics(iterations, IterationType::NORMAL,
inner_iter_profiler.current_duration(), E_0,
f, Scalar(0), Scalar(0), Scalar(0),
solver.hessian_regularization(), α, α_max,
α_reduction_factor, Scalar(1));
print_iteration_diagnostics(
iterations, IterationType::NORMAL,
inner_iter_profiler.current_duration(), E_0, f, Scalar(0), Scalar(0),
Scalar(0), solver.hessian_regularization(),
solver.constraint_jacobian_regularization(),
p_x.template lpNorm<Eigen::Infinity>(), Scalar(1), α, α_max,
α_reduction_factor, Scalar(1));
}
++iterations;

8
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton_matrix_callbacks.hpp vendored
View File

@@ -7,6 +7,8 @@
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include "sleipnir/optimization/solver/util/problem_scaling.hpp"
namespace slp {
/// Matrix callbacks for the Newton's method solver.
@@ -70,6 +72,8 @@ struct NewtonMatrixCallbacks {
///
/// L(x) = f(x)
///
/// Only the lower triangle is used.
///
/// <table>
/// <tr>
/// <th>Variable</th>
@@ -88,6 +92,10 @@ struct NewtonMatrixCallbacks {
/// </tr>
/// </table>
std::function<SparseMatrix(const DenseVector& x)> H;
/// Automatic problem scaling factors. Used to scale the cost and tolerance
/// inside the Newton solver.
ProblemScaling<Scalar> scaling;
};
} // namespace slp

106
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp.hpp vendored
View File

@@ -5,7 +5,6 @@
#include <chrono>
#include <cmath>
#include <functional>
#include <limits>
#include <span>
#include <Eigen/Core>
@@ -121,7 +120,7 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
solve_profilers.emplace_back(" ↳ KKT system solve");
solve_profilers.emplace_back(" ↳ line search");
solve_profilers.emplace_back(" ↳ SOC");
solve_profilers.emplace_back(" ↳ next iter prep");
solve_profilers.emplace_back(" ↳ feas. restoration");
solve_profilers.emplace_back(" ↳ f(x)");
solve_profilers.emplace_back(" ↳ ∇f(x)");
solve_profilers.emplace_back(" ↳ ∇²ₓₓL");
@@ -139,7 +138,7 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
auto& kkt_system_solve_prof = solve_profilers[7];
auto& line_search_prof = solve_profilers[8];
auto& soc_prof = solve_profilers[9];
auto& next_iter_prep_prof = solve_profilers[10];
auto& feasibility_restoration_prof = solve_profilers[10];
// Set up profiled matrix callbacks
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
@@ -176,7 +175,8 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
[&](const DenseVector& x) -> SparseMatrix {
ScopedProfiler prof{A_e_prof};
return matrix_callbacks.A_e(x);
}};
},
matrix_callbacks.scaling};
#else
const auto& matrices = matrix_callbacks;
#endif
@@ -198,6 +198,12 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
slp_assert(A_e.rows() == matrices.num_equality_constraints);
slp_assert(A_e.cols() == matrices.num_decision_variables);
DenseVector trial_x;
DenseVector trial_y;
Scalar trial_f;
DenseVector trial_c_e;
// Check for overconstrained problem
if (matrices.num_equality_constraints > matrices.num_decision_variables) {
if (options.diagnostics) {
@@ -220,8 +226,12 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
// Kept outside the loop so its storage can be reused
gch::small_vector<Eigen::Triplet<Scalar>> triplets;
RegularizedLDLT<Scalar> solver{matrices.num_decision_variables,
matrices.num_equality_constraints};
const int lhs_rows =
matrices.num_decision_variables + matrices.num_equality_constraints;
RegularizedLDLT<Scalar> solver{
// Use sparse solver if lower triangle fills < 25% of system
H.nonZeros() + A_e.nonZeros() < 0.25 * lhs_rows * lhs_rows,
matrices.num_decision_variables, matrices.num_equality_constraints};
// Variables for determining when a step is acceptable
constexpr Scalar α_reduction_factor(0.5);
@@ -230,7 +240,8 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
int full_step_rejected_counter = 0;
// Error
Scalar E_0 = std::numeric_limits<Scalar>::infinity();
Scalar E_0 = unscaled_kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
matrices.scaling, g, A_e, c_e, y);
setup_prof.stop();
@@ -331,11 +342,11 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
// Loop until a step is accepted
while (1) {
DenseVector trial_x = x + α * step.p_x;
DenseVector trial_y = y + α * step.p_y;
trial_x = x + α * step.p_x;
trial_y = y + α * step.p_y;
Scalar trial_f = matrices.f(trial_x);
DenseVector trial_c_e = matrices.c_e(trial_x);
trial_f = matrices.f(trial_x);
trial_c_e = matrices.c_e(trial_x);
// If f(xₖ + αpₖˣ) or cₑ(xₖ + αpₖˣ) aren't finite, reduce step size
// immediately
@@ -372,6 +383,8 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
Scalar α_soc = α;
DenseVector c_e_soc = c_e;
Scalar soc_constraint_violation = next_constraint_violation;
bool step_acceptable = false;
for (int soc_iteration = 0; soc_iteration < 5 && !step_acceptable;
++soc_iteration) {
@@ -380,17 +393,18 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
scope_exit soc_exit{[&] {
soc_profiler.stop();
if (options.diagnostics) {
if (options.diagnostics && step_acceptable) {
print_iteration_diagnostics(
iterations,
step_acceptable ? IterationType::ACCEPTED_SOC
: IterationType::REJECTED_SOC,
iterations, IterationType::SECOND_ORDER_CORRECTION,
soc_profiler.current_duration(),
kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
g, A_e, trial_c_e, trial_y),
trial_f, trial_c_e.template lpNorm<1>(), Scalar(0), Scalar(0),
solver.hessian_regularization(), α_soc, Scalar(1),
α_reduction_factor, Scalar(1));
solver.hessian_regularization(),
solver.constraint_jacobian_regularization(),
soc_step.p_x.template lpNorm<Eigen::Infinity>(),
soc_step.p_y.template lpNorm<Eigen::Infinity>(), α_soc,
Scalar(1), α_reduction_factor, Scalar(1));
}
}};
@@ -412,23 +426,26 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
trial_f = matrices.f(trial_x);
trial_c_e = matrices.c_e(trial_x);
// Check whether the filter accepts trial iterate
FilterEntry trial_entry{trial_f, trial_c_e};
if (filter.try_add(current_entry, trial_entry, step.p_x, g, α)) {
step = soc_step;
α = α_soc;
step_acceptable = true;
break;
}
// Constraint violation scale factor for second-order corrections
constexpr Scalar κ_soc(0.99);
// If constraint violation hasn't been sufficiently reduced, stop
// making second-order corrections
next_constraint_violation = trial_c_e.template lpNorm<1>();
if (next_constraint_violation > κ_soc * prev_constraint_violation) {
if (next_constraint_violation > κ_soc * soc_constraint_violation) {
break;
}
// Check whether filter accepts trial iterate
FilterEntry trial_entry{trial_f, trial_c_e};
if (filter.try_add(current_entry, trial_entry, step.p_x, g, α)) {
step = soc_step;
α = α_soc;
step_acceptable = true;
}
soc_constraint_violation = next_constraint_violation;
}
if (step_acceptable) {
@@ -469,6 +486,7 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
trial_x = x + α_max * step.p_x;
trial_y = y + α_max * step.p_y;
trial_f = matrices.f(trial_x);
trial_c_e = matrices.c_e(trial_x);
Scalar next_kkt_error = kkt_error<Scalar, KKTErrorType::ONE_NORM>(
@@ -476,8 +494,6 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
// If the step using αᵐᵃˣ reduced the KKT error, accept it anyway
if (next_kkt_error <= Scalar(0.999) * current_kkt_error) {
α = α_max;
// Accept step
break;
}
@@ -490,6 +506,9 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
line_search_profiler.stop();
if (call_feasibility_restoration) {
ScopedProfiler feasibility_restoration_profiler{
feasibility_restoration_prof};
FilterEntry initial_entry{matrices.f(x), c_e};
// Feasibility restoration phase
@@ -512,47 +531,50 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
Scalar(0.9) * initial_entry.constraint_violation &&
filter.try_add(initial_entry, trial_entry, trial_x - x, g, α);
});
auto status =
feasibility_restoration<Scalar>(matrices, callbacks, options, x, y);
auto status = feasibility_restoration<Scalar>(matrices, callbacks,
options, x, y, iterations);
if (status != ExitStatus::SUCCESS) {
// Report failure
return status;
}
f = matrices.f(x);
c_e = matrices.c_e(x);
} else {
// If full step was accepted, reset full-step rejected counter
if (α == α_max) {
full_step_rejected_counter = 0;
}
// xₖ₊₁ = xₖ + αₖpₖˣ
// yₖ₊₁ = yₖ + αₖpₖʸ
x += α * step.p_x;
y += α * step.p_y;
// Update iterates
x = trial_x;
y = trial_y;
f = trial_f;
c_e = trial_c_e;
}
// Update autodiff for Jacobians and Hessian
f = matrices.f(x);
A_e = matrices.A_e(x);
g = matrices.g(x);
H = matrices.H(x, y);
ScopedProfiler next_iter_prep_profiler{next_iter_prep_prof};
c_e = matrices.c_e(x);
// Update the error
E_0 = kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(g, A_e, c_e, y);
E_0 = unscaled_kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
matrices.scaling, g, A_e, c_e, y);
next_iter_prep_profiler.stop();
inner_iter_profiler.stop();
if (options.diagnostics) {
print_iteration_diagnostics(iterations, IterationType::NORMAL,
inner_iter_profiler.current_duration(), E_0,
f, c_e.template lpNorm<1>(), Scalar(0),
Scalar(0), solver.hessian_regularization(), α,
α_max, α_reduction_factor, α);
Scalar(0), solver.hessian_regularization(),
solver.constraint_jacobian_regularization(),
step.p_x.template lpNorm<Eigen::Infinity>(),
step.p_y.template lpNorm<Eigen::Infinity>(),
α, α_max, α_reduction_factor, α);
}
++iterations;

10
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp_matrix_callbacks.hpp vendored
View File

@@ -7,6 +7,8 @@
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include "sleipnir/optimization/solver/util/problem_scaling.hpp"
namespace slp {
/// Matrix callbacks for the Sequential Quadratic Programming (SQP) solver.
@@ -73,6 +75,8 @@ struct SQPMatrixCallbacks {
///
/// L(x, y) = f(x) − yᵀcₑ(x)
///
/// Only the lower triangle is used.
///
/// <table>
/// <tr>
/// <th>Variable</th>
@@ -99,6 +103,8 @@ struct SQPMatrixCallbacks {
/// Constraint part of Lagrangian Hessian ∇ₓₓ²(−yᵀcₑ(x)) getter.
///
/// Only the lower triangle is used.
///
/// <table>
/// <tr>
/// <th>Variable</th>
@@ -171,6 +177,10 @@ struct SQPMatrixCallbacks {
/// </tr>
/// </table>
std::function<SparseMatrix(const DenseVector& x)> A_e;
/// Automatic problem scaling factors. Used to scale the cost, constraints,
/// and tolerance inside the SQP solver.
ProblemScaling<Scalar> scaling;
};
} // namespace slp

4
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/all_finite.hpp vendored
View File

@@ -6,6 +6,8 @@
#include <Eigen/SparseCore>
namespace slp {
/// Returns true if elements of sparse matrix are all finite.
///
/// @param mat Sparse matrix.
@@ -24,3 +26,5 @@ bool all_finite(const Eigen::SparseCompressedBase<Derived>& mat) {
return true;
}
} // namespace slp

4
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/append_as_triplets.hpp vendored
View File

@@ -7,6 +7,8 @@
#include <Eigen/SparseCore>
#include <gch/small_vector.hpp>
namespace slp {
/// Appends sparse matrices to list of triplets at the given offset.
///
/// The triplets are appended in column-major order (e.g., first column of mat1,
@@ -58,3 +60,5 @@ void append_diagonal_as_triplets(
triplets.emplace_back(row_offset + row, col_offset + row, diag[row]);
}
}
} // namespace slp

210
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/dense_regularized_ldlt.hpp vendored Normal file
View File

@@ -0,0 +1,210 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <algorithm>
#include <limits>
#include <Eigen/Cholesky>
#include <Eigen/Core>
#include "sleipnir/optimization/solver/util/inertia.hpp"
namespace slp {
/// Solves dense systems of linear equations using a regularized LDLT
/// factorization.
///
/// @tparam Scalar Scalar type.
template <typename Scalar>
class DenseRegularizedLDLT {
public:
/// Type alias for dense matrix.
using DenseMatrix = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
/// Type alias for dense vector.
using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
/// Constructs a DenseRegularizedLDLT instance.
///
/// @param num_decision_variables The number of decision variables in the
/// system.
/// @param num_equality_constraints The number of equality constraints in the
/// system.
DenseRegularizedLDLT(int num_decision_variables, int num_equality_constraints)
: m_num_decision_variables{num_decision_variables},
m_num_equality_constraints{num_equality_constraints} {}
/// Constructs a DenseRegularizedLDLT instance.
///
/// @param num_decision_variables The number of decision variables in the
/// system.
/// @param num_equality_constraints The number of equality constraints in the
/// system.
/// @param γ_min The minimum constraint regularization.
DenseRegularizedLDLT(int num_decision_variables, int num_equality_constraints,
Scalar γ_min)
: m_num_decision_variables{num_decision_variables},
m_num_equality_constraints{num_equality_constraints},
m_γ_min{γ_min} {}
/// Reports whether previous computation was successful.
///
/// @return Whether previous computation was successful.
Eigen::ComputationInfo info() const { return m_info; }
/// Computes the regularized LDLT factorization of a matrix.
///
/// @param lhs Left-hand side of the system.
/// @return The factorization.
DenseRegularizedLDLT& compute(const DenseMatrix& lhs) {
m_info = m_solver.compute(lhs).info();
if (m_info == Eigen::Success) {
auto D = m_solver.vectorD();
// If the inertia is ideal and D from LDLT is sufficiently far from zero,
// don't regularize the system
if (Inertia{D} == ideal_inertia &&
(D.cwiseAbs().array() >= Scalar(1e-4)).all()) {
m_prev_δ = Scalar(0);
m_prev_γ = Scalar(0);
return *this;
}
}
// We'll give lhs the correct inertia by adding [δI, 0; 0, −γI] where δ and
// γ regularize the Hessian and equality constraint Jacobian respectively.
// If the previous δ was zero, attempt a small value. Otherwise, attempt a
// smaller value than the previous δ so δ trends downward.
Scalar δ = m_prev_δ == Scalar(0)
? Scalar(1e-4)
: std::max(m_prev_δ / Scalar(2),
std::numeric_limits<Scalar>::epsilon());
// Start γ at the minimum to minimize equality constraint Jacobian
// distortion.
Scalar γ = m_γ_min;
while (true) {
m_info = m_solver.compute(lhs + regularization(δ, γ)).info();
if (m_info == Eigen::Success) {
Inertia inertia{m_solver.vectorD()};
if (inertia == ideal_inertia) {
// If the inertia is ideal, report success
m_prev_δ = δ;
m_prev_γ = γ;
return *this;
} else if (inertia.zero > 0) {
if (γ == Scalar(0)) {
// If there's zero eigenvalues and γ = 0, increase γ to potentially
// compensate for a rank-deficient equality constraint Jacobian
γ = Scalar(1e-10);
} else {
// If there's zero eigenvalues and γ > 0, increase δ and γ to drive
// all eigenvalues away from zero
δ *= Scalar(10);
γ *= Scalar(10);
}
} else if (inertia.negative > ideal_inertia.negative) {
// If there's too many negative eigenvalues, increase δ to add more
// positive eigenvalues
δ *= Scalar(10);
} else if (inertia.positive > ideal_inertia.positive) {
// If there's too many positive eigenvalues, increase γ to add more
// negative eigenvalues
γ = γ == Scalar(0) ? Scalar(1e-10) : γ * Scalar(10);
}
} else {
// If the decomposition failed, increase δ and γ to drive all
// eigenvalues away from zero
δ *= Scalar(10);
γ = γ == Scalar(0) ? Scalar(1e-10) : γ * Scalar(10);
}
// If the lhs perturbation is too high, report failure. This can be caused
// by ill-conditioning.
if (δ > Scalar(1e20) || γ > Scalar(1e20)) {
m_info = Eigen::NumericalIssue;
m_prev_δ = δ;
m_prev_γ = γ;
return *this;
}
}
}
/// Solves the system of equations using a regularized LDLT factorization.
///
/// @param rhs Right-hand side of the system.
/// @return The solution.
template <typename Rhs>
DenseVector solve(const Eigen::MatrixBase<Rhs>& rhs) const {
return m_solver.solve(rhs);
}
/// Solves the system of equations using a regularized LDLT factorization.
///
/// @param rhs Right-hand side of the system.
/// @return The solution.
template <typename Rhs>
DenseVector solve(const Eigen::SparseMatrixBase<Rhs>& rhs) const {
return m_solver.solve(rhs.toDense());
}
/// Returns the Hessian regularization factor.
///
/// @return Hessian regularization factor.
Scalar hessian_regularization() const { return m_prev_δ; }
/// Returns the constraint Jacobian regularization factor.
///
/// @return Constraint Jacobian regularization factor.
Scalar constraint_jacobian_regularization() const { return m_prev_γ; }
private:
using Solver = Eigen::LDLT<DenseMatrix>;
Solver m_solver;
Eigen::ComputationInfo m_info = Eigen::Success;
/// The number of decision variables in the system.
int m_num_decision_variables = 0;
/// The number of equality constraints in the system.
int m_num_equality_constraints = 0;
/// The minimum constraint regularization.
Scalar m_γ_min{1e-10};
/// The ideal system inertia.
Inertia ideal_inertia{m_num_decision_variables, m_num_equality_constraints,
0};
/// The value of δ from the previous run of compute().
Scalar m_prev_δ{0};
/// The value of γ from the previous run of compute().
Scalar m_prev_γ{0};
/// Returns regularization matrix.
///
/// [δI 0]
/// [ 0 −γI]
///
/// @param δ The Hessian regularization factor.
/// @param γ The equality constraint Jacobian regularization factor.
/// @return Regularization matrix.
DenseMatrix regularization(Scalar δ, Scalar γ) const {
DenseVector vec{m_num_decision_variables + m_num_equality_constraints};
vec.segment(0, m_num_decision_variables).setConstant(δ);
vec.segment(m_num_decision_variables, m_num_equality_constraints)
.setConstant(-γ);
return vec.asDiagonal().toDenseMatrix();
}
};
} // namespace slp

82
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp vendored
View File

@@ -33,7 +33,7 @@ ExitStatus interior_point(
Eigen::Vector<Scalar, Eigen::Dynamic>& x,
Eigen::Vector<Scalar, Eigen::Dynamic>& s,
Eigen::Vector<Scalar, Eigen::Dynamic>& y,
Eigen::Vector<Scalar, Eigen::Dynamic>& z, Scalar& μ);
Eigen::Vector<Scalar, Eigen::Dynamic>& z, Scalar& μ, int& iterations);
/// Computes initial values for p and n in feasibility restoration.
///
@@ -111,6 +111,7 @@ compute_p_n(const Eigen::Vector<Scalar, Eigen::Dynamic>& c, Scalar ρ,
/// @param[in,out] x The decision variables from the normal solve.
/// @param[in,out] y The equality constraint dual variables from the normal
/// solve.
/// @param[in,out] iterations The iteration counter.
/// @return The exit status.
template <typename Scalar>
ExitStatus feasibility_restoration(
@@ -118,7 +119,7 @@ ExitStatus feasibility_restoration(
std::span<std::function<bool(const IterationInfo<Scalar>& info)>>
iteration_callbacks,
const Options& options, Eigen::Vector<Scalar, Eigen::Dynamic>& x,
Eigen::Vector<Scalar, Eigen::Dynamic>& y) {
Eigen::Vector<Scalar, Eigen::Dynamic>& y, int& iterations) {
// Feasibility restoration
//
// min ρ Σ (pₑ + nₑ) + ζ/2 (x - xᵣ)ᵀDᵣ(x - xᵣ)
@@ -148,12 +149,14 @@ ExitStatus feasibility_restoration(
constexpr Scalar ρ(1e3);
const Scalar μ(options.tolerance / 10.0);
const Scalar ζ = sqrt(μ);
const DenseVector c_e = matrices.c_e(x);
Scalar fr_μ = std::max(μ, c_e.template lpNorm<Eigen::Infinity>());
const Scalar ζ = sqrt(fr_μ);
const auto& x_r = x;
const auto [p_e_0, n_e_0] = compute_p_n(c_e, ρ, μ);
const auto [p_e_0, n_e_0] = compute_p_n(c_e, ρ, fr_μ);
// Dᵣ = diag(min(1, 1/xᵣ[i]²) for i in x.rows())
const DiagonalMatrix D_r =
@@ -166,10 +169,15 @@ ExitStatus feasibility_restoration(
DenseVector fr_y = DenseVector::Zero(num_eq);
// Force the duals to start with perfect complementarity with the slacks
DenseVector fr_z{2 * num_eq};
fr_z << μ * p_e_0.cwiseInverse(), μ * n_e_0.cwiseInverse();
fr_z << fr_μ * p_e_0.cwiseInverse(), fr_μ * n_e_0.cwiseInverse();
Scalar fr_μ = std::max(μ, c_e.template lpNorm<Eigen::Infinity>());
// Inherit the parent problem's scaling for the constraints, and use no
// scaling for the cost function since it has changed. The new rows introduced
// are not scaled.
const ProblemScaling<Scalar> fr_scaling{Scalar(1), matrices.scaling.c_e,
DenseVector::Ones(2 * num_eq)};
InteriorPointMatrixCallbacks<Scalar> fr_matrix_callbacks{
static_cast<int>(fr_x.rows()),
@@ -289,14 +297,15 @@ ExitStatus feasibility_restoration(
SparseMatrix A_i_p{2 * num_eq, x_p.rows()};
A_i_p.setFromSortedTriplets(triplets.begin(), triplets.end());
return A_i_p;
}};
},
fr_scaling};
auto status = interior_point<Scalar>(fr_matrix_callbacks, iteration_callbacks,
options, true,
auto status = interior_point<Scalar>(
fr_matrix_callbacks, iteration_callbacks, options, true,
#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
{},
Eigen::ArrayX<bool>::Constant(2 * num_eq, true),
#endif
fr_x, fr_s, fr_y, fr_z, fr_μ);
fr_x, fr_s, fr_y, fr_z, fr_μ, iterations);
x = fr_x.segment(0, x.rows());
@@ -331,16 +340,21 @@ ExitStatus feasibility_restoration(
/// @param[in,out] z The current inequality constraint duals from the normal
/// solve.
/// @param[in] μ Barrier parameter.
/// @param[in,out] iterations The iteration counter.
/// @return The exit status.
template <typename Scalar>
ExitStatus feasibility_restoration(
const InteriorPointMatrixCallbacks<Scalar>& matrix_callbacks,
std::span<std::function<bool(const IterationInfo<Scalar>& info)>>
iteration_callbacks,
const Options& options, Eigen::Vector<Scalar, Eigen::Dynamic>& x,
const Options& options,
#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
const Eigen::ArrayX<bool>& bound_constraint_mask,
#endif
Eigen::Vector<Scalar, Eigen::Dynamic>& x,
Eigen::Vector<Scalar, Eigen::Dynamic>& s,
Eigen::Vector<Scalar, Eigen::Dynamic>& y,
Eigen::Vector<Scalar, Eigen::Dynamic>& z, Scalar μ) {
Eigen::Vector<Scalar, Eigen::Dynamic>& z, Scalar μ, int& iterations) {
// Feasibility restoration
//
// min ρ Σ (pₑ + nₑ + pᵢ + nᵢ) + ζ/2 (x - xᵣ)ᵀDᵣ(x - xᵣ)
@@ -374,14 +388,17 @@ ExitStatus feasibility_restoration(
const auto& num_ineq = matrices.num_inequality_constraints;
constexpr Scalar ρ(1e3);
const Scalar ζ = sqrt(μ);
const DenseVector c_e = matrices.c_e(x);
const DenseVector c_i = matrices.c_i(x);
Scalar fr_μ = std::max({μ, c_e.template lpNorm<Eigen::Infinity>(),
(c_i - s).template lpNorm<Eigen::Infinity>()});
const Scalar ζ = sqrt(fr_μ);
const auto& x_r = x;
const auto [p_e_0, n_e_0] = compute_p_n(c_e, ρ, μ);
const auto [p_i_0, n_i_0] = compute_p_n((c_i - s).eval(), ρ, μ);
const auto [p_e_0, n_e_0] = compute_p_n(c_e, ρ, fr_μ);
const auto [p_i_0, n_i_0] = compute_p_n((c_i - s).eval(), ρ, fr_μ);
// Dᵣ = diag(min(1, 1/xᵣ[i]²) for i in x.rows())
const DiagonalMatrix D_r =
@@ -396,12 +413,20 @@ ExitStatus feasibility_restoration(
DenseVector fr_y = DenseVector::Zero(c_e.rows());
// Force the duals to start with perfect complementarity with the slacks
DenseVector fr_z{c_i.rows() + 2 * num_eq + 2 * num_ineq};
fr_z << z.cwiseMin(ρ), μ * p_e_0.cwiseInverse(), μ * n_e_0.cwiseInverse(),
μ * p_i_0.cwiseInverse(), μ * n_i_0.cwiseInverse();
fr_z << fr_μ * s.cwiseInverse(), fr_μ * p_e_0.cwiseInverse(),
fr_μ * n_e_0.cwiseInverse(), fr_μ * p_i_0.cwiseInverse(),
fr_μ * n_i_0.cwiseInverse();
Scalar fr_μ = std::max({μ, c_e.template lpNorm<Eigen::Infinity>(),
(c_i - s).template lpNorm<Eigen::Infinity>()});
// Inherit the parent problem's scaling for the constraints, and use no
// scaling for the cost function since it has changed. The new rows introduced
// are not scaled.
DenseVector fr_d_c_i{c_i.rows() + 2 * num_eq + 2 * num_ineq};
fr_d_c_i << matrices.scaling.c_i,
DenseVector::Ones(2 * num_eq + 2 * num_ineq);
const ProblemScaling<Scalar> fr_scaling{Scalar(1), matrices.scaling.c_e,
fr_d_c_i};
InteriorPointMatrixCallbacks<Scalar> fr_matrix_callbacks{
static_cast<int>(fr_x.rows()),
@@ -564,14 +589,21 @@ ExitStatus feasibility_restoration(
SparseMatrix A_i_p{2 * num_eq + 3 * num_ineq, x_p.rows()};
A_i_p.setFromSortedTriplets(triplets.begin(), triplets.end());
return A_i_p;
}};
},
fr_scaling};
auto status = interior_point<Scalar>(fr_matrix_callbacks, iteration_callbacks,
options, true,
#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
{},
Eigen::ArrayX<bool> fr_bound_constraint_mask{2 * num_eq + 3 * num_ineq};
fr_bound_constraint_mask.segment(0, num_ineq) = bound_constraint_mask;
fr_bound_constraint_mask.segment(num_ineq, 2 * num_eq + 2 * num_ineq) = true;
#endif
fr_x, fr_s, fr_y, fr_z, fr_μ);
auto status = interior_point<Scalar>(
fr_matrix_callbacks, iteration_callbacks, options, true,
#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
fr_bound_constraint_mask,
#endif
fr_x, fr_s, fr_y, fr_z, fr_μ, iterations);
x = fr_x.segment(0, x.rows());
s = fr_s.segment(0, s.rows());

1
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/filter.hpp vendored
View File

@@ -4,7 +4,6 @@
#include <algorithm>
#include <cmath>
#include <limits>
#include <Eigen/Core>
#include <Eigen/SparseCore>

49
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/inertia.hpp vendored
View File

@@ -2,7 +2,10 @@
#pragma once
#include <limits>
#include <Eigen/Core>
#include <fmt/format.h>
namespace slp {
@@ -36,9 +39,9 @@ class Inertia {
template <typename Scalar>
explicit Inertia(const Eigen::Vector<Scalar, Eigen::Dynamic>& D) {
for (const auto& elem : D) {
if (elem > Scalar(0)) {
if (elem > std::numeric_limits<Scalar>::epsilon()) {
++positive;
} else if (elem < Scalar(0)) {
} else if (elem < -std::numeric_limits<Scalar>::epsilon()) {
++negative;
} else {
++zero;
@@ -56,9 +59,9 @@ class Inertia {
const Eigen::Diagonal<
const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>>& D) {
for (const auto& elem : D) {
if (elem > Scalar(0)) {
if (elem > std::numeric_limits<Scalar>::epsilon()) {
++positive;
} else if (elem < Scalar(0)) {
} else if (elem < -std::numeric_limits<Scalar>::epsilon()) {
++negative;
} else {
++zero;
@@ -73,3 +76,41 @@ class Inertia {
};
} // namespace slp
// @cond Suppress Doxygen
/// Formatter for Inertia.
template <>
struct fmt::formatter<slp::Inertia> {
/// Parses format string.
///
/// @param ctx Format parse context.
/// @return Format parse context iterator.
constexpr auto parse(fmt::format_parse_context& ctx) {
return m_underlying.parse(ctx);
}
/// Formats Inertia.
///
/// @tparam FmtContext Format context type.
/// @param inertia Inertia.
/// @param ctx Format context.
/// @return Format context iterator.
template <typename FmtContext>
constexpr auto format(const slp::Inertia& inertia, FmtContext& ctx) const {
auto out = ctx.out();
out = fmt::format_to(out, "(");
out = m_underlying.format(inertia.positive, ctx);
out = fmt::format_to(out, ", ");
out = m_underlying.format(inertia.negative, ctx);
out = fmt::format_to(out, ", ");
out = m_underlying.format(inertia.zero, ctx);
return fmt::format_to(out, ")");
}
private:
fmt::formatter<int> m_underlying;
};
// @endcond

12
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/is_locally_infeasible.hpp vendored
View File

@@ -12,10 +12,8 @@ namespace slp {
/// Returns true if the problem's equality constraints are locally infeasible.
///
/// @tparam Scalar Scalar type.
/// @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 A_e Equality constraint Jacobian Aₑ(x).
/// @param c_e Equality constraints cₑ(x).
template <typename Scalar>
bool is_equality_locally_infeasible(
const Eigen::SparseMatrix<Scalar>& A_e,
@@ -33,10 +31,8 @@ bool is_equality_locally_infeasible(
/// Returns true if the problem's inequality constraints are locally infeasible.
///
/// @tparam Scalar Scalar type.
/// @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at
/// the current iterate.
/// @param c_i The problem's inequality constraints cᵢ(x) evaluated at the
/// current iterate.
/// @param A_i Inequality constraint Jacobian Aᵢ(x).
/// @param c_i Inequality constraints cᵢ(x).
template <typename Scalar>
bool is_inequality_locally_infeasible(
const Eigen::SparseMatrix<Scalar>& A_i,

137
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/kkt_error.hpp vendored
View File

@@ -7,6 +7,8 @@
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include "sleipnir/optimization/solver/util/problem_scaling.hpp"
// See docs/algorithms.md#Works_cited for citation definitions
namespace slp {
@@ -22,8 +24,8 @@ enum class KKTErrorType {
/// Returns the KKT error for Newton's method.
///
/// @tparam Scalar Scalar type.
/// @tparam T The type of KKT error to compute.
/// @param g Gradient of the cost function ∇f.
/// @tparam T Type of KKT error to compute.
/// @param g Cost function gradient ∇f.
template <typename Scalar, KKTErrorType T>
Scalar kkt_error(const Eigen::Vector<Scalar, Eigen::Dynamic>& g) {
// The KKT conditions from docs/algorithms.md:
@@ -40,12 +42,10 @@ Scalar kkt_error(const Eigen::Vector<Scalar, Eigen::Dynamic>& g) {
/// Returns the KKT error for Sequential Quadratic Programming.
///
/// @tparam Scalar Scalar type.
/// @tparam T The type of KKT error to compute.
/// @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.
/// @tparam T Type of KKT error to compute.
/// @param g Cost function gradient ∇f.
/// @param A_e Equality constraint Jacobian Aₑ(x).
/// @param c_e Equality constraints cₑ(x).
/// @param y Equality constraint dual variables.
template <typename Scalar, KKTErrorType T>
Scalar kkt_error(const Eigen::Vector<Scalar, Eigen::Dynamic>& g,
@@ -79,16 +79,12 @@ Scalar kkt_error(const Eigen::Vector<Scalar, Eigen::Dynamic>& g,
/// Returns the KKT error for the interior-point method.
///
/// @tparam Scalar Scalar type.
/// @tparam T The type of KKT error to compute.
/// @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 A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at
/// the current iterate.
/// @param c_i The problem's inequality constraints cᵢ(x) evaluated at the
/// current iterate.
/// @tparam T Type of KKT error to compute.
/// @param g Cost function gradient ∇f.
/// @param A_e Equality constraint Jacobian Aₑ(x).
/// @param c_e Equality constraints cₑ(x).
/// @param A_i Inequality constraint Jacobian Aᵢ(x).
/// @param c_i Inequality constraints cᵢ(x).
/// @param s Inequality constraint slack variables.
/// @param y Equality constraint dual variables.
/// @param z Inequality constraint dual variables.
@@ -149,4 +145,109 @@ Scalar kkt_error(const Eigen::Vector<Scalar, Eigen::Dynamic>& g,
}
}
/// Returns the unscaled KKT error for Newton's method.
///
/// @tparam Scalar Scalar type.
/// @tparam T Type of KKT error to compute.
/// @param scaling Problem scaling.
/// @param g Scaled cost function gradient d_f·∇f.
template <typename Scalar, KKTErrorType T>
Scalar unscaled_kkt_error(const ProblemScaling<Scalar>& scaling,
const Eigen::Vector<Scalar, Eigen::Dynamic>& g) {
using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
if (scaling.is_identity()) {
return kkt_error<Scalar, T>(g);
}
const DenseVector g_unscaled = (Scalar(1) / scaling.f) * g;
return kkt_error<Scalar, T>(g_unscaled);
}
/// Returns the unscaled KKT error for Sequential Quadratic Programming.
///
/// @tparam Scalar Scalar type.
/// @tparam T Type of KKT error to compute.
/// @param scaling Problem scaling.
/// @param g Scaled cost function gradient d_f·∇f.
/// @param A_e Scaled equality constraint Jacobian D_cₑ·Aₑ(x).
/// @param c_e Scaled equality constraints D_cₑ·cₑ(x).
/// @param y Scaled equality constraint dual variables.
template <typename Scalar, KKTErrorType T>
Scalar unscaled_kkt_error(const ProblemScaling<Scalar>& scaling,
const Eigen::Vector<Scalar, Eigen::Dynamic>& g,
const Eigen::SparseMatrix<Scalar>& A_e,
const Eigen::Vector<Scalar, Eigen::Dynamic>& c_e,
const Eigen::Vector<Scalar, Eigen::Dynamic>& y) {
using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
using SparseMatrix = Eigen::SparseMatrix<Scalar>;
if (scaling.is_identity()) {
return kkt_error<Scalar, T>(g, A_e, c_e, y);
}
const Scalar inv_d_f = Scalar(1) / scaling.f;
const DenseVector inv_d_c_e = scaling.c_e.cwiseInverse();
const DenseVector g_unscaled = inv_d_f * g;
const SparseMatrix A_e_unscaled = inv_d_c_e.asDiagonal() * A_e;
const DenseVector c_e_unscaled = inv_d_c_e.cwiseProduct(c_e);
const DenseVector y_unscaled = scaling.c_e.cwiseProduct(y) * inv_d_f;
return kkt_error<Scalar, T>(g_unscaled, A_e_unscaled, c_e_unscaled,
y_unscaled);
}
/// Returns the unscaled KKT error for the interior-point method.
///
/// @tparam Scalar Scalar type.
/// @tparam T Type of KKT error to compute.
/// @param scaling Problem scaling.
/// @param g Scaled cost function gradient d_f·∇f.
/// @param A_e Scaled equality constraint Jacobian D_cₑ·Aₑ(x).
/// @param c_e Scaled equality constraints D_cₑ·cₑ(x).
/// @param A_i Scaled inequality constraint Jacobian D_cᵢ·Aᵢ(x).
/// @param c_i Scaled inequality constraints D_cᵢ·cᵢ(x).
/// @param s Scaled inequality constraint slack variables.
/// @param y Scaled equality constraint dual variables.
/// @param z Scaled inequality constraint dual variables.
/// @param μ Scaled barrier parameter.
template <typename Scalar, KKTErrorType T>
Scalar unscaled_kkt_error(const ProblemScaling<Scalar>& scaling,
const Eigen::Vector<Scalar, Eigen::Dynamic>& g,
const Eigen::SparseMatrix<Scalar>& A_e,
const Eigen::Vector<Scalar, Eigen::Dynamic>& c_e,
const Eigen::SparseMatrix<Scalar>& A_i,
const Eigen::Vector<Scalar, Eigen::Dynamic>& c_i,
const Eigen::Vector<Scalar, Eigen::Dynamic>& s,
const Eigen::Vector<Scalar, Eigen::Dynamic>& y,
const Eigen::Vector<Scalar, Eigen::Dynamic>& z,
Scalar μ) {
using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
using SparseMatrix = Eigen::SparseMatrix<Scalar>;
if (scaling.is_identity()) {
return kkt_error<Scalar, T>(g, A_e, c_e, A_i, c_i, s, y, z, μ);
}
const Scalar inv_d_f = Scalar(1) / scaling.f;
const DenseVector inv_d_c_e = scaling.c_e.cwiseInverse();
const DenseVector inv_d_c_i = scaling.c_i.cwiseInverse();
const DenseVector g_unscaled = inv_d_f * g;
const SparseMatrix A_e_unscaled = inv_d_c_e.asDiagonal() * A_e;
const DenseVector c_e_unscaled = inv_d_c_e.cwiseProduct(c_e);
const SparseMatrix A_i_unscaled = inv_d_c_i.asDiagonal() * A_i;
const DenseVector c_i_unscaled = inv_d_c_i.cwiseProduct(c_i);
const DenseVector s_unscaled = inv_d_c_i.cwiseProduct(s);
const DenseVector y_unscaled = scaling.c_e.cwiseProduct(y) * inv_d_f;
const DenseVector z_unscaled = scaling.c_i.cwiseProduct(z) * inv_d_f;
const Scalar μ_unscaled = inv_d_f * μ;
return kkt_error<Scalar, T>(g_unscaled, A_e_unscaled, c_e_unscaled,
A_i_unscaled, c_i_unscaled, s_unscaled,
y_unscaled, z_unscaled, μ_unscaled);
}
} // namespace slp

13
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/lagrange_multiplier_estimate.hpp vendored
View File

@@ -27,9 +27,8 @@ struct LagrangeMultiplierEstimate {
/// Estimates Lagrange multipliers for SQP.
///
/// @tparam Scalar Scalar type.
/// @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 g Cost function gradient ∇f.
/// @param A_e Equality constraint Jacobian Aₑ(x).
template <typename Scalar>
Eigen::Vector<Scalar, Eigen::Dynamic> lagrange_multiplier_estimate(
const Eigen::SparseVector<Scalar>& g,
@@ -47,11 +46,9 @@ Eigen::Vector<Scalar, Eigen::Dynamic> lagrange_multiplier_estimate(
/// Estimates Lagrange multipliers for interior-point method.
///
/// @tparam Scalar Scalar type.
/// @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 A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at
/// the current iterate.
/// @param g Cost function gradient ∇f.
/// @param A_e Equality constraint Jacobian Aₑ(x).
/// @param A_i Inequality constraint Jacobian Aᵢ(x).
/// @param s Inequality constraint slack variables.
/// @param μ Barrier parameter.
template <typename Scalar>

116
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/problem_scaling.hpp vendored Normal file
View File

@@ -0,0 +1,116 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <algorithm>
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include "sleipnir/optimization/solver/util/sparse_inf_norms.hpp"
// See docs/algorithms.md#Works_cited for citation definitions
namespace slp {
/// Automatic problem scaling factors.
///
/// @tparam Scalar Scalar type.
template <typename Scalar>
struct ProblemScaling {
/// Type alias for dense vector.
using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
/// Type alias for sparse matrix.
using SparseMatrix = Eigen::SparseMatrix<Scalar>;
/// Type alias for sparse vector.
using SparseVector = Eigen::SparseVector<Scalar>;
/// Cost scaling factor d_f.
Scalar f = Scalar(1);
/// Equality constraint scaling factors d_cₑ.
DenseVector c_e;
/// Inequality constraint scaling factors d_cᵢ.
DenseVector c_i;
/// Constructs identity problem scaling.
ProblemScaling() = default;
/// Constructs problem scaling from explicit factors.
///
/// @param f Cost scaling factor d_f.
/// @param c_e Equality constraint scaling factors d_cₑ.
/// @param c_i Inequality constraint scaling factors d_cᵢ.
ProblemScaling(Scalar f, const DenseVector& c_e, const DenseVector& c_i)
: f{f}, c_e{c_e}, c_i{c_i} {}
/// Computes Newton problem scaling.
///
/// Scales the cost so the largest gradient component at the starting point
/// is at most gₘₐₓ:
///
/// d_f = min(1, gₘₐₓ / ‖∇f(x₀)‖_∞)
///
/// See §3.8 Automatic Scaling of the Problem Statement in [2].
///
/// @param g Cost gradient ∇f, evaluated at the starting point.
explicit ProblemScaling(const DenseVector& g) {
constexpr Scalar g_max(100);
f = std::min(Scalar(1), g_max / g.template lpNorm<Eigen::Infinity>());
}
/// Computes SQP problem scaling.
///
/// Scales the cost and each equality constraint so the largest gradient
/// component at the starting point is at most gₘₐₓ:
///
/// d_f = min(1, gₘₐₓ / ‖∇f(x₀)‖_∞)
/// d_cₑ[j] = min(1, gₘₐₓ / ‖∇cₑⱼ(x₀)‖_∞)
///
/// See §3.8 Automatic Scaling of the Problem Statement in [2].
///
/// @param g Cost gradient ∇f, evaluated at the starting point.
/// @param A_e Equality constraint Jacobian Aₑ, evaluated at the starting
/// point.
ProblemScaling(const DenseVector& g, const SparseMatrix& A_e) {
constexpr Scalar g_max(100);
f = std::min(Scalar(1), g_max / g.template lpNorm<Eigen::Infinity>());
c_e = (g_max / sparse_inf_norms(A_e).array()).min(Scalar(1)).matrix();
}
/// Computes interior-point problem scaling.
///
/// Scales the cost and each constraint so the largest gradient
/// component at the starting point is at most gₘₐₓ:
///
/// d_f = min(1, gₘₐₓ / ‖∇f(x₀)‖_∞)
/// d_c[j] = min(1, gₘₐₓ / ‖∇cⱼ(x₀)‖_∞)
///
/// See §3.8 Automatic Scaling of the Problem Statement in [2].
///
/// @param g Cost gradient ∇f, evaluated at the starting point.
/// @param A_e Equality constraint Jacobian Aₑ, evaluated at the starting
/// point.
/// @param A_i Inequality constraint Jacobian Aᵢ, evaluated at the starting
/// point.
ProblemScaling(const DenseVector& g, const SparseMatrix& A_e,
const SparseMatrix& A_i) {
constexpr Scalar g_max(100);
f = std::min(Scalar(1), g_max / g.template lpNorm<Eigen::Infinity>());
c_e = (g_max / sparse_inf_norms(A_e).array()).min(Scalar(1)).matrix();
c_i = (g_max / sparse_inf_norms(A_i).array()).min(Scalar(1)).matrix();
}
/// Whether the problem scaling is identity.
///
/// @return True if the problem scaling is identity.
bool is_identity() const {
return f == Scalar(1) && c_e.size() == 0 && c_i.size() == 0;
}
};
} // namespace slp

213
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/regularized_ldlt.hpp vendored
View File

@@ -2,14 +2,11 @@
#pragma once
#include <Eigen/Cholesky>
#include <Eigen/Core>
#include <Eigen/SparseCholesky>
#include <Eigen/SparseCore>
#include "sleipnir/optimization/solver/util/inertia.hpp"
// See docs/algorithms.md#Works_cited for citation definitions
#include "sleipnir/optimization/solver/util/dense_regularized_ldlt.hpp"
#include "sleipnir/optimization/solver/util/sparse_regularized_ldlt.hpp"
namespace slp {
@@ -19,8 +16,6 @@ namespace slp {
template <typename Scalar>
class RegularizedLDLT {
public:
/// Type alias for dense matrix.
using DenseMatrix = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
/// Type alias for dense vector.
using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
/// Type alias for sparse matrix.
@@ -28,118 +23,60 @@ class RegularizedLDLT {
/// Constructs a RegularizedLDLT instance.
///
/// @param use_sparse_solver Whether to use sparse or dense solver.
/// @param num_decision_variables The number of decision variables in the
/// system.
/// @param num_equality_constraints The number of equality constraints in the
/// system.
RegularizedLDLT(int num_decision_variables, int num_equality_constraints)
: m_num_decision_variables{num_decision_variables},
m_num_equality_constraints{num_equality_constraints} {}
RegularizedLDLT(bool use_sparse_solver, int num_decision_variables,
int num_equality_constraints)
: m_use_sparse_solver{use_sparse_solver},
m_sparse_solver{num_decision_variables, num_equality_constraints},
m_dense_solver{num_decision_variables, num_equality_constraints} {}
/// Constructs a RegularizedLDLT instance.
///
/// @param use_sparse_solver Whether to use sparse or dense solver.
/// @param num_decision_variables The number of decision variables in the
/// system.
/// @param num_equality_constraints The number of equality constraints in the
/// system.
/// @param γ_min The minimum constraint regularization.
RegularizedLDLT(int num_decision_variables, int num_equality_constraints,
Scalar γ_min)
: m_num_decision_variables{num_decision_variables},
m_num_equality_constraints{num_equality_constraints},
m_γ_min{γ_min} {}
RegularizedLDLT(bool use_sparse_solver, int num_decision_variables,
int num_equality_constraints, Scalar γ_min)
: m_use_sparse_solver{use_sparse_solver},
m_sparse_solver{num_decision_variables, num_equality_constraints,
γ_min},
m_dense_solver{num_decision_variables, num_equality_constraints,
γ_min} {}
/// Reports whether previous computation was successful.
///
/// @return Whether previous computation was successful.
Eigen::ComputationInfo info() const { return m_info; }
Eigen::ComputationInfo info() const {
if (m_use_sparse_solver) {
return m_sparse_solver.info();
} else {
return m_dense_solver.info();
}
}
/// Computes the regularized LDLT factorization of a matrix.
///
/// In sparse mode, the matrix's symbolic decomposition is reused in
/// subsequent calls, so subsequent calls must be given a matrix with the same
/// sparsity pattern.
///
/// @param lhs Left-hand side of the system.
/// @return The factorization.
RegularizedLDLT& compute(const SparseMatrix& lhs) {
// The regularization procedure is based on algorithm B.1 of [1]
// Max density is 50% due to the caller only providing the lower triangle.
// We consider less than 25% to be sparse.
m_is_sparse = lhs.nonZeros() < 0.25 * lhs.size();
m_info = m_is_sparse ? compute_sparse(lhs).info()
: m_dense_solver.compute(lhs).info();
if (m_info == Eigen::Success) {
auto D =
m_is_sparse ? m_sparse_solver.vectorD() : m_dense_solver.vectorD();
// If the inertia is ideal and D from LDLT is sufficiently far from zero,
// don't regularize the system
if (Inertia{D} == ideal_inertia &&
(D.cwiseAbs().array() >= Scalar(1e-4)).all()) {
m_prev_δ = Scalar(0);
return *this;
}
if (m_use_sparse_solver) {
m_sparse_solver.compute(lhs);
} else {
m_dense_solver.compute(lhs);
}
// Also regularize the Hessian. If the Hessian wasn't regularized in a
// previous run of compute(), start at small values of δ and γ. Otherwise,
// attempt a δ and γ half as big as the previous run so δ and γ can trend
// downwards over time.
Scalar δ = m_prev_δ == Scalar(0) ? Scalar(1e-4) : m_prev_δ / Scalar(2);
Scalar γ = m_γ_min;
while (true) {
Inertia inertia;
// Regularize lhs by adding a multiple of the identity matrix
//
// lhs = [H + AᵢᵀΣAᵢ + δI Aₑᵀ]
// [ Aₑ −γI]
if (m_is_sparse) {
m_info = compute_sparse(lhs + regularization(δ, γ)).info();
if (m_info == Eigen::Success) {
inertia = Inertia{m_sparse_solver.vectorD()};
}
} else {
m_info = m_dense_solver.compute(lhs + regularization(δ, γ)).info();
if (m_info == Eigen::Success) {
inertia = Inertia{m_dense_solver.vectorD()};
}
}
if (m_info == Eigen::Success) {
if (inertia == ideal_inertia) {
// If the inertia is ideal, store δ and return
m_prev_δ = δ;
return *this;
} else if (inertia.zero > 0) {
// If there's zero eigenvalues, increase δ and γ by an order of
// magnitude and try again
δ *= Scalar(10);
γ *= Scalar(10);
} else if (inertia.negative > ideal_inertia.negative) {
// If there's too many negative eigenvalues, increase δ by an order of
// magnitude and try again
δ *= Scalar(10);
} else if (inertia.positive > ideal_inertia.positive) {
// If there's too many positive eigenvalues, increase γ by an order of
// magnitude and try again
γ = γ == Scalar(0) ? Scalar(1e-10) : γ * Scalar(10);
}
} else {
// If the decomposition failed, increase δ and γ by an order of
// magnitude and try again
δ *= Scalar(10);
γ *= Scalar(10);
}
// If the Hessian perturbation is too high, report failure. This can be
// caused by ill-conditioning.
if (δ > Scalar(1e20) || γ > Scalar(1e20)) {
m_info = Eigen::NumericalIssue;
return *this;
}
}
return *this;
}
/// Solves the system of equations using a regularized LDLT factorization.
@@ -148,7 +85,7 @@ class RegularizedLDLT {
/// @return The solution.
template <typename Rhs>
DenseVector solve(const Eigen::MatrixBase<Rhs>& rhs) const {
if (m_is_sparse) {
if (m_use_sparse_solver) {
return m_sparse_solver.solve(rhs);
} else {
return m_dense_solver.solve(rhs);
@@ -161,77 +98,39 @@ class RegularizedLDLT {
/// @return The solution.
template <typename Rhs>
DenseVector solve(const Eigen::SparseMatrixBase<Rhs>& rhs) const {
if (m_is_sparse) {
if (m_use_sparse_solver) {
return m_sparse_solver.solve(rhs);
} else {
return m_dense_solver.solve(rhs.toDense());
return m_dense_solver.solve(rhs);
}
}
/// Returns the Hessian regularization factor.
///
/// @return Hessian regularization factor.
Scalar hessian_regularization() const { return m_prev_δ; }
Scalar hessian_regularization() const {
if (m_use_sparse_solver) {
return m_sparse_solver.hessian_regularization();
} else {
return m_dense_solver.hessian_regularization();
}
}
/// Returns the constraint Jacobian regularization factor.
///
/// @return Constraint Jacobian regularization factor.
Scalar constraint_jacobian_regularization() const {
if (m_use_sparse_solver) {
return m_sparse_solver.constraint_jacobian_regularization();
} else {
return m_dense_solver.constraint_jacobian_regularization();
}
}
private:
using SparseSolver = Eigen::SimplicialLDLT<SparseMatrix>;
using DenseSolver = Eigen::LDLT<DenseMatrix>;
SparseSolver m_sparse_solver;
DenseSolver m_dense_solver;
bool m_is_sparse = true;
Eigen::ComputationInfo m_info = Eigen::Success;
/// The number of decision variables in the system.
int m_num_decision_variables = 0;
/// The number of equality constraints in the system.
int m_num_equality_constraints = 0;
/// The minimum constraint regularization.
Scalar m_γ_min{1e-10};
/// The ideal system inertia.
Inertia ideal_inertia{m_num_decision_variables, m_num_equality_constraints,
0};
/// The value of δ from the previous run of compute().
Scalar m_prev_δ{0};
// Number of non-zeros in LHS.
int m_non_zeros = -1;
/// Computes factorization of a sparse matrix.
///
/// @param lhs Matrix to factorize.
/// @return The factorization.
SparseSolver& compute_sparse(const SparseMatrix& lhs) {
// Reanalize lhs's sparsity pattern if it changed
int non_zeros = lhs.nonZeros();
if (m_non_zeros != non_zeros) {
m_sparse_solver.analyzePattern(lhs);
m_non_zeros = non_zeros;
}
m_sparse_solver.factorize(lhs);
return m_sparse_solver;
}
/// Returns regularization matrix.
///
/// @param δ The Hessian regularization factor.
/// @param γ The equality constraint Jacobian regularization factor.
/// @return Regularization matrix.
SparseMatrix regularization(Scalar δ, Scalar γ) const {
DenseVector vec{m_num_decision_variables + m_num_equality_constraints};
vec.segment(0, m_num_decision_variables).setConstant(δ);
vec.segment(m_num_decision_variables, m_num_equality_constraints)
.setConstant(-γ);
return SparseMatrix{vec.asDiagonal()};
}
bool m_use_sparse_solver;
SparseRegularizedLDLT<Scalar> m_sparse_solver;
DenseRegularizedLDLT<Scalar> m_dense_solver;
};
} // namespace slp

34
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/sparse_inf_norms.hpp vendored Normal file
View File

@@ -0,0 +1,34 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <algorithm>
#include <Eigen/Core>
#include <Eigen/SparseCore>
namespace slp {
/// Computes the row-wise infinity norm of a sparse matrix.
///
/// @param mat Sparse matrix.
/// @return A dense vector where each element is the infinity norm of the
/// corresponding row of the sparse matrix.
template <typename Derived>
auto sparse_inf_norms(const Eigen::SparseCompressedBase<Derived>& mat)
-> Eigen::Vector<typename Derived::Scalar, Eigen::Dynamic> {
using Scalar = typename Derived::Scalar;
using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
DenseVector norms = DenseVector::Zero(mat.rows());
for (int k = 0; k < mat.outerSize(); ++k) {
for (typename Derived::InnerIterator it{mat, k}; it; ++it) {
using std::abs;
norms[it.row()] = std::max(norms[it.row()], abs(it.value()));
}
}
return norms;
}
} // namespace slp

227
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/sparse_regularized_ldlt.hpp vendored Normal file
View File

@@ -0,0 +1,227 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <algorithm>
#include <limits>
#include <Eigen/Core>
#include <Eigen/SparseCholesky>
#include <Eigen/SparseCore>
#include "sleipnir/optimization/solver/util/inertia.hpp"
namespace slp {
/// Solves sparse systems of linear equations using a regularized LDLT
/// factorization.
///
/// @tparam Scalar Scalar type.
template <typename Scalar>
class SparseRegularizedLDLT {
public:
/// Type alias for dense vector.
using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
/// Type alias for sparse matrix.
using SparseMatrix = Eigen::SparseMatrix<Scalar>;
/// Constructs a SparseRegularizedLDLT instance.
///
/// @param num_decision_variables The number of decision variables in the
/// system.
/// @param num_equality_constraints The number of equality constraints in the
/// system.
SparseRegularizedLDLT(int num_decision_variables,
int num_equality_constraints)
: m_num_decision_variables{num_decision_variables},
m_num_equality_constraints{num_equality_constraints} {}
/// Constructs a SparseRegularizedLDLT instance.
///
/// @param num_decision_variables The number of decision variables in the
/// system.
/// @param num_equality_constraints The number of equality constraints in the
/// system.
/// @param γ_min The minimum constraint regularization.
SparseRegularizedLDLT(int num_decision_variables,
int num_equality_constraints, Scalar γ_min)
: m_num_decision_variables{num_decision_variables},
m_num_equality_constraints{num_equality_constraints},
m_γ_min{γ_min} {}
/// Reports whether previous computation was successful.
///
/// @return Whether previous computation was successful.
Eigen::ComputationInfo info() const { return m_info; }
/// Computes the regularized LDLT factorization of a matrix.
///
/// The matrix's symbolic decomposition is reused in subsequent calls, so
/// subsequent calls must be given a matrix with the same sparsity pattern.
///
/// @param lhs Left-hand side of the system.
/// @return The factorization.
SparseRegularizedLDLT& compute(const SparseMatrix& lhs) {
// Regularization with zeros ensures the pattern analysis in the sparse
// solver is reused by all factorizations
SparseMatrix unregularized_lhs = lhs + regularization(Scalar(0), Scalar(0));
if (!m_analyzed_pattern) {
m_solver.analyzePattern(unregularized_lhs);
m_analyzed_pattern = true;
}
m_solver.factorize(unregularized_lhs);
m_info = m_solver.info();
if (m_info == Eigen::Success) {
auto D = m_solver.vectorD();
// If the inertia is ideal and D from LDLT is sufficiently far from zero,
// don't regularize the system
if (Inertia{D} == ideal_inertia &&
(D.cwiseAbs().array() >= Scalar(1e-4)).all()) {
m_prev_δ = Scalar(0);
m_prev_γ = Scalar(0);
return *this;
}
}
// We'll give lhs the correct inertia by adding [δI, 0; 0, −γI] where δ and
// γ regularize the Hessian and equality constraint Jacobian respectively.
// If the previous δ was zero, attempt a small value. Otherwise, attempt a
// smaller value than the previous δ so δ trends downward.
Scalar δ = m_prev_δ == Scalar(0)
? Scalar(1e-4)
: std::max(m_prev_δ / Scalar(2),
std::numeric_limits<Scalar>::epsilon());
// Start γ at the minimum to minimize equality constraint Jacobian
// distortion.
Scalar γ = m_γ_min;
while (true) {
m_solver.factorize(lhs + regularization(δ, γ));
m_info = m_solver.info();
if (m_info == Eigen::Success) {
Inertia inertia{m_solver.vectorD()};
if (inertia == ideal_inertia) {
// If the inertia is ideal, report success
m_prev_δ = δ;
m_prev_γ = γ;
return *this;
} else if (inertia.zero > 0) {
if (γ == Scalar(0)) {
// If there's zero eigenvalues and γ = 0, increase γ to potentially
// compensate for a rank-deficient equality constraint Jacobian
γ = Scalar(1e-10);
} else {
// If there's zero eigenvalues and γ > 0, increase δ and γ to drive
// all eigenvalues away from zero
δ *= Scalar(10);
γ *= Scalar(10);
}
} else if (inertia.negative > ideal_inertia.negative) {
// If there's too many negative eigenvalues, increase δ to add more
// positive eigenvalues
δ *= Scalar(10);
} else if (inertia.positive > ideal_inertia.positive) {
// If there's too many positive eigenvalues, increase γ to add more
// negative eigenvalues
γ = γ == Scalar(0) ? Scalar(1e-10) : γ * Scalar(10);
}
} else {
// If the decomposition failed, increase δ and γ to drive all
// eigenvalues away from zero
δ *= Scalar(10);
γ = γ == Scalar(0) ? Scalar(1e-10) : γ * Scalar(10);
}
// If the lhs perturbation is too high, report failure. This can be caused
// by ill-conditioning.
if (δ > Scalar(1e20) || γ > Scalar(1e20)) {
m_info = Eigen::NumericalIssue;
m_prev_δ = δ;
m_prev_γ = γ;
return *this;
}
}
}
/// Solves the system of equations using a regularized LDLT factorization.
///
/// @param rhs Right-hand side of the system.
/// @return The solution.
template <typename Rhs>
DenseVector solve(const Eigen::MatrixBase<Rhs>& rhs) const {
return m_solver.solve(rhs);
}
/// Solves the system of equations using a regularized LDLT factorization.
///
/// @param rhs Right-hand side of the system.
/// @return The solution.
template <typename Rhs>
DenseVector solve(const Eigen::SparseMatrixBase<Rhs>& rhs) const {
return m_solver.solve(rhs);
}
/// Returns the Hessian regularization factor.
///
/// @return Hessian regularization factor.
Scalar hessian_regularization() const { return m_prev_δ; }
/// Returns the constraint Jacobian regularization factor.
///
/// @return Constraint Jacobian regularization factor.
Scalar constraint_jacobian_regularization() const { return m_prev_γ; }
private:
using Solver = Eigen::SimplicialLDLT<SparseMatrix>;
Solver m_solver;
bool m_analyzed_pattern = false;
Eigen::ComputationInfo m_info = Eigen::Success;
/// The number of decision variables in the system.
int m_num_decision_variables = 0;
/// The number of equality constraints in the system.
int m_num_equality_constraints = 0;
/// The minimum constraint regularization.
Scalar m_γ_min{1e-10};
/// The ideal system inertia.
Inertia ideal_inertia{m_num_decision_variables, m_num_equality_constraints,
0};
/// The value of δ from the previous run of compute().
Scalar m_prev_δ{0};
/// The value of γ from the previous run of compute().
Scalar m_prev_γ{0};
/// Returns regularization matrix.
///
/// [δI 0]
/// [ 0 −γI]
///
/// @param δ The Hessian regularization factor.
/// @param γ The equality constraint Jacobian regularization factor.
/// @return Regularization matrix.
SparseMatrix regularization(Scalar δ, Scalar γ) const {
DenseVector vec{m_num_decision_variables + m_num_equality_constraints};
vec.segment(0, m_num_decision_variables).setConstant(δ);
vec.segment(m_num_decision_variables, m_num_equality_constraints)
.setConstant(-γ);
return SparseMatrix{vec.asDiagonal()};
}
};
} // namespace slp

87
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/print_diagnostics.hpp vendored
View File

@@ -25,10 +25,8 @@ namespace slp {
enum class IterationType : uint8_t {
/// Normal iteration.
NORMAL,
/// Accepted second-order correction iteration.
ACCEPTED_SOC,
/// Rejected second-order correction iteration.
REJECTED_SOC,
/// Second-order correction iteration.
SECOND_ORDER_CORRECTION,
/// Feasibility restoration iteration.
FEASIBILITY_RESTORATION
};
@@ -73,8 +71,8 @@ std::string power_of_10(Scalar value) {
if (exponent < 0) {
output += "⁻";
}
constexpr std::array strs = {"⁰", "¹", "²", "³", "⁴",
"⁵", "⁶", "⁷", "⁸", "⁹"};
constexpr std::array strs{"⁰", "¹", "²", "³", "⁴",
"⁵", "⁶", "⁷", "⁸", "⁹"};
for (const auto& digit : digits | std::views::reverse) {
output += strs[digit];
}
@@ -183,6 +181,9 @@ inline void print_bound_constraint_global_infeasibility_error(
/// @param complementarity The complementarity.
/// @param μ The barrier parameter.
/// @param δ The Hessian regularization factor.
/// @param γ The constraint Jacobian regularization factor.
/// @param full_primal_step_inf_norm The infinity norm of the full primal step.
/// @param full_dual_step_inf_norm The infinity norm of the full dual step.
/// @param primal_α The primal step size.
/// @param primal_α_max The max primal step size.
/// @param α_reduction_factor Factor by which primal_α is reduced during
@@ -193,33 +194,23 @@ void print_iteration_diagnostics(int iterations, IterationType type,
const std::chrono::duration<Rep, Period>& time,
Scalar error, Scalar cost,
Scalar infeasibility, Scalar complementarity,
Scalar μ, Scalar δ, Scalar primal_α,
Scalar primal_α_max, Scalar α_reduction_factor,
Scalar dual_α) {
Scalar μ, Scalar δ, Scalar γ,
Scalar full_primal_step_inf_norm,
Scalar full_dual_step_inf_norm,
Scalar primal_α, Scalar primal_α_max,
Scalar α_reduction_factor, Scalar dual_α) {
if (iterations % 20 == 0) {
if (iterations == 0) {
slp::print("┏");
slp::println("┏{:━^119}┓", "");
} else {
slp::print("┢");
}
slp::print(
"{:━^4}┯{:━^4}┯{:━^9}┯{:━^12}┯{:━^13}┯{:━^12}┯{:━^12}┯{:━^8}┯{:━^5}┯"
"{:━^8}┯{:━^8}┯{:━^2}",
"", "", "", "", "", "", "", "", "", "", "", "");
if (iterations == 0) {
slp::println("┓");
} else {
slp::println("┪");
slp::println("┢{:━^119}┪", "");
}
slp::println(
"┃{:^4}│{:^4}│{:^9}│{:^12}│{:^13}│{:^12}│{:^12}│{:^8}│{:^5}│{:^8}│{:^8}"
"│{:^2}┃",
"iter", "type", "time (ms)", "error", "cost", "infeas.", "complement.",
"μ", "reg", "primal α", "dual α", "↩");
slp::println(
"┡{:━^4}┷{:━^4}┷{:━^9}┷{:━^12}┷{:━^13}┷{:━^12}┷{:━^12}┷{:━^8}┷{:━^5}┷"
"{:━^8}┷{:━^8}┷{:━^2}┩",
"", "", "", "", "", "", "", "", "", "", "", "");
"┃{:^4} {:^9} {:^10} {:^11} {:^10} {:^8} {:^8} {:^5} {:^5} {:^8} "
"{:^8} {:^8} {:^8} {:^2}┃",
"iter", "duration", "error", "cost", "infeas.", "complem.", "μ", "δ",
"γ", "|p_pr|", "|p_du|", "α_pr", "α_du", "↩");
slp::println("┡{:━^119}┩", "");
}
// For the number of backtracks, we want x such that:
@@ -236,12 +227,13 @@ void print_iteration_diagnostics(int iterations, IterationType type,
int backtracks =
static_cast<int>(log(primal_α / primal_α_max) / log(α_reduction_factor));
constexpr std::array ITERATION_TYPES = {"norm", "✓SOC", "XSOC", "rest"};
constexpr std::array ITERATION_TYPES{" ", "s", "r"};
slp::println(
"│{:4} {:4} {:9.3f} {:12e} {:13e} {:12e} {:12e} {:.2e} {:<5} {:.2e} "
"{:.2e} {:2d}│",
"│{:4} {:1} {:9.3f} {:10.4e} {:11.4e} {:10.4e} {:8.2e} {:8.2e} {:<5} "
"{:<5} {:8.2e} {:8.2e} {:8.2e} {:8.2e} {:2d}│",
iterations, ITERATION_TYPES[slp::to_underlying(type)], to_ms(time), error,
cost, infeasibility, complementarity, μ, power_of_10(δ), primal_α, dual_α,
cost, infeasibility, complementarity, μ, power_of_10(δ), power_of_10(γ),
full_primal_step_inf_norm, full_dual_step_inf_norm, primal_α, dual_α,
backtracks);
}
#else
@@ -251,7 +243,7 @@ void print_iteration_diagnostics(int iterations, IterationType type,
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
/// Prints bottom of iteration diagnostics table.
inline void print_bottom_iteration_diagnostics() {
slp::println("└{:─^108}┘", "");
slp::println("└{:─^119}┘", "");
}
#else
#define print_bottom_iteration_diagnostics(...)
@@ -269,7 +261,7 @@ std::string histogram(double value) {
double ipart;
int fpart = static_cast<int>(std::modf(value * Width, &ipart) * 8);
constexpr std::array strs = {" ", "▏", "▎", "▍", "▌", "▋", "▊", "▉", "█"};
constexpr std::array strs{" ", "▏", "▎", "▍", "▌", "▋", "▊", "▉", "█"};
std::string hist;
int index = 0;
@@ -297,10 +289,10 @@ inline void print_solver_diagnostics(
const gch::small_vector<SolveProfiler>& solve_profilers) {
auto solve_duration = to_ms(solve_profilers[0].total_duration());
slp::println("┏{:━^21}┯{:━^18}┯{:━^10}┯{:━^9}┯{:━^4}┓", "", "", "", "", "");
slp::println("┃{:^21}│{:^18}│{:^10}│{:^9}│{:^4}┃", "solver trace", "percent",
"total (ms)", "each (ms)", "runs");
slp::println("┡{:━^21}┷{:━^18}┷{:━^10}┷{:━^9}┷{:━^4}┩", "", "", "", "", "");
slp::println("┏{:━^66}┓", "");
slp::println("┃{:^21} {:^18} {:^10} {:^9} {:^4}┃", "time trace", "percentage",
"total", "each", "runs");
slp::println("┡{:━^66}┩", "");
for (auto& profiler : solve_profilers) {
double norm = solve_duration == 0.0
@@ -326,23 +318,26 @@ inline void print_setup_diagnostics(
const gch::small_vector<SetupProfiler>& setup_profilers) {
auto setup_duration = to_ms(setup_profilers[0].duration());
// Print link to diagnostic output description
slp::println(
"See https://sleipnirgroup.github.io/Sleipnir/md_usage.html#output for "
"diagnostic output description.\n");
// Print heading
slp::println("┏{:━^21}┯{:━^18}┯{:━^10}┯{:━^9}┯{:━^4}┓", "", "", "", "", "");
slp::println("┃{:^21}│{:^18}│{:^10}│{:^9}│{:^4}┃", "setup trace", "percent",
"total (ms)", "each (ms)", "runs");
slp::println("┡{:━^21}┷{:━^18}┷{:━^10}┷{:━^9}┷{:━^4}┩", "", "", "", "", "");
slp::println("┏{:━^50}┓", "");
slp::println("┃{:^21} {:^18} {:^9}┃", "time trace", "percentage", "duration");
slp::println("┡{:━^50}┩", "");
// Print setup profilers
for (auto& profiler : setup_profilers) {
double norm = setup_duration == 0.0
? (&profiler == &setup_profilers[0] ? 1.0 : 0.0)
: to_ms(profiler.duration()) / setup_duration;
slp::println("│{:<21} {:>6.2f}%▕{}▏ {:>10.3f} {:>9.3f} {:>4}│",
profiler.name(), norm * 100.0, histogram<9>(norm),
to_ms(profiler.duration()), to_ms(profiler.duration()), "1");
slp::println("│{:<21} {:>6.2f}%▕{}▏ {:>9.3f}│", profiler.name(),
norm * 100.0, histogram<9>(norm), to_ms(profiler.duration()));
}
slp::println("└{:─^66}┘", "");
slp::println("└{:─^50}┘", "");
}
#else
#define print_setup_diagnostics(...)

2
wpimath/src/test/java/org/wpilib/math/optimization/FlywheelOCPTest.java
View File

@@ -103,7 +103,7 @@ class FlywheelOCPTest {
// steady-states.
assertEquals(u, problem.U().value(0, k), 2.0);
} else {
assertEquals(u, problem.U().value(0, k), 1e-4);
assertEquals(u, problem.U().value(0, k), 2e-4);
}
}

5
wpimath/src/test/java/org/wpilib/math/optimization/solver/ExitStatusTest.java
View File

@@ -27,13 +27,16 @@ class ExitStatusTest {
problem.minimize(x.times(x));
problem.addCallback(info -> false);
x.setValue(1.0);
assertEquals(ExitStatus.SUCCESS, problem.solve());
problem.addCallback(info -> true);
x.setValue(1.0);
assertEquals(ExitStatus.CALLBACK_REQUESTED_STOP, problem.solve());
problem.clearCallbacks();
problem.addCallback(info -> false);
x.setValue(1.0);
assertEquals(ExitStatus.SUCCESS, problem.solve());
}
@@ -187,6 +190,7 @@ class ExitStatusTest {
try (var problem = new Problem()) {
var x = problem.decisionVariable();
x.setValue(1.0);
problem.minimize(x.times(x));
@@ -207,6 +211,7 @@ class ExitStatusTest {
try (var problem = new Problem()) {
var x = problem.decisionVariable();
x.setValue(1.0);
problem.minimize(x.times(x));