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 Sleipnir (#7973)

Browse Source
This commit is contained in:
Tyler Veness
2025-05-27 07:24:15 -07:00
committed by GitHub
parent 5368e8c6ed
commit de718f7ae5
90 changed files with 11188 additions and 7917 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
upstream_utils/sleipnir.py
View File

@@ -24,10 +24,7 @@ def copy_upstream_src(wpilib_root):
# Copy Sleipnir header files into allwpilib
include_files = [
os.path.join(dp, f)
for dp, dn, fn in os.walk("include")
for f in fn
if not f.endswith("small_vector.hpp")
os.path.join(dp, f) for dp, dn, fn in os.walk("include") for f in fn
]
include_files = copy_to(
include_files, os.path.join(wpimath, "src/main/native/thirdparty/sleipnir")
@@ -44,12 +41,42 @@ def copy_upstream_src(wpilib_root):
os.path.join(wpimath, "src/main/native/thirdparty/sleipnir", filename),
)
# Write shim for wpi::SmallVector
try:
os.mkdir(
os.path.join(wpimath, "src/main/native/thirdparty/sleipnir/include/gch")
)
except:
pass
with open(
os.path.join(
wpimath,
"src/main/native/thirdparty/sleipnir/include/gch/small_vector.hpp",
),
"w",
) as f:
f.write(
"""// Copyright (c) Sleipnir contributors
#pragma once
#include <wpi/SmallVector.h>
namespace gch {
template <typename T>
using small_vector = wpi::SmallVector<T>;
} // namespace gch
"""
)
def main():
name = "sleipnir"
url = "https://github.com/SleipnirGroup/Sleipnir"
# main on 2024-12-07
tag = "01206ab17d741f4c45a7faeb56b8a5442df1681c"
# main on 2025-05-18
tag = "2cc18ff6d25ee0a9bd0f9993a0a41a61a28bda3e"
sleipnir = Lib(name, url, tag, copy_upstream_src)
sleipnir.main()

43
upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch
View File

@@ -1,30 +1,32 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Wed, 29 May 2024 16:29:55 -0700
Subject: [PATCH 1/3] Use fmtlib
Subject: [PATCH 1/8] Use fmtlib
---
include/.styleguide | 1 +
include/sleipnir/util/Print.hpp | 31 ++++++++++++++++++-------------
2 files changed, 19 insertions(+), 13 deletions(-)
include/sleipnir/util/print.hpp | 31 ++++++++++++++++++-------------
src/optimization/problem.cpp | 2 +-
3 files changed, 20 insertions(+), 14 deletions(-)
diff --git a/include/.styleguide b/include/.styleguide
index 8fb61fdf9cc5ceff633d3126f0579eef25a1326f..6a7f8ed28f9cb037c9746a7e0ef5e110481d9825 100644
index 1b6652d3d5886cf8c9eca0d855c21031775bad7c..4f4c76204071f90bf49eddb8c2aceb583b5e09ba 100644
--- a/include/.styleguide
+++ b/include/.styleguide
@@ -12,4 +12,5 @@ licenseUpdateExclude {
@@ -8,5 +8,6 @@ cppSrcFileInclude {
includeOtherLibs {
^Eigen/
+ ^fmt/
^gch/
}
diff --git a/include/sleipnir/util/Print.hpp b/include/sleipnir/util/Print.hpp
index a746cb77b70f095bb15f4c493295cb925bc74cd3..c01fd4ac679df854b885293d681ea1e0984626fa 100644
--- a/include/sleipnir/util/Print.hpp
+++ b/include/sleipnir/util/Print.hpp
@@ -3,52 +3,57 @@
#pragma once
diff --git a/include/sleipnir/util/print.hpp b/include/sleipnir/util/print.hpp
index fe430352dabf4cd6a890dc8007237c7a261dfd4b..055d5c9fa246201f1d8ae7ddca00b1159aeb2a57 100644
--- a/include/sleipnir/util/print.hpp
+++ b/include/sleipnir/util/print.hpp
@@ -4,10 +4,15 @@
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
#include <cstdio>
-#include <print>
#include <system_error>
@@ -36,7 +38,11 @@ index a746cb77b70f095bb15f4c493295cb925bc74cd3..c01fd4ac679df854b885293d681ea1e0
+#include <fmt/core.h>
+#endif
+
namespace sleipnir {
#endif
namespace slp {
@@ -15,45 +20,45 @@ namespace slp {
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
/**
- * Wrapper around std::print() that squelches write failure exceptions.
@@ -93,3 +99,16 @@ index a746cb77b70f095bb15f4c493295cb925bc74cd3..c01fd4ac679df854b885293d681ea1e0
} catch (const std::system_error&) {
}
}
diff --git a/src/optimization/problem.cpp b/src/optimization/problem.cpp
index 31115490867146ec166604bcc61731d7891a9f22..81863808d329a53d4162ce0624a3b8e8afc32dfc 100644
--- a/src/optimization/problem.cpp
+++ b/src/optimization/problem.cpp
@@ -335,7 +335,7 @@ void Problem::print_exit_conditions([[maybe_unused]] const Options& options) {
slp::println(" ↳ executed {} iterations", options.max_iterations);
}
if (std::isfinite(options.timeout.count())) {
- slp::println(" ↳ {} elapsed", options.timeout);
+ slp::println(" ↳ {} elapsed", options.timeout.count());
}
}

639
upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch
View File

@@ -1,610 +1,77 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Sun, 16 Jun 2024 12:08:49 -0700
Subject: [PATCH 2/3] Use wpi::SmallVector
Subject: [PATCH 2/8] Use wpi::SmallVector
---
include/.styleguide | 1 +
include/sleipnir/autodiff/Expression.hpp | 13 +++++++------
include/sleipnir/autodiff/ExpressionGraph.hpp | 15 ++++++++-------
include/sleipnir/autodiff/Hessian.hpp | 4 ++--
include/sleipnir/autodiff/Jacobian.hpp | 10 +++++-----
include/sleipnir/autodiff/Variable.hpp | 10 +++++-----
include/sleipnir/autodiff/VariableMatrix.hpp | 4 ++--
include/sleipnir/optimization/Multistart.hpp | 7 ++++---
.../optimization/OptimizationProblem.hpp | 8 ++++----
include/sleipnir/util/Pool.hpp | 7 ++++---
include/sleipnir/util/Spy.hpp | 4 ++--
src/.styleguide | 1 +
src/optimization/solver/InteriorPoint.cpp | 4 ++--
src/optimization/solver/SQP.cpp | 4 ++--
.../solver/util/FeasibilityRestoration.hpp | 18 +++++++++---------
src/optimization/solver/util/Filter.hpp | 4 ++--
16 files changed, 60 insertions(+), 54 deletions(-)
include/sleipnir/autodiff/expression.hpp | 4 ++--
include/sleipnir/autodiff/variable.hpp | 5 ++---
include/sleipnir/autodiff/variable_matrix.hpp | 4 ++--
3 files changed, 6 insertions(+), 7 deletions(-)
diff --git a/include/.styleguide b/include/.styleguide
index 6a7f8ed28f9cb037c9746a7e0ef5e110481d9825..efa36cee1fb593ae810032340c64f1854fbbc523 100644
--- a/include/.styleguide
+++ b/include/.styleguide
@@ -13,4 +13,5 @@ licenseUpdateExclude {
includeOtherLibs {
^Eigen/
^fmt/
+ ^wpi/
}
diff --git a/include/sleipnir/autodiff/Expression.hpp b/include/sleipnir/autodiff/Expression.hpp
index dd53755ccae88e3975d1b5e6b13ac464bd4efcce..ef9a15bf69d8cae6b2196513b72ec4b359cc8752 100644
--- a/include/sleipnir/autodiff/Expression.hpp
+++ b/include/sleipnir/autodiff/Expression.hpp
@@ -11,11 +11,12 @@
#include <numbers>
#include <utility>
diff --git a/include/sleipnir/autodiff/expression.hpp b/include/sleipnir/autodiff/expression.hpp
index 873e1c27559d92eb1b3a217890ca41bdc65af122..1c5f84d22a0bed70869121acabd527825ba90adb 100644
--- a/include/sleipnir/autodiff/expression.hpp
+++ b/include/sleipnir/autodiff/expression.hpp
@@ -30,7 +30,7 @@ inline constexpr bool USE_POOL_ALLOCATOR = true;
struct Expression;
+#include <wpi/SmallVector.h>
+
#include "sleipnir/autodiff/ExpressionType.hpp"
#include "sleipnir/util/IntrusiveSharedPtr.hpp"
#include "sleipnir/util/Pool.hpp"
#include "sleipnir/util/SymbolExports.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir::detail {
@@ -29,8 +30,8 @@ inline constexpr bool kUsePoolAllocator = true;
struct SLEIPNIR_DLLEXPORT Expression;
-inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr);
-inline constexpr void IntrusiveSharedPtrDecRefCount(Expression* expr);
+inline void IntrusiveSharedPtrIncRefCount(Expression* expr);
+inline void IntrusiveSharedPtrDecRefCount(Expression* expr);
inline constexpr void inc_ref_count(Expression* expr);
-inline constexpr void dec_ref_count(Expression* expr);
+inline void dec_ref_count(Expression* expr);
/**
* Typedef for intrusive shared pointer to Expression.
@@ -418,7 +419,7 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt(const ExpressionPtr& x);
@@ -680,7 +680,7 @@ inline constexpr void inc_ref_count(Expression* expr) {
*
* @param expr The shared pointer's managed object.
*/
-inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr) {
+inline void IntrusiveSharedPtrIncRefCount(Expression* expr) {
++expr->refCount;
}
@@ -427,12 +428,12 @@ inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr) {
*
* @param expr The shared pointer's managed object.
*/
-inline constexpr void IntrusiveSharedPtrDecRefCount(Expression* expr) {
+inline void IntrusiveSharedPtrDecRefCount(Expression* expr) {
-inline constexpr void dec_ref_count(Expression* expr) {
+inline void dec_ref_count(Expression* expr) {
// If a deeply nested tree is being deallocated all at once, calling the
// Expression destructor when expr's refcount reaches zero can cause a stack
// overflow. Instead, we iterate over its children to decrement their
// refcounts and deallocate them.
- small_vector<Expression*> stack;
+ wpi::SmallVector<Expression*> stack;
stack.emplace_back(expr);
while (!stack.empty()) {
diff --git a/include/sleipnir/autodiff/ExpressionGraph.hpp b/include/sleipnir/autodiff/ExpressionGraph.hpp
index c614195d82ad022dfd0c3f6f8240b042c0056c2f..714bcbb82907e754138347334c7fca8a7ccf055d 100644
--- a/include/sleipnir/autodiff/ExpressionGraph.hpp
+++ b/include/sleipnir/autodiff/ExpressionGraph.hpp
@@ -4,10 +4,11 @@
#include <span>
+#include <wpi/SmallVector.h>
+
#include "sleipnir/autodiff/Expression.hpp"
#include "sleipnir/util/FunctionRef.hpp"
#include "sleipnir/util/SymbolExports.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir::detail {
@@ -36,7 +37,7 @@ class SLEIPNIR_DLLEXPORT ExpressionGraph {
// https://en.wikipedia.org/wiki/Breadth-first_search
// BFS list sorted from parent to child.
- small_vector<Expression*> stack;
+ wpi::SmallVector<Expression*> stack;
stack.emplace_back(root.Get());
@@ -119,7 +120,7 @@ class SLEIPNIR_DLLEXPORT ExpressionGraph {
*
* @param wrt Variables with respect to which to compute the gradient.
diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp
index 14eb1d3b95069e143699e1488f3081c4cd9de07c..9f79a82763213dc712cce4c2a322289d57645032 100644
--- a/include/sleipnir/autodiff/variable.hpp
+++ b/include/sleipnir/autodiff/variable.hpp
@@ -47,7 +47,7 @@ class SLEIPNIR_DLLEXPORT Variable {
/**
* Constructs an empty Variable.
*/
- small_vector<ExpressionPtr> GenerateGradientTree(
+ wpi::SmallVector<ExpressionPtr> GenerateGradientTree(
std::span<const ExpressionPtr> wrt) const {
// Read docs/algorithms.md#Reverse_accumulation_automatic_differentiation
// for background on reverse accumulation automatic differentiation.
@@ -128,7 +129,7 @@ class SLEIPNIR_DLLEXPORT ExpressionGraph {
wrt[row]->row = row;
}
- small_vector<ExpressionPtr> grad;
+ wpi::SmallVector<ExpressionPtr> grad;
grad.reserve(wrt.size());
for (size_t row = 0; row < wrt.size(); ++row) {
grad.emplace_back(MakeExpressionPtr());
@@ -231,13 +232,13 @@ class SLEIPNIR_DLLEXPORT ExpressionGraph {
private:
// List that maps nodes to their respective row.
- small_vector<int> m_rowList;
+ wpi::SmallVector<int> m_rowList;
// List for updating adjoints
- small_vector<Expression*> m_adjointList;
+ wpi::SmallVector<Expression*> m_adjointList;
// List for updating values
- small_vector<Expression*> m_valueList;
+ wpi::SmallVector<Expression*> m_valueList;
};
} // namespace sleipnir::detail
diff --git a/include/sleipnir/autodiff/Hessian.hpp b/include/sleipnir/autodiff/Hessian.hpp
index 4563aa234bd7b0ec22e12d2fc0b6f4569bee7f39..2e60d89e95280bdac422b0d7dab955ba111b0059 100644
--- a/include/sleipnir/autodiff/Hessian.hpp
+++ b/include/sleipnir/autodiff/Hessian.hpp
@@ -6,6 +6,7 @@
#include <Eigen/Core>
#include <Eigen/SparseCore>
+#include <wpi/SmallVector.h>
#include "sleipnir/autodiff/ExpressionGraph.hpp"
#include "sleipnir/autodiff/Jacobian.hpp"
@@ -13,7 +14,6 @@
#include "sleipnir/autodiff/Variable.hpp"
#include "sleipnir/autodiff/VariableMatrix.hpp"
#include "sleipnir/util/SymbolExports.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir {
@@ -36,7 +36,7 @@ class SLEIPNIR_DLLEXPORT Hessian {
Hessian(Variable variable, const VariableMatrix& wrt) noexcept
: m_jacobian{
[&] {
- small_vector<detail::ExpressionPtr> wrtVec;
+ wpi::SmallVector<detail::ExpressionPtr> wrtVec;
wrtVec.reserve(wrt.size());
for (auto& elem : wrt) {
wrtVec.emplace_back(elem.expr);
diff --git a/include/sleipnir/autodiff/Jacobian.hpp b/include/sleipnir/autodiff/Jacobian.hpp
index ac00c11ef8c947cbe95c58081bdbfb42bf901051..0c660156c80f94539383b8f0d01d7853e41e0297 100644
--- a/include/sleipnir/autodiff/Jacobian.hpp
+++ b/include/sleipnir/autodiff/Jacobian.hpp
@@ -5,13 +5,13 @@
#include <utility>
#include <Eigen/SparseCore>
+#include <wpi/SmallVector.h>
#include "sleipnir/autodiff/ExpressionGraph.hpp"
#include "sleipnir/autodiff/Profiler.hpp"
#include "sleipnir/autodiff/Variable.hpp"
#include "sleipnir/autodiff/VariableMatrix.hpp"
#include "sleipnir/util/SymbolExports.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir {
@@ -81,7 +81,7 @@ class SLEIPNIR_DLLEXPORT Jacobian {
VariableMatrix Get() const {
VariableMatrix result{m_variables.Rows(), m_wrt.Rows()};
- small_vector<detail::ExpressionPtr> wrtVec;
+ wpi::SmallVector<detail::ExpressionPtr> wrtVec;
wrtVec.reserve(m_wrt.size());
for (auto& elem : m_wrt) {
wrtVec.emplace_back(elem.expr);
@@ -138,16 +138,16 @@ class SLEIPNIR_DLLEXPORT Jacobian {
VariableMatrix m_variables;
VariableMatrix m_wrt;
- small_vector<detail::ExpressionGraph> m_graphs;
+ wpi::SmallVector<detail::ExpressionGraph> m_graphs;
Eigen::SparseMatrix<double> m_J{m_variables.Rows(), m_wrt.Rows()};
// Cached triplets for gradients of linear rows
- small_vector<Eigen::Triplet<double>> m_cachedTriplets;
+ wpi::SmallVector<Eigen::Triplet<double>> m_cachedTriplets;
// List of row indices for nonlinear rows whose graients will be computed in
// Value()
- small_vector<int> m_nonlinearRows;
+ wpi::SmallVector<int> m_nonlinearRows;
Profiler m_profiler;
};
diff --git a/include/sleipnir/autodiff/Variable.hpp b/include/sleipnir/autodiff/Variable.hpp
index c04d629f482efe59497570ca1fd9b09a4af2ae1e..d192fb96e7984b7c0ca30262668c41e5e84ca34e 100644
--- a/include/sleipnir/autodiff/Variable.hpp
+++ b/include/sleipnir/autodiff/Variable.hpp
@@ -10,6 +10,7 @@
#include <vector>
#include <Eigen/Core>
+#include <wpi/SmallVector.h>
#include "sleipnir/autodiff/Expression.hpp"
#include "sleipnir/autodiff/ExpressionGraph.hpp"
@@ -17,7 +18,6 @@
#include "sleipnir/util/Concepts.hpp"
#include "sleipnir/util/Print.hpp"
#include "sleipnir/util/SymbolExports.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir {
@@ -445,8 +445,8 @@ template <typename LHS, typename RHS>
(ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
(!std::same_as<std::decay_t<LHS>, double> ||
!std::same_as<std::decay_t<RHS>, double>)
-small_vector<Variable> MakeConstraints(LHS&& lhs, RHS&& rhs) {
- small_vector<Variable> constraints;
+wpi::SmallVector<Variable> MakeConstraints(LHS&& lhs, RHS&& rhs) {
+ wpi::SmallVector<Variable> constraints;
if constexpr (ScalarLike<std::decay_t<LHS>> &&
ScalarLike<std::decay_t<RHS>>) {
@@ -534,7 +534,7 @@ small_vector<Variable> MakeConstraints(LHS&& lhs, RHS&& rhs) {
*/
struct SLEIPNIR_DLLEXPORT EqualityConstraints {
/// A vector of scalar equality constraints.
- small_vector<Variable> constraints;
+ wpi::SmallVector<Variable> constraints;
- explicit constexpr Variable(std::nullptr_t) : expr{nullptr} {}
+ explicit Variable(std::nullptr_t) : expr{nullptr} {}
/**
* Concatenates multiple equality constraints.
@@ -596,7 +596,7 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints {
*/
struct SLEIPNIR_DLLEXPORT InequalityConstraints {
/// A vector of scalar inequality constraints.
- small_vector<Variable> constraints;
+ wpi::SmallVector<Variable> constraints;
* Constructs a Variable from a floating point type.
@@ -77,8 +77,7 @@ class SLEIPNIR_DLLEXPORT Variable {
*
* @param expr The autodiff variable.
*/
- explicit constexpr Variable(detail::ExpressionPtr&& expr)
- : expr{std::move(expr)} {}
+ explicit Variable(detail::ExpressionPtr&& expr) : expr{std::move(expr)} {}
/**
* Concatenates multiple inequality constraints.
diff --git a/include/sleipnir/autodiff/VariableMatrix.hpp b/include/sleipnir/autodiff/VariableMatrix.hpp
index 47452b8988b3a1a96a78d28644200b1c4cdc89c9..57b09913d15e9590873ad7bf62e2baff9fbc5df9 100644
--- a/include/sleipnir/autodiff/VariableMatrix.hpp
+++ b/include/sleipnir/autodiff/VariableMatrix.hpp
@@ -11,6 +11,7 @@
#include <vector>
#include <Eigen/Core>
+#include <wpi/SmallVector.h>
#include "sleipnir/autodiff/Slice.hpp"
#include "sleipnir/autodiff/Variable.hpp"
@@ -18,7 +19,6 @@
#include "sleipnir/util/Assert.hpp"
#include "sleipnir/util/FunctionRef.hpp"
#include "sleipnir/util/SymbolExports.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir {
@@ -946,7 +946,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
}
private:
- small_vector<Variable> m_storage;
+ wpi::SmallVector<Variable> m_storage;
int m_rows = 0;
int m_cols = 0;
};
diff --git a/include/sleipnir/optimization/Multistart.hpp b/include/sleipnir/optimization/Multistart.hpp
index 8055713a2492a9c8473f047a2fb9fe7ca57753c3..09b1b2f3bf33c35ae0aeddb9b5d47c0d80c68cec 100644
--- a/include/sleipnir/optimization/Multistart.hpp
+++ b/include/sleipnir/optimization/Multistart.hpp
@@ -6,9 +6,10 @@
#include <future>
#include <span>
+#include <wpi/SmallVector.h>
+
#include "sleipnir/optimization/SolverStatus.hpp"
#include "sleipnir/util/FunctionRef.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir {
@@ -44,14 +45,14 @@ MultistartResult<DecisionVariables> Multistart(
const DecisionVariables& initialGuess)>
solve,
std::span<const DecisionVariables> initialGuesses) {
- small_vector<std::future<MultistartResult<DecisionVariables>>> futures;
+ wpi::SmallVector<std::future<MultistartResult<DecisionVariables>>> futures;
futures.reserve(initialGuesses.size());
for (const auto& initialGuess : initialGuesses) {
futures.emplace_back(std::async(std::launch::async, solve, initialGuess));
}
- small_vector<MultistartResult<DecisionVariables>> results;
+ wpi::SmallVector<MultistartResult<DecisionVariables>> results;
results.reserve(futures.size());
for (auto& future : futures) {
diff --git a/include/sleipnir/optimization/OptimizationProblem.hpp b/include/sleipnir/optimization/OptimizationProblem.hpp
index 569dcdee507881ceb412585ca811927072552c15..66883fed98ad087010fb153bd91effce6e047928 100644
--- a/include/sleipnir/optimization/OptimizationProblem.hpp
+++ b/include/sleipnir/optimization/OptimizationProblem.hpp
@@ -11,6 +11,7 @@
#include <utility>
#include <Eigen/Core>
+#include <wpi/SmallVector.h>
#include "sleipnir/autodiff/Variable.hpp"
#include "sleipnir/autodiff/VariableMatrix.hpp"
@@ -22,7 +23,6 @@
#include "sleipnir/optimization/solver/SQP.hpp"
#include "sleipnir/util/Print.hpp"
#include "sleipnir/util/SymbolExports.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir {
@@ -364,16 +364,16 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem {
private:
// The list of decision variables, which are the root of the problem's
// expression tree
- small_vector<Variable> m_decisionVariables;
+ wpi::SmallVector<Variable> m_decisionVariables;
// The cost function: f(x)
std::optional<Variable> m_f;
// The list of equality constraints: cₑ(x) = 0
- small_vector<Variable> m_equalityConstraints;
+ wpi::SmallVector<Variable> m_equalityConstraints;
// The list of inequality constraints: cᵢ(x) ≥ 0
- small_vector<Variable> m_inequalityConstraints;
+ wpi::SmallVector<Variable> m_inequalityConstraints;
// The user callback
std::function<bool(const SolverIterationInfo& info)> m_callback =
diff --git a/include/sleipnir/util/Pool.hpp b/include/sleipnir/util/Pool.hpp
index 441fa701d4972bc14973c6269d56d4a124deaee5..1951bd1ee8f3bee8d4a3c044c99354b0fd10031d 100644
--- a/include/sleipnir/util/Pool.hpp
+++ b/include/sleipnir/util/Pool.hpp
@@ -5,8 +5,9 @@
#include <cstddef>
#include <memory>
+#include <wpi/SmallVector.h>
+
#include "sleipnir/util/SymbolExports.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir {
@@ -78,8 +79,8 @@ class SLEIPNIR_DLLEXPORT PoolResource {
}
private:
- small_vector<std::unique_ptr<std::byte[]>> m_buffer;
- small_vector<void*> m_freeList;
+ wpi::SmallVector<std::unique_ptr<std::byte[]>> m_buffer;
+ wpi::SmallVector<void*> m_freeList;
size_t blocksPerChunk;
* Assignment operator for double.
diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp
index 410f12873cfdf5d0d484653c6c3dac74ed96348a..1c6f9e8dade8bebce7aec18bbb9b5491acb1d977 100644
--- a/include/sleipnir/autodiff/variable_matrix.hpp
+++ b/include/sleipnir/autodiff/variable_matrix.hpp
@@ -1120,14 +1120,14 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
*
* @return Begin iterator.
*/
- const_iterator cbegin() const { return const_iterator{m_storage.cbegin()}; }
+ const_iterator cbegin() const { return const_iterator{m_storage.begin()}; }
/**
diff --git a/include/sleipnir/util/Spy.hpp b/include/sleipnir/util/Spy.hpp
index cb9b4e191545e96c2bade5f8f99b0bec376b656b..7f526a2d9968e76b385a0ddfb2edf5bab7274fb0 100644
--- a/include/sleipnir/util/Spy.hpp
+++ b/include/sleipnir/util/Spy.hpp
@@ -7,9 +7,9 @@
#include <string_view>
* Returns end iterator.
*
* @return End iterator.
*/
- const_iterator cend() const { return const_iterator{m_storage.cend()}; }
+ const_iterator cend() const { return const_iterator{m_storage.end()}; }
#include <Eigen/SparseCore>
+#include <wpi/SmallVector.h>
#include "sleipnir/util/SymbolExports.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir {
@@ -32,7 +32,7 @@ SLEIPNIR_DLLEXPORT inline void Spy(std::ostream& file,
const int cells_width = mat.cols() + 1;
const int cells_height = mat.rows();
- small_vector<uint8_t> cells;
+ wpi::SmallVector<uint8_t> cells;
// Allocate space for matrix of characters plus trailing newlines
cells.reserve(cells_width * cells_height);
diff --git a/src/.styleguide b/src/.styleguide
index f3b2f0cf9e60b3a86b9654ff2b381f9c48734ff6..ad739cea6dce6f6cb586f538d1d30b92503484c1 100644
--- a/src/.styleguide
+++ b/src/.styleguide
@@ -8,4 +8,5 @@ cppSrcFileInclude {
includeOtherLibs {
^Eigen/
+ ^wpi/
}
diff --git a/src/optimization/solver/InteriorPoint.cpp b/src/optimization/solver/InteriorPoint.cpp
index a09d9866d05731c8ce53122b3d1a850803883df4..d3981c59d163927e3e5ba602c3323f6e1429c475 100644
--- a/src/optimization/solver/InteriorPoint.cpp
+++ b/src/optimization/solver/InteriorPoint.cpp
@@ -9,6 +9,7 @@
#include <limits>
#include <Eigen/SparseCholesky>
+#include <wpi/SmallVector.h>
#include "optimization/RegularizedLDLT.hpp"
#include "optimization/solver/util/ErrorEstimate.hpp"
@@ -23,7 +24,6 @@
#include "sleipnir/optimization/SolverExitCondition.hpp"
#include "sleipnir/util/Print.hpp"
#include "sleipnir/util/Spy.hpp"
-#include "sleipnir/util/small_vector.hpp"
#include "util/ScopeExit.hpp"
#include "util/ToMilliseconds.hpp"
@@ -226,7 +226,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
};
// Kept outside the loop so its storage can be reused
- small_vector<Eigen::Triplet<double>> triplets;
+ wpi::SmallVector<Eigen::Triplet<double>> triplets;
RegularizedLDLT solver;
diff --git a/src/optimization/solver/SQP.cpp b/src/optimization/solver/SQP.cpp
index 77b9632e1da37361c995a8579d1d83a2756d6d88..662abc2fb6e311767b0fbb3a61121ce78549a3f6 100644
--- a/src/optimization/solver/SQP.cpp
+++ b/src/optimization/solver/SQP.cpp
@@ -9,6 +9,7 @@
#include <limits>
#include <Eigen/SparseCholesky>
+#include <wpi/SmallVector.h>
#include "optimization/RegularizedLDLT.hpp"
#include "optimization/solver/util/ErrorEstimate.hpp"
@@ -22,7 +23,6 @@
#include "sleipnir/optimization/SolverExitCondition.hpp"
#include "sleipnir/util/Print.hpp"
#include "sleipnir/util/Spy.hpp"
-#include "sleipnir/util/small_vector.hpp"
#include "util/ScopeExit.hpp"
#include "util/ToMilliseconds.hpp"
@@ -155,7 +155,7 @@ void SQP(std::span<Variable> decisionVariables,
Filter filter{f};
// Kept outside the loop so its storage can be reused
- small_vector<Eigen::Triplet<double>> triplets;
+ wpi::SmallVector<Eigen::Triplet<double>> triplets;
RegularizedLDLT solver;
diff --git a/src/optimization/solver/util/FeasibilityRestoration.hpp b/src/optimization/solver/util/FeasibilityRestoration.hpp
index feefe137adf0832b094a36d61201b15962138ded..79b5d99ae27de6049ba098888a965291e6b677fa 100644
--- a/src/optimization/solver/util/FeasibilityRestoration.hpp
+++ b/src/optimization/solver/util/FeasibilityRestoration.hpp
@@ -8,6 +8,7 @@
#include <span>
#include <Eigen/Core>
+#include <wpi/SmallVector.h>
#include "sleipnir/autodiff/Variable.hpp"
#include "sleipnir/autodiff/VariableMatrix.hpp"
@@ -16,7 +17,6 @@
#include "sleipnir/optimization/SolverStatus.hpp"
#include "sleipnir/optimization/solver/InteriorPoint.hpp"
#include "sleipnir/util/FunctionRef.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir {
@@ -57,7 +57,7 @@ inline void FeasibilityRestoration(
constexpr double ρ = 1000.0;
double μ = config.tolerance / 10.0;
- small_vector<Variable> fr_decisionVariables;
+ wpi::SmallVector<Variable> fr_decisionVariables;
fr_decisionVariables.reserve(decisionVariables.size() +
2 * equalityConstraints.size());
@@ -70,7 +70,7 @@ inline void FeasibilityRestoration(
fr_decisionVariables.emplace_back();
}
- auto it = fr_decisionVariables.cbegin();
+ auto it = fr_decisionVariables.begin();
VariableMatrix xAD{std::span{it, it + decisionVariables.size()}};
it += decisionVariables.size();
@@ -128,7 +128,7 @@ inline void FeasibilityRestoration(
}
// cₑ(x) - pₑ + nₑ = 0
- small_vector<Variable> fr_equalityConstraints;
+ wpi::SmallVector<Variable> fr_equalityConstraints;
fr_equalityConstraints.assign(equalityConstraints.begin(),
equalityConstraints.end());
for (size_t row = 0; row < fr_equalityConstraints.size(); ++row) {
@@ -137,7 +137,7 @@ inline void FeasibilityRestoration(
}
// cᵢ(x) - s - pᵢ + nᵢ = 0
- small_vector<Variable> fr_inequalityConstraints;
+ wpi::SmallVector<Variable> fr_inequalityConstraints;
// pₑ ≥ 0
std::copy(p_e.begin(), p_e.end(),
@@ -227,7 +227,7 @@ inline void FeasibilityRestoration(
constexpr double ρ = 1000.0;
- small_vector<Variable> fr_decisionVariables;
+ wpi::SmallVector<Variable> fr_decisionVariables;
fr_decisionVariables.reserve(decisionVariables.size() +
2 * equalityConstraints.size() +
2 * inequalityConstraints.size());
@@ -243,7 +243,7 @@ inline void FeasibilityRestoration(
fr_decisionVariables.emplace_back();
}
- auto it = fr_decisionVariables.cbegin();
+ auto it = fr_decisionVariables.begin();
VariableMatrix xAD{std::span{it, it + decisionVariables.size()}};
it += decisionVariables.size();
@@ -319,7 +319,7 @@ inline void FeasibilityRestoration(
}
// cₑ(x) - pₑ + nₑ = 0
- small_vector<Variable> fr_equalityConstraints;
+ wpi::SmallVector<Variable> fr_equalityConstraints;
fr_equalityConstraints.assign(equalityConstraints.begin(),
equalityConstraints.end());
for (size_t row = 0; row < fr_equalityConstraints.size(); ++row) {
@@ -328,7 +328,7 @@ inline void FeasibilityRestoration(
}
// cᵢ(x) - s - pᵢ + nᵢ = 0
- small_vector<Variable> fr_inequalityConstraints;
+ wpi::SmallVector<Variable> fr_inequalityConstraints;
fr_inequalityConstraints.assign(inequalityConstraints.begin(),
inequalityConstraints.end());
for (size_t row = 0; row < fr_inequalityConstraints.size(); ++row) {
diff --git a/src/optimization/solver/util/Filter.hpp b/src/optimization/solver/util/Filter.hpp
index f19236928c59623bc0a3ce87b659f0756997f821..0c14efd7b8afa6cef398f5a7d383c54dbf64ec68 100644
--- a/src/optimization/solver/util/Filter.hpp
+++ b/src/optimization/solver/util/Filter.hpp
@@ -8,9 +8,9 @@
#include <utility>
#include <Eigen/Core>
+#include <wpi/SmallVector.h>
#include "sleipnir/autodiff/Variable.hpp"
-#include "sleipnir/util/small_vector.hpp"
// See docs/algorithms.md#Works_cited for citation definitions.
@@ -177,7 +177,7 @@ class Filter {
private:
Variable* m_f = nullptr;
- small_vector<FilterEntry> m_filter;
+ wpi::SmallVector<FilterEntry> m_filter;
};
} // namespace sleipnir
/**
* Returns number of elements in matrix.

31
upstream_utils/sleipnir_patches/0003-Suppress-clang-tidy-false-positives.patch
View File

@@ -1,31 +0,0 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Wed, 26 Jun 2024 12:13:33 -0700
Subject: [PATCH 3/3] Suppress clang-tidy false positives
---
include/sleipnir/autodiff/Variable.hpp | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/include/sleipnir/autodiff/Variable.hpp b/include/sleipnir/autodiff/Variable.hpp
index d192fb96e7984b7c0ca30262668c41e5e84ca34e..f25c6d153310a01700ee2390ecf35ffa8af7df11 100644
--- a/include/sleipnir/autodiff/Variable.hpp
+++ b/include/sleipnir/autodiff/Variable.hpp
@@ -541,7 +541,7 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints {
*
* @param equalityConstraints The list of EqualityConstraints to concatenate.
*/
- EqualityConstraints(
+ EqualityConstraints( // NOLINT
std::initializer_list<EqualityConstraints> equalityConstraints) {
for (const auto& elem : equalityConstraints) {
constraints.insert(constraints.end(), elem.constraints.begin(),
@@ -604,7 +604,7 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints {
* @param inequalityConstraints The list of InequalityConstraints to
* concatenate.
*/
- InequalityConstraints(
+ InequalityConstraints( // NOLINT
std::initializer_list<InequalityConstraints> inequalityConstraints) {
for (const auto& elem : inequalityConstraints) {
constraints.insert(constraints.end(), elem.constraints.begin(),

41
upstream_utils/sleipnir_patches/0003-Use-wpi-byteswap.patch Normal file
View File

@@ -0,0 +1,41 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Tue, 28 Jan 2025 22:19:14 -0800
Subject: [PATCH 3/8] Use wpi::byteswap()
---
include/.styleguide | 1 +
include/sleipnir/util/spy.hpp | 3 ++-
2 files changed, 3 insertions(+), 1 deletion(-)
diff --git a/include/.styleguide b/include/.styleguide
index 4f4c76204071f90bf49eddb8c2aceb583b5e09ba..03938557c2600a7a1f72c6b93c935602f5acb2b2 100644
--- a/include/.styleguide
+++ b/include/.styleguide
@@ -10,4 +10,5 @@ includeOtherLibs {
^Eigen/
^fmt/
^gch/
+ ^wpi/
}
diff --git a/include/sleipnir/util/spy.hpp b/include/sleipnir/util/spy.hpp
index a2f94803e3744cee771669210d1af883160e9896..8cd7d4353aad20153af5cd7a818fa55889d35721 100644
--- a/include/sleipnir/util/spy.hpp
+++ b/include/sleipnir/util/spy.hpp
@@ -12,6 +12,7 @@
#include <string_view>
#include <Eigen/SparseCore>
+#include <wpi/bit.h>
#include "sleipnir/util/symbol_exports.hpp"
@@ -115,7 +116,7 @@ class SLEIPNIR_DLLEXPORT Spy {
*/
void write32le(int32_t num) {
if constexpr (std::endian::native != std::endian::little) {
- num = std::byteswap(num);
+ num = wpi::byteswap(num);
}
m_file.write(reinterpret_cast<char*>(&num), sizeof(num));
}

63
upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch Normal file
View File

@@ -0,0 +1,63 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Tue, 28 Jan 2025 22:19:31 -0800
Subject: [PATCH 4/8] Replace std::to_underlying()
---
src/optimization/problem.cpp | 9 ++++-----
src/util/print_diagnostics.hpp | 6 +++---
2 files changed, 7 insertions(+), 8 deletions(-)
diff --git a/src/optimization/problem.cpp b/src/optimization/problem.cpp
index 81863808d329a53d4162ce0624a3b8e8afc32dfc..c3319fc0a927cf452871a2db08d5edff87ac8eea 100644
--- a/src/optimization/problem.cpp
+++ b/src/optimization/problem.cpp
@@ -7,7 +7,6 @@
#include <memory>
#include <optional>
#include <ranges>
-#include <utility>
#include <Eigen/Core>
#include <Eigen/SparseCore>
@@ -346,11 +345,11 @@ void Problem::print_problem_analysis() {
// Print problem structure
slp::println("\nProblem structure:");
slp::println(" ↳ {} cost function",
- types[std::to_underlying(cost_function_type())]);
+ types[static_cast<uint8_t>(cost_function_type())]);
slp::println(" ↳ {} equality constraints",
- types[std::to_underlying(equality_constraint_type())]);
+ types[static_cast<uint8_t>(equality_constraint_type())]);
slp::println(" ↳ {} inequality constraints",
- types[std::to_underlying(inequality_constraint_type())]);
+ types[static_cast<uint8_t>(inequality_constraint_type())]);
if (m_decision_variables.size() == 1) {
slp::print("\n1 decision variable\n");
@@ -362,7 +361,7 @@ void Problem::print_problem_analysis() {
[](const gch::small_vector<Variable>& constraints) {
std::array<size_t, 5> counts{};
for (const auto& constraint : constraints) {
- ++counts[std::to_underlying(constraint.type())];
+ ++counts[static_cast<uint8_t>(constraint.type())];
}
for (const auto& [count, name] :
std::views::zip(counts, std::array{"empty", "constant", "linear",
diff --git a/src/util/print_diagnostics.hpp b/src/util/print_diagnostics.hpp
index fde36957c0258f6e3cd435ef6224d60407012ff7..82e0e082b0e40153dcb2fcd2c655a412a8a9540a 100644
--- a/src/util/print_diagnostics.hpp
+++ b/src/util/print_diagnostics.hpp
@@ -238,9 +238,9 @@ void print_iteration_diagnostics(int iterations, IterationType type,
slp::println(
"│{:4} {:4} {:9.3f} {:12e} {:13e} {:12e} {:12e} {:.2e} {:<5} {:.2e} "
"{:.2e} {:2d}│",
- iterations, ITERATION_TYPES[std::to_underlying(type)], to_ms(time), error,
- cost, infeasibility, complementarity, μ, power_of_10(δ), primal_α, dual_α,
- backtracks);
+ iterations, ITERATION_TYPES[static_cast<uint8_t>(type)], to_ms(time),
+ error, cost, infeasibility, complementarity, μ, power_of_10(δ), primal_α,
+ dual_α, backtracks);
}
#else
#define print_iteration_diagnostics(...)

53
upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch Normal file
View File

@@ -0,0 +1,53 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Sat, 8 Feb 2025 13:42:36 -0800
Subject: [PATCH 5/8] Replace std::views::zip()
---
include/sleipnir/autodiff/adjoint_expression_graph.hpp | 5 ++++-
src/optimization/problem.cpp | 9 +++++----
2 files changed, 9 insertions(+), 5 deletions(-)
diff --git a/include/sleipnir/autodiff/adjoint_expression_graph.hpp b/include/sleipnir/autodiff/adjoint_expression_graph.hpp
index 4b4f3303faed766d3ac39829870514f50d9a582f..4576e19c9695caf4407fbbb592afe32d8252a0db 100644
--- a/include/sleipnir/autodiff/adjoint_expression_graph.hpp
+++ b/include/sleipnir/autodiff/adjoint_expression_graph.hpp
@@ -155,7 +155,10 @@ class AdjointExpressionGraph {
}
}
} else {
- for (const auto& [col, node] : std::views::zip(m_col_list, m_top_list)) {
+ 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 != 0.0) {
triplets.emplace_back(row, col, node->adjoint);
diff --git a/src/optimization/problem.cpp b/src/optimization/problem.cpp
index c3319fc0a927cf452871a2db08d5edff87ac8eea..5532b3962409e2140132e79241da4fba0f36bc78 100644
--- a/src/optimization/problem.cpp
+++ b/src/optimization/problem.cpp
@@ -6,7 +6,6 @@
#include <cmath>
#include <memory>
#include <optional>
-#include <ranges>
#include <Eigen/Core>
#include <Eigen/SparseCore>
@@ -363,9 +362,11 @@ void Problem::print_problem_analysis() {
for (const auto& constraint : constraints) {
++counts[static_cast<uint8_t>(constraint.type())];
}
- for (const auto& [count, name] :
- std::views::zip(counts, std::array{"empty", "constant", "linear",
- "quadratic", "nonlinear"})) {
+ for (size_t i = 0; i < counts.size(); ++i) {
+ constexpr std::array names{"empty", "constant", "linear", "quadratic",
+ "nonlinear"};
+ const auto& count = counts[i];
+ const auto& name = names[i];
if (count > 0) {
slp::println(" ↳ {} {}", count, name);
}

22
upstream_utils/sleipnir_patches/0006-Suppress-clang-tidy-false-positives.patch Normal file
View File

@@ -0,0 +1,22 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Mon, 10 Feb 2025 11:37:02 -0800
Subject: [PATCH 6/8] Suppress clang-tidy false positives
---
include/sleipnir/autodiff/variable.hpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp
index 9f79a82763213dc712cce4c2a322289d57645032..17e7eb7cc2c7c7599eaba97d8ec80972524c1599 100644
--- a/include/sleipnir/autodiff/variable.hpp
+++ b/include/sleipnir/autodiff/variable.hpp
@@ -626,7 +626,7 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints {
* @param inequality_constraints The list of InequalityConstraints to
* concatenate.
*/
- InequalityConstraints(
+ InequalityConstraints( // NOLINT
std::initializer_list<InequalityConstraints> inequality_constraints) {
for (const auto& elem : inequality_constraints) {
constraints.insert(constraints.end(), elem.constraints.begin(),

34
upstream_utils/sleipnir_patches/0007-Suppress-GCC-12-warning-false-positive.patch Normal file
View File

@@ -0,0 +1,34 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Mon, 24 Feb 2025 15:12:03 -0800
Subject: [PATCH 7/8] Suppress GCC 12 warning false positive
---
include/sleipnir/autodiff/variable_matrix.hpp | 7 +++++++
1 file changed, 7 insertions(+)
diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp
index 1c6f9e8dade8bebce7aec18bbb9b5491acb1d977..dee43f926d304e1f4900bd57b99cd613e808f58e 100644
--- a/include/sleipnir/autodiff/variable_matrix.hpp
+++ b/include/sleipnir/autodiff/variable_matrix.hpp
@@ -573,6 +573,10 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
VariableMatrix result(VariableMatrix::empty, lhs.rows(), rhs.cols());
+#if __GNUC__ >= 12
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
+#endif
for (int i = 0; i < lhs.rows(); ++i) {
for (int j = 0; j < rhs.cols(); ++j) {
Variable sum;
@@ -590,6 +594,9 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
result[i, j] = sum;
}
}
+#if __GNUC__ >= 12
+#pragma GCC diagnostic pop
+#endif
return result;
}

915
upstream_utils/sleipnir_patches/0008-Revert-Use-multidimensional-array-subscript-operator.patch Normal file
View File

@@ -0,0 +1,915 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Sat, 12 Apr 2025 16:28:47 -0700
Subject: [PATCH 8/8] Revert "Use multidimensional array subscript operator
(#843)"
This reverts commit f9b2c450bbbf6f14b194b8b81708d032a6431ee0.
---
include/sleipnir/autodiff/hessian.hpp | 4 +-
include/sleipnir/autodiff/jacobian.hpp | 4 +-
include/sleipnir/autodiff/variable.hpp | 26 +----
include/sleipnir/autodiff/variable_block.hpp | 70 +++++------
include/sleipnir/autodiff/variable_matrix.hpp | 110 ++++++------------
include/sleipnir/control/ocp.hpp | 14 +--
include/sleipnir/optimization/problem.hpp | 6 +-
src/autodiff/variable_matrix.cpp | 66 +++++------
8 files changed, 118 insertions(+), 182 deletions(-)
diff --git a/include/sleipnir/autodiff/hessian.hpp b/include/sleipnir/autodiff/hessian.hpp
index 4ad097a8117dac47566a3c6896d281004147be70..8b048ab3ba0d671397cfdadcd137ac67bef1b441 100644
--- a/include/sleipnir/autodiff/hessian.hpp
+++ b/include/sleipnir/autodiff/hessian.hpp
@@ -103,9 +103,9 @@ class SLEIPNIR_DLLEXPORT Hessian {
auto grad = m_graphs[row].generate_gradient_tree(m_wrt);
for (int col = 0; col < m_wrt.rows(); ++col) {
if (grad[col].expr != nullptr) {
- result[row, col] = std::move(grad[col]);
+ result(row, col) = std::move(grad[col]);
} else {
- result[row, col] = Variable{0.0};
+ result(row, col) = Variable{0.0};
}
}
}
diff --git a/include/sleipnir/autodiff/jacobian.hpp b/include/sleipnir/autodiff/jacobian.hpp
index 787fca8ccd3fd6e46c5d31ab980704e6a5e99402..7e7e1340d065d35412f43b27fac7d8a719b7e5b5 100644
--- a/include/sleipnir/autodiff/jacobian.hpp
+++ b/include/sleipnir/autodiff/jacobian.hpp
@@ -95,9 +95,9 @@ class SLEIPNIR_DLLEXPORT Jacobian {
auto grad = m_graphs[row].generate_gradient_tree(m_wrt);
for (int col = 0; col < m_wrt.rows(); ++col) {
if (grad[col].expr != nullptr) {
- result[row, col] = std::move(grad[col]);
+ result(row, col) = std::move(grad[col]);
} else {
- result[row, col] = Variable{0.0};
+ result(row, col) = Variable{0.0};
}
}
}
diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp
index 17e7eb7cc2c7c7599eaba97d8ec80972524c1599..03b929c778c03186cc5b461a2e855da23034457a 100644
--- a/include/sleipnir/autodiff/variable.hpp
+++ b/include/sleipnir/autodiff/variable.hpp
@@ -505,11 +505,7 @@ gch::small_vector<Variable> make_constraints(LHS&& lhs, RHS&& rhs) {
for (int row = 0; row < rhs.rows(); ++row) {
for (int col = 0; col < rhs.cols(); ++col) {
// Make right-hand side zero
- if constexpr (EigenMatrixLike<std::decay_t<RHS>>) {
- constraints.emplace_back(lhs - rhs(row, col));
- } else {
- constraints.emplace_back(lhs - rhs[row, col]);
- }
+ constraints.emplace_back(lhs - rhs(row, col));
}
}
} else if constexpr (MatrixLike<LHS> && ScalarLike<RHS>) {
@@ -518,11 +514,7 @@ gch::small_vector<Variable> make_constraints(LHS&& lhs, RHS&& rhs) {
for (int row = 0; row < lhs.rows(); ++row) {
for (int col = 0; col < lhs.cols(); ++col) {
// Make right-hand side zero
- if constexpr (EigenMatrixLike<std::decay_t<LHS>>) {
- constraints.emplace_back(lhs(row, col) - rhs);
- } else {
- constraints.emplace_back(lhs[row, col] - rhs);
- }
+ constraints.emplace_back(lhs(row, col) - rhs);
}
}
} else if constexpr (MatrixLike<LHS> && MatrixLike<RHS>) {
@@ -532,19 +524,7 @@ gch::small_vector<Variable> make_constraints(LHS&& lhs, RHS&& rhs) {
for (int row = 0; row < lhs.rows(); ++row) {
for (int col = 0; col < lhs.cols(); ++col) {
// Make right-hand side zero
- if constexpr (EigenMatrixLike<std::decay_t<LHS>> &&
- EigenMatrixLike<std::decay_t<RHS>>) {
- constraints.emplace_back(lhs(row, col) - rhs(row, col));
- } else if constexpr (EigenMatrixLike<std::decay_t<LHS>> &&
- SleipnirMatrixLike<std::decay_t<RHS>>) {
- constraints.emplace_back(lhs(row, col) - rhs[row, col]);
- } else if constexpr (SleipnirMatrixLike<std::decay_t<LHS>> &&
- EigenMatrixLike<std::decay_t<RHS>>) {
- constraints.emplace_back(lhs[row, col] - rhs(row, col));
- } else if constexpr (SleipnirMatrixLike<std::decay_t<LHS>> &&
- SleipnirMatrixLike<std::decay_t<RHS>>) {
- constraints.emplace_back(lhs[row, col] - rhs[row, col]);
- }
+ constraints.emplace_back(lhs(row, col) - rhs(row, col));
}
}
}
diff --git a/include/sleipnir/autodiff/variable_block.hpp b/include/sleipnir/autodiff/variable_block.hpp
index f1c1ca0dc3fde663c3e74f6fca4b89b119cf377d..632d44beb5b3dae29b9829c52a6168fee39fe537 100644
--- a/include/sleipnir/autodiff/variable_block.hpp
+++ b/include/sleipnir/autodiff/variable_block.hpp
@@ -50,7 +50,7 @@ class VariableBlock {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] = values[row, col];
+ (*this)(row, col) = values(row, col);
}
}
}
@@ -85,7 +85,7 @@ class VariableBlock {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] = values[row, col];
+ (*this)(row, col) = values(row, col);
}
}
}
@@ -152,7 +152,7 @@ class VariableBlock {
VariableBlock<Mat>& operator=(ScalarLike auto value) {
slp_assert(rows() == 1 && cols() == 1);
- (*this)[0, 0] = value;
+ (*this)(0, 0) = value;
return *this;
}
@@ -167,7 +167,7 @@ class VariableBlock {
void set_value(double value) {
slp_assert(rows() == 1 && cols() == 1);
- (*this)[0, 0].set_value(value);
+ (*this)(0, 0).set_value(value);
}
/**
@@ -182,7 +182,7 @@ class VariableBlock {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] = values(row, col);
+ (*this)(row, col) = values(row, col);
}
}
@@ -201,7 +201,7 @@ class VariableBlock {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col].set_value(values(row, col));
+ (*this)(row, col).set_value(values(row, col));
}
}
}
@@ -217,7 +217,7 @@ class VariableBlock {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] = values[row, col];
+ (*this)(row, col) = values(row, col);
}
}
return *this;
@@ -234,7 +234,7 @@ class VariableBlock {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] = std::move(values[row, col]);
+ (*this)(row, col) = std::move(values(row, col));
}
}
return *this;
@@ -247,13 +247,13 @@ class VariableBlock {
* @param col The scalar subblock's column.
* @return A scalar subblock at the given row and column.
*/
- Variable& operator[](int row, int col)
+ Variable& operator()(int row, int col)
requires(!std::is_const_v<Mat>)
{
slp_assert(row >= 0 && row < rows());
slp_assert(col >= 0 && col < cols());
- return (*m_mat)[m_row_slice.start + row * m_row_slice.step,
- m_col_slice.start + col * m_col_slice.step];
+ return (*m_mat)(m_row_slice.start + row * m_row_slice.step,
+ m_col_slice.start + col * m_col_slice.step);
}
/**
@@ -263,11 +263,11 @@ class VariableBlock {
* @param col The scalar subblock's column.
* @return A scalar subblock at the given row and column.
*/
- const Variable& operator[](int row, int col) const {
+ const Variable& operator()(int row, int col) const {
slp_assert(row >= 0 && row < rows());
slp_assert(col >= 0 && col < cols());
- return (*m_mat)[m_row_slice.start + row * m_row_slice.step,
- m_col_slice.start + col * m_col_slice.step];
+ return (*m_mat)(m_row_slice.start + row * m_row_slice.step,
+ m_col_slice.start + col * m_col_slice.step);
}
/**
@@ -280,7 +280,7 @@ class VariableBlock {
requires(!std::is_const_v<Mat>)
{
slp_assert(row >= 0 && row < rows() * cols());
- return (*this)[row / cols(), row % cols()];
+ return (*this)(row / cols(), row % cols());
}
/**
@@ -291,7 +291,7 @@ class VariableBlock {
*/
const Variable& operator[](int row) const {
slp_assert(row >= 0 && row < rows() * cols());
- return (*this)[row / cols(), row % cols()];
+ return (*this)(row / cols(), row % cols());
}
/**
@@ -309,8 +309,8 @@ class VariableBlock {
slp_assert(col_offset >= 0 && col_offset <= cols());
slp_assert(block_rows >= 0 && block_rows <= rows() - row_offset);
slp_assert(block_cols >= 0 && block_cols <= cols() - col_offset);
- return (*this)[Slice{row_offset, row_offset + block_rows, 1},
- Slice{col_offset, col_offset + block_cols, 1}];
+ return (*this)({row_offset, row_offset + block_rows, 1},
+ {col_offset, col_offset + block_cols, 1});
}
/**
@@ -328,8 +328,8 @@ class VariableBlock {
slp_assert(col_offset >= 0 && col_offset <= cols());
slp_assert(block_rows >= 0 && block_rows <= rows() - row_offset);
slp_assert(block_cols >= 0 && block_cols <= cols() - col_offset);
- return (*this)[Slice{row_offset, row_offset + block_rows, 1},
- Slice{col_offset, col_offset + block_cols, 1}];
+ return (*this)({row_offset, row_offset + block_rows, 1},
+ {col_offset, col_offset + block_cols, 1});
}
/**
@@ -339,7 +339,7 @@ class VariableBlock {
* @param col_slice The column slice.
* @return A slice of the variable matrix.
*/
- VariableBlock<Mat> operator[](Slice row_slice, Slice col_slice) {
+ VariableBlock<Mat> operator()(Slice row_slice, Slice col_slice) {
int row_slice_length = row_slice.adjust(m_row_slice_length);
int col_slice_length = col_slice.adjust(m_col_slice_length);
return VariableBlock{
@@ -359,7 +359,7 @@ class VariableBlock {
* @param col_slice The column slice.
* @return A slice of the variable matrix.
*/
- const VariableBlock<const Mat> operator[](Slice row_slice,
+ const VariableBlock<const Mat> operator()(Slice row_slice,
Slice col_slice) const {
int row_slice_length = row_slice.adjust(m_row_slice_length);
int col_slice_length = col_slice.adjust(m_col_slice_length);
@@ -385,7 +385,7 @@ class VariableBlock {
* @param col_slice_length The column slice length.
* @return A slice of the variable matrix.
*/
- VariableBlock<Mat> operator[](Slice row_slice, int row_slice_length,
+ VariableBlock<Mat> operator()(Slice row_slice, int row_slice_length,
Slice col_slice, int col_slice_length) {
return VariableBlock{
*m_mat,
@@ -409,7 +409,7 @@ class VariableBlock {
* @param col_slice_length The column slice length.
* @return A slice of the variable matrix.
*/
- const VariableBlock<const Mat> operator[](Slice row_slice,
+ const VariableBlock<const Mat> operator()(Slice row_slice,
int row_slice_length,
Slice col_slice,
int col_slice_length) const {
@@ -524,7 +524,7 @@ class VariableBlock {
VariableBlock<Mat>& operator*=(const ScalarLike auto& rhs) {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] *= rhs;
+ (*this)(row, col) *= rhs;
}
}
@@ -542,7 +542,7 @@ class VariableBlock {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] /= rhs[0, 0];
+ (*this)(row, col) /= rhs(0, 0);
}
}
@@ -558,7 +558,7 @@ class VariableBlock {
VariableBlock<Mat>& operator/=(const ScalarLike auto& rhs) {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] /= rhs;
+ (*this)(row, col) /= rhs;
}
}
@@ -576,7 +576,7 @@ class VariableBlock {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] += rhs[row, col];
+ (*this)(row, col) += rhs(row, col);
}
}
@@ -594,7 +594,7 @@ class VariableBlock {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] += rhs;
+ (*this)(row, col) += rhs;
}
}
@@ -612,7 +612,7 @@ class VariableBlock {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] -= rhs[row, col];
+ (*this)(row, col) -= rhs(row, col);
}
}
@@ -630,7 +630,7 @@ class VariableBlock {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] -= rhs;
+ (*this)(row, col) -= rhs;
}
}
@@ -655,7 +655,7 @@ class VariableBlock {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- result[col, row] = (*this)[row, col];
+ result(col, row) = (*this)(row, col);
}
}
@@ -686,8 +686,8 @@ class VariableBlock {
double value(int row, int col) {
slp_assert(row >= 0 && row < rows());
slp_assert(col >= 0 && col < cols());
- return (*m_mat)[m_row_slice.start + row * m_row_slice.step,
- m_col_slice.start + col * m_col_slice.step]
+ return (*m_mat)(m_row_slice.start + row * m_row_slice.step,
+ m_col_slice.start + col * m_col_slice.step)
.value();
}
@@ -731,7 +731,7 @@ class VariableBlock {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- result[row, col] = unary_op((*this)[row, col]);
+ result(row, col) = unary_op((*this)(row, col));
}
}
diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp
index dee43f926d304e1f4900bd57b99cd613e808f58e..4dc2cea00cb9491035a9b4795be3562186991c7a 100644
--- a/include/sleipnir/autodiff/variable_matrix.hpp
+++ b/include/sleipnir/autodiff/variable_matrix.hpp
@@ -211,7 +211,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int row = 0; row < values.rows(); ++row) {
for (int col = 0; col < values.cols(); ++col) {
- (*this)[row, col] = values(row, col);
+ (*this)(row, col) = values(row, col);
}
}
@@ -229,7 +229,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
VariableMatrix& operator=(ScalarLike auto value) {
slp_assert(rows() == 1 && cols() == 1);
- (*this)[0, 0] = value;
+ (*this)(0, 0) = value;
return *this;
}
@@ -246,7 +246,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int row = 0; row < values.rows(); ++row) {
for (int col = 0; col < values.cols(); ++col) {
- (*this)[row, col].set_value(values(row, col));
+ (*this)(row, col).set_value(values(row, col));
}
}
}
@@ -280,7 +280,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
m_storage.reserve(rows() * cols());
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- m_storage.emplace_back(values[row, col]);
+ m_storage.emplace_back(values(row, col));
}
}
}
@@ -295,7 +295,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
m_storage.reserve(rows() * cols());
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- m_storage.emplace_back(values[row, col]);
+ m_storage.emplace_back(values(row, col));
}
}
}
@@ -340,7 +340,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
* @param col The block column.
* @return A block pointing to the given row and column.
*/
- Variable& operator[](int row, int col) {
+ Variable& operator()(int row, int col) {
slp_assert(row >= 0 && row < rows());
slp_assert(col >= 0 && col < cols());
return m_storage[row * cols() + col];
@@ -353,7 +353,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
* @param col The block column.
* @return A block pointing to the given row and column.
*/
- const Variable& operator[](int row, int col) const {
+ const Variable& operator()(int row, int col) const {
slp_assert(row >= 0 && row < rows());
slp_assert(col >= 0 && col < cols());
return m_storage[row * cols() + col];
@@ -426,7 +426,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
* @param col_slice The column slice.
* @return A slice of the variable matrix.
*/
- VariableBlock<VariableMatrix> operator[](Slice row_slice, Slice col_slice) {
+ VariableBlock<VariableMatrix> operator()(Slice row_slice, Slice col_slice) {
int row_slice_length = row_slice.adjust(rows());
int col_slice_length = col_slice.adjust(cols());
return VariableBlock{*this, std::move(row_slice), row_slice_length,
@@ -440,7 +440,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
* @param col_slice The column slice.
* @return A slice of the variable matrix.
*/
- const VariableBlock<const VariableMatrix> operator[](Slice row_slice,
+ const VariableBlock<const VariableMatrix> operator()(Slice row_slice,
Slice col_slice) const {
int row_slice_length = row_slice.adjust(rows());
int col_slice_length = col_slice.adjust(cols());
@@ -461,7 +461,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
* @return A slice of the variable matrix.
*
*/
- VariableBlock<VariableMatrix> operator[](Slice row_slice,
+ VariableBlock<VariableMatrix> operator()(Slice row_slice,
int row_slice_length,
Slice col_slice,
int col_slice_length) {
@@ -481,7 +481,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
* @param col_slice_length The column slice length.
* @return A slice of the variable matrix.
*/
- const VariableBlock<const VariableMatrix> operator[](
+ const VariableBlock<const VariableMatrix> operator()(
Slice row_slice, int row_slice_length, Slice col_slice,
int col_slice_length) const {
return VariableBlock{*this, std::move(row_slice), row_slice_length,
@@ -581,17 +581,9 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int j = 0; j < rhs.cols(); ++j) {
Variable sum;
for (int k = 0; k < lhs.cols(); ++k) {
- if constexpr (SleipnirMatrixLike<LHS> && SleipnirMatrixLike<RHS>) {
- sum += lhs[i, k] * rhs[k, j];
- } else if constexpr (SleipnirMatrixLike<LHS> &&
- EigenMatrixLike<RHS>) {
- sum += lhs[i, k] * rhs(k, j);
- } else if constexpr (EigenMatrixLike<LHS> &&
- SleipnirMatrixLike<RHS>) {
- sum += lhs(i, k) * rhs[k, j];
- }
+ sum += lhs(i, k) * rhs(k, j);
}
- result[i, j] = sum;
+ result(i, j) = sum;
}
}
#if __GNUC__ >= 12
@@ -613,7 +605,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
- result[row, col] = lhs[row, col] * rhs;
+ result(row, col) = lhs(row, col) * rhs;
}
}
@@ -632,11 +624,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
- if constexpr (SleipnirMatrixLike<decltype(lhs)>) {
- result[row, col] = lhs[row, col] * rhs;
- } else {
- result[row, col] = lhs(row, col) * rhs;
- }
+ result(row, col) = lhs(row, col) * rhs;
}
}
@@ -655,7 +643,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
- result[row, col] = rhs[row, col] * lhs;
+ result(row, col) = rhs(row, col) * lhs;
}
}
@@ -674,11 +662,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
- if constexpr (SleipnirMatrixLike<decltype(rhs)>) {
- result[row, col] = rhs[row, col] * lhs;
- } else {
- result[row, col] = rhs(row, col) * lhs;
- }
+ result(row, col) = rhs(row, col) * lhs;
}
}
@@ -698,13 +682,9 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int j = 0; j < rhs.cols(); ++j) {
Variable sum;
for (int k = 0; k < cols(); ++k) {
- if constexpr (SleipnirMatrixLike<decltype(rhs)>) {
- sum += (*this)[i, k] * rhs[k, j];
- } else {
- sum += (*this)[i, k] * rhs(k, j);
- }
+ sum += (*this)(i, k) * rhs(k, j);
}
- (*this)[i, j] = sum;
+ (*this)(i, j) = sum;
}
}
@@ -720,7 +700,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
VariableMatrix& operator*=(const ScalarLike auto& rhs) {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < rhs.cols(); ++col) {
- (*this)[row, col] *= rhs;
+ (*this)(row, col) *= rhs;
}
}
@@ -740,11 +720,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
- if constexpr (SleipnirMatrixLike<decltype(lhs)>) {
- result[row, col] = lhs[row, col] / rhs;
- } else {
- result[row, col] = lhs(row, col) / rhs;
- }
+ result(row, col) = lhs(row, col) / rhs;
}
}
@@ -760,7 +736,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
VariableMatrix& operator/=(const ScalarLike auto& rhs) {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] /= rhs;
+ (*this)(row, col) /= rhs;
}
}
@@ -784,13 +760,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
- if constexpr (SleipnirMatrixLike<LHS> && SleipnirMatrixLike<RHS>) {
- result[row, col] = lhs[row, col] + rhs[row, col];
- } else if constexpr (SleipnirMatrixLike<LHS> && EigenMatrixLike<RHS>) {
- result[row, col] = lhs[row, col] + rhs(row, col);
- } else if constexpr (EigenMatrixLike<LHS> && SleipnirMatrixLike<RHS>) {
- result[row, col] = lhs(row, col) + rhs[row, col];
- }
+ result(row, col) = lhs(row, col) + rhs(row, col);
}
}
@@ -808,11 +778,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- if constexpr (SleipnirMatrixLike<decltype(rhs)>) {
- (*this)[row, col] += rhs[row, col];
- } else {
- (*this)[row, col] += rhs(row, col);
- }
+ (*this)(row, col) += rhs(row, col);
}
}
@@ -830,7 +796,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] += rhs;
+ (*this)(row, col) += rhs;
}
}
@@ -854,13 +820,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
- if constexpr (SleipnirMatrixLike<LHS> && SleipnirMatrixLike<RHS>) {
- result[row, col] = lhs[row, col] - rhs[row, col];
- } else if constexpr (SleipnirMatrixLike<LHS> && EigenMatrixLike<RHS>) {
- result[row, col] = lhs[row, col] - rhs(row, col);
- } else if constexpr (EigenMatrixLike<LHS> && SleipnirMatrixLike<RHS>) {
- result[row, col] = lhs(row, col) - rhs[row, col];
- }
+ result(row, col) = lhs(row, col) - rhs(row, col);
}
}
@@ -878,11 +838,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- if constexpr (SleipnirMatrixLike<decltype(rhs)>) {
- (*this)[row, col] -= rhs[row, col];
- } else {
- (*this)[row, col] -= rhs(row, col);
- }
+ (*this)(row, col) -= rhs(row, col);
}
}
@@ -900,7 +856,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] -= rhs;
+ (*this)(row, col) -= rhs;
}
}
@@ -918,7 +874,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
- result[row, col] = -lhs[row, col];
+ result(row, col) = -lhs(row, col);
}
}
@@ -930,7 +886,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
*/
operator Variable() const { // NOLINT
slp_assert(rows() == 1 && cols() == 1);
- return (*this)[0, 0];
+ return (*this)(0, 0);
}
/**
@@ -943,7 +899,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- result[col, row] = (*this)[row, col];
+ result(col, row) = (*this)(row, col);
}
}
@@ -1017,7 +973,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- result[row, col] = unary_op((*this)[row, col]);
+ result(row, col) = unary_op((*this)(row, col));
}
}
@@ -1199,7 +1155,7 @@ SLEIPNIR_DLLEXPORT inline VariableMatrix cwise_reduce(
for (int row = 0; row < lhs.rows(); ++row) {
for (int col = 0; col < lhs.cols(); ++col) {
- result[row, col] = binary_op(lhs[row, col], rhs[row, col]);
+ result(row, col) = binary_op(lhs(row, col), rhs(row, col));
}
}
diff --git a/include/sleipnir/control/ocp.hpp b/include/sleipnir/control/ocp.hpp
index 282520fb852d8588b96846eb5b4952bf47d1309f..d9174426669281e68a5c09d298cfd5bcd3be3776 100644
--- a/include/sleipnir/control/ocp.hpp
+++ b/include/sleipnir/control/ocp.hpp
@@ -180,7 +180,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
if (m_timestep_method == TimestepMethod::FIXED) {
m_DT = VariableMatrix{1, m_num_steps + 1};
for (int i = 0; i < num_steps + 1; ++i) {
- m_DT[0, i] = m_dt.count();
+ m_DT(0, i) = m_dt.count();
}
} else if (m_timestep_method == TimestepMethod::VARIABLE_SINGLE) {
Variable dt = decision_variable();
@@ -189,12 +189,12 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
// Set the member variable matrix to track the decision variable
m_DT = VariableMatrix{1, m_num_steps + 1};
for (int i = 0; i < num_steps + 1; ++i) {
- m_DT[0, i] = dt;
+ m_DT(0, i) = dt;
}
} else if (m_timestep_method == TimestepMethod::VARIABLE) {
m_DT = decision_variable(1, m_num_steps + 1);
for (int i = 0; i < num_steps + 1; ++i) {
- m_DT[0, i].set_value(m_dt.count());
+ m_DT(0, i).set_value(m_dt.count());
}
}
@@ -270,7 +270,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
for (int i = 0; i < m_num_steps + 1; ++i) {
auto x = X().col(i);
auto u = U().col(i);
- auto dt = this->dt()[0, i];
+ auto dt = this->dt()(0, i);
callback(time, x, u, dt);
time += dt;
@@ -377,7 +377,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
// Derivation at https://mec560sbu.github.io/2016/09/30/direct_collocation/
for (int i = 0; i < m_num_steps; ++i) {
- Variable h = dt()[0, i];
+ Variable h = dt()(0, i);
auto& f = m_dynamics_function;
@@ -412,7 +412,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
auto x_begin = X().col(i);
auto x_end = X().col(i + 1);
auto u = U().col(i);
- Variable dt = this->dt()[0, i];
+ Variable dt = this->dt()(0, i);
if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) {
subject_to(x_end == rk4<const decltype(m_dynamics_function)&,
@@ -433,7 +433,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
auto x_begin = X().col(i);
auto x_end = X().col(i + 1);
auto u = U().col(i);
- Variable dt = this->dt()[0, i];
+ Variable dt = this->dt()(0, i);
if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) {
x_end = rk4<const decltype(m_dynamics_function)&, VariableMatrix,
diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp
index b7a868657c704487049efaf6b3972b1f7b72bfb4..b484ec08d6c50bf42fbaa1d5b4c66a20cb11a922 100644
--- a/include/sleipnir/optimization/problem.hpp
+++ b/include/sleipnir/optimization/problem.hpp
@@ -78,7 +78,7 @@ class SLEIPNIR_DLLEXPORT Problem {
for (int row = 0; row < rows; ++row) {
for (int col = 0; col < cols; ++col) {
m_decision_variables.emplace_back();
- vars[row, col] = m_decision_variables.back();
+ vars(row, col) = m_decision_variables.back();
}
}
@@ -113,8 +113,8 @@ class SLEIPNIR_DLLEXPORT Problem {
for (int row = 0; row < rows; ++row) {
for (int col = 0; col <= row; ++col) {
m_decision_variables.emplace_back();
- vars[row, col] = m_decision_variables.back();
- vars[col, row] = m_decision_variables.back();
+ vars(row, col) = m_decision_variables.back();
+ vars(col, row) = m_decision_variables.back();
}
}
diff --git a/src/autodiff/variable_matrix.cpp b/src/autodiff/variable_matrix.cpp
index decdc70809189d309708774ec60603fe73c50ecc..71f8153d345750d79fa41cf7af14ac766fcad2a4 100644
--- a/src/autodiff/variable_matrix.cpp
+++ b/src/autodiff/variable_matrix.cpp
@@ -12,17 +12,17 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
if (A.rows() == 1 && A.cols() == 1) {
// Compute optimal inverse instead of using Eigen's general solver
- return B[0, 0] / A[0, 0];
+ return B(0, 0) / A(0, 0);
} else if (A.rows() == 2 && A.cols() == 2) {
// Compute optimal inverse instead of using Eigen's general solver
//
// [a b]⁻¹ ___1___ [ d −b]
// [c d] = ad − bc [−c a]
- const auto& a = A[0, 0];
- const auto& b = A[0, 1];
- const auto& c = A[1, 0];
- const auto& d = A[1, 1];
+ const auto& a = A(0, 0);
+ const auto& b = A(0, 1);
+ const auto& c = A(1, 0);
+ const auto& d = A(1, 1);
slp::VariableMatrix adj_A{{d, -b}, {-c, a}};
auto det_A = a * d - b * c;
@@ -39,15 +39,15 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
//
// https://www.wolframalpha.com/input?i=inverse+%7B%7Ba%2C+b%2C+c%7D%2C+%7Bd%2C+e%2C+f%7D%2C+%7Bg%2C+h%2C+i%7D%7D
- const auto& a = A[0, 0];
- const auto& b = A[0, 1];
- const auto& c = A[0, 2];
- const auto& d = A[1, 0];
- const auto& e = A[1, 1];
- const auto& f = A[1, 2];
- const auto& g = A[2, 0];
- const auto& h = A[2, 1];
- const auto& i = A[2, 2];
+ const auto& a = A(0, 0);
+ const auto& b = A(0, 1);
+ const auto& c = A(0, 2);
+ const auto& d = A(1, 0);
+ const auto& e = A(1, 1);
+ const auto& f = A(1, 2);
+ const auto& g = A(2, 0);
+ const auto& h = A(2, 1);
+ const auto& i = A(2, 2);
auto ae = a * e;
auto af = a * f;
@@ -87,22 +87,22 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
//
// https://www.wolframalpha.com/input?i=inverse+%7B%7Ba%2C+b%2C+c%2C+d%7D%2C+%7Be%2C+f%2C+g%2C+h%7D%2C+%7Bi%2C+j%2C+k%2C+l%7D%2C+%7Bm%2C+n%2C+o%2C+p%7D%7D
- const auto& a = A[0, 0];
- const auto& b = A[0, 1];
- const auto& c = A[0, 2];
- const auto& d = A[0, 3];
- const auto& e = A[1, 0];
- const auto& f = A[1, 1];
- const auto& g = A[1, 2];
- const auto& h = A[1, 3];
- const auto& i = A[2, 0];
- const auto& j = A[2, 1];
- const auto& k = A[2, 2];
- const auto& l = A[2, 3];
- const auto& m = A[3, 0];
- const auto& n = A[3, 1];
- const auto& o = A[3, 2];
- const auto& p = A[3, 3];
+ const auto& a = A(0, 0);
+ const auto& b = A(0, 1);
+ const auto& c = A(0, 2);
+ const auto& d = A(0, 3);
+ const auto& e = A(1, 0);
+ const auto& f = A(1, 1);
+ const auto& g = A(1, 2);
+ const auto& h = A(1, 3);
+ const auto& i = A(2, 0);
+ const auto& j = A(2, 1);
+ const auto& k = A(2, 2);
+ const auto& l = A(2, 3);
+ const auto& m = A(3, 0);
+ const auto& n = A(3, 1);
+ const auto& o = A(3, 2);
+ const auto& p = A(3, 3);
auto afk = a * f * k;
auto afl = a * f * l;
@@ -232,14 +232,14 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
MatrixXv eigen_A{A.rows(), A.cols()};
for (int row = 0; row < A.rows(); ++row) {
for (int col = 0; col < A.cols(); ++col) {
- eigen_A(row, col) = A[row, col];
+ eigen_A(row, col) = A(row, col);
}
}
MatrixXv eigen_B{B.rows(), B.cols()};
for (int row = 0; row < B.rows(); ++row) {
for (int col = 0; col < B.cols(); ++col) {
- eigen_B(row, col) = B[row, col];
+ eigen_B(row, col) = B(row, col);
}
}
@@ -248,7 +248,7 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
VariableMatrix X{A.cols(), B.cols()};
for (int row = 0; row < X.rows(); ++row) {
for (int col = 0; col < X.cols(); ++col) {
- X[row, col] = eigen_X(row, col);
+ X(row, col) = eigen_X(row, col);
}
}

46
wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp
View File

@@ -7,8 +7,8 @@
#include <algorithm>
#include <limits>
#include <sleipnir/autodiff/Gradient.hpp>
#include <sleipnir/autodiff/Hessian.hpp>
#include <sleipnir/autodiff/gradient.hpp>
#include <sleipnir/autodiff/hessian.hpp>
#include "frc/EigenCore.h"
#include "frc/system/NumericalIntegration.h"
@@ -18,7 +18,7 @@ using namespace frc;
units::volt_t ArmFeedforward::Calculate(
units::unit_t<Angle> currentAngle, units::unit_t<Velocity> currentVelocity,
units::unit_t<Velocity> nextVelocity) const {
using VarMat = sleipnir::VariableMatrix;
using VarMat = slp::VariableMatrix;
// Small kₐ values make the solver ill-conditioned
if (kA < units::unit_t<ka_unit>{1e-1}) {
@@ -32,37 +32,37 @@ units::volt_t ArmFeedforward::Calculate(
Matrixd<2, 1> B{{0.0}, {1.0 / kA.value()}};
const auto& f = [&](const VarMat& x, const VarMat& u) -> VarMat {
VarMat c{{0.0},
{-(kS / kA).value() * sleipnir::sign(x(1)) -
(kG / kA).value() * sleipnir::cos(x(0))}};
{-(kS / kA).value() * slp::sign(x[1]) -
(kG / kA).value() * slp::cos(x[0])}};
return A * x + B * u + c;
};
Vectord<2> r_k{currentAngle.value(), currentVelocity.value()};
sleipnir::Variable u_k;
slp::Variable u_k;
// Initial guess
auto acceleration = (nextVelocity - currentVelocity) / m_dt;
u_k.SetValue((kS * wpi::sgn(currentVelocity.value()) + kV * currentVelocity +
kA * acceleration + kG * units::math::cos(currentAngle))
.value());
u_k.set_value((kS * wpi::sgn(currentVelocity.value()) + kV * currentVelocity +
kA * acceleration + kG * units::math::cos(currentAngle))
.value());
auto r_k1 = RK4<decltype(f), VarMat, VarMat>(f, r_k, u_k, m_dt);
// Minimize difference between desired and actual next velocity
auto cost =
(nextVelocity.value() - r_k1(1)) * (nextVelocity.value() - r_k1(1));
(nextVelocity.value() - r_k1[1]) * (nextVelocity.value() - r_k1[1]);
// Refine solution via Newton's method
{
auto xAD = u_k;
double x = xAD.Value();
double x = xAD.value();
sleipnir::Gradient gradientF{cost, xAD};
Eigen::SparseVector<double> g = gradientF.Value();
slp::Gradient gradientF{cost, xAD};
Eigen::SparseVector<double> g = gradientF.value();
sleipnir::Hessian hessianF{cost, xAD};
Eigen::SparseMatrix<double> H = hessianF.Value();
slp::Hessian hessianF{cost, xAD};
Eigen::SparseMatrix<double> H = hessianF.value();
double error_k = std::numeric_limits<double>::infinity();
double error_k1 = std::abs(g.coeff(0));
@@ -81,31 +81,31 @@ units::volt_t ArmFeedforward::Calculate(
// Shrink step until cost goes down
{
double oldCost = cost.Value();
double oldCost = cost.value();
double α = 1.0;
double trial_x = x + α * p_x;
xAD.SetValue(trial_x);
xAD.set_value(trial_x);
while (cost.Value() > oldCost) {
while (cost.value() > oldCost) {
α *= 0.5;
trial_x = x + α * p_x;
xAD.SetValue(trial_x);
xAD.set_value(trial_x);
}
x = trial_x;
}
xAD.SetValue(x);
xAD.set_value(x);
g = gradientF.Value();
H = hessianF.Value();
g = gradientF.value();
H = hessianF.value();
error_k1 = std::abs(g.coeff(0));
}
}
return units::volt_t{u_k.Value()};
return units::volt_t{u_k.value()};
}

28
wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp
View File

@@ -4,7 +4,7 @@
#include "frc/geometry/Ellipse2d.h"
#include <sleipnir/optimization/OptimizationProblem.hpp>
#include <sleipnir/optimization/problem.hpp>
using namespace frc;
@@ -20,31 +20,29 @@ Translation2d Ellipse2d::Nearest(const Translation2d& point) const {
// Find nearest point
{
namespace slp = sleipnir;
sleipnir::OptimizationProblem problem;
slp::Problem problem;
// Point on ellipse
auto x = problem.DecisionVariable();
x.SetValue(rotPoint.X().value());
auto y = problem.DecisionVariable();
y.SetValue(rotPoint.Y().value());
auto x = problem.decision_variable();
x.set_value(rotPoint.X().value());
auto y = problem.decision_variable();
y.set_value(rotPoint.Y().value());
problem.Minimize(slp::pow(x - rotPoint.X().value(), 2) +
problem.minimize(slp::pow(x - rotPoint.X().value(), 2) +
slp::pow(y - rotPoint.Y().value(), 2));
// (x − x_c)²/a² + (y − y_c)²/b² = 1
// b²(x − x_c)² + a²(y − y_c)² = a²b²
double a2 = m_xSemiAxis.value() * m_xSemiAxis.value();
double b2 = m_ySemiAxis.value() * m_ySemiAxis.value();
problem.SubjectTo(b2 * slp::pow(x - m_center.X().value(), 2) +
a2 * slp::pow(y - m_center.Y().value(), 2) ==
a2 * b2);
problem.subject_to(b2 * slp::pow(x - m_center.X().value(), 2) +
a2 * slp::pow(y - m_center.Y().value(), 2) ==
a2 * b2);
problem.Solve();
problem.solve();
rotPoint = frc::Translation2d{units::meter_t{x.Value()},
units::meter_t{y.Value()}};
rotPoint = frc::Translation2d{units::meter_t{x.value()},
units::meter_t{y.value()}};
}
// Undo rotation

3
wpimath/src/main/native/thirdparty/sleipnir/.styleguide vendored
View File

@@ -10,13 +10,14 @@ modifiableFileExclude {
\.patch$
\.png$
\.svg$
jormungandr/cpp/Docstrings\.hpp$
jormungandr/cpp/docstrings\.hpp$
jormungandr/py\.typed$
}
includeOtherLibs {
^Eigen/
^catch2/
^gch/
^nanobind/
^sleipnir/
}

5
wpimath/src/main/native/thirdparty/sleipnir/include/.styleguide vendored
View File

@@ -6,12 +6,9 @@ cppSrcFileInclude {
\.cpp$
}
licenseUpdateExclude {
include/sleipnir/util/small_vector\.hpp$
}
includeOtherLibs {
^Eigen/
^fmt/
^gch/
^wpi/
}

12
wpimath/src/main/native/thirdparty/sleipnir/include/gch/small_vector.hpp vendored Normal file
View File

@@ -0,0 +1,12 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <wpi/SmallVector.h>
namespace gch {
template <typename T>
using small_vector = wpi::SmallVector<T>;
} // namespace gch

1144
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Expression.hpp vendored
View File

File diff suppressed because it is too large Load Diff

244
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/ExpressionGraph.hpp vendored
View File

@@ -1,244 +0,0 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <span>
#include <wpi/SmallVector.h>
#include "sleipnir/autodiff/Expression.hpp"
#include "sleipnir/util/FunctionRef.hpp"
#include "sleipnir/util/SymbolExports.hpp"
namespace sleipnir::detail {
/**
* This class is an adaptor type that performs value updates of an expression's
* computational graph in a way that skips duplicates.
*/
class SLEIPNIR_DLLEXPORT ExpressionGraph {
public:
/**
* Generates the deduplicated computational graph for the given expression.
*
* @param root The root node of the expression.
*/
explicit ExpressionGraph(ExpressionPtr& root) {
// If the root type is a constant, Update() is a no-op, so there's no work
// to do
if (root == nullptr || root->type == ExpressionType::kConstant) {
return;
}
// Breadth-first search (BFS) is used as opposed to a depth-first search
// (DFS) to avoid counting duplicate nodes multiple times. A list of nodes
// ordered from parent to child with no duplicates is generated.
//
// https://en.wikipedia.org/wiki/Breadth-first_search
// BFS list sorted from parent to child.
wpi::SmallVector<Expression*> stack;
stack.emplace_back(root.Get());
// Initialize the number of instances of each node in the tree
// (Expression::duplications)
while (!stack.empty()) {
auto currentNode = stack.back();
stack.pop_back();
for (auto&& arg : currentNode->args) {
// Only continue if the node is not a constant and hasn't already been
// explored.
if (arg != nullptr && arg->type != ExpressionType::kConstant) {
// If this is the first instance of the node encountered (it hasn't
// been explored yet), add it to stack so it's recursed upon
if (arg->duplications == 0) {
stack.push_back(arg.Get());
}
++arg->duplications;
}
}
}
stack.emplace_back(root.Get());
while (!stack.empty()) {
auto currentNode = stack.back();
stack.pop_back();
// BFS lists sorted from parent to child.
m_rowList.emplace_back(currentNode->row);
m_adjointList.emplace_back(currentNode);
if (currentNode->valueFunc != nullptr) {
// Constants are skipped because they have no valueFunc and don't need
// to be updated
m_valueList.emplace_back(currentNode);
}
for (auto&& arg : currentNode->args) {
// Only add node if it's not a constant and doesn't already exist in the
// tape.
if (arg != nullptr && arg->type != ExpressionType::kConstant) {
// Once the number of node visitations equals the number of
// duplications (the counter hits zero), add it to the stack. Note
// that this means the node is only enqueued once.
--arg->duplications;
if (arg->duplications == 0) {
stack.push_back(arg.Get());
}
}
}
}
}
/**
* Update the values of all nodes in this computational tree based on the
* values of their dependent nodes.
*/
void Update() {
// Traverse the BFS list backward from child to parent and update the value
// of each node.
for (auto it = m_valueList.rbegin(); it != m_valueList.rend(); ++it) {
auto& node = *it;
auto& lhs = node->args[0];
auto& rhs = node->args[1];
if (lhs != nullptr) {
if (rhs != nullptr) {
node->value = node->valueFunc(lhs->value, rhs->value);
} else {
node->value = node->valueFunc(lhs->value, 0.0);
}
}
}
}
/**
* Returns the variable's gradient tree.
*
* @param wrt Variables with respect to which to compute the gradient.
*/
wpi::SmallVector<ExpressionPtr> GenerateGradientTree(
std::span<const ExpressionPtr> wrt) const {
// Read docs/algorithms.md#Reverse_accumulation_automatic_differentiation
// for background on reverse accumulation automatic differentiation.
for (size_t row = 0; row < wrt.size(); ++row) {
wrt[row]->row = row;
}
wpi::SmallVector<ExpressionPtr> grad;
grad.reserve(wrt.size());
for (size_t row = 0; row < wrt.size(); ++row) {
grad.emplace_back(MakeExpressionPtr());
}
// Zero adjoints. The root node's adjoint is 1.0 as df/df is always 1.
if (m_adjointList.size() > 0) {
m_adjointList[0]->adjointExpr = MakeExpressionPtr(1.0);
for (auto it = m_adjointList.begin() + 1; it != m_adjointList.end();
++it) {
auto& node = *it;
node->adjointExpr = MakeExpressionPtr();
}
}
// df/dx = (df/dy)(dy/dx). The adjoint of x is equal to the adjoint of y
// multiplied by dy/dx. If there are multiple "paths" from the root node to
// variable; the variable's adjoint is the sum of each path's adjoint
// contribution.
for (auto node : m_adjointList) {
auto& lhs = node->args[0];
auto& rhs = node->args[1];
if (lhs != nullptr && !lhs->IsConstant(0.0)) {
lhs->adjointExpr = lhs->adjointExpr +
node->gradientFuncs[0](lhs, rhs, node->adjointExpr);
}
if (rhs != nullptr && !rhs->IsConstant(0.0)) {
rhs->adjointExpr = rhs->adjointExpr +
node->gradientFuncs[1](lhs, rhs, node->adjointExpr);
}
// If variable is a leaf node, assign its adjoint to the gradient.
if (node->row != -1) {
grad[node->row] = node->adjointExpr;
}
}
// Unlink adjoints to avoid circular references between them and their
// parent expressions. This ensures all expressions are returned to the free
// list.
for (auto node : m_adjointList) {
for (auto& arg : node->args) {
if (arg != nullptr) {
arg->adjointExpr = nullptr;
}
}
}
for (size_t row = 0; row < wrt.size(); ++row) {
wrt[row]->row = -1;
}
return grad;
}
/**
* Updates the adjoints in the expression graph, effectively computing the
* gradient.
*
* @param func A function that takes two arguments: an int for the gradient
* row, and a double for the adjoint (gradient value).
*/
void ComputeAdjoints(function_ref<void(int row, double adjoint)> func) {
// Zero adjoints. The root node's adjoint is 1.0 as df/df is always 1.
m_adjointList[0]->adjoint = 1.0;
for (auto it = m_adjointList.begin() + 1; it != m_adjointList.end(); ++it) {
auto& node = *it;
node->adjoint = 0.0;
}
// df/dx = (df/dy)(dy/dx). The adjoint of x is equal to the adjoint of y
// multiplied by dy/dx. If there are multiple "paths" from the root node to
// variable; the variable's adjoint is the sum of each path's adjoint
// contribution.
for (size_t col = 0; col < m_adjointList.size(); ++col) {
auto& node = m_adjointList[col];
auto& lhs = node->args[0];
auto& rhs = node->args[1];
if (lhs != nullptr) {
if (rhs != nullptr) {
lhs->adjoint += node->gradientValueFuncs[0](lhs->value, rhs->value,
node->adjoint);
rhs->adjoint += node->gradientValueFuncs[1](lhs->value, rhs->value,
node->adjoint);
} else {
lhs->adjoint +=
node->gradientValueFuncs[0](lhs->value, 0.0, node->adjoint);
}
}
// If variable is a leaf node, assign its adjoint to the gradient.
int row = m_rowList[col];
if (row != -1) {
func(row, node->adjoint);
}
}
}
private:
// List that maps nodes to their respective row.
wpi::SmallVector<int> m_rowList;
// List for updating adjoints
wpi::SmallVector<Expression*> m_adjointList;
// List for updating values
wpi::SmallVector<Expression*> m_valueList;
};
} // namespace sleipnir::detail

27
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/ExpressionType.hpp vendored
View File

@@ -1,27 +0,0 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <stdint.h>
namespace sleipnir {
/**
* Expression type.
*
* Used for autodiff caching.
*/
enum class ExpressionType : uint8_t {
/// There is no expression.
kNone,
/// The expression is a constant.
kConstant,
/// The expression is composed of linear and lower-order operators.
kLinear,
/// The expression is composed of quadratic and lower-order operators.
kQuadratic,
/// The expression is composed of nonlinear and lower-order operators.
kNonlinear
};
} // namespace sleipnir

79
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Hessian.hpp vendored
View File

@@ -1,79 +0,0 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <utility>
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <wpi/SmallVector.h>
#include "sleipnir/autodiff/ExpressionGraph.hpp"
#include "sleipnir/autodiff/Jacobian.hpp"
#include "sleipnir/autodiff/Profiler.hpp"
#include "sleipnir/autodiff/Variable.hpp"
#include "sleipnir/autodiff/VariableMatrix.hpp"
#include "sleipnir/util/SymbolExports.hpp"
namespace sleipnir {
/**
* This class calculates the Hessian of a variable with respect to a vector of
* variables.
*
* The gradient tree is cached so subsequent Hessian calculations are faster,
* and the Hessian is only recomputed if the variable expression is nonlinear.
*/
class SLEIPNIR_DLLEXPORT Hessian {
public:
/**
* Constructs a Hessian object.
*
* @param variable Variable of which to compute the Hessian.
* @param wrt Vector of variables with respect to which to compute the
* Hessian.
*/
Hessian(Variable variable, const VariableMatrix& wrt) noexcept
: m_jacobian{
[&] {
wpi::SmallVector<detail::ExpressionPtr> wrtVec;
wrtVec.reserve(wrt.size());
for (auto& elem : wrt) {
wrtVec.emplace_back(elem.expr);
}
auto grad =
detail::ExpressionGraph{variable.expr}.GenerateGradientTree(
wrtVec);
VariableMatrix ret{wrt.Rows()};
for (int row = 0; row < ret.Rows(); ++row) {
ret(row) = Variable{std::move(grad[row])};
}
return ret;
}(),
wrt} {}
/**
* Returns the Hessian as a VariableMatrix.
*
* This is useful when constructing optimization problems with derivatives in
* them.
*/
VariableMatrix Get() const { return m_jacobian.Get(); }
/**
* Evaluates the Hessian at wrt's value.
*/
const Eigen::SparseMatrix<double>& Value() { return m_jacobian.Value(); }
/**
* Returns the profiler.
*/
Profiler& GetProfiler() { return m_jacobian.GetProfiler(); }
private:
Jacobian m_jacobian;
};
} // namespace sleipnir

155
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Jacobian.hpp vendored
View File

@@ -1,155 +0,0 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <utility>
#include <Eigen/SparseCore>
#include <wpi/SmallVector.h>
#include "sleipnir/autodiff/ExpressionGraph.hpp"
#include "sleipnir/autodiff/Profiler.hpp"
#include "sleipnir/autodiff/Variable.hpp"
#include "sleipnir/autodiff/VariableMatrix.hpp"
#include "sleipnir/util/SymbolExports.hpp"
namespace sleipnir {
/**
* This class calculates the Jacobian of a vector of variables with respect to a
* vector of variables.
*
* The Jacobian is only recomputed if the variable expression is quadratic or
* higher order.
*/
class SLEIPNIR_DLLEXPORT Jacobian {
public:
/**
* Constructs a Jacobian object.
*
* @param variables Vector of variables of which to compute the Jacobian.
* @param wrt Vector of variables with respect to which to compute the
* Jacobian.
*/
Jacobian(const VariableMatrix& variables, const VariableMatrix& wrt) noexcept
: m_variables{std::move(variables)}, m_wrt{std::move(wrt)} {
m_profiler.StartSetup();
for (int row = 0; row < m_wrt.Rows(); ++row) {
m_wrt(row).expr->row = row;
}
for (Variable variable : m_variables) {
m_graphs.emplace_back(variable.expr);
}
// Reserve triplet space for 99% sparsity
m_cachedTriplets.reserve(m_variables.Rows() * m_wrt.Rows() * 0.01);
for (int row = 0; row < m_variables.Rows(); ++row) {
if (m_variables(row).Type() == ExpressionType::kLinear) {
// 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].ComputeAdjoints([&](int col, double adjoint) {
m_cachedTriplets.emplace_back(row, col, adjoint);
});
} else if (m_variables(row).Type() > ExpressionType::kLinear) {
// If the row is quadratic or nonlinear, add it to the list of nonlinear
// rows to be recomputed in Value().
m_nonlinearRows.emplace_back(row);
}
}
for (int row = 0; row < m_wrt.Rows(); ++row) {
m_wrt(row).expr->row = -1;
}
if (m_nonlinearRows.empty()) {
m_J.setFromTriplets(m_cachedTriplets.begin(), m_cachedTriplets.end());
}
m_profiler.StopSetup();
}
/**
* Returns the Jacobian as a VariableMatrix.
*
* This is useful when constructing optimization problems with derivatives in
* them.
*/
VariableMatrix Get() const {
VariableMatrix result{m_variables.Rows(), m_wrt.Rows()};
wpi::SmallVector<detail::ExpressionPtr> wrtVec;
wrtVec.reserve(m_wrt.size());
for (auto& elem : m_wrt) {
wrtVec.emplace_back(elem.expr);
}
for (int row = 0; row < m_variables.Rows(); ++row) {
auto grad = m_graphs[row].GenerateGradientTree(wrtVec);
for (int col = 0; col < m_wrt.Rows(); ++col) {
result(row, col) = Variable{std::move(grad[col])};
}
}
return result;
}
/**
* Evaluates the Jacobian at wrt's value.
*/
const Eigen::SparseMatrix<double>& Value() {
if (m_nonlinearRows.empty()) {
return m_J;
}
m_profiler.StartSolve();
for (auto& graph : m_graphs) {
graph.Update();
}
// Copy the cached triplets so triplets added for the nonlinear rows are
// thrown away at the end of the function
auto triplets = m_cachedTriplets;
// Compute each nonlinear row of the Jacobian
for (int row : m_nonlinearRows) {
m_graphs[row].ComputeAdjoints([&](int col, double adjoint) {
triplets.emplace_back(row, col, adjoint);
});
}
m_J.setFromTriplets(triplets.begin(), triplets.end());
m_profiler.StopSolve();
return m_J;
}
/**
* Returns the profiler.
*/
Profiler& GetProfiler() { return m_profiler; }
private:
VariableMatrix m_variables;
VariableMatrix m_wrt;
wpi::SmallVector<detail::ExpressionGraph> m_graphs;
Eigen::SparseMatrix<double> m_J{m_variables.Rows(), m_wrt.Rows()};
// Cached triplets for gradients of linear rows
wpi::SmallVector<Eigen::Triplet<double>> m_cachedTriplets;
// List of row indices for nonlinear rows whose graients will be computed in
// Value()
wpi::SmallVector<int> m_nonlinearRows;
Profiler m_profiler;
};
} // namespace sleipnir

79
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Profiler.hpp vendored
View File

@@ -1,79 +0,0 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <chrono>
#include "sleipnir/util/SymbolExports.hpp"
namespace sleipnir {
/**
* Records the number of profiler measurements (start/stop pairs) and the
* average duration between each start and stop call.
*/
class SLEIPNIR_DLLEXPORT Profiler {
public:
/**
* Tell the profiler to start measuring setup time.
*/
void StartSetup() { m_setupStartTime = std::chrono::system_clock::now(); }
/**
* Tell the profiler to stop measuring setup time.
*/
void StopSetup() {
m_setupDuration = std::chrono::system_clock::now() - m_setupStartTime;
}
/**
* Tell the profiler to start measuring solve time.
*/
void StartSolve() { m_solveStartTime = std::chrono::system_clock::now(); }
/**
* Tell the profiler to stop measuring solve time, increment the number of
* averages, and incorporate the latest measurement into the average.
*/
void StopSolve() {
auto now = std::chrono::system_clock::now();
++m_solveMeasurements;
m_averageSolveDuration =
(m_solveMeasurements - 1.0) / m_solveMeasurements *
m_averageSolveDuration +
1.0 / m_solveMeasurements * (now - m_solveStartTime);
}
/**
* The setup duration in milliseconds as a double.
*/
double SetupDuration() const {
using std::chrono::duration_cast;
using std::chrono::nanoseconds;
return duration_cast<nanoseconds>(m_setupDuration).count() / 1e6;
}
/**
* The number of solve measurements taken.
*/
int SolveMeasurements() const { return m_solveMeasurements; }
/**
* The average solve duration in milliseconds as a double.
*/
double AverageSolveDuration() const {
using std::chrono::duration_cast;
using std::chrono::nanoseconds;
return duration_cast<nanoseconds>(m_averageSolveDuration).count() / 1e6;
}
private:
std::chrono::system_clock::time_point m_setupStartTime;
std::chrono::duration<double> m_setupDuration{0.0};
int m_solveMeasurements = 0;
std::chrono::duration<double> m_averageSolveDuration{0.0};
std::chrono::system_clock::time_point m_solveStartTime;
};
} // namespace sleipnir

765
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableBlock.hpp vendored
View File

@@ -1,765 +0,0 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <concepts>
#include <type_traits>
#include <utility>
#include <Eigen/Core>
#include "sleipnir/autodiff/Slice.hpp"
#include "sleipnir/autodiff/Variable.hpp"
#include "sleipnir/util/Assert.hpp"
#include "sleipnir/util/FunctionRef.hpp"
namespace sleipnir {
/**
* A submatrix of autodiff variables with reference semantics.
*
* @tparam Mat The type of the matrix whose storage this class points to.
*/
template <typename Mat>
class VariableBlock {
public:
VariableBlock(const VariableBlock<Mat>& values) = default;
/**
* Assigns a VariableBlock to the block.
*
* @param values VariableBlock of values.
*/
VariableBlock<Mat>& operator=(const VariableBlock<Mat>& values) {
if (this == &values) {
return *this;
}
if (m_mat == nullptr) {
m_mat = values.m_mat;
m_rowSlice = values.m_rowSlice;
m_rowSliceLength = values.m_rowSliceLength;
m_colSlice = values.m_colSlice;
m_colSliceLength = values.m_colSliceLength;
} else {
Assert(Rows() == values.Rows());
Assert(Cols() == values.Cols());
for (int row = 0; row < Rows(); ++row) {
for (int col = 0; col < Cols(); ++col) {
(*this)(row, col) = values(row, col);
}
}
}
return *this;
}
VariableBlock(VariableBlock<Mat>&&) = default;
/**
* Assigns a VariableBlock to the block.
*
* @param values VariableBlock of values.
*/
VariableBlock<Mat>& operator=(VariableBlock<Mat>&& values) {
if (this == &values) {
return *this;
}
if (m_mat == nullptr) {
m_mat = values.m_mat;
m_rowSlice = values.m_rowSlice;
m_rowSliceLength = values.m_rowSliceLength;
m_colSlice = values.m_colSlice;
m_colSliceLength = values.m_colSliceLength;
} else {
Assert(Rows() == values.Rows());
Assert(Cols() == values.Cols());
for (int row = 0; row < Rows(); ++row) {
for (int col = 0; col < Cols(); ++col) {
(*this)(row, col) = values(row, col);
}
}
}
return *this;
}
/**
* Constructs a Variable block pointing to all of the given matrix.
*
* @param mat The matrix to which to point.
*/
VariableBlock(Mat& mat) // NOLINT
: m_mat{&mat},
m_rowSlice{0, mat.Rows(), 1},
m_rowSliceLength{m_rowSlice.Adjust(mat.Rows())},
m_colSlice{0, mat.Cols(), 1},
m_colSliceLength{m_colSlice.Adjust(mat.Cols())} {}
/**
* Constructs a Variable block pointing to a subset of the given matrix.
*
* @param mat The matrix to which to point.
* @param rowOffset The block's row offset.
* @param colOffset The block's column offset.
* @param blockRows The number of rows in the block.
* @param blockCols The number of columns in the block.
*/
VariableBlock(Mat& mat, int rowOffset, int colOffset, int blockRows,
int blockCols)
: m_mat{&mat},
m_rowSlice{rowOffset, rowOffset + blockRows, 1},
m_rowSliceLength{m_rowSlice.Adjust(mat.Rows())},
m_colSlice{colOffset, colOffset + blockCols, 1},
m_colSliceLength{m_colSlice.Adjust(mat.Cols())} {}
/**
* Constructs a Variable block pointing to a subset of the given matrix.
*
* Note that the slices are taken as is rather than adjusted.
*
* @param mat The matrix to which to point.
* @param rowSlice The block's row slice.
* @param rowSliceLength The block's row length.
* @param colSlice The block's column slice.
* @param colSliceLength The block's column length.
*/
VariableBlock(Mat& mat, Slice rowSlice, int rowSliceLength, Slice colSlice,
int colSliceLength)
: m_mat{&mat},
m_rowSlice{std::move(rowSlice)},
m_rowSliceLength{rowSliceLength},
m_colSlice{std::move(colSlice)},
m_colSliceLength{colSliceLength} {}
/**
* Assigns a double to the block.
*
* This only works for blocks with one row and one column.
*/
VariableBlock<Mat>& operator=(double value) {
Assert(Rows() == 1 && Cols() == 1);
(*this)(0, 0) = value;
return *this;
}
/**
* Assigns a double to the block.
*
* This only works for blocks with one row and one column.
*
* @param value Value to assign.
*/
void SetValue(double value) {
Assert(Rows() == 1 && Cols() == 1);
(*this)(0, 0).SetValue(value);
}
/**
* Assigns an Eigen matrix to the block.
*
* @param values Eigen matrix of values to assign.
*/
template <typename Derived>
VariableBlock<Mat>& operator=(const Eigen::MatrixBase<Derived>& values) {
Assert(Rows() == values.rows());
Assert(Cols() == values.cols());
for (int row = 0; row < Rows(); ++row) {
for (int col = 0; col < Cols(); ++col) {
(*this)(row, col) = values(row, col);
}
}
return *this;
}
/**
* Sets block's internal values.
*
* @param values Eigen matrix of values.
*/
template <typename Derived>
requires std::same_as<typename Derived::Scalar, double>
void SetValue(const Eigen::MatrixBase<Derived>& values) {
Assert(Rows() == values.rows());
Assert(Cols() == values.cols());
for (int row = 0; row < Rows(); ++row) {
for (int col = 0; col < Cols(); ++col) {
(*this)(row, col).SetValue(values(row, col));
}
}
}
/**
* Assigns a VariableMatrix to the block.
*
* @param values VariableMatrix of values.
*/
VariableBlock<Mat>& operator=(const Mat& values) {
Assert(Rows() == values.Rows());
Assert(Cols() == values.Cols());
for (int row = 0; row < Rows(); ++row) {
for (int col = 0; col < Cols(); ++col) {
(*this)(row, col) = values(row, col);
}
}
return *this;
}
/**
* Assigns a VariableMatrix to the block.
*
* @param values VariableMatrix of values.
*/
VariableBlock<Mat>& operator=(Mat&& values) {
Assert(Rows() == values.Rows());
Assert(Cols() == values.Cols());
for (int row = 0; row < Rows(); ++row) {
for (int col = 0; col < Cols(); ++col) {
(*this)(row, col) = std::move(values(row, col));
}
}
return *this;
}
/**
* Returns a scalar subblock at the given row and column.
*
* @param row The scalar subblock's row.
* @param col The scalar subblock's column.
*/
Variable& operator()(int row, int col)
requires(!std::is_const_v<Mat>)
{
Assert(row >= 0 && row < Rows());
Assert(col >= 0 && col < Cols());
return (*m_mat)(m_rowSlice.start + row * m_rowSlice.step,
m_colSlice.start + col * m_colSlice.step);
}
/**
* Returns a scalar subblock at the given row and column.
*
* @param row The scalar subblock's row.
* @param col The scalar subblock's column.
*/
const Variable& operator()(int row, int col) const {
Assert(row >= 0 && row < Rows());
Assert(col >= 0 && col < Cols());
return (*m_mat)(m_rowSlice.start + row * m_rowSlice.step,
m_colSlice.start + col * m_colSlice.step);
}
/**
* Returns a scalar subblock at the given row.
*
* @param row The scalar subblock's row.
*/
Variable& operator()(int row)
requires(!std::is_const_v<Mat>)
{
Assert(row >= 0 && row < Rows() * Cols());
return (*this)(row / Cols(), row % Cols());
}
/**
* Returns a scalar subblock at the given row.
*
* @param row The scalar subblock's row.
*/
const Variable& operator()(int row) const {
Assert(row >= 0 && row < Rows() * Cols());
return (*this)(row / Cols(), row % Cols());
}
/**
* Returns a block of the variable matrix.
*
* @param rowOffset The row offset of the block selection.
* @param colOffset The column offset of the block selection.
* @param blockRows The number of rows in the block selection.
* @param blockCols The number of columns in the block selection.
*/
VariableBlock<Mat> Block(int rowOffset, int colOffset, int blockRows,
int blockCols) {
Assert(rowOffset >= 0 && rowOffset <= Rows());
Assert(colOffset >= 0 && colOffset <= Cols());
Assert(blockRows >= 0 && blockRows <= Rows() - rowOffset);
Assert(blockCols >= 0 && blockCols <= Cols() - colOffset);
return (*this)({rowOffset, rowOffset + blockRows, 1},
{colOffset, colOffset + blockCols, 1});
}
/**
* Returns a block slice of the variable matrix.
*
* @param rowOffset The row offset of the block selection.
* @param colOffset The column offset of the block selection.
* @param blockRows The number of rows in the block selection.
* @param blockCols The number of columns in the block selection.
*/
const VariableBlock<const Mat> Block(int rowOffset, int colOffset,
int blockRows, int blockCols) const {
Assert(rowOffset >= 0 && rowOffset <= Rows());
Assert(colOffset >= 0 && colOffset <= Cols());
Assert(blockRows >= 0 && blockRows <= Rows() - rowOffset);
Assert(blockCols >= 0 && blockCols <= Cols() - colOffset);
return (*this)({rowOffset, rowOffset + blockRows, 1},
{colOffset, colOffset + blockCols, 1});
}
/**
* Returns a slice of the variable matrix.
*
* @param rowSlice The row slice.
* @param colSlice The column slice.
*/
VariableBlock<Mat> operator()(Slice rowSlice, Slice colSlice) {
int rowSliceLength = rowSlice.Adjust(m_rowSliceLength);
int colSliceLength = colSlice.Adjust(m_colSliceLength);
return VariableBlock{
*m_mat,
{m_rowSlice.start + rowSlice.start * m_rowSlice.step,
m_rowSlice.start + rowSlice.stop, m_rowSlice.step * rowSlice.step},
rowSliceLength,
{m_colSlice.start + colSlice.start * m_colSlice.step,
m_colSlice.start + colSlice.stop, m_colSlice.step * colSlice.step},
colSliceLength};
}
/**
* Returns a slice of the variable matrix.
*
* @param rowSlice The row slice.
* @param colSlice The column slice.
*/
const VariableBlock<const Mat> operator()(Slice rowSlice,
Slice colSlice) const {
int rowSliceLength = rowSlice.Adjust(m_rowSliceLength);
int colSliceLength = colSlice.Adjust(m_colSliceLength);
return VariableBlock{
*m_mat,
{m_rowSlice.start + rowSlice.start * m_rowSlice.step,
m_rowSlice.start + rowSlice.stop, m_rowSlice.step * rowSlice.step},
rowSliceLength,
{m_colSlice.start + colSlice.start * m_colSlice.step,
m_colSlice.start + colSlice.stop, m_colSlice.step * colSlice.step},
colSliceLength};
}
/**
* Returns a slice of the variable matrix.
*
* The given slices aren't adjusted. This overload is for Python bindings
* only.
*
* @param rowSlice The row slice.
* @param rowSliceLength The row slice length.
* @param colSlice The column slice.
* @param colSliceLength The column slice length.
*/
VariableBlock<Mat> operator()(Slice rowSlice, int rowSliceLength,
Slice colSlice, int colSliceLength) {
return VariableBlock{
*m_mat,
{m_rowSlice.start + rowSlice.start * m_rowSlice.step,
m_rowSlice.start + rowSlice.stop, m_rowSlice.step * rowSlice.step},
rowSliceLength,
{m_colSlice.start + colSlice.start * m_colSlice.step,
m_colSlice.start + colSlice.stop, m_colSlice.step * colSlice.step},
colSliceLength};
}
/**
* Returns a slice of the variable matrix.
*
* The given slices aren't adjusted. This overload is for Python bindings
* only.
*
* @param rowSlice The row slice.
* @param rowSliceLength The row slice length.
* @param colSlice The column slice.
* @param colSliceLength The column slice length.
*/
const VariableBlock<const Mat> operator()(Slice rowSlice, int rowSliceLength,
Slice colSlice,
int colSliceLength) const {
return VariableBlock{
*m_mat,
{m_rowSlice.start + rowSlice.start * m_rowSlice.step,
m_rowSlice.start + rowSlice.stop, m_rowSlice.step * rowSlice.step},
rowSliceLength,
{m_colSlice.start + colSlice.start * m_colSlice.step,
m_colSlice.start + colSlice.stop, m_colSlice.step * colSlice.step},
colSliceLength};
}
/**
* Returns a segment of the variable vector.
*
* @param offset The offset of the segment.
* @param length The length of the segment.
*/
VariableBlock<Mat> Segment(int offset, int length) {
Assert(offset >= 0 && offset < Rows() * Cols());
Assert(length >= 0 && length <= Rows() * Cols() - offset);
return Block(offset, 0, length, 1);
}
/**
* Returns a segment of the variable vector.
*
* @param offset The offset of the segment.
* @param length The length of the segment.
*/
const VariableBlock<Mat> Segment(int offset, int length) const {
Assert(offset >= 0 && offset < Rows() * Cols());
Assert(length >= 0 && length <= Rows() * Cols() - offset);
return Block(offset, 0, length, 1);
}
/**
* Returns a row slice of the variable matrix.
*
* @param row The row to slice.
*/
VariableBlock<Mat> Row(int row) {
Assert(row >= 0 && row < Rows());
return Block(row, 0, 1, Cols());
}
/**
* Returns a row slice of the variable matrix.
*
* @param row The row to slice.
*/
VariableBlock<const Mat> Row(int row) const {
Assert(row >= 0 && row < Rows());
return Block(row, 0, 1, Cols());
}
/**
* Returns a column slice of the variable matrix.
*
* @param col The column to slice.
*/
VariableBlock<Mat> Col(int col) {
Assert(col >= 0 && col < Cols());
return Block(0, col, Rows(), 1);
}
/**
* Returns a column slice of the variable matrix.
*
* @param col The column to slice.
*/
VariableBlock<const Mat> Col(int col) const {
Assert(col >= 0 && col < Cols());
return Block(0, col, Rows(), 1);
}
/**
* Compound matrix multiplication-assignment operator.
*
* @param rhs Variable to multiply.
*/
VariableBlock<Mat>& operator*=(const VariableBlock<Mat>& rhs) {
Assert(Cols() == rhs.Rows() && Cols() == rhs.Cols());
for (int i = 0; i < Rows(); ++i) {
for (int j = 0; j < rhs.Cols(); ++j) {
Variable sum;
for (int k = 0; k < Cols(); ++k) {
sum += (*this)(i, k) * rhs(k, j);
}
(*this)(i, j) = sum;
}
}
return *this;
}
/**
* Compound matrix multiplication-assignment operator (only enabled when lhs
* is a scalar).
*
* @param rhs Variable to multiply.
*/
VariableBlock& operator*=(double rhs) {
for (int row = 0; row < Rows(); ++row) {
for (int col = 0; col < Cols(); ++col) {
(*this)(row, col) *= rhs;
}
}
return *this;
}
/**
* Compound matrix division-assignment operator (only enabled when rhs
* is a scalar).
*
* @param rhs Variable to divide.
*/
VariableBlock<Mat>& operator/=(const VariableBlock<Mat>& rhs) {
Assert(rhs.Rows() == 1 && rhs.Cols() == 1);
for (int row = 0; row < Rows(); ++row) {
for (int col = 0; col < Cols(); ++col) {
(*this)(row, col) /= rhs(0, 0);
}
}
return *this;
}
/**
* Compound matrix division-assignment operator (only enabled when rhs
* is a scalar).
*
* @param rhs Variable to divide.
*/
VariableBlock<Mat>& operator/=(double rhs) {
for (int row = 0; row < Rows(); ++row) {
for (int col = 0; col < Cols(); ++col) {
(*this)(row, col) /= rhs;
}
}
return *this;
}
/**
* Compound addition-assignment operator.
*
* @param rhs Variable to add.
*/
VariableBlock<Mat>& operator+=(const VariableBlock<Mat>& rhs) {
for (int row = 0; row < Rows(); ++row) {
for (int col = 0; col < Cols(); ++col) {
(*this)(row, col) += rhs(row, col);
}
}
return *this;
}
/**
* Compound subtraction-assignment operator.
*
* @param rhs Variable to subtract.
*/
VariableBlock<Mat>& operator-=(const VariableBlock<Mat>& rhs) {
for (int row = 0; row < Rows(); ++row) {
for (int col = 0; col < Cols(); ++col) {
(*this)(row, col) -= rhs(row, col);
}
}
return *this;
}
/**
* Returns the transpose of the variable matrix.
*/
std::remove_cv_t<Mat> T() const {
std::remove_cv_t<Mat> result{Cols(), Rows()};
for (int row = 0; row < Rows(); ++row) {
for (int col = 0; col < Cols(); ++col) {
result(col, row) = (*this)(row, col);
}
}
return result;
}
/**
* Returns number of rows in the matrix.
*/
int Rows() const { return m_rowSliceLength; }
/**
* Returns number of columns in the matrix.
*/
int Cols() const { return m_colSliceLength; }
/**
* Returns an element of the variable matrix.
*
* @param row The row of the element to return.
* @param col The column of the element to return.
*/
double Value(int row, int col) {
Assert(row >= 0 && row < Rows());
Assert(col >= 0 && col < Cols());
return (*m_mat)(m_rowSlice.start + row * m_rowSlice.step,
m_colSlice.start + col * m_colSlice.step)
.Value();
}
/**
* Returns a row of the variable column vector.
*
* @param index The index of the element to return.
*/
double Value(int index) {
Assert(index >= 0 && index < Rows() * Cols());
return Value(index / Cols(), index % Cols());
}
/**
* Returns the contents of the variable matrix.
*/
Eigen::MatrixXd Value() {
Eigen::MatrixXd result{Rows(), Cols()};
for (int row = 0; row < Rows(); ++row) {
for (int col = 0; col < Cols(); ++col) {
result(row, col) = Value(row, col);
}
}
return result;
}
/**
* Transforms the matrix coefficient-wise with an unary operator.
*
* @param unaryOp The unary operator to use for the transform operation.
*/
std::remove_cv_t<Mat> CwiseTransform(
function_ref<Variable(const Variable& x)> unaryOp) const {
std::remove_cv_t<Mat> result{Rows(), Cols()};
for (int row = 0; row < Rows(); ++row) {
for (int col = 0; col < Cols(); ++col) {
result(row, col) = unaryOp((*this)(row, col));
}
}
return result;
}
class iterator {
public:
using iterator_category = std::forward_iterator_tag;
using value_type = Variable;
using difference_type = std::ptrdiff_t;
using pointer = Variable*;
using reference = Variable&;
iterator(VariableBlock<Mat>* mat, int row, int col)
: m_mat{mat}, m_row{row}, m_col{col} {}
iterator& operator++() {
++m_col;
if (m_col == m_mat->Cols()) {
m_col = 0;
++m_row;
}
return *this;
}
iterator operator++(int) {
iterator retval = *this;
++(*this);
return retval;
}
bool operator==(const iterator&) const = default;
reference operator*() { return (*m_mat)(m_row, m_col); }
private:
VariableBlock<Mat>* m_mat;
int m_row;
int m_col;
};
class const_iterator {
public:
using iterator_category = std::forward_iterator_tag;
using value_type = Variable;
using difference_type = std::ptrdiff_t;
using pointer = Variable*;
using const_reference = const Variable&;
const_iterator(const VariableBlock<Mat>* mat, int row, int col)
: m_mat{mat}, m_row{row}, m_col{col} {}
const_iterator& operator++() {
++m_col;
if (m_col == m_mat->Cols()) {
m_col = 0;
++m_row;
}
return *this;
}
const_iterator operator++(int) {
const_iterator retval = *this;
++(*this);
return retval;
}
bool operator==(const const_iterator&) const = default;
const_reference operator*() const { return (*m_mat)(m_row, m_col); }
private:
const VariableBlock<Mat>* m_mat;
int m_row;
int m_col;
};
/**
* Returns begin iterator.
*/
iterator begin() { return iterator(this, 0, 0); }
/**
* Returns end iterator.
*/
iterator end() { return iterator(this, Rows(), 0); }
/**
* Returns begin iterator.
*/
const_iterator begin() const { return const_iterator(this, 0, 0); }
/**
* Returns end iterator.
*/
const_iterator end() const { return const_iterator(this, Rows(), 0); }
/**
* Returns begin iterator.
*/
const_iterator cbegin() const { return const_iterator(this, 0, 0); }
/**
* Returns end iterator.
*/
const_iterator cend() const { return const_iterator(this, Rows(), 0); }
/**
* Returns number of elements in matrix.
*/
size_t size() const { return Rows() * Cols(); }
private:
Mat* m_mat = nullptr;
Slice m_rowSlice;
int m_rowSliceLength = 0;
Slice m_colSlice;
int m_colSliceLength = 0;
};
} // namespace sleipnir

1096
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableMatrix.hpp vendored
View File

File diff suppressed because it is too large Load Diff

178
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/adjoint_expression_graph.hpp vendored Normal file
View File

@@ -0,0 +1,178 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <ranges>
#include <utility>
#include <Eigen/SparseCore>
#include <gch/small_vector.hpp>
#include "sleipnir/autodiff/expression_graph.hpp"
#include "sleipnir/autodiff/variable.hpp"
#include "sleipnir/autodiff/variable_matrix.hpp"
namespace slp::detail {
/**
* This class is an adaptor type that performs value updates of an expression's
* adjoint graph.
*/
class AdjointExpressionGraph {
public:
/**
* Generates the adjoint graph for the given expression.
*
* @param root The root node of the expression.
*/
explicit AdjointExpressionGraph(const Variable& root)
: m_top_list{topological_sort(root.expr)} {
for (const auto& node : m_top_list) {
m_col_list.emplace_back(node->col);
}
}
/**
* Update the values of all nodes in this adjoint graph based on the values of
* their dependent nodes.
*/
void update_values() { detail::update_values(m_top_list); }
/**
* Returns the variable's gradient tree.
*
* This function lazily allocates variables, so elements of the returned
* VariableMatrix will be empty if the corresponding element of wrt had no
* adjoint. Ensure Variable::expr isn't nullptr before calling member
* functions.
*
* @param wrt Variables with respect to which to compute the gradient.
* @return The variable's gradient tree.
*/
VariableMatrix generate_gradient_tree(const VariableMatrix& wrt) const {
// Read docs/algorithms.md#Reverse_accumulation_automatic_differentiation
// for background on reverse accumulation automatic differentiation.
if (m_top_list.empty()) {
return VariableMatrix{VariableMatrix::empty, wrt.rows(), 1};
}
// Set root node's adjoint to 1 since df/df is 1
m_top_list[0]->adjoint_expr = make_expression_ptr<ConstExpression>(1.0);
// df/dx = (df/dy)(dy/dx). The adjoint of x is equal to the adjoint of y
// multiplied by dy/dx. If there are multiple "paths" from the root node to
// variable; the variable's adjoint is the sum of each path's adjoint
// contribution.
for (auto& node : m_top_list) {
auto& lhs = node->args[0];
auto& rhs = node->args[1];
if (lhs != nullptr) {
lhs->adjoint_expr += node->grad_expr_l(lhs, rhs, node->adjoint_expr);
if (rhs != nullptr) {
rhs->adjoint_expr += node->grad_expr_r(lhs, rhs, node->adjoint_expr);
}
}
}
// Move gradient tree to return value
VariableMatrix grad{VariableMatrix::empty, wrt.rows(), 1};
for (int row = 0; row < grad.rows(); ++row) {
grad[row] = Variable{std::move(wrt[row].expr->adjoint_expr)};
}
// Unlink adjoints to avoid circular references between them and their
// parent expressions. This ensures all expressions are returned to the free
// list.
for (auto& node : m_top_list) {
node->adjoint_expr = nullptr;
}
return grad;
}
/**
* Updates the adjoints in the expression graph (computes the gradient) then
* appends the adjoints of wrt to the sparse matrix triplets.
*
* @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_adjoint_triplets(
gch::small_vector<Eigen::Triplet<double>>& triplets, int row,
const VariableMatrix& wrt) 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 = 0.0;
}
}
if (m_top_list.empty()) {
return;
}
// Set root node's adjoint to 1 since df/df is 1
m_top_list[0]->adjoint = 1.0;
// Zero the rest of the adjoints
for (auto& node : m_top_list | std::views::drop(1)) {
node->adjoint = 0.0;
}
// df/dx = (df/dy)(dy/dx). The adjoint of x is equal to the adjoint of y
// multiplied by dy/dx. If there are multiple "paths" from the root node to
// variable; the variable's adjoint is the sum of each path's adjoint
// contribution.
for (const auto& node : m_top_list) {
auto& lhs = node->args[0];
auto& rhs = node->args[1];
if (lhs != nullptr) {
if (rhs != nullptr) {
lhs->adjoint += node->grad_l(lhs->val, rhs->val, node->adjoint);
rhs->adjoint += node->grad_r(lhs->val, rhs->val, node->adjoint);
} else {
lhs->adjoint += node->grad_l(lhs->val, 0.0, node->adjoint);
}
}
}
// 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;
// Append adjoints of wrt to sparse matrix triplets
if (node->adjoint != 0.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 != 0.0) {
triplets.emplace_back(row, col, node->adjoint);
}
}
}
}
private:
// Topological sort of graph from parent to child
gch::small_vector<Expression*> m_top_list;
// List that maps nodes to their respective column
gch::small_vector<int> m_col_list;
};
} // namespace slp::detail

1758
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression.hpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff

94
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_graph.hpp vendored Normal file
View File

@@ -0,0 +1,94 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <ranges>
#include <gch/small_vector.hpp>
#include "sleipnir/autodiff/expression.hpp"
namespace slp::detail {
/**
* Generate a topological sort of an expression graph from parent to child.
*
* https://en.wikipedia.org/wiki/Topological_sorting
*
* @param root The root node of the expression.
*/
inline gch::small_vector<Expression*> topological_sort(
const ExpressionPtr& root) {
gch::small_vector<Expression*> list;
// If the root type is a constant, Update() is a no-op, so there's no work
// to do
if (root == nullptr || root->type() == ExpressionType::CONSTANT) {
return list;
}
// Stack of nodes to explore
gch::small_vector<Expression*> stack;
// Enumerate incoming edges for each node via depth-first search
stack.emplace_back(root.get());
while (!stack.empty()) {
auto node = stack.back();
stack.pop_back();
for (auto& arg : node->args) {
// If the node hasn't been explored yet, add it to the stack
if (arg != nullptr && ++arg->incoming_edges == 1) {
stack.push_back(arg.get());
}
}
}
// Generate topological sort of graph from parent to child.
//
// A node is only added to the stack after all its incoming edges have been
// traversed. Expression::incoming_edges is a decrementing counter for
// tracking this.
//
// https://en.wikipedia.org/wiki/Topological_sorting
stack.emplace_back(root.get());
while (!stack.empty()) {
auto node = stack.back();
stack.pop_back();
list.emplace_back(node);
for (auto& arg : node->args) {
// If we traversed all this node's incoming edges, add it to the stack
if (arg != nullptr && --arg->incoming_edges == 0) {
stack.push_back(arg.get());
}
}
}
return list;
}
/**
* Update the values of all nodes in this graph based on the values of
* their dependent nodes.
*
* @param list Topological sort of graph from parent to child.
*/
inline void update_values(const gch::small_vector<Expression*>& list) {
// Traverse graph from child to parent and update values
for (auto& node : list | std::views::reverse) {
auto& lhs = node->args[0];
auto& rhs = node->args[1];
if (lhs != nullptr) {
if (rhs != nullptr) {
node->val = node->value(lhs->val, rhs->val);
} else {
node->val = node->value(lhs->val, 0.0);
}
}
}
}
} // namespace slp::detail

56
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_type.hpp vendored Normal file
View File

@@ -0,0 +1,56 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <stdint.h>
#include <string_view>
#include "sleipnir/util/symbol_exports.hpp"
namespace slp {
/**
* Expression type.
*
* Used for autodiff caching.
*/
enum class ExpressionType : uint8_t {
/// There is no expression.
NONE,
/// The expression is a constant.
CONSTANT,
/// The expression is composed of linear and lower-order operators.
LINEAR,
/// The expression is composed of quadratic and lower-order operators.
QUADRATIC,
/// The expression is composed of nonlinear and lower-order operators.
NONLINEAR
};
/**
* Returns user-readable message corresponding to the expression type.
*
* @param type Expression type.
*/
SLEIPNIR_DLLEXPORT constexpr std::string_view to_message(
const ExpressionType& type) {
using enum ExpressionType;
switch (type) {
case NONE:
return "none";
case CONSTANT:
return "constant";
case LINEAR:
return "linear";
case QUADRATIC:
return "quadratic";
case NONLINEAR:
return "nonlinear";
default:
return "unknown";
}
}
} // namespace slp

35
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Gradient.hpp → wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient.hpp vendored
View File

@@ -6,13 +6,13 @@
#include <Eigen/SparseCore>
#include "sleipnir/autodiff/Jacobian.hpp"
#include "sleipnir/autodiff/Profiler.hpp"
#include "sleipnir/autodiff/Variable.hpp"
#include "sleipnir/autodiff/VariableMatrix.hpp"
#include "sleipnir/util/SymbolExports.hpp"
#include "sleipnir/autodiff/jacobian.hpp"
#include "sleipnir/autodiff/variable.hpp"
#include "sleipnir/autodiff/variable_matrix.hpp"
#include "sleipnir/util/concepts.hpp"
#include "sleipnir/util/symbol_exports.hpp"
namespace sleipnir {
namespace slp {
/**
* This class calculates the gradient of a a variable with respect to a vector
@@ -30,7 +30,7 @@ class SLEIPNIR_DLLEXPORT Gradient {
* @param wrt Variable with respect to which to compute the gradient.
*/
Gradient(Variable variable, Variable wrt) noexcept
: Gradient{std::move(variable), VariableMatrix{wrt}} {}
: m_jacobian{std::move(variable), std::move(wrt)} {}
/**
* Constructs a Gradient object.
@@ -39,35 +39,34 @@ class SLEIPNIR_DLLEXPORT Gradient {
* @param wrt Vector of variables with respect to which to compute the
* gradient.
*/
Gradient(Variable variable, const VariableMatrix& wrt) noexcept
: m_jacobian{variable, wrt} {}
Gradient(Variable variable, SleipnirMatrixLike auto wrt) noexcept
: m_jacobian{VariableMatrix{std::move(variable)}, std::move(wrt)} {}
/**
* Returns the gradient as a VariableMatrix.
*
* This is useful when constructing optimization problems with derivatives in
* them.
*
* @return The gradient as a VariableMatrix.
*/
VariableMatrix Get() const { return m_jacobian.Get().T(); }
VariableMatrix get() const { return m_jacobian.get().T(); }
/**
* Evaluates the gradient at wrt's value.
*
* @return The gradient at wrt's value.
*/
const Eigen::SparseVector<double>& Value() {
m_g = m_jacobian.Value();
const Eigen::SparseVector<double>& value() {
m_g = m_jacobian.value();
return m_g;
}
/**
* Returns the profiler.
*/
Profiler& GetProfiler() { return m_jacobian.GetProfiler(); }
private:
Eigen::SparseVector<double> m_g;
Jacobian m_jacobian;
};
} // namespace sleipnir
} // namespace slp

169
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/hessian.hpp vendored Normal file
View File

@@ -0,0 +1,169 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <utility>
#include <Eigen/SparseCore>
#include <gch/small_vector.hpp>
#include "sleipnir/autodiff/adjoint_expression_graph.hpp"
#include "sleipnir/autodiff/variable.hpp"
#include "sleipnir/autodiff/variable_matrix.hpp"
#include "sleipnir/util/concepts.hpp"
#include "sleipnir/util/symbol_exports.hpp"
namespace slp {
/**
* This class calculates the Hessian of a variable with respect to a vector of
* variables.
*
* The gradient tree is cached so subsequent Hessian calculations are faster,
* and the Hessian is only recomputed if the variable expression is nonlinear.
*
* @tparam UpLo Which part of the Hessian to compute (Lower or Lower | Upper).
*/
template <int UpLo>
requires(UpLo == Eigen::Lower) || (UpLo == (Eigen::Lower | Eigen::Upper))
class SLEIPNIR_DLLEXPORT Hessian {
public:
/**
* Constructs a Hessian object.
*
* @param variable Variable of which to compute the Hessian.
* @param wrt Variable with respect to which to compute the Hessian.
*/
Hessian(Variable variable, Variable wrt) noexcept
: Hessian{std::move(variable), VariableMatrix{std::move(wrt)}} {}
/**
* Constructs a Hessian object.
*
* @param variable Variable of which to compute the Hessian.
* @param wrt Vector of variables with respect to which to compute the
* Hessian.
*/
Hessian(Variable variable, SleipnirMatrixLike auto wrt) noexcept
: m_variables{detail::AdjointExpressionGraph{variable}
.generate_gradient_tree(wrt)},
m_wrt{wrt} {
// Initialize column each expression's adjoint occupies in the Jacobian
for (size_t col = 0; col < m_wrt.size(); ++col) {
m_wrt[col].expr->col = col;
}
for (auto& variable : m_variables) {
m_graphs.emplace_back(variable);
}
// Reset col to -1
for (auto& node : m_wrt) {
node.expr->col = -1;
}
for (int row = 0; row < m_variables.rows(); ++row) {
if (m_variables[row].expr == nullptr) {
continue;
}
if (m_variables[row].type() == ExpressionType::LINEAR) {
// 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_adjoint_triplets(m_cached_triplets, row, m_wrt);
} 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().
m_nonlinear_rows.emplace_back(row);
}
}
if (m_nonlinear_rows.empty()) {
m_H.setFromTriplets(m_cached_triplets.begin(), m_cached_triplets.end());
if constexpr (UpLo == Eigen::Lower) {
m_H = m_H.triangularView<Eigen::Lower>();
}
}
}
/**
* Returns the Hessian as a VariableMatrix.
*
* This is useful when constructing optimization problems with derivatives in
* them.
*
* @return The Hessian as a VariableMatrix.
*/
VariableMatrix get() const {
VariableMatrix result{VariableMatrix::empty, m_variables.rows(),
m_wrt.rows()};
for (int row = 0; row < m_variables.rows(); ++row) {
auto grad = m_graphs[row].generate_gradient_tree(m_wrt);
for (int col = 0; col < m_wrt.rows(); ++col) {
if (grad[col].expr != nullptr) {
result(row, col) = std::move(grad[col]);
} else {
result(row, col) = Variable{0.0};
}
}
}
return result;
}
/**
* Evaluates the Hessian at wrt's value.
*
* @return The Hessian at wrt's value.
*/
const Eigen::SparseMatrix<double>& value() {
if (m_nonlinear_rows.empty()) {
return m_H;
}
for (auto& graph : m_graphs) {
graph.update_values();
}
// Copy the cached triplets so triplets added for the nonlinear rows are
// thrown away at the end of the function
auto triplets = m_cached_triplets;
// Compute each nonlinear row of the Hessian
for (int row : m_nonlinear_rows) {
m_graphs[row].append_adjoint_triplets(triplets, row, m_wrt);
}
if (!triplets.empty()) {
m_H.setFromTriplets(triplets.begin(), triplets.end());
if constexpr (UpLo == Eigen::Lower) {
m_H = m_H.triangularView<Eigen::Lower>();
}
} else {
// setFromTriplets() is a no-op on empty triplets, so explicitly zero out
// the storage
m_H.setZero();
}
return m_H;
}
private:
VariableMatrix m_variables;
VariableMatrix m_wrt;
gch::small_vector<detail::AdjointExpressionGraph> m_graphs;
Eigen::SparseMatrix<double> m_H{m_variables.rows(), m_wrt.rows()};
// Cached triplets for gradients of linear rows
gch::small_vector<Eigen::Triplet<double>> m_cached_triplets;
// List of row indices for nonlinear rows whose graients will be computed in
// Value()
gch::small_vector<int> m_nonlinear_rows;
};
} // namespace slp

158
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/jacobian.hpp vendored Normal file
View File

@@ -0,0 +1,158 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <utility>
#include <Eigen/SparseCore>
#include <gch/small_vector.hpp>
#include "sleipnir/autodiff/adjoint_expression_graph.hpp"
#include "sleipnir/autodiff/variable.hpp"
#include "sleipnir/autodiff/variable_matrix.hpp"
#include "sleipnir/util/concepts.hpp"
#include "sleipnir/util/symbol_exports.hpp"
namespace slp {
/**
* This class calculates the Jacobian of a vector of variables with respect to a
* vector of variables.
*
* The Jacobian is only recomputed if the variable expression is quadratic or
* higher order.
*/
class SLEIPNIR_DLLEXPORT Jacobian {
public:
/**
* Constructs a Jacobian object.
*
* @param variable Variable of which to compute the Jacobian.
* @param wrt Variable with respect to which to compute the Jacobian.
*/
Jacobian(Variable variable, Variable wrt) noexcept
: Jacobian{VariableMatrix{std::move(variable)},
VariableMatrix{std::move(wrt)}} {}
/**
* Constructs a Jacobian object.
*
* @param variables Vector of variables of which to compute the Jacobian.
* @param wrt Vector of variables with respect to which to compute the
* Jacobian.
*/
Jacobian(VariableMatrix variables, SleipnirMatrixLike auto wrt) noexcept
: m_variables{std::move(variables)}, m_wrt{std::move(wrt)} {
// Initialize column each expression's adjoint occupies in the Jacobian
for (size_t col = 0; col < m_wrt.size(); ++col) {
m_wrt[col].expr->col = col;
}
for (auto& variable : m_variables) {
m_graphs.emplace_back(variable);
}
// Reset col to -1
for (auto& node : m_wrt) {
node.expr->col = -1;
}
for (int row = 0; row < m_variables.rows(); ++row) {
if (m_variables[row].expr == nullptr) {
continue;
}
if (m_variables[row].type() == ExpressionType::LINEAR) {
// 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_adjoint_triplets(m_cached_triplets, row, m_wrt);
} 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().
m_nonlinear_rows.emplace_back(row);
}
}
if (m_nonlinear_rows.empty()) {
m_J.setFromTriplets(m_cached_triplets.begin(), m_cached_triplets.end());
}
}
/**
* Returns the Jacobian as a VariableMatrix.
*
* This is useful when constructing optimization problems with derivatives in
* them.
*
* @return The Jacobian as a VariableMatrix.
*/
VariableMatrix get() const {
VariableMatrix result{VariableMatrix::empty, m_variables.rows(),
m_wrt.rows()};
for (int row = 0; row < m_variables.rows(); ++row) {
auto grad = m_graphs[row].generate_gradient_tree(m_wrt);
for (int col = 0; col < m_wrt.rows(); ++col) {
if (grad[col].expr != nullptr) {
result(row, col) = std::move(grad[col]);
} else {
result(row, col) = Variable{0.0};
}
}
}
return result;
}
/**
* Evaluates the Jacobian at wrt's value.
*
* @return The Jacobian at wrt's value.
*/
const Eigen::SparseMatrix<double>& value() {
if (m_nonlinear_rows.empty()) {
return m_J;
}
for (auto& graph : m_graphs) {
graph.update_values();
}
// Copy the cached triplets so triplets added for the nonlinear rows are
// thrown away at the end of the function
auto triplets = m_cached_triplets;
// Compute each nonlinear row of the Jacobian
for (int row : m_nonlinear_rows) {
m_graphs[row].append_adjoint_triplets(triplets, row, m_wrt);
}
if (!triplets.empty()) {
m_J.setFromTriplets(triplets.begin(), triplets.end());
} else {
// setFromTriplets() is a no-op on empty triplets, so explicitly zero out
// the storage
m_J.setZero();
}
return m_J;
}
private:
VariableMatrix m_variables;
VariableMatrix m_wrt;
gch::small_vector<detail::AdjointExpressionGraph> m_graphs;
Eigen::SparseMatrix<double> m_J{m_variables.rows(), m_wrt.rows()};
// Cached triplets for gradients of linear rows
gch::small_vector<Eigen::Triplet<double>> m_cached_triplets;
// List of row indices for nonlinear rows whose graients will be computed in
// Value()
gch::small_vector<int> m_nonlinear_rows;
};
} // namespace slp

46
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Slice.hpp → wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/slice.hpp vendored
View File

@@ -6,18 +6,28 @@
#include <limits>
#include <utility>
#include "sleipnir/util/Assert.hpp"
#include "sleipnir/util/SymbolExports.hpp"
#include "sleipnir/util/assert.hpp"
#include "sleipnir/util/symbol_exports.hpp"
namespace sleipnir {
namespace slp {
namespace slicing {
/**
* Type tag used to designate an omitted argument of the slice.
*/
struct none_t {};
/**
* Designates an omitted argument of the slice.
*/
static inline constexpr none_t _;
} // namespace slicing
/**
* Represents a sequence of elements in an iterable object.
*/
class SLEIPNIR_DLLEXPORT Slice {
public:
/// Start index (inclusive).
@@ -36,14 +46,20 @@ class SLEIPNIR_DLLEXPORT Slice {
/**
* Constructs a slice.
*
* @param stop Slice stop index (exclusive).
*/
template <typename Stop>
requires std::same_as<Stop, slicing::none_t> ||
std::convertible_to<Stop, int>
constexpr Slice(Stop stop) // NOLINT
: Slice(slicing::_, std::move(stop), 1) {}
constexpr Slice(slicing::none_t) // NOLINT
: Slice(0, std::numeric_limits<int>::max(), 1) {}
/**
* Constructs a slice.
*
* @param start Slice start index (inclusive).
*/
constexpr Slice(int start) { // NOLINT
this->start = start;
this->stop = (start == -1) ? std::numeric_limits<int>::max() : start + 1;
this->step = 1;
}
/**
* Constructs a slice.
@@ -77,7 +93,7 @@ class SLEIPNIR_DLLEXPORT Slice {
if constexpr (std::same_as<Step, slicing::none_t>) {
this->step = 1;
} else {
Assert(step != 0);
slp_assert(step != 0);
this->step = step;
}
@@ -115,9 +131,9 @@ class SLEIPNIR_DLLEXPORT Slice {
* @param length The sequence length.
* @return The slice length.
*/
constexpr int Adjust(int length) {
Assert(step != 0);
Assert(step >= -std::numeric_limits<int>::max());
constexpr int adjust(int length) {
slp_assert(step != 0);
slp_assert(step >= -std::numeric_limits<int>::max());
if (start < 0) {
start += length;
@@ -155,4 +171,4 @@ class SLEIPNIR_DLLEXPORT Slice {
}
};
} // namespace sleipnir
} // namespace slp

324
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Variable.hpp → wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable.hpp vendored
View File

@@ -5,23 +5,32 @@
#include <algorithm>
#include <concepts>
#include <initializer_list>
#include <source_location>
#include <type_traits>
#include <utility>
#include <vector>
#include <Eigen/Core>
#include <wpi/SmallVector.h>
#include <gch/small_vector.hpp>
#include "sleipnir/autodiff/Expression.hpp"
#include "sleipnir/autodiff/ExpressionGraph.hpp"
#include "sleipnir/util/Assert.hpp"
#include "sleipnir/util/Concepts.hpp"
#include "sleipnir/util/Print.hpp"
#include "sleipnir/util/SymbolExports.hpp"
#include "sleipnir/autodiff/expression.hpp"
#include "sleipnir/autodiff/expression_graph.hpp"
#include "sleipnir/util/assert.hpp"
#include "sleipnir/util/concepts.hpp"
#include "sleipnir/util/symbol_exports.hpp"
namespace sleipnir {
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
#include "sleipnir/util/print.hpp"
#endif
namespace slp {
// Forward declarations for friend declarations in Variable
namespace detail {
class AdjointExpressionGraph;
} // namespace detail
template <int UpLo = Eigen::Lower | Eigen::Upper>
requires(UpLo == Eigen::Lower) || (UpLo == (Eigen::Lower | Eigen::Upper))
class SLEIPNIR_DLLEXPORT Hessian;
class SLEIPNIR_DLLEXPORT Jacobian;
@@ -36,11 +45,25 @@ class SLEIPNIR_DLLEXPORT Variable {
Variable() = default;
/**
* Constructs a Variable from a double.
* Constructs an empty Variable.
*/
explicit Variable(std::nullptr_t) : expr{nullptr} {}
/**
* Constructs a Variable from a floating point type.
*
* @param value The value of the Variable.
*/
Variable(double value) : expr{detail::MakeExpressionPtr(value)} {} // NOLINT
Variable(std::floating_point auto value) // NOLINT
: expr{detail::make_expression_ptr<detail::ConstExpression>(value)} {}
/**
* Constructs a Variable from an integral type.
*
* @param value The value of the Variable.
*/
Variable(std::integral auto value) // NOLINT
: expr{detail::make_expression_ptr<detail::ConstExpression>(value)} {}
/**
* Constructs a Variable pointing to the specified expression.
@@ -60,9 +83,10 @@ class SLEIPNIR_DLLEXPORT Variable {
* Assignment operator for double.
*
* @param value The value of the Variable.
* @return This variable.
*/
Variable& operator=(double value) {
expr = detail::MakeExpressionPtr(value);
expr = detail::make_expression_ptr<detail::ConstExpression>(value);
return *this;
}
@@ -72,19 +96,22 @@ class SLEIPNIR_DLLEXPORT Variable {
*
* @param value The value of the Variable.
*/
void SetValue(double value) {
if (expr->IsConstant(0.0)) {
expr = detail::MakeExpressionPtr(value);
void set_value(double value) {
if (expr->is_constant(0.0)) {
expr = detail::make_expression_ptr<detail::ConstExpression>(value);
} else {
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
// We only need to check the first argument since unary and binary
// operators both use it
if (expr->args[0] != nullptr && !expr->args[0]->IsConstant(0.0)) {
sleipnir::println(
if (expr->args[0] != nullptr) {
auto location = std::source_location::current();
slp::println(
stderr,
"WARNING: {}:{}: Modified the value of a dependent variable",
__FILE__, __LINE__);
"WARNING: {}:{}: {}: Modified the value of a dependent variable",
location.file_name(), location.line(), location.function_name());
}
expr->value = value;
#endif
expr->val = value;
}
}
@@ -93,6 +120,7 @@ class SLEIPNIR_DLLEXPORT Variable {
*
* @param lhs Operator left-hand side.
* @param rhs Operator right-hand side.
* @return Result of multiplication.
*/
friend SLEIPNIR_DLLEXPORT Variable operator*(const Variable& lhs,
const Variable& rhs) {
@@ -103,6 +131,7 @@ class SLEIPNIR_DLLEXPORT Variable {
* Variable-Variable compound multiplication operator.
*
* @param rhs Operator right-hand side.
* @return Result of multiplication.
*/
Variable& operator*=(const Variable& rhs) {
*this = *this * rhs;
@@ -114,6 +143,7 @@ class SLEIPNIR_DLLEXPORT Variable {
*
* @param lhs Operator left-hand side.
* @param rhs Operator right-hand side.
* @return Result of division.
*/
friend SLEIPNIR_DLLEXPORT Variable operator/(const Variable& lhs,
const Variable& rhs) {
@@ -124,6 +154,7 @@ class SLEIPNIR_DLLEXPORT Variable {
* Variable-Variable compound division operator.
*
* @param rhs Operator right-hand side.
* @return Result of division.
*/
Variable& operator/=(const Variable& rhs) {
*this = *this / rhs;
@@ -135,6 +166,7 @@ class SLEIPNIR_DLLEXPORT Variable {
*
* @param lhs Operator left-hand side.
* @param rhs Operator right-hand side.
* @return Result of addition.
*/
friend SLEIPNIR_DLLEXPORT Variable operator+(const Variable& lhs,
const Variable& rhs) {
@@ -145,6 +177,7 @@ class SLEIPNIR_DLLEXPORT Variable {
* Variable-Variable compound addition operator.
*
* @param rhs Operator right-hand side.
* @return Result of addition.
*/
Variable& operator+=(const Variable& rhs) {
*this = *this + rhs;
@@ -156,6 +189,7 @@ class SLEIPNIR_DLLEXPORT Variable {
*
* @param lhs Operator left-hand side.
* @param rhs Operator right-hand side.
* @return Result of subtraction.
*/
friend SLEIPNIR_DLLEXPORT Variable operator-(const Variable& lhs,
const Variable& rhs) {
@@ -166,6 +200,7 @@ class SLEIPNIR_DLLEXPORT Variable {
* Variable-Variable compound subtraction operator.
*
* @param rhs Operator right-hand side.
* @return Result of subtraction.
*/
Variable& operator-=(const Variable& rhs) {
*this = *this - rhs;
@@ -192,25 +227,38 @@ class SLEIPNIR_DLLEXPORT Variable {
/**
* Returns the value of this variable.
*
* @return The value of this variable.
*/
double Value() {
// Updates the value of this variable based on the values of its dependent
// variables
detail::ExpressionGraph{expr}.Update();
double value() {
if (!m_graph_initialized) {
m_graph = detail::topological_sort(expr);
m_graph_initialized = true;
}
detail::update_values(m_graph);
return expr->value;
return expr->val;
}
/**
* Returns the type of this expression (constant, linear, quadratic, or
* nonlinear).
*
* @return The type of this expression.
*/
ExpressionType Type() const { return expr->type; }
ExpressionType type() const { return expr->type(); }
private:
/// The expression node.
/// The expression node
detail::ExpressionPtr expr =
detail::MakeExpressionPtr(0.0, ExpressionType::kLinear);
detail::make_expression_ptr<detail::DecisionVariableExpression>();
/// Used to update the value of this variable based on the values of its
/// dependent variables
gch::small_vector<detail::Expression*> m_graph;
/// Used for lazy initialization of m_graph
bool m_graph_initialized = false;
friend SLEIPNIR_DLLEXPORT Variable abs(const Variable& x);
friend SLEIPNIR_DLLEXPORT Variable acos(const Variable& x);
@@ -237,6 +285,9 @@ class SLEIPNIR_DLLEXPORT Variable {
friend SLEIPNIR_DLLEXPORT Variable hypot(const Variable& x, const Variable& y,
const Variable& z);
friend class detail::AdjointExpressionGraph;
template <int UpLo>
requires(UpLo == Eigen::Lower) || (UpLo == (Eigen::Lower | Eigen::Upper))
friend class SLEIPNIR_DLLEXPORT Hessian;
friend class SLEIPNIR_DLLEXPORT Jacobian;
};
@@ -425,8 +476,7 @@ SLEIPNIR_DLLEXPORT inline Variable tanh(const Variable& x) {
*/
SLEIPNIR_DLLEXPORT inline Variable hypot(const Variable& x, const Variable& y,
const Variable& z) {
return Variable{sleipnir::sqrt(sleipnir::pow(x, 2) + sleipnir::pow(y, 2) +
sleipnir::pow(z, 2))};
return Variable{slp::sqrt(slp::pow(x, 2) + slp::pow(y, 2) + slp::pow(z, 2))};
}
/**
@@ -441,85 +491,38 @@ SLEIPNIR_DLLEXPORT inline Variable hypot(const Variable& x, const Variable& y,
* @param rhs Right-hand side.
*/
template <typename LHS, typename RHS>
requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
(ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
(!std::same_as<std::decay_t<LHS>, double> ||
!std::same_as<std::decay_t<RHS>, double>)
wpi::SmallVector<Variable> MakeConstraints(LHS&& lhs, RHS&& rhs) {
wpi::SmallVector<Variable> constraints;
requires(ScalarLike<LHS> || MatrixLike<LHS>) &&
(ScalarLike<RHS> || MatrixLike<RHS>) &&
(SleipnirType<LHS> || SleipnirType<RHS>)
gch::small_vector<Variable> make_constraints(LHS&& lhs, RHS&& rhs) {
gch::small_vector<Variable> constraints;
if constexpr (ScalarLike<std::decay_t<LHS>> &&
ScalarLike<std::decay_t<RHS>>) {
if constexpr (ScalarLike<LHS> && ScalarLike<RHS>) {
constraints.emplace_back(lhs - rhs);
} else if constexpr (ScalarLike<std::decay_t<LHS>> &&
MatrixLike<std::decay_t<RHS>>) {
int rows;
int cols;
if constexpr (EigenMatrixLike<std::decay_t<RHS>>) {
rows = rhs.rows();
cols = rhs.cols();
} else {
rows = rhs.Rows();
cols = rhs.Cols();
}
} else if constexpr (ScalarLike<LHS> && MatrixLike<RHS>) {
constraints.reserve(rhs.rows() * rhs.cols());
constraints.reserve(rows * cols);
for (int row = 0; row < rows; ++row) {
for (int col = 0; col < cols; ++col) {
for (int row = 0; row < rhs.rows(); ++row) {
for (int col = 0; col < rhs.cols(); ++col) {
// Make right-hand side zero
constraints.emplace_back(lhs - rhs(row, col));
}
}
} else if constexpr (MatrixLike<std::decay_t<LHS>> &&
ScalarLike<std::decay_t<RHS>>) {
int rows;
int cols;
if constexpr (EigenMatrixLike<std::decay_t<LHS>>) {
rows = lhs.rows();
cols = lhs.cols();
} else {
rows = lhs.Rows();
cols = lhs.Cols();
}
} else if constexpr (MatrixLike<LHS> && ScalarLike<RHS>) {
constraints.reserve(lhs.rows() * lhs.cols());
constraints.reserve(rows * cols);
for (int row = 0; row < rows; ++row) {
for (int col = 0; col < cols; ++col) {
for (int row = 0; row < lhs.rows(); ++row) {
for (int col = 0; col < lhs.cols(); ++col) {
// Make right-hand side zero
constraints.emplace_back(lhs(row, col) - rhs);
}
}
} else if constexpr (MatrixLike<std::decay_t<LHS>> &&
MatrixLike<std::decay_t<RHS>>) {
int lhsRows;
int lhsCols;
if constexpr (EigenMatrixLike<std::decay_t<LHS>>) {
lhsRows = lhs.rows();
lhsCols = lhs.cols();
} else {
lhsRows = lhs.Rows();
lhsCols = lhs.Cols();
}
} else if constexpr (MatrixLike<LHS> && MatrixLike<RHS>) {
slp_assert(lhs.rows() == rhs.rows() && lhs.cols() == rhs.cols());
constraints.reserve(lhs.rows() * lhs.cols());
[[maybe_unused]]
int rhsRows;
[[maybe_unused]]
int rhsCols;
if constexpr (EigenMatrixLike<std::decay_t<RHS>>) {
rhsRows = rhs.rows();
rhsCols = rhs.cols();
} else {
rhsRows = rhs.Rows();
rhsCols = rhs.Cols();
}
Assert(lhsRows == rhsRows && lhsCols == rhsCols);
constraints.reserve(lhsRows * lhsCols);
for (int row = 0; row < lhsRows; ++row) {
for (int col = 0; col < lhsCols; ++col) {
for (int row = 0; row < lhs.rows(); ++row) {
for (int col = 0; col < lhs.cols(); ++col) {
// Make right-hand side zero
constraints.emplace_back(lhs(row, col) - rhs(row, col));
}
@@ -534,16 +537,16 @@ wpi::SmallVector<Variable> MakeConstraints(LHS&& lhs, RHS&& rhs) {
*/
struct SLEIPNIR_DLLEXPORT EqualityConstraints {
/// A vector of scalar equality constraints.
wpi::SmallVector<Variable> constraints;
gch::small_vector<Variable> constraints;
/**
* Concatenates multiple equality constraints.
*
* @param equalityConstraints The list of EqualityConstraints to concatenate.
* @param equality_constraints The list of EqualityConstraints to concatenate.
*/
EqualityConstraints( // NOLINT
std::initializer_list<EqualityConstraints> equalityConstraints) {
for (const auto& elem : equalityConstraints) {
EqualityConstraints(
std::initializer_list<EqualityConstraints> equality_constraints) {
for (const auto& elem : equality_constraints) {
constraints.insert(constraints.end(), elem.constraints.begin(),
elem.constraints.end());
}
@@ -554,11 +557,11 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints {
*
* This overload is for Python bindings only.
*
* @param equalityConstraints The list of EqualityConstraints to concatenate.
* @param equality_constraints The list of EqualityConstraints to concatenate.
*/
explicit EqualityConstraints(
const std::vector<EqualityConstraints>& equalityConstraints) {
for (const auto& elem : equalityConstraints) {
const std::vector<EqualityConstraints>& equality_constraints) {
for (const auto& elem : equality_constraints) {
constraints.insert(constraints.end(), elem.constraints.begin(),
elem.constraints.end());
}
@@ -574,20 +577,19 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints {
* @param rhs Right-hand side.
*/
template <typename LHS, typename RHS>
requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
(ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
(!std::same_as<std::decay_t<LHS>, double> ||
!std::same_as<std::decay_t<RHS>, double>)
requires(ScalarLike<LHS> || MatrixLike<LHS>) &&
(ScalarLike<RHS> || MatrixLike<RHS>) &&
(SleipnirType<LHS> || SleipnirType<RHS>)
EqualityConstraints(LHS&& lhs, RHS&& rhs)
: constraints{MakeConstraints(lhs, rhs)} {}
: constraints{make_constraints(lhs, rhs)} {}
/**
* Implicit conversion operator to bool.
*/
operator bool() { // NOLINT
return std::all_of(
constraints.begin(), constraints.end(),
[](auto& constraint) { return constraint.Value() == 0.0; });
return std::ranges::all_of(constraints, [](auto& constraint) {
return constraint.value() == 0.0;
});
}
};
@@ -596,17 +598,17 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints {
*/
struct SLEIPNIR_DLLEXPORT InequalityConstraints {
/// A vector of scalar inequality constraints.
wpi::SmallVector<Variable> constraints;
gch::small_vector<Variable> constraints;
/**
* Concatenates multiple inequality constraints.
*
* @param inequalityConstraints The list of InequalityConstraints to
* @param inequality_constraints The list of InequalityConstraints to
* concatenate.
*/
InequalityConstraints( // NOLINT
std::initializer_list<InequalityConstraints> inequalityConstraints) {
for (const auto& elem : inequalityConstraints) {
std::initializer_list<InequalityConstraints> inequality_constraints) {
for (const auto& elem : inequality_constraints) {
constraints.insert(constraints.end(), elem.constraints.begin(),
elem.constraints.end());
}
@@ -617,12 +619,12 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints {
*
* This overload is for Python bindings only.
*
* @param inequalityConstraints The list of InequalityConstraints to
* @param inequality_constraints The list of InequalityConstraints to
* concatenate.
*/
explicit InequalityConstraints(
const std::vector<InequalityConstraints>& inequalityConstraints) {
for (const auto& elem : inequalityConstraints) {
const std::vector<InequalityConstraints>& inequality_constraints) {
for (const auto& elem : inequality_constraints) {
constraints.insert(constraints.end(), elem.constraints.begin(),
elem.constraints.end());
}
@@ -638,20 +640,19 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints {
* @param rhs Right-hand side.
*/
template <typename LHS, typename RHS>
requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
(ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
(!std::same_as<std::decay_t<LHS>, double> ||
!std::same_as<std::decay_t<RHS>, double>)
requires(ScalarLike<LHS> || MatrixLike<LHS>) &&
(ScalarLike<RHS> || MatrixLike<RHS>) &&
(SleipnirType<LHS> || SleipnirType<RHS>)
InequalityConstraints(LHS&& lhs, RHS&& rhs)
: constraints{MakeConstraints(lhs, rhs)} {}
: constraints{make_constraints(lhs, rhs)} {}
/**
* Implicit conversion operator to bool.
*/
operator bool() { // NOLINT
return std::all_of(
constraints.begin(), constraints.end(),
[](auto& constraint) { return constraint.Value() >= 0.0; });
return std::ranges::all_of(constraints, [](auto& constraint) {
return constraint.value() >= 0.0;
});
}
};
@@ -662,10 +663,9 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints {
* @param rhs Left-hand side.
*/
template <typename LHS, typename RHS>
requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
(ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
(!std::same_as<std::decay_t<LHS>, double> ||
!std::same_as<std::decay_t<RHS>, double>)
requires(ScalarLike<LHS> || MatrixLike<LHS>) &&
(ScalarLike<RHS> || MatrixLike<RHS>) &&
(SleipnirType<LHS> || SleipnirType<RHS>)
EqualityConstraints operator==(LHS&& lhs, RHS&& rhs) {
return EqualityConstraints{lhs, rhs};
}
@@ -678,10 +678,9 @@ EqualityConstraints operator==(LHS&& lhs, RHS&& rhs) {
* @param rhs Left-hand side.
*/
template <typename LHS, typename RHS>
requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
(ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
(!std::same_as<std::decay_t<LHS>, double> ||
!std::same_as<std::decay_t<RHS>, double>)
requires(ScalarLike<LHS> || MatrixLike<LHS>) &&
(ScalarLike<RHS> || MatrixLike<RHS>) &&
(SleipnirType<LHS> || SleipnirType<RHS>)
InequalityConstraints operator<(LHS&& lhs, RHS&& rhs) {
return rhs >= lhs;
}
@@ -694,10 +693,9 @@ InequalityConstraints operator<(LHS&& lhs, RHS&& rhs) {
* @param rhs Left-hand side.
*/
template <typename LHS, typename RHS>
requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
(ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
(!std::same_as<std::decay_t<LHS>, double> ||
!std::same_as<std::decay_t<RHS>, double>)
requires(ScalarLike<LHS> || MatrixLike<LHS>) &&
(ScalarLike<RHS> || MatrixLike<RHS>) &&
(SleipnirType<LHS> || SleipnirType<RHS>)
InequalityConstraints operator<=(LHS&& lhs, RHS&& rhs) {
return rhs >= lhs;
}
@@ -710,10 +708,9 @@ InequalityConstraints operator<=(LHS&& lhs, RHS&& rhs) {
* @param rhs Left-hand side.
*/
template <typename LHS, typename RHS>
requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
(ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
(!std::same_as<std::decay_t<LHS>, double> ||
!std::same_as<std::decay_t<RHS>, double>)
requires(ScalarLike<LHS> || MatrixLike<LHS>) &&
(ScalarLike<RHS> || MatrixLike<RHS>) &&
(SleipnirType<LHS> || SleipnirType<RHS>)
InequalityConstraints operator>(LHS&& lhs, RHS&& rhs) {
return lhs >= rhs;
}
@@ -726,15 +723,14 @@ InequalityConstraints operator>(LHS&& lhs, RHS&& rhs) {
* @param rhs Left-hand side.
*/
template <typename LHS, typename RHS>
requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
(ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
(!std::same_as<std::decay_t<LHS>, double> ||
!std::same_as<std::decay_t<RHS>, double>)
requires(ScalarLike<LHS> || MatrixLike<LHS>) &&
(ScalarLike<RHS> || MatrixLike<RHS>) &&
(SleipnirType<LHS> || SleipnirType<RHS>)
InequalityConstraints operator>=(LHS&& lhs, RHS&& rhs) {
return InequalityConstraints{lhs, rhs};
}
} // namespace sleipnir
} // namespace slp
namespace Eigen {
@@ -742,20 +738,28 @@ namespace Eigen {
* NumTraits specialization that allows instantiating Eigen types with Variable.
*/
template <>
struct NumTraits<sleipnir::Variable> : NumTraits<double> {
using Real = sleipnir::Variable;
using NonInteger = sleipnir::Variable;
using Nested = sleipnir::Variable;
struct NumTraits<slp::Variable> : NumTraits<double> {
/// Real type.
using Real = slp::Variable;
/// Non-integer type.
using NonInteger = slp::Variable;
/// Nested type.
using Nested = slp::Variable;
enum {
IsComplex = 0,
IsInteger = 0,
IsSigned = 1,
RequireInitialization = 1,
ReadCost = 1,
AddCost = 3,
MulCost = 3
};
/// Is complex.
static constexpr int IsComplex = 0;
/// Is integer.
static constexpr int IsInteger = 0;
/// Is signed.
static constexpr int IsSigned = 1;
/// Require initialization.
static constexpr int RequireInitialization = 1;
/// Read cost.
static constexpr int ReadCost = 1;
/// Add cost.
static constexpr int AddCost = 3;
/// Multiply cost.
static constexpr int MulCost = 3;
};
} // namespace Eigen

872
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_block.hpp vendored Normal file
View File

@@ -0,0 +1,872 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <concepts>
#include <type_traits>
#include <utility>
#include <Eigen/Core>
#include "sleipnir/autodiff/slice.hpp"
#include "sleipnir/autodiff/variable.hpp"
#include "sleipnir/util/assert.hpp"
#include "sleipnir/util/function_ref.hpp"
namespace slp {
/**
* A submatrix of autodiff variables with reference semantics.
*
* @tparam Mat The type of the matrix whose storage this class points to.
*/
template <typename Mat>
class VariableBlock {
public:
/**
* Copy constructor.
*/
VariableBlock(const VariableBlock<Mat>&) = default;
/**
* Assigns a VariableBlock to the block.
*
* @param values VariableBlock of values.
* @return This VariableBlock.
*/
VariableBlock<Mat>& operator=(const VariableBlock<Mat>& values) {
if (this == &values) {
return *this;
}
if (m_mat == nullptr) {
m_mat = values.m_mat;
m_row_slice = values.m_row_slice;
m_row_slice_length = values.m_row_slice_length;
m_col_slice = values.m_col_slice;
m_col_slice_length = values.m_col_slice_length;
} else {
slp_assert(rows() == values.rows() && cols() == values.cols());
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
(*this)(row, col) = values(row, col);
}
}
}
return *this;
}
/**
* Move constructor.
*/
VariableBlock(VariableBlock<Mat>&&) = default;
/**
* Assigns a VariableBlock to the block.
*
* @param values VariableBlock of values.
* @return This VariableBlock.
*/
VariableBlock<Mat>& operator=(VariableBlock<Mat>&& values) {
if (this == &values) {
return *this;
}
if (m_mat == nullptr) {
m_mat = values.m_mat;
m_row_slice = values.m_row_slice;
m_row_slice_length = values.m_row_slice_length;
m_col_slice = values.m_col_slice;
m_col_slice_length = values.m_col_slice_length;
} else {
slp_assert(rows() == values.rows() && cols() == values.cols());
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
(*this)(row, col) = values(row, col);
}
}
}
return *this;
}
/**
* Constructs a Variable block pointing to all of the given matrix.
*
* @param mat The matrix to which to point.
*/
VariableBlock(Mat& mat) // NOLINT
: m_mat{&mat},
m_row_slice{0, mat.rows(), 1},
m_row_slice_length{m_row_slice.adjust(mat.rows())},
m_col_slice{0, mat.cols(), 1},
m_col_slice_length{m_col_slice.adjust(mat.cols())} {}
/**
* Constructs a Variable block pointing to a subset of the given matrix.
*
* @param mat The matrix to which to point.
* @param row_offset The block's row offset.
* @param col_offset The block's column offset.
* @param block_rows The number of rows in the block.
* @param block_cols The number of columns in the block.
*/
VariableBlock(Mat& mat, int row_offset, int col_offset, int block_rows,
int block_cols)
: m_mat{&mat},
m_row_slice{row_offset, row_offset + block_rows, 1},
m_row_slice_length{m_row_slice.adjust(mat.rows())},
m_col_slice{col_offset, col_offset + block_cols, 1},
m_col_slice_length{m_col_slice.adjust(mat.cols())} {}
/**
* Constructs a Variable block pointing to a subset of the given matrix.
*
* Note that the slices are taken as is rather than adjusted.
*
* @param mat The matrix to which to point.
* @param row_slice The block's row slice.
* @param row_slice_length The block's row length.
* @param col_slice The block's column slice.
* @param col_slice_length The block's column length.
*/
VariableBlock(Mat& mat, Slice row_slice, int row_slice_length,
Slice col_slice, int col_slice_length)
: m_mat{&mat},
m_row_slice{std::move(row_slice)},
m_row_slice_length{row_slice_length},
m_col_slice{std::move(col_slice)},
m_col_slice_length{col_slice_length} {}
/**
* Assigns a double to the block.
*
* This only works for blocks with one row and one column.
*
* @param value Value to assign.
* @return This VariableBlock.
*/
VariableBlock<Mat>& operator=(ScalarLike auto value) {
slp_assert(rows() == 1 && cols() == 1);
(*this)(0, 0) = value;
return *this;
}
/**
* Assigns a double to the block.
*
* This only works for blocks with one row and one column.
*
* @param value Value to assign.
*/
void set_value(double value) {
slp_assert(rows() == 1 && cols() == 1);
(*this)(0, 0).set_value(value);
}
/**
* Assigns an Eigen matrix to the block.
*
* @param values Eigen matrix of values to assign.
* @return This VariableBlock.
*/
template <typename Derived>
VariableBlock<Mat>& operator=(const Eigen::MatrixBase<Derived>& values) {
slp_assert(rows() == values.rows() && cols() == values.cols());
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
(*this)(row, col) = values(row, col);
}
}
return *this;
}
/**
* Sets block's internal values.
*
* @param values Eigen matrix of values.
*/
template <typename Derived>
requires std::same_as<typename Derived::Scalar, double>
void set_value(const Eigen::MatrixBase<Derived>& values) {
slp_assert(rows() == values.rows() && cols() == values.cols());
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
(*this)(row, col).set_value(values(row, col));
}
}
}
/**
* Assigns a VariableMatrix to the block.
*
* @param values VariableMatrix of values.
* @return This VariableBlock.
*/
VariableBlock<Mat>& operator=(const Mat& values) {
slp_assert(rows() == values.rows() && cols() == values.cols());
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
(*this)(row, col) = values(row, col);
}
}
return *this;
}
/**
* Assigns a VariableMatrix to the block.
*
* @param values VariableMatrix of values.
* @return This VariableBlock.
*/
VariableBlock<Mat>& operator=(Mat&& values) {
slp_assert(rows() == values.rows() && cols() == values.cols());
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
(*this)(row, col) = std::move(values(row, col));
}
}
return *this;
}
/**
* Returns a scalar subblock at the given row and column.
*
* @param row The scalar subblock's row.
* @param col The scalar subblock's column.
* @return A scalar subblock at the given row and column.
*/
Variable& operator()(int row, int col)
requires(!std::is_const_v<Mat>)
{
slp_assert(row >= 0 && row < rows());
slp_assert(col >= 0 && col < cols());
return (*m_mat)(m_row_slice.start + row * m_row_slice.step,
m_col_slice.start + col * m_col_slice.step);
}
/**
* Returns a scalar subblock at the given row and column.
*
* @param row The scalar subblock's row.
* @param col The scalar subblock's column.
* @return A scalar subblock at the given row and column.
*/
const Variable& operator()(int row, int col) const {
slp_assert(row >= 0 && row < rows());
slp_assert(col >= 0 && col < cols());
return (*m_mat)(m_row_slice.start + row * m_row_slice.step,
m_col_slice.start + col * m_col_slice.step);
}
/**
* Returns a scalar subblock at the given row.
*
* @param row The scalar subblock's row.
* @return A scalar subblock at the given row.
*/
Variable& operator[](int row)
requires(!std::is_const_v<Mat>)
{
slp_assert(row >= 0 && row < rows() * cols());
return (*this)(row / cols(), row % cols());
}
/**
* Returns a scalar subblock at the given row.
*
* @param row The scalar subblock's row.
* @return A scalar subblock at the given row.
*/
const Variable& operator[](int row) const {
slp_assert(row >= 0 && row < rows() * cols());
return (*this)(row / cols(), row % cols());
}
/**
* Returns a block of the variable matrix.
*
* @param row_offset The row offset of the block selection.
* @param col_offset The column offset of the block selection.
* @param block_rows The number of rows in the block selection.
* @param block_cols The number of columns in the block selection.
* @return A block of the variable matrix.
*/
VariableBlock<Mat> block(int row_offset, int col_offset, int block_rows,
int block_cols) {
slp_assert(row_offset >= 0 && row_offset <= rows());
slp_assert(col_offset >= 0 && col_offset <= cols());
slp_assert(block_rows >= 0 && block_rows <= rows() - row_offset);
slp_assert(block_cols >= 0 && block_cols <= cols() - col_offset);
return (*this)({row_offset, row_offset + block_rows, 1},
{col_offset, col_offset + block_cols, 1});
}
/**
* Returns a block slice of the variable matrix.
*
* @param row_offset The row offset of the block selection.
* @param col_offset The column offset of the block selection.
* @param block_rows The number of rows in the block selection.
* @param block_cols The number of columns in the block selection.
* @return A block slice of the variable matrix.
*/
const VariableBlock<const Mat> block(int row_offset, int col_offset,
int block_rows, int block_cols) const {
slp_assert(row_offset >= 0 && row_offset <= rows());
slp_assert(col_offset >= 0 && col_offset <= cols());
slp_assert(block_rows >= 0 && block_rows <= rows() - row_offset);
slp_assert(block_cols >= 0 && block_cols <= cols() - col_offset);
return (*this)({row_offset, row_offset + block_rows, 1},
{col_offset, col_offset + block_cols, 1});
}
/**
* Returns a slice of the variable matrix.
*
* @param row_slice The row slice.
* @param col_slice The column slice.
* @return A slice of the variable matrix.
*/
VariableBlock<Mat> operator()(Slice row_slice, Slice col_slice) {
int row_slice_length = row_slice.adjust(m_row_slice_length);
int col_slice_length = col_slice.adjust(m_col_slice_length);
return VariableBlock{
*m_mat,
{m_row_slice.start + row_slice.start * m_row_slice.step,
m_row_slice.start + row_slice.stop, m_row_slice.step * row_slice.step},
row_slice_length,
{m_col_slice.start + col_slice.start * m_col_slice.step,
m_col_slice.start + col_slice.stop, m_col_slice.step * col_slice.step},
col_slice_length};
}
/**
* Returns a slice of the variable matrix.
*
* @param row_slice The row slice.
* @param col_slice The column slice.
* @return A slice of the variable matrix.
*/
const VariableBlock<const Mat> operator()(Slice row_slice,
Slice col_slice) const {
int row_slice_length = row_slice.adjust(m_row_slice_length);
int col_slice_length = col_slice.adjust(m_col_slice_length);
return VariableBlock{
*m_mat,
{m_row_slice.start + row_slice.start * m_row_slice.step,
m_row_slice.start + row_slice.stop, m_row_slice.step * row_slice.step},
row_slice_length,
{m_col_slice.start + col_slice.start * m_col_slice.step,
m_col_slice.start + col_slice.stop, m_col_slice.step * col_slice.step},
col_slice_length};
}
/**
* Returns a slice of the variable matrix.
*
* The given slices aren't adjusted. This overload is for Python bindings
* only.
*
* @param row_slice The row slice.
* @param row_slice_length The row slice length.
* @param col_slice The column slice.
* @param col_slice_length The column slice length.
* @return A slice of the variable matrix.
*/
VariableBlock<Mat> operator()(Slice row_slice, int row_slice_length,
Slice col_slice, int col_slice_length) {
return VariableBlock{
*m_mat,
{m_row_slice.start + row_slice.start * m_row_slice.step,
m_row_slice.start + row_slice.stop, m_row_slice.step * row_slice.step},
row_slice_length,
{m_col_slice.start + col_slice.start * m_col_slice.step,
m_col_slice.start + col_slice.stop, m_col_slice.step * col_slice.step},
col_slice_length};
}
/**
* Returns a slice of the variable matrix.
*
* The given slices aren't adjusted. This overload is for Python bindings
* only.
*
* @param row_slice The row slice.
* @param row_slice_length The row slice length.
* @param col_slice The column slice.
* @param col_slice_length The column slice length.
* @return A slice of the variable matrix.
*/
const VariableBlock<const Mat> operator()(Slice row_slice,
int row_slice_length,
Slice col_slice,
int col_slice_length) const {
return VariableBlock{
*m_mat,
{m_row_slice.start + row_slice.start * m_row_slice.step,
m_row_slice.start + row_slice.stop, m_row_slice.step * row_slice.step},
row_slice_length,
{m_col_slice.start + col_slice.start * m_col_slice.step,
m_col_slice.start + col_slice.stop, m_col_slice.step * col_slice.step},
col_slice_length};
}
/**
* Returns a segment of the variable vector.
*
* @param offset The offset of the segment.
* @param length The length of the segment.
* @return A segment of the variable vector.
*/
VariableBlock<Mat> segment(int offset, int length) {
slp_assert(offset >= 0 && offset < rows() * cols());
slp_assert(length >= 0 && length <= rows() * cols() - offset);
return block(offset, 0, length, 1);
}
/**
* Returns a segment of the variable vector.
*
* @param offset The offset of the segment.
* @param length The length of the segment.
* @return A segment of the variable vector.
*/
const VariableBlock<Mat> segment(int offset, int length) const {
slp_assert(offset >= 0 && offset < rows() * cols());
slp_assert(length >= 0 && length <= rows() * cols() - offset);
return block(offset, 0, length, 1);
}
/**
* Returns a row slice of the variable matrix.
*
* @param row The row to slice.
* @return A row slice of the variable matrix.
*/
VariableBlock<Mat> row(int row) {
slp_assert(row >= 0 && row < rows());
return block(row, 0, 1, cols());
}
/**
* Returns a row slice of the variable matrix.
*
* @param row The row to slice.
* @return A row slice of the variable matrix.
*/
VariableBlock<const Mat> row(int row) const {
slp_assert(row >= 0 && row < rows());
return block(row, 0, 1, cols());
}
/**
* Returns a column slice of the variable matrix.
*
* @param col The column to slice.
* @return A column slice of the variable matrix.
*/
VariableBlock<Mat> col(int col) {
slp_assert(col >= 0 && col < cols());
return block(0, col, rows(), 1);
}
/**
* Returns a column slice of the variable matrix.
*
* @param col The column to slice.
* @return A column slice of the variable matrix.
*/
VariableBlock<const Mat> col(int col) const {
slp_assert(col >= 0 && col < cols());
return block(0, col, rows(), 1);
}
/**
* Compound matrix multiplication-assignment operator.
*
* @param rhs Variable to multiply.
* @return Result of multiplication.
*/
VariableBlock<Mat>& operator*=(const MatrixLike auto& rhs) {
slp_assert(cols() == rhs.rows() && cols() == rhs.cols());
for (int i = 0; i < rows(); ++i) {
for (int j = 0; j < rhs.cols(); ++j) {
Variable sum;
for (int k = 0; k < cols(); ++k) {
sum += (*this)(i, k) * rhs(k, j);
}
(*this)(i, j) = sum;
}
}
return *this;
}
/**
* Compound matrix multiplication-assignment operator.
*
* @param rhs Variable to multiply.
* @return Result of multiplication.
*/
VariableBlock<Mat>& operator*=(const ScalarLike auto& rhs) {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
(*this)(row, col) *= rhs;
}
}
return *this;
}
/**
* Compound matrix division-assignment operator.
*
* @param rhs Variable to divide.
* @return Result of division.
*/
VariableBlock<Mat>& operator/=(const MatrixLike auto& rhs) {
slp_assert(rhs.rows() == 1 && rhs.cols() == 1);
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
(*this)(row, col) /= rhs(0, 0);
}
}
return *this;
}
/**
* Compound matrix division-assignment operator.
*
* @param rhs Variable to divide.
* @return Result of division.
*/
VariableBlock<Mat>& operator/=(const ScalarLike auto& rhs) {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
(*this)(row, col) /= rhs;
}
}
return *this;
}
/**
* Compound addition-assignment operator.
*
* @param rhs Variable to add.
* @return Result of addition.
*/
VariableBlock<Mat>& operator+=(const MatrixLike auto& rhs) {
slp_assert(rows() == rhs.rows() && cols() == rhs.cols());
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
(*this)(row, col) += rhs(row, col);
}
}
return *this;
}
/**
* Compound addition-assignment operator.
*
* @param rhs Variable to add.
* @return Result of addition.
*/
VariableBlock<Mat>& operator+=(const ScalarLike auto& rhs) {
slp_assert(rows() == 1 && cols() == 1);
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
(*this)(row, col) += rhs;
}
}
return *this;
}
/**
* Compound subtraction-assignment operator.
*
* @param rhs Variable to subtract.
* @return Result of subtraction.
*/
VariableBlock<Mat>& operator-=(const MatrixLike auto& rhs) {
slp_assert(rows() == rhs.rows() && cols() == rhs.cols());
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
(*this)(row, col) -= rhs(row, col);
}
}
return *this;
}
/**
* Compound subtraction-assignment operator.
*
* @param rhs Variable to subtract.
* @return Result of subtraction.
*/
VariableBlock<Mat>& operator-=(const ScalarLike auto& rhs) {
slp_assert(rows() == 1 && cols() == 1);
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
(*this)(row, col) -= rhs;
}
}
return *this;
}
/**
* Implicit conversion operator from 1x1 VariableBlock to Variable.
*/
operator Variable() const { // NOLINT
slp_assert(rows() == 1 && cols() == 1);
return (*this)(0, 0);
}
/**
* Returns the transpose of the variable matrix.
*
* @return The transpose of the variable matrix.
*/
std::remove_cv_t<Mat> T() const {
std::remove_cv_t<Mat> result{Mat::empty, cols(), rows()};
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
result(col, row) = (*this)(row, col);
}
}
return result;
}
/**
* Returns the number of rows in the matrix.
*
* @return The number of rows in the matrix.
*/
int rows() const { return m_row_slice_length; }
/**
* Returns the number of columns in the matrix.
*
* @return The number of columns in the matrix.
*/
int cols() const { return m_col_slice_length; }
/**
* Returns an element of the variable matrix.
*
* @param row The row of the element to return.
* @param col The column of the element to return.
* @return An element of the variable matrix.
*/
double value(int row, int col) {
slp_assert(row >= 0 && row < rows());
slp_assert(col >= 0 && col < cols());
return (*m_mat)(m_row_slice.start + row * m_row_slice.step,
m_col_slice.start + col * m_col_slice.step)
.value();
}
/**
* Returns a row of the variable column vector.
*
* @param index The index of the element to return.
* @return A row of the variable column vector.
*/
double value(int index) {
slp_assert(index >= 0 && index < rows() * cols());
return value(index / cols(), index % cols());
}
/**
* Returns the contents of the variable matrix.
*
* @return The contents of the variable matrix.
*/
Eigen::MatrixXd value() {
Eigen::MatrixXd result{rows(), cols()};
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
result(row, col) = value(row, col);
}
}
return result;
}
/**
* Transforms the matrix coefficient-wise with an unary operator.
*
* @param unary_op The unary operator to use for the transform operation.
* @return Result of the unary operator.
*/
std::remove_cv_t<Mat> cwise_transform(
function_ref<Variable(const Variable& x)> unary_op) const {
std::remove_cv_t<Mat> result{Mat::empty, rows(), cols()};
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
result(row, col) = unary_op((*this)(row, col));
}
}
return result;
}
#ifndef DOXYGEN_SHOULD_SKIP_THIS
class iterator {
public:
using iterator_category = std::forward_iterator_tag;
using value_type = Variable;
using difference_type = std::ptrdiff_t;
using pointer = Variable*;
using reference = Variable&;
constexpr iterator() noexcept = default;
constexpr iterator(VariableBlock<Mat>* mat, int index) noexcept
: m_mat{mat}, m_index{index} {}
constexpr iterator& operator++() noexcept {
++m_index;
return *this;
}
constexpr iterator operator++(int) noexcept {
iterator retval = *this;
++(*this);
return retval;
}
constexpr bool operator==(const iterator&) const noexcept = default;
constexpr reference operator*() const noexcept { return (*m_mat)[m_index]; }
private:
VariableBlock<Mat>* m_mat = nullptr;
int m_index = 0;
};
class const_iterator {
public:
using iterator_category = std::forward_iterator_tag;
using value_type = Variable;
using difference_type = std::ptrdiff_t;
using pointer = Variable*;
using const_reference = const Variable&;
constexpr const_iterator() noexcept = default;
constexpr const_iterator(const VariableBlock<Mat>* mat, int index) noexcept
: m_mat{mat}, m_index{index} {}
constexpr const_iterator& operator++() noexcept {
++m_index;
return *this;
}
constexpr const_iterator operator++(int) noexcept {
const_iterator retval = *this;
++(*this);
return retval;
}
constexpr bool operator==(const const_iterator&) const noexcept = default;
constexpr const_reference operator*() const noexcept {
return (*m_mat)[m_index];
}
private:
const VariableBlock<Mat>* m_mat = nullptr;
int m_index = 0;
};
#endif // DOXYGEN_SHOULD_SKIP_THIS
/**
* Returns begin iterator.
*
* @return Begin iterator.
*/
iterator begin() { return iterator(this, 0); }
/**
* Returns end iterator.
*
* @return End iterator.
*/
iterator end() { return iterator(this, rows() * cols()); }
/**
* Returns begin iterator.
*
* @return Begin iterator.
*/
const_iterator begin() const { return const_iterator(this, 0); }
/**
* Returns end iterator.
*
* @return End iterator.
*/
const_iterator end() const { return const_iterator(this, rows() * cols()); }
/**
* Returns begin iterator.
*
* @return Begin iterator.
*/
const_iterator cbegin() const { return const_iterator(this, 0); }
/**
* Returns end iterator.
*
* @return End iterator.
*/
const_iterator cend() const { return const_iterator(this, rows() * cols()); }
/**
* Returns number of elements in matrix.
*
* @return Number of elements in matrix.
*/
size_t size() const { return rows() * cols(); }
private:
Mat* m_mat = nullptr;
Slice m_row_slice;
int m_row_slice_length = 0;
Slice m_col_slice;
int m_col_slice_length = 0;
};
} // namespace slp

1283
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_matrix.hpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff

342
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/OCPSolver.hpp → wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/ocp.hpp vendored
View File

@@ -7,14 +7,14 @@
#include <chrono>
#include <utility>
#include "sleipnir/autodiff/VariableMatrix.hpp"
#include "sleipnir/optimization/OptimizationProblem.hpp"
#include "sleipnir/util/Assert.hpp"
#include "sleipnir/util/Concepts.hpp"
#include "sleipnir/util/FunctionRef.hpp"
#include "sleipnir/util/SymbolExports.hpp"
#include "sleipnir/autodiff/variable_matrix.hpp"
#include "sleipnir/optimization/problem.hpp"
#include "sleipnir/util/assert.hpp"
#include "sleipnir/util/concepts.hpp"
#include "sleipnir/util/function_ref.hpp"
#include "sleipnir/util/symbol_exports.hpp"
namespace sleipnir {
namespace slp {
/**
* Performs 4th order Runge-Kutta integration of dx/dt = f(t, x, u) for dt.
@@ -26,7 +26,7 @@ namespace sleipnir {
* @param dt The time over which to integrate.
*/
template <typename F, typename State, typename Input, typename Time>
State RK4(F&& f, State x, Input u, Time t0, Time dt) {
State rk4(F&& f, State x, Input u, Time t0, Time dt) {
auto halfdt = dt * 0.5;
State k1 = f(t0, x, u, dt);
State k2 = f(t0 + halfdt, x + k1 * halfdt, u, dt);
@@ -42,13 +42,13 @@ State RK4(F&& f, State x, Input u, Time t0, Time dt) {
enum class TranscriptionMethod : uint8_t {
/// Each state is a decision variable constrained to the integrated dynamics
/// of the previous state.
kDirectTranscription,
DIRECT_TRANSCRIPTION,
/// The trajectory is modeled as a series of cubic polynomials where the
/// centerpoint slope is constrained.
kDirectCollocation,
DIRECT_COLLOCATION,
/// States depend explicitly as a function of all previous states and all
/// previous inputs.
kSingleShooting
SINGLE_SHOOTING
};
/**
@@ -56,9 +56,9 @@ enum class TranscriptionMethod : uint8_t {
*/
enum class DynamicsType : uint8_t {
/// The dynamics are a function in the form dx/dt = f(t, x, u).
kExplicitODE,
EXPLICIT_ODE,
/// The dynamics are a function in the form xₖ₊₁ = f(t, xₖ, uₖ).
kDiscrete
DISCRETE
};
/**
@@ -66,12 +66,12 @@ enum class DynamicsType : uint8_t {
*/
enum class TimestepMethod : uint8_t {
/// The timestep is a fixed constant.
kFixed,
FIXED,
/// The timesteps are allowed to vary as independent decision variables.
kVariable,
VARIABLE,
/// The timesteps are equal length but allowed to vary as a single decision
/// variable.
kVariableSingle
VARIABLE_SINGLE
};
/**
@@ -100,179 +100,177 @@ enum class TimestepMethod : uint8_t {
* https://underactuated.mit.edu/trajopt.html goes into more detail on each
* transcription method.
*/
class SLEIPNIR_DLLEXPORT OCPSolver : public OptimizationProblem {
class SLEIPNIR_DLLEXPORT OCP : public Problem {
public:
/**
* Build an optimization problem using a system evolution function (explicit
* ODE or discrete state transition function).
*
* @param numStates The number of system states.
* @param numInputs The number of system inputs.
* @param num_states The number of system states.
* @param num_inputs The number of system inputs.
* @param dt The timestep for fixed-step integration.
* @param numSteps The number of control points.
* @param num_steps The number of control points.
* @param dynamics Function representing an explicit or implicit ODE, or a
* discrete state transition function.
* - Explicit: dx/dt = f(x, u, *)
* - Implicit: f([x dx/dt]', u, *) = 0
* - State transition: xₖ₊₁ = f(xₖ, uₖ)
* @param dynamicsType The type of system evolution function.
* @param timestepMethod The timestep method.
* @param dynamics_type The type of system evolution function.
* @param timestep_method The timestep method.
* @param method The transcription method.
*/
OCPSolver(
int numStates, int numInputs, std::chrono::duration<double> dt,
int numSteps,
OCP(int num_states, int num_inputs, std::chrono::duration<double> dt,
int num_steps,
function_ref<VariableMatrix(const VariableMatrix& x,
const VariableMatrix& u)>
dynamics,
DynamicsType dynamicsType = DynamicsType::kExplicitODE,
TimestepMethod timestepMethod = TimestepMethod::kFixed,
TranscriptionMethod method = TranscriptionMethod::kDirectTranscription)
: OCPSolver{numStates,
numInputs,
dt,
numSteps,
[=]([[maybe_unused]] const VariableMatrix& t,
const VariableMatrix& x, const VariableMatrix& u,
[[maybe_unused]]
const VariableMatrix& dt) -> VariableMatrix {
return dynamics(x, u);
},
dynamicsType,
timestepMethod,
method} {}
DynamicsType dynamics_type = DynamicsType::EXPLICIT_ODE,
TimestepMethod timestep_method = TimestepMethod::FIXED,
TranscriptionMethod method = TranscriptionMethod::DIRECT_TRANSCRIPTION)
: OCP{num_states,
num_inputs,
dt,
num_steps,
[=]([[maybe_unused]] const VariableMatrix& t,
const VariableMatrix& x, const VariableMatrix& u,
[[maybe_unused]]
const VariableMatrix& dt) -> VariableMatrix {
return dynamics(x, u);
},
dynamics_type,
timestep_method,
method} {}
/**
* Build an optimization problem using a system evolution function (explicit
* ODE or discrete state transition function).
*
* @param numStates The number of system states.
* @param numInputs The number of system inputs.
* @param num_states The number of system states.
* @param num_inputs The number of system inputs.
* @param dt The timestep for fixed-step integration.
* @param numSteps The number of control points.
* @param num_steps The number of control points.
* @param dynamics Function representing an explicit or implicit ODE, or a
* discrete state transition function.
* - Explicit: dx/dt = f(t, x, u, *)
* - Implicit: f(t, [x dx/dt]', u, *) = 0
* - State transition: xₖ₊₁ = f(t, xₖ, uₖ, dt)
* @param dynamicsType The type of system evolution function.
* @param timestepMethod The timestep method.
* @param dynamics_type The type of system evolution function.
* @param timestep_method The timestep method.
* @param method The transcription method.
*/
OCPSolver(
int numStates, int numInputs, std::chrono::duration<double> dt,
int numSteps,
OCP(int num_states, int num_inputs, std::chrono::duration<double> dt,
int num_steps,
function_ref<VariableMatrix(const Variable& t, const VariableMatrix& x,
const VariableMatrix& u, const Variable& dt)>
dynamics,
DynamicsType dynamicsType = DynamicsType::kExplicitODE,
TimestepMethod timestepMethod = TimestepMethod::kFixed,
TranscriptionMethod method = TranscriptionMethod::kDirectTranscription)
: m_numStates{numStates},
m_numInputs{numInputs},
DynamicsType dynamics_type = DynamicsType::EXPLICIT_ODE,
TimestepMethod timestep_method = TimestepMethod::FIXED,
TranscriptionMethod method = TranscriptionMethod::DIRECT_TRANSCRIPTION)
: m_num_states{num_states},
m_num_inputs{num_inputs},
m_dt{dt},
m_numSteps{numSteps},
m_transcriptionMethod{method},
m_dynamicsType{dynamicsType},
m_dynamicsFunction{std::move(dynamics)},
m_timestepMethod{timestepMethod} {
// u is numSteps + 1 so that the final constraintFunction evaluation works
m_U = DecisionVariable(m_numInputs, m_numSteps + 1);
m_num_steps{num_steps},
m_transcription_method{method},
m_dynamics_type{dynamics_type},
m_dynamics_function{std::move(dynamics)},
m_timestep_method{timestep_method} {
// u is num_steps + 1 so that the final constraint function evaluation works
m_U = decision_variable(m_num_inputs, m_num_steps + 1);
if (m_timestepMethod == TimestepMethod::kFixed) {
m_DT = VariableMatrix{1, m_numSteps + 1};
for (int i = 0; i < numSteps + 1; ++i) {
if (m_timestep_method == TimestepMethod::FIXED) {
m_DT = VariableMatrix{1, m_num_steps + 1};
for (int i = 0; i < num_steps + 1; ++i) {
m_DT(0, i) = m_dt.count();
}
} else if (m_timestepMethod == TimestepMethod::kVariableSingle) {
Variable DT = DecisionVariable();
DT.SetValue(m_dt.count());
} else if (m_timestep_method == TimestepMethod::VARIABLE_SINGLE) {
Variable dt = decision_variable();
dt.set_value(m_dt.count());
// Set the member variable matrix to track the decision variable
m_DT = VariableMatrix{1, m_numSteps + 1};
for (int i = 0; i < numSteps + 1; ++i) {
m_DT(0, i) = DT;
m_DT = VariableMatrix{1, m_num_steps + 1};
for (int i = 0; i < num_steps + 1; ++i) {
m_DT(0, i) = dt;
}
} else if (m_timestepMethod == TimestepMethod::kVariable) {
m_DT = DecisionVariable(1, m_numSteps + 1);
for (int i = 0; i < numSteps + 1; ++i) {
m_DT(0, i).SetValue(m_dt.count());
} else if (m_timestep_method == TimestepMethod::VARIABLE) {
m_DT = decision_variable(1, m_num_steps + 1);
for (int i = 0; i < num_steps + 1; ++i) {
m_DT(0, i).set_value(m_dt.count());
}
}
if (m_transcriptionMethod == TranscriptionMethod::kDirectTranscription) {
m_X = DecisionVariable(m_numStates, m_numSteps + 1);
ConstrainDirectTranscription();
} else if (m_transcriptionMethod ==
TranscriptionMethod::kDirectCollocation) {
m_X = DecisionVariable(m_numStates, m_numSteps + 1);
ConstrainDirectCollocation();
} else if (m_transcriptionMethod == TranscriptionMethod::kSingleShooting) {
if (m_transcription_method == TranscriptionMethod::DIRECT_TRANSCRIPTION) {
m_X = decision_variable(m_num_states, m_num_steps + 1);
constrain_direct_transcription();
} else if (m_transcription_method ==
TranscriptionMethod::DIRECT_COLLOCATION) {
m_X = decision_variable(m_num_states, m_num_steps + 1);
constrain_direct_collocation();
} else if (m_transcription_method == TranscriptionMethod::SINGLE_SHOOTING) {
// In single-shooting the states aren't decision variables, but instead
// depend on the input and previous states
m_X = VariableMatrix{m_numStates, m_numSteps + 1};
ConstrainSingleShooting();
m_X = VariableMatrix{m_num_states, m_num_steps + 1};
constrain_single_shooting();
}
}
/**
* Utility function to constrain the initial state.
*
* @param initialState the initial state to constrain to.
* @param initial_state the initial state to constrain to.
*/
template <typename T>
requires ScalarLike<T> || MatrixLike<T>
void ConstrainInitialState(const T& initialState) {
SubjectTo(InitialState() == initialState);
void constrain_initial_state(const T& initial_state) {
subject_to(this->initial_state() == initial_state);
}
/**
* Utility function to constrain the final state.
*
* @param finalState the final state to constrain to.
* @param final_state the final state to constrain to.
*/
template <typename T>
requires ScalarLike<T> || MatrixLike<T>
void ConstrainFinalState(const T& finalState) {
SubjectTo(FinalState() == finalState);
void constrain_final_state(const T& final_state) {
subject_to(this->final_state() == final_state);
}
/**
* Set the constraint evaluation function. This function is called
* `numSteps+1` times, with the corresponding state and input
* `num_steps+1` times, with the corresponding state and input
* VariableMatrices.
*
* @param callback The callback f(x, u) where x is the state and u is the
* input vector.
*/
void ForEachStep(
void for_each_step(
const function_ref<void(const VariableMatrix& x, const VariableMatrix& u)>
callback) {
for (int i = 0; i < m_numSteps + 1; ++i) {
auto x = X().Col(i);
auto u = U().Col(i);
for (int i = 0; i < m_num_steps + 1; ++i) {
auto x = X().col(i);
auto u = U().col(i);
callback(x, u);
}
}
/**
* Set the constraint evaluation function. This function is called
* `numSteps+1` times, with the corresponding state and input
* `num_steps+1` times, with the corresponding state and input
* VariableMatrices.
*
* @param callback The callback f(t, x, u, dt) where t is time, x is the state
* vector, u is the input vector, and dt is the timestep duration.
*/
void ForEachStep(
void for_each_step(
const function_ref<void(const Variable& t, const VariableMatrix& x,
const VariableMatrix& u, const Variable& dt)>
callback) {
Variable time = 0.0;
for (int i = 0; i < m_numSteps + 1; ++i) {
auto x = X().Col(i);
auto u = U().Col(i);
auto dt = DT()(0, i);
for (int i = 0; i < m_num_steps + 1; ++i) {
auto x = X().col(i);
auto u = U().col(i);
auto dt = this->dt()(0, i);
callback(time, x, u, dt);
time += dt;
@@ -282,115 +280,115 @@ class SLEIPNIR_DLLEXPORT OCPSolver : public OptimizationProblem {
/**
* Convenience function to set a lower bound on the input.
*
* @param lowerBound The lower bound that inputs must always be above. Must be
* shaped (numInputs)x1.
* @param lower_bound The lower bound that inputs must always be above. Must
* be shaped (num_inputs)x1.
*/
template <typename T>
requires ScalarLike<T> || MatrixLike<T>
void SetLowerInputBound(const T& lowerBound) {
for (int i = 0; i < m_numSteps + 1; ++i) {
SubjectTo(U().Col(i) >= lowerBound);
void set_lower_input_bound(const T& lower_bound) {
for (int i = 0; i < m_num_steps + 1; ++i) {
subject_to(U().col(i) >= lower_bound);
}
}
/**
* Convenience function to set an upper bound on the input.
*
* @param upperBound The upper bound that inputs must always be below. Must be
* shaped (numInputs)x1.
* @param upper_bound The upper bound that inputs must always be below. Must
* be shaped (num_inputs)x1.
*/
template <typename T>
requires ScalarLike<T> || MatrixLike<T>
void SetUpperInputBound(const T& upperBound) {
for (int i = 0; i < m_numSteps + 1; ++i) {
SubjectTo(U().Col(i) <= upperBound);
void set_upper_input_bound(const T& upper_bound) {
for (int i = 0; i < m_num_steps + 1; ++i) {
subject_to(U().col(i) <= upper_bound);
}
}
/**
* Convenience function to set a lower bound on the timestep.
*
* @param minTimestep The minimum timestep.
* @param min_timestep The minimum timestep.
*/
void SetMinTimestep(std::chrono::duration<double> minTimestep) {
SubjectTo(DT() >= minTimestep.count());
void set_min_timestep(std::chrono::duration<double> min_timestep) {
subject_to(dt() >= min_timestep.count());
}
/**
* Convenience function to set an upper bound on the timestep.
*
* @param maxTimestep The maximum timestep.
* @param max_timestep The maximum timestep.
*/
void SetMaxTimestep(std::chrono::duration<double> maxTimestep) {
SubjectTo(DT() <= maxTimestep.count());
void set_max_timestep(std::chrono::duration<double> max_timestep) {
subject_to(dt() <= max_timestep.count());
}
/**
* Get the state variables. After the problem is solved, this will contain the
* optimized trajectory.
*
* Shaped (numStates)x(numSteps+1).
* Shaped (num_states)x(num_steps+1).
*
* @returns The state variable matrix.
* @return The state variable matrix.
*/
VariableMatrix& X() { return m_X; };
VariableMatrix& X() { return m_X; }
/**
* Get the input variables. After the problem is solved, this will contain the
* inputs corresponding to the optimized trajectory.
*
* Shaped (numInputs)x(numSteps+1), although the last input step is unused in
* the trajectory.
* Shaped (num_inputs)x(num_steps+1), although the last input step is unused
* in the trajectory.
*
* @returns The input variable matrix.
* @return The input variable matrix.
*/
VariableMatrix& U() { return m_U; };
VariableMatrix& U() { return m_U; }
/**
* Get the timestep variables. After the problem is solved, this will contain
* the timesteps corresponding to the optimized trajectory.
*
* Shaped 1x(numSteps+1), although the last timestep is unused in
* Shaped 1x(num_steps+1), although the last timestep is unused in
* the trajectory.
*
* @returns The timestep variable matrix.
* @return The timestep variable matrix.
*/
VariableMatrix& DT() { return m_DT; };
VariableMatrix& dt() { return m_DT; }
/**
* Convenience function to get the initial state in the trajectory.
*
* @returns The initial state of the trajectory.
* @return The initial state of the trajectory.
*/
VariableMatrix InitialState() { return m_X.Col(0); }
VariableMatrix initial_state() { return m_X.col(0); }
/**
* Convenience function to get the final state in the trajectory.
*
* @returns The final state of the trajectory.
* @return The final state of the trajectory.
*/
VariableMatrix FinalState() { return m_X.Col(m_numSteps); }
VariableMatrix final_state() { return m_X.col(m_num_steps); }
private:
void ConstrainDirectCollocation() {
Assert(m_dynamicsType == DynamicsType::kExplicitODE);
void constrain_direct_collocation() {
slp_assert(m_dynamics_type == DynamicsType::EXPLICIT_ODE);
Variable time = 0.0;
// Derivation at https://mec560sbu.github.io/2016/09/30/direct_collocation/
for (int i = 0; i < m_numSteps; ++i) {
Variable h = DT()(0, i);
for (int i = 0; i < m_num_steps; ++i) {
Variable h = dt()(0, i);
auto& f = m_dynamicsFunction;
auto& f = m_dynamics_function;
auto t_begin = time;
auto t_end = t_begin + h;
auto x_begin = X().Col(i);
auto x_end = X().Col(i + 1);
auto x_begin = X().col(i);
auto x_end = X().col(i + 1);
auto u_begin = U().Col(i);
auto u_end = U().Col(i + 1);
auto u_begin = U().col(i);
auto u_end = U().col(i + 1);
auto xdot_begin = f(t_begin, x_begin, u_begin, h);
auto xdot_end = f(t_end, x_end, u_end, h);
@@ -401,71 +399,71 @@ class SLEIPNIR_DLLEXPORT OCPSolver : public OptimizationProblem {
auto x_c = 0.5 * (x_begin + x_end) + h / 8 * (xdot_begin - xdot_end);
auto u_c = 0.5 * (u_begin + u_end);
SubjectTo(xdot_c == f(t_c, x_c, u_c, h));
subject_to(xdot_c == f(t_c, x_c, u_c, h));
time += h;
}
}
void ConstrainDirectTranscription() {
void constrain_direct_transcription() {
Variable time = 0.0;
for (int i = 0; i < m_numSteps; ++i) {
auto x_begin = X().Col(i);
auto x_end = X().Col(i + 1);
auto u = U().Col(i);
Variable dt = DT()(0, i);
for (int i = 0; i < m_num_steps; ++i) {
auto x_begin = X().col(i);
auto x_end = X().col(i + 1);
auto u = U().col(i);
Variable dt = this->dt()(0, i);
if (m_dynamicsType == DynamicsType::kExplicitODE) {
SubjectTo(x_end == RK4<const decltype(m_dynamicsFunction)&,
VariableMatrix, VariableMatrix, Variable>(
m_dynamicsFunction, x_begin, u, time, dt));
} else if (m_dynamicsType == DynamicsType::kDiscrete) {
SubjectTo(x_end == m_dynamicsFunction(time, x_begin, u, dt));
if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) {
subject_to(x_end == rk4<const decltype(m_dynamics_function)&,
VariableMatrix, VariableMatrix, Variable>(
m_dynamics_function, x_begin, u, time, dt));
} else if (m_dynamics_type == DynamicsType::DISCRETE) {
subject_to(x_end == m_dynamics_function(time, x_begin, u, dt));
}
time += dt;
}
}
void ConstrainSingleShooting() {
void constrain_single_shooting() {
Variable time = 0.0;
for (int i = 0; i < m_numSteps; ++i) {
auto x_begin = X().Col(i);
auto x_end = X().Col(i + 1);
auto u = U().Col(i);
Variable dt = DT()(0, i);
for (int i = 0; i < m_num_steps; ++i) {
auto x_begin = X().col(i);
auto x_end = X().col(i + 1);
auto u = U().col(i);
Variable dt = this->dt()(0, i);
if (m_dynamicsType == DynamicsType::kExplicitODE) {
x_end = RK4<const decltype(m_dynamicsFunction)&, VariableMatrix,
VariableMatrix, Variable>(m_dynamicsFunction, x_begin, u,
if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) {
x_end = rk4<const decltype(m_dynamics_function)&, VariableMatrix,
VariableMatrix, Variable>(m_dynamics_function, x_begin, u,
time, dt);
} else if (m_dynamicsType == DynamicsType::kDiscrete) {
x_end = m_dynamicsFunction(time, x_begin, u, dt);
} else if (m_dynamics_type == DynamicsType::DISCRETE) {
x_end = m_dynamics_function(time, x_begin, u, dt);
}
time += dt;
}
}
int m_numStates;
int m_numInputs;
int m_num_states;
int m_num_inputs;
std::chrono::duration<double> m_dt;
int m_numSteps;
TranscriptionMethod m_transcriptionMethod;
int m_num_steps;
TranscriptionMethod m_transcription_method;
DynamicsType m_dynamicsType;
DynamicsType m_dynamics_type;
function_ref<VariableMatrix(const Variable& t, const VariableMatrix& x,
const VariableMatrix& u, const Variable& dt)>
m_dynamicsFunction;
m_dynamics_function;
TimestepMethod m_timestepMethod;
TimestepMethod m_timestep_method;
VariableMatrix m_X;
VariableMatrix m_U;
VariableMatrix m_DT;
};
} // namespace sleipnir
} // namespace slp

386
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp vendored
View File

@@ -1,386 +0,0 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <algorithm>
#include <array>
#include <concepts>
#include <functional>
#include <iterator>
#include <optional>
#include <utility>
#include <Eigen/Core>
#include <wpi/SmallVector.h>
#include "sleipnir/autodiff/Variable.hpp"
#include "sleipnir/autodiff/VariableMatrix.hpp"
#include "sleipnir/optimization/SolverConfig.hpp"
#include "sleipnir/optimization/SolverExitCondition.hpp"
#include "sleipnir/optimization/SolverIterationInfo.hpp"
#include "sleipnir/optimization/SolverStatus.hpp"
#include "sleipnir/optimization/solver/InteriorPoint.hpp"
#include "sleipnir/optimization/solver/SQP.hpp"
#include "sleipnir/util/Print.hpp"
#include "sleipnir/util/SymbolExports.hpp"
namespace sleipnir {
/**
* This class allows the user to pose a constrained nonlinear optimization
* problem in natural mathematical notation and solve it.
*
* This class supports problems of the form:
@verbatim
minₓ f(x)
subject to cₑ(x) = 0
cᵢ(x) ≥ 0
@endverbatim
*
* where f(x) is the scalar cost function, x is the vector of decision variables
* (variables the solver can tweak to minimize the cost function), cᵢ(x) are the
* inequality constraints, and cₑ(x) are the equality constraints. Constraints
* are equations or inequalities of the decision variables that constrain what
* values the solver is allowed to use when searching for an optimal solution.
*
* The nice thing about this class is users don't have to put their system in
* the form shown above manually; they can write it in natural mathematical form
* and it'll be converted for them.
*/
class SLEIPNIR_DLLEXPORT OptimizationProblem {
public:
/**
* Construct the optimization problem.
*/
OptimizationProblem() noexcept = default;
/**
* Create a decision variable in the optimization problem.
*/
[[nodiscard]]
Variable DecisionVariable() {
m_decisionVariables.emplace_back();
return m_decisionVariables.back();
}
/**
* Create a matrix of decision variables in the optimization problem.
*
* @param rows Number of matrix rows.
* @param cols Number of matrix columns.
*/
[[nodiscard]]
VariableMatrix DecisionVariable(int rows, int cols = 1) {
m_decisionVariables.reserve(m_decisionVariables.size() + rows * cols);
VariableMatrix vars{rows, cols};
for (int row = 0; row < rows; ++row) {
for (int col = 0; col < cols; ++col) {
m_decisionVariables.emplace_back();
vars(row, col) = m_decisionVariables.back();
}
}
return vars;
}
/**
* Create a symmetric matrix of decision variables in the optimization
* problem.
*
* Variable instances are reused across the diagonal, which helps reduce
* problem dimensionality.
*
* @param rows Number of matrix rows.
*/
[[nodiscard]]
VariableMatrix SymmetricDecisionVariable(int rows) {
// We only need to store the lower triangle of an n x n symmetric matrix;
// the other elements are duplicates. The lower triangle has (n² + n)/2
// elements.
//
// n
// Σ k = (n² + n)/2
// k=1
m_decisionVariables.reserve(m_decisionVariables.size() +
(rows * rows + rows) / 2);
VariableMatrix vars{rows, rows};
for (int row = 0; row < rows; ++row) {
for (int col = 0; col <= row; ++col) {
m_decisionVariables.emplace_back();
vars(row, col) = m_decisionVariables.back();
vars(col, row) = m_decisionVariables.back();
}
}
return vars;
}
/**
* Tells the solver to minimize the output of the given cost function.
*
* Note that this is optional. If only constraints are specified, the solver
* will find the closest solution to the initial conditions that's in the
* feasible set.
*
* @param cost The cost function to minimize.
*/
void Minimize(const Variable& cost) {
m_f = cost;
status.costFunctionType = m_f.value().Type();
}
/**
* Tells the solver to minimize the output of the given cost function.
*
* Note that this is optional. If only constraints are specified, the solver
* will find the closest solution to the initial conditions that's in the
* feasible set.
*
* @param cost The cost function to minimize.
*/
void Minimize(Variable&& cost) {
m_f = std::move(cost);
status.costFunctionType = m_f.value().Type();
}
/**
* Tells the solver to maximize the output of the given objective function.
*
* Note that this is optional. If only constraints are specified, the solver
* will find the closest solution to the initial conditions that's in the
* feasible set.
*
* @param objective The objective function to maximize.
*/
void Maximize(const Variable& objective) {
// Maximizing a cost function is the same as minimizing its negative
m_f = -objective;
status.costFunctionType = m_f.value().Type();
}
/**
* Tells the solver to maximize the output of the given objective function.
*
* Note that this is optional. If only constraints are specified, the solver
* will find the closest solution to the initial conditions that's in the
* feasible set.
*
* @param objective The objective function to maximize.
*/
void Maximize(Variable&& objective) {
// Maximizing a cost function is the same as minimizing its negative
m_f = -std::move(objective);
status.costFunctionType = m_f.value().Type();
}
/**
* Tells the solver to solve the problem while satisfying the given equality
* constraint.
*
* @param constraint The constraint to satisfy.
*/
void SubjectTo(const EqualityConstraints& constraint) {
// Get the highest order equality constraint expression type
for (const auto& c : constraint.constraints) {
status.equalityConstraintType =
std::max(status.equalityConstraintType, c.Type());
}
m_equalityConstraints.reserve(m_equalityConstraints.size() +
constraint.constraints.size());
std::copy(constraint.constraints.begin(), constraint.constraints.end(),
std::back_inserter(m_equalityConstraints));
}
/**
* Tells the solver to solve the problem while satisfying the given equality
* constraint.
*
* @param constraint The constraint to satisfy.
*/
void SubjectTo(EqualityConstraints&& constraint) {
// Get the highest order equality constraint expression type
for (const auto& c : constraint.constraints) {
status.equalityConstraintType =
std::max(status.equalityConstraintType, c.Type());
}
m_equalityConstraints.reserve(m_equalityConstraints.size() +
constraint.constraints.size());
std::copy(constraint.constraints.begin(), constraint.constraints.end(),
std::back_inserter(m_equalityConstraints));
}
/**
* Tells the solver to solve the problem while satisfying the given inequality
* constraint.
*
* @param constraint The constraint to satisfy.
*/
void SubjectTo(const InequalityConstraints& constraint) {
// Get the highest order inequality constraint expression type
for (const auto& c : constraint.constraints) {
status.inequalityConstraintType =
std::max(status.inequalityConstraintType, c.Type());
}
m_inequalityConstraints.reserve(m_inequalityConstraints.size() +
constraint.constraints.size());
std::copy(constraint.constraints.begin(), constraint.constraints.end(),
std::back_inserter(m_inequalityConstraints));
}
/**
* Tells the solver to solve the problem while satisfying the given inequality
* constraint.
*
* @param constraint The constraint to satisfy.
*/
void SubjectTo(InequalityConstraints&& constraint) {
// Get the highest order inequality constraint expression type
for (const auto& c : constraint.constraints) {
status.inequalityConstraintType =
std::max(status.inequalityConstraintType, c.Type());
}
m_inequalityConstraints.reserve(m_inequalityConstraints.size() +
constraint.constraints.size());
std::copy(constraint.constraints.begin(), constraint.constraints.end(),
std::back_inserter(m_inequalityConstraints));
}
/**
* Solve the optimization problem. The solution will be stored in the original
* variables used to construct the problem.
*
* @param config Configuration options for the solver.
*/
SolverStatus Solve(const SolverConfig& config = SolverConfig{}) {
// Create the initial value column vector
Eigen::VectorXd x{m_decisionVariables.size()};
for (size_t i = 0; i < m_decisionVariables.size(); ++i) {
x(i) = m_decisionVariables[i].Value();
}
status.exitCondition = SolverExitCondition::kSuccess;
// If there's no cost function, make it zero and continue
if (!m_f.has_value()) {
m_f = Variable();
}
if (config.diagnostics) {
constexpr std::array kExprTypeToName{"empty", "constant", "linear",
"quadratic", "nonlinear"};
// Print cost function and constraint expression types
sleipnir::println(
"The cost function is {}.",
kExprTypeToName[static_cast<int>(status.costFunctionType)]);
sleipnir::println(
"The equality constraints are {}.",
kExprTypeToName[static_cast<int>(status.equalityConstraintType)]);
sleipnir::println(
"The inequality constraints are {}.",
kExprTypeToName[static_cast<int>(status.inequalityConstraintType)]);
sleipnir::println("");
// Print problem dimensionality
sleipnir::println("Number of decision variables: {}",
m_decisionVariables.size());
sleipnir::println("Number of equality constraints: {}",
m_equalityConstraints.size());
sleipnir::println("Number of inequality constraints: {}\n",
m_inequalityConstraints.size());
}
// If the problem is empty or constant, there's nothing to do
if (status.costFunctionType <= ExpressionType::kConstant &&
status.equalityConstraintType <= ExpressionType::kConstant &&
status.inequalityConstraintType <= ExpressionType::kConstant) {
return status;
}
// Solve the optimization problem
if (m_inequalityConstraints.empty()) {
SQP(m_decisionVariables, m_equalityConstraints, m_f.value(), m_callback,
config, x, &status);
} else {
Eigen::VectorXd s = Eigen::VectorXd::Ones(m_inequalityConstraints.size());
InteriorPoint(m_decisionVariables, m_equalityConstraints,
m_inequalityConstraints, m_f.value(), m_callback, config,
false, x, s, &status);
}
if (config.diagnostics) {
sleipnir::println("Exit condition: {}", ToMessage(status.exitCondition));
}
// Assign the solution to the original Variable instances
VariableMatrix{m_decisionVariables}.SetValue(x);
return status;
}
/**
* Sets a callback to be called at each solver iteration.
*
* The callback for this overload should return void.
*
* @param callback The callback.
*/
template <typename F>
requires requires(F callback, const SolverIterationInfo& info) {
{ callback(info) } -> std::same_as<void>;
}
void Callback(F&& callback) {
m_callback = [=, callback = std::forward<F>(callback)](
const SolverIterationInfo& info) {
callback(info);
return false;
};
}
/**
* Sets a callback to be called at each solver iteration.
*
* The callback for this overload should return bool.
*
* @param callback The callback. Returning true from the callback causes the
* solver to exit early with the solution it has so far.
*/
template <typename F>
requires requires(F callback, const SolverIterationInfo& info) {
{ callback(info) } -> std::same_as<bool>;
}
void Callback(F&& callback) {
m_callback = std::forward<F>(callback);
}
private:
// The list of decision variables, which are the root of the problem's
// expression tree
wpi::SmallVector<Variable> m_decisionVariables;
// The cost function: f(x)
std::optional<Variable> m_f;
// The list of equality constraints: cₑ(x) = 0
wpi::SmallVector<Variable> m_equalityConstraints;
// The list of inequality constraints: cᵢ(x) ≥ 0
wpi::SmallVector<Variable> m_inequalityConstraints;
// The user callback
std::function<bool(const SolverIterationInfo& info)> m_callback =
[](const SolverIterationInfo&) { return false; };
// The solver status
SolverStatus status;
};
} // namespace sleipnir

54
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverConfig.hpp vendored
View File

@@ -1,54 +0,0 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <chrono>
#include <limits>
#include "sleipnir/util/SymbolExports.hpp"
namespace sleipnir {
/**
* Solver configuration.
*/
struct SLEIPNIR_DLLEXPORT SolverConfig {
/// The solver will stop once the error is below this tolerance.
double tolerance = 1e-8;
/// The maximum number of solver iterations before returning a solution.
int maxIterations = 5000;
/// The solver will stop once the error is below this tolerance for
/// `acceptableIterations` iterations. This is useful in cases where the
/// solver might not be able to achieve the desired level of accuracy due to
/// floating-point round-off.
double acceptableTolerance = 1e-6;
/// The solver will stop once the error is below `acceptableTolerance` for
/// this many iterations.
int maxAcceptableIterations = 15;
/// The maximum elapsed wall clock time before returning a solution.
std::chrono::duration<double> timeout{
std::numeric_limits<double>::infinity()};
/// Enables the feasible interior-point method. When the inequality
/// constraints are all feasible, step sizes are reduced when necessary to
/// prevent them becoming infeasible again. This is useful when parts of the
/// problem are ill-conditioned in infeasible regions (e.g., square root of a
/// negative value). This can slow or prevent progress toward a solution
/// though, so only enable it if necessary.
bool feasibleIPM = false;
/// Enables diagnostic prints.
bool diagnostics = false;
/// Enables writing sparsity patterns of H, Aₑ, and Aᵢ to files named H.spy,
/// A_e.spy, and A_i.spy respectively during solve.
///
/// Use tools/spy.py to plot them.
bool spy = false;
};
} // namespace sleipnir

80
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverExitCondition.hpp vendored
View File

@@ -1,80 +0,0 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <stdint.h>
#include <string_view>
#include "sleipnir/util/SymbolExports.hpp"
namespace sleipnir {
/**
* Solver exit condition.
*/
enum class SolverExitCondition : int8_t {
/// Solved the problem to the desired tolerance.
kSuccess = 0,
/// Solved the problem to an acceptable tolerance, but not the desired one.
kSolvedToAcceptableTolerance = 1,
/// The solver returned its solution so far after the user requested a stop.
kCallbackRequestedStop = 2,
/// The solver determined the problem to be overconstrained and gave up.
kTooFewDOFs = -1,
/// The solver determined the problem to be locally infeasible and gave up.
kLocallyInfeasible = -2,
/// The solver failed to reach the desired tolerance, and feasibility
/// restoration failed to converge.
kFeasibilityRestorationFailed = -3,
/// The solver encountered nonfinite initial cost or constraints and gave up.
kNonfiniteInitialCostOrConstraints = -4,
/// The solver encountered diverging primal iterates xₖ and/or sₖ and gave up.
kDivergingIterates = -5,
/// The solver returned its solution so far after exceeding the maximum number
/// of iterations.
kMaxIterationsExceeded = -6,
/// The solver returned its solution so far after exceeding the maximum
/// elapsed wall clock time.
kTimeout = -7
};
/**
* Returns user-readable message corresponding to the exit condition.
*
* @param exitCondition Solver exit condition.
*/
SLEIPNIR_DLLEXPORT constexpr std::string_view ToMessage(
const SolverExitCondition& exitCondition) {
using enum SolverExitCondition;
switch (exitCondition) {
case kSuccess:
return "solved to desired tolerance";
case kSolvedToAcceptableTolerance:
return "solved to acceptable tolerance";
case kCallbackRequestedStop:
return "callback requested stop";
case kTooFewDOFs:
return "problem has too few degrees of freedom";
case kLocallyInfeasible:
return "problem is locally infeasible";
case kFeasibilityRestorationFailed:
return "solver failed to reach the desired tolerance, and feasibility "
"restoration failed to converge";
case kNonfiniteInitialCostOrConstraints:
return "solver encountered nonfinite initial cost or constraints and "
"gave up";
case kDivergingIterates:
return "solver encountered diverging primal iterates xₖ and/or sₖ and "
"gave up";
case kMaxIterationsExceeded:
return "solution returned after maximum iterations exceeded";
case kTimeout:
return "solution returned after maximum wall clock time exceeded";
default:
return "unknown";
}
}
} // namespace sleipnir

32
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverStatus.hpp vendored
View File

@@ -1,32 +0,0 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include "sleipnir/autodiff/ExpressionType.hpp"
#include "sleipnir/optimization/SolverExitCondition.hpp"
#include "sleipnir/util/SymbolExports.hpp"
namespace sleipnir {
/**
* Return value of OptimizationProblem::Solve() containing the cost function and
* constraint types and solver's exit condition.
*/
struct SLEIPNIR_DLLEXPORT SolverStatus {
/// The cost function type detected by the solver.
ExpressionType costFunctionType = ExpressionType::kNone;
/// The equality constraint type detected by the solver.
ExpressionType equalityConstraintType = ExpressionType::kNone;
/// The inequality constraint type detected by the solver.
ExpressionType inequalityConstraintType = ExpressionType::kNone;
/// The solver's exit condition.
SolverExitCondition exitCondition = SolverExitCondition::kSuccess;
/// The solution's cost.
double cost = 0.0;
};
} // namespace sleipnir

50
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Multistart.hpp → wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/multistart.hpp vendored
View File

@@ -6,12 +6,12 @@
#include <future>
#include <span>
#include <wpi/SmallVector.h>
#include <gch/small_vector.hpp>
#include "sleipnir/optimization/SolverStatus.hpp"
#include "sleipnir/util/FunctionRef.hpp"
#include "sleipnir/optimization/solver/exit_status.hpp"
#include "sleipnir/util/function_ref.hpp"
namespace sleipnir {
namespace slp {
/**
* The result of a multistart solve.
@@ -21,7 +21,11 @@ namespace sleipnir {
*/
template <typename DecisionVariables>
struct MultistartResult {
SolverStatus status;
/// The solver exit status.
ExitStatus status;
/// The solution's cost.
double cost;
/// The decision variables.
DecisionVariables variables;
};
@@ -37,39 +41,37 @@ struct MultistartResult {
* guess.
* @param solve A user-provided function that takes a decision variable initial
* guess and returns a MultistartResult.
* @param initialGuesses A list of decision variable initial guesses to try.
* @param initial_guesses A list of decision variable initial guesses to try.
*/
template <typename DecisionVariables>
MultistartResult<DecisionVariables> Multistart(
function_ref<MultistartResult<DecisionVariables>(
const DecisionVariables& initialGuess)>
const DecisionVariables& initial_guess)>
solve,
std::span<const DecisionVariables> initialGuesses) {
wpi::SmallVector<std::future<MultistartResult<DecisionVariables>>> futures;
futures.reserve(initialGuesses.size());
std::span<const DecisionVariables> initial_guesses) {
gch::small_vector<std::future<MultistartResult<DecisionVariables>>> futures;
futures.reserve(initial_guesses.size());
for (const auto& initialGuess : initialGuesses) {
futures.emplace_back(std::async(std::launch::async, solve, initialGuess));
for (const auto& initial_guess : initial_guesses) {
futures.emplace_back(std::async(std::launch::async, solve, initial_guess));
}
wpi::SmallVector<MultistartResult<DecisionVariables>> results;
gch::small_vector<MultistartResult<DecisionVariables>> results;
results.reserve(futures.size());
for (auto& future : futures) {
results.emplace_back(future.get());
}
return *std::min_element(
results.cbegin(), results.cend(), [](const auto& a, const auto& b) {
// Prioritize successful solve
if (a.status.exitCondition == SolverExitCondition::kSuccess &&
b.status.exitCondition != SolverExitCondition::kSuccess) {
return true;
}
return *std::ranges::min_element(results, [](const auto& a, const auto& b) {
// Prioritize successful solve
if (a.status == ExitStatus::SUCCESS && b.status != ExitStatus::SUCCESS) {
return true;
}
// Otherwise prioritize solution with lower cost
return a.status.cost < b.status.cost;
});
// Otherwise prioritize solution with lower cost
return a.cost < b.cost;
});
}
} // namespace sleipnir
} // namespace slp

342
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp vendored Normal file
View File

@@ -0,0 +1,342 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <algorithm>
#include <concepts>
#include <functional>
#include <iterator>
#include <optional>
#include <ranges>
#include <utility>
#include <Eigen/Core>
#include <gch/small_vector.hpp>
#include "sleipnir/autodiff/expression_type.hpp"
#include "sleipnir/autodiff/variable.hpp"
#include "sleipnir/autodiff/variable_matrix.hpp"
#include "sleipnir/optimization/solver/exit_status.hpp"
#include "sleipnir/optimization/solver/iteration_info.hpp"
#include "sleipnir/optimization/solver/options.hpp"
#include "sleipnir/util/symbol_exports.hpp"
namespace slp {
/**
* This class allows the user to pose a constrained nonlinear optimization
* problem in natural mathematical notation and solve it.
*
* This class supports problems of the form:
@verbatim
minₓ f(x)
subject to cₑ(x) = 0
cᵢ(x) ≥ 0
@endverbatim
*
* where f(x) is the scalar cost function, x is the vector of decision variables
* (variables the solver can tweak to minimize the cost function), cᵢ(x) are the
* inequality constraints, and cₑ(x) are the equality constraints. Constraints
* are equations or inequalities of the decision variables that constrain what
* values the solver is allowed to use when searching for an optimal solution.
*
* The nice thing about this class is users don't have to put their system in
* the form shown above manually; they can write it in natural mathematical form
* and it'll be converted for them.
*/
class SLEIPNIR_DLLEXPORT Problem {
public:
/**
* Construct the optimization problem.
*/
Problem() noexcept = default;
/**
* Create a decision variable in the optimization problem.
*
* @return A decision variable in the optimization problem.
*/
[[nodiscard]]
Variable decision_variable() {
m_decision_variables.emplace_back();
return m_decision_variables.back();
}
/**
* Create a matrix of decision variables in the optimization problem.
*
* @param rows Number of matrix rows.
* @param cols Number of matrix columns.
* @return A matrix of decision variables in the optimization problem.
*/
[[nodiscard]]
VariableMatrix decision_variable(int rows, int cols = 1) {
m_decision_variables.reserve(m_decision_variables.size() + rows * cols);
VariableMatrix vars{rows, cols};
for (int row = 0; row < rows; ++row) {
for (int col = 0; col < cols; ++col) {
m_decision_variables.emplace_back();
vars(row, col) = m_decision_variables.back();
}
}
return vars;
}
/**
* Create a symmetric matrix of decision variables in the optimization
* problem.
*
* Variable instances are reused across the diagonal, which helps reduce
* problem dimensionality.
*
* @param rows Number of matrix rows.
* @return A symmetric matrix of decision varaibles in the optimization
* problem.
*/
[[nodiscard]]
VariableMatrix symmetric_decision_variable(int rows) {
// We only need to store the lower triangle of an n x n symmetric matrix;
// the other elements are duplicates. The lower triangle has (n² + n)/2
// elements.
//
// n
// Σ k = (n² + n)/2
// k=1
m_decision_variables.reserve(m_decision_variables.size() +
(rows * rows + rows) / 2);
VariableMatrix vars{rows, rows};
for (int row = 0; row < rows; ++row) {
for (int col = 0; col <= row; ++col) {
m_decision_variables.emplace_back();
vars(row, col) = m_decision_variables.back();
vars(col, row) = m_decision_variables.back();
}
}
return vars;
}
/**
* Tells the solver to minimize the output of the given cost function.
*
* Note that this is optional. If only constraints are specified, the solver
* will find the closest solution to the initial conditions that's in the
* feasible set.
*
* @param cost The cost function to minimize.
*/
void minimize(const Variable& cost) { m_f = cost; }
/**
* Tells the solver to minimize the output of the given cost function.
*
* Note that this is optional. If only constraints are specified, the solver
* will find the closest solution to the initial conditions that's in the
* feasible set.
*
* @param cost The cost function to minimize.
*/
void minimize(Variable&& cost) { m_f = std::move(cost); }
/**
* Tells the solver to maximize the output of the given objective function.
*
* Note that this is optional. If only constraints are specified, the solver
* will find the closest solution to the initial conditions that's in the
* feasible set.
*
* @param objective The objective function to maximize.
*/
void maximize(const Variable& objective) {
// Maximizing a cost function is the same as minimizing its negative
m_f = -objective;
}
/**
* Tells the solver to maximize the output of the given objective function.
*
* Note that this is optional. If only constraints are specified, the solver
* will find the closest solution to the initial conditions that's in the
* feasible set.
*
* @param objective The objective function to maximize.
*/
void maximize(Variable&& objective) {
// Maximizing a cost function is the same as minimizing its negative
m_f = -std::move(objective);
}
/**
* Tells the solver to solve the problem while satisfying the given equality
* constraint.
*
* @param constraint The constraint to satisfy.
*/
void subject_to(const EqualityConstraints& constraint) {
m_equality_constraints.reserve(m_equality_constraints.size() +
constraint.constraints.size());
std::ranges::copy(constraint.constraints,
std::back_inserter(m_equality_constraints));
}
/**
* Tells the solver to solve the problem while satisfying the given equality
* constraint.
*
* @param constraint The constraint to satisfy.
*/
void subject_to(EqualityConstraints&& constraint) {
m_equality_constraints.reserve(m_equality_constraints.size() +
constraint.constraints.size());
std::ranges::copy(constraint.constraints,
std::back_inserter(m_equality_constraints));
}
/**
* Tells the solver to solve the problem while satisfying the given inequality
* constraint.
*
* @param constraint The constraint to satisfy.
*/
void subject_to(const InequalityConstraints& constraint) {
m_inequality_constraints.reserve(m_inequality_constraints.size() +
constraint.constraints.size());
std::ranges::copy(constraint.constraints,
std::back_inserter(m_inequality_constraints));
}
/**
* Tells the solver to solve the problem while satisfying the given inequality
* constraint.
*
* @param constraint The constraint to satisfy.
*/
void subject_to(InequalityConstraints&& constraint) {
m_inequality_constraints.reserve(m_inequality_constraints.size() +
constraint.constraints.size());
std::ranges::copy(constraint.constraints,
std::back_inserter(m_inequality_constraints));
}
/**
* Returns the cost function's type.
*
* @return The cost function's type.
*/
ExpressionType cost_function_type() const {
if (m_f) {
return m_f.value().type();
} else {
return ExpressionType::NONE;
}
}
/**
* Returns the type of the highest order equality constraint.
*
* @return The type of the highest order equality constraint.
*/
ExpressionType equality_constraint_type() const {
if (!m_equality_constraints.empty()) {
return std::ranges::max(m_equality_constraints, {}, &Variable::type)
.type();
} else {
return ExpressionType::NONE;
}
}
/**
* Returns the type of the highest order inequality constraint.
*
* @return The type of the highest order inequality constraint.
*/
ExpressionType inequality_constraint_type() const {
if (!m_inequality_constraints.empty()) {
return std::ranges::max(m_inequality_constraints, {}, &Variable::type)
.type();
} else {
return ExpressionType::NONE;
}
}
/**
* Solve the optimization problem. The solution will be stored in the original
* variables used to construct the problem.
*
* @param options Solver options.
* @param spy Enables writing sparsity patterns of H, Aₑ, and Aᵢ to files
* named H.spy, A_e.spy, and A_i.spy respectively during solve. Use
* tools/spy.py to plot them.
* @return The solver status.
*/
ExitStatus solve(const Options& options = Options{},
[[maybe_unused]] bool spy = false);
/**
* Adds a callback to be called at the beginning of each solver iteration.
*
* The callback for this overload should return void.
*
* @param callback The callback.
*/
template <typename F>
requires requires(F callback, const IterationInfo& info) {
{ callback(info) } -> std::same_as<void>;
}
void add_callback(F&& callback) {
m_iteration_callbacks.emplace_back(
[=, callback = std::forward<F>(callback)](const IterationInfo& info) {
callback(info);
return false;
});
}
/**
* Adds a callback to be called at the beginning of each solver iteration.
*
* The callback for this overload should return bool.
*
* @param callback The callback. Returning true from the callback causes the
* solver to exit early with the solution it has so far.
*/
template <typename F>
requires requires(F callback, const IterationInfo& info) {
{ callback(info) } -> std::same_as<bool>;
}
void add_callback(F&& callback) {
m_iteration_callbacks.emplace_back(std::forward<F>(callback));
}
/**
* Clears the registered callbacks.
*/
void clear_callbacks() { m_iteration_callbacks.clear(); }
private:
// The list of decision variables, which are the root of the problem's
// expression tree
gch::small_vector<Variable> m_decision_variables;
// The cost function: f(x)
std::optional<Variable> m_f;
// The list of equality constraints: cₑ(x) = 0
gch::small_vector<Variable> m_equality_constraints;
// The list of inequality constraints: cᵢ(x) ≥ 0
gch::small_vector<Variable> m_inequality_constraints;
// The iteration callbacks
gch::small_vector<std::function<bool(const IterationInfo& info)>>
m_iteration_callbacks;
void print_exit_conditions([[maybe_unused]] const Options& options);
void print_problem_analysis();
};
} // namespace slp

55
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/InteriorPoint.hpp vendored
View File

@@ -1,55 +0,0 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <span>
#include <Eigen/Core>
#include "sleipnir/autodiff/Variable.hpp"
#include "sleipnir/optimization/SolverConfig.hpp"
#include "sleipnir/optimization/SolverIterationInfo.hpp"
#include "sleipnir/optimization/SolverStatus.hpp"
#include "sleipnir/util/FunctionRef.hpp"
#include "sleipnir/util/SymbolExports.hpp"
namespace sleipnir {
/**
Finds the optimal solution to a nonlinear program using the interior-point
method.
A nonlinear program has the form:
@verbatim
min_x f(x)
subject to cₑ(x) = 0
cᵢ(x) ≥ 0
@endverbatim
where f(x) is the cost function, cₑ(x) are the equality constraints, and cᵢ(x)
are the inequality constraints.
@param[in] decisionVariables The list of decision variables.
@param[in] equalityConstraints The list of equality constraints.
@param[in] inequalityConstraints The list of inequality constraints.
@param[in] f The cost function.
@param[in] callback The user callback.
@param[in] config Configuration options for the solver.
@param[in] feasibilityRestoration Whether to use feasibility restoration instead
of the normal algorithm.
@param[in,out] x The initial guess and output location for the decision
variables.
@param[in,out] s The initial guess and output location for the inequality
constraint slack variables.
@param[out] status The solver status.
*/
SLEIPNIR_DLLEXPORT void InteriorPoint(
std::span<Variable> decisionVariables,
std::span<Variable> equalityConstraints,
std::span<Variable> inequalityConstraints, Variable& f,
function_ref<bool(const SolverIterationInfo& info)> callback,
const SolverConfig& config, bool feasibilityRestoration, Eigen::VectorXd& x,
Eigen::VectorXd& s, SolverStatus* status);
} // namespace sleipnir

46
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/SQP.hpp vendored
View File

@@ -1,46 +0,0 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <span>
#include <Eigen/Core>
#include "sleipnir/autodiff/Variable.hpp"
#include "sleipnir/optimization/SolverConfig.hpp"
#include "sleipnir/optimization/SolverIterationInfo.hpp"
#include "sleipnir/optimization/SolverStatus.hpp"
#include "sleipnir/util/FunctionRef.hpp"
#include "sleipnir/util/SymbolExports.hpp"
namespace sleipnir {
/**
Finds the optimal solution to a nonlinear program using Sequential Quadratic
Programming (SQP).
A nonlinear program has the form:
@verbatim
min_x f(x)
subject to cₑ(x) = 0
@endverbatim
where f(x) is the cost function and cₑ(x) are the equality constraints.
@param[in] decisionVariables The list of decision variables.
@param[in] equalityConstraints The list of equality constraints.
@param[in] f The cost function.
@param[in] callback The user callback.
@param[in] config Configuration options for the solver.
@param[in,out] x The initial guess and output location for the decision
variables.
@param[out] status The solver status.
*/
SLEIPNIR_DLLEXPORT void SQP(
std::span<Variable> decisionVariables,
std::span<Variable> equalityConstraints, Variable& f,
function_ref<bool(const SolverIterationInfo& info)> callback,
const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status);
} // namespace sleipnir

82
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/exit_status.hpp vendored Normal file
View File

@@ -0,0 +1,82 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <stdint.h>
#include <string_view>
#include "sleipnir/util/symbol_exports.hpp"
namespace slp {
/**
* Solver exit status. Negative values indicate failure.
*/
enum class ExitStatus : int8_t {
/// Solved the problem to the desired tolerance.
SUCCESS = 0,
/// The solver returned its solution so far after the user requested a stop.
CALLBACK_REQUESTED_STOP = 1,
/// The solver determined the problem to be overconstrained and gave up.
TOO_FEW_DOFS = -1,
/// The solver determined the problem to be locally infeasible and gave up.
LOCALLY_INFEASIBLE = -2,
/// The problem setup frontend determined the problem to have an empty
/// feasible region.
GLOBALLY_INFEASIBLE = -3,
/// The linear system factorization failed.
FACTORIZATION_FAILED = -4,
/// The backtracking line search failed, and the problem isn't locally
/// infeasible.
LINE_SEARCH_FAILED = -5,
/// The solver encountered nonfinite initial cost or constraints and gave up.
NONFINITE_INITIAL_COST_OR_CONSTRAINTS = -6,
/// The solver encountered diverging primal iterates xₖ and/or sₖ and gave up.
DIVERGING_ITERATES = -7,
/// The solver returned its solution so far after exceeding the maximum number
/// of iterations.
MAX_ITERATIONS_EXCEEDED = -8,
/// The solver returned its solution so far after exceeding the maximum
/// elapsed wall clock time.
TIMEOUT = -9,
};
/**
* Returns user-readable message corresponding to the solver exit status.
*
* @param exit_status Solver exit status.
*/
SLEIPNIR_DLLEXPORT constexpr std::string_view to_message(
const ExitStatus& exit_status) {
using enum ExitStatus;
switch (exit_status) {
case SUCCESS:
return "success";
case CALLBACK_REQUESTED_STOP:
return "callback requested stop";
case TOO_FEW_DOFS:
return "too few degrees of freedom";
case LOCALLY_INFEASIBLE:
return "locally infeasible";
case GLOBALLY_INFEASIBLE:
return "globally infeasible";
case FACTORIZATION_FAILED:
return "factorization failed";
case LINE_SEARCH_FAILED:
return "line search failed";
case NONFINITE_INITIAL_COST_OR_CONSTRAINTS:
return "nonfinite initial cost or constraints";
case DIVERGING_ITERATES:
return "diverging iterates";
case MAX_ITERATIONS_EXCEEDED:
return "max iterations exceeded";
case TIMEOUT:
return "timeout";
default:
return "unknown";
}
}
} // namespace slp

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

@@ -0,0 +1,232 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <functional>
#include <span>
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include "sleipnir/optimization/solver/exit_status.hpp"
#include "sleipnir/optimization/solver/iteration_info.hpp"
#include "sleipnir/optimization/solver/options.hpp"
#include "sleipnir/util/symbol_exports.hpp"
namespace slp {
/**
* Matrix callbacks for the interior-point method solver.
*/
struct SLEIPNIR_DLLEXPORT InteriorPointMatrixCallbacks {
/// Cost function value f(x) getter.
///
/// <table>
/// <tr>
/// <th>Variable</th>
/// <th>Rows</th>
/// <th>Columns</th>
/// </tr>
/// <tr>
/// <td>x</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>f(x)</td>
/// <td>1</td>
/// <td>1</td>
/// </tr>
/// </table>
std::function<double(const Eigen::VectorXd& x)> f;
/// Cost function gradient ∇f(x) getter.
///
/// <table>
/// <tr>
/// <th>Variable</th>
/// <th>Rows</th>
/// <th>Columns</th>
/// </tr>
/// <tr>
/// <td>x</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>∇f(x)</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// </table>
std::function<Eigen::SparseVector<double>(const Eigen::VectorXd& x)> g;
/// Lagrangian Hessian ∇ₓₓ²L(x, y, z) getter.
///
/// L(xₖ, yₖ, zₖ) = f(xₖ) − yₖᵀcₑ(xₖ) − zₖᵀcᵢ(xₖ)
///
/// <table>
/// <tr>
/// <th>Variable</th>
/// <th>Rows</th>
/// <th>Columns</th>
/// </tr>
/// <tr>
/// <td>x</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>y</td>
/// <td>num_equality_constraints</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>z</td>
/// <td>num_inequality_constraints</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>∇ₓₓ²L(x, y, z)</td>
/// <td>num_decision_variables</td>
/// <td>num_decision_variables</td>
/// </tr>
/// </table>
std::function<Eigen::SparseMatrix<double>(const Eigen::VectorXd& x,
const Eigen::VectorXd& y,
const Eigen::VectorXd& z)>
H;
/// Equality constraint value cₑ(x) getter.
///
/// <table>
/// <tr>
/// <th>Variable</th>
/// <th>Rows</th>
/// <th>Columns</th>
/// </tr>
/// <tr>
/// <td>x</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>cₑ(x)</td>
/// <td>num_equality_constraints</td>
/// <td>1</td>
/// </tr>
/// </table>
std::function<Eigen::VectorXd(const Eigen::VectorXd& x)> c_e;
/// Equality constraint Jacobian ∂cₑ/∂x getter.
///
/// @verbatim
/// [∇ᵀcₑ₁(xₖ)]
/// Aₑ(x) = [∇ᵀcₑ₂(xₖ)]
/// [ ⋮ ]
/// [∇ᵀcₑₘ(xₖ)]
/// @endverbatim
///
/// <table>
/// <tr>
/// <th>Variable</th>
/// <th>Rows</th>
/// <th>Columns</th>
/// </tr>
/// <tr>
/// <td>x</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>Aₑ(x)</td>
/// <td>num_equality_constraints</td>
/// <td>num_decision_variables</td>
/// </tr>
/// </table>
std::function<Eigen::SparseMatrix<double>(const Eigen::VectorXd& x)> A_e;
/// Inequality constraint value cᵢ(x) getter.
///
/// <table>
/// <tr>
/// <th>Variable</th>
/// <th>Rows</th>
/// <th>Columns</th>
/// </tr>
/// <tr>
/// <td>x</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>cᵢ(x)</td>
/// <td>num_inequality_constraints</td>
/// <td>1</td>
/// </tr>
/// </table>
std::function<Eigen::VectorXd(const Eigen::VectorXd& x)> c_i;
/// Inequality constraint Jacobian ∂cᵢ/∂x getter.
///
/// @verbatim
/// [∇ᵀcᵢ₁(xₖ)]
/// Aᵢ(x) = [∇ᵀcᵢ₂(xₖ)]
/// [ ⋮ ]
/// [∇ᵀcᵢₘ(xₖ)]
/// @endverbatim
///
/// <table>
/// <tr>
/// <th>Variable</th>
/// <th>Rows</th>
/// <th>Columns</th>
/// </tr>
/// <tr>
/// <td>x</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>Aᵢ(x)</td>
/// <td>num_inequality_constraints</td>
/// <td>num_decision_variables</td>
/// </tr>
/// </table>
std::function<Eigen::SparseMatrix<double>(const Eigen::VectorXd& x)> A_i;
};
/**
Finds the optimal solution to a nonlinear program using the interior-point
method.
A nonlinear program has the form:
@verbatim
min_x f(x)
subject to cₑ(x) = 0
cᵢ(x) ≥ 0
@endverbatim
where f(x) is the cost function, cₑ(x) are the equality constraints, and cᵢ(x)
are the inequality constraints.
@param[in] matrix_callbacks Matrix callbacks.
@param[in] iteration_callbacks The list of callbacks to call at the beginning of
each iteration.
@param[in] options Solver options.
@param[in,out] x The initial guess and output location for the decision
variables.
@return The exit status.
*/
SLEIPNIR_DLLEXPORT ExitStatus
interior_point(const InteriorPointMatrixCallbacks& matrix_callbacks,
std::span<std::function<bool(const IterationInfo& info)>>
iteration_callbacks,
const Options& options,
#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
const Eigen::ArrayX<bool>& bound_constraint_mask,
#endif
Eigen::VectorXd& x);
} // namespace slp

11
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverIterationInfo.hpp → wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/iteration_info.hpp vendored
View File

@@ -5,21 +5,18 @@
#include <Eigen/Core>
#include <Eigen/SparseCore>
namespace sleipnir {
namespace slp {
/**
* Solver iteration information exposed to a user callback.
* Solver iteration information exposed to an iteration callback.
*/
struct SolverIterationInfo {
struct IterationInfo {
/// The solver iteration.
int iteration;
/// The decision variables.
const Eigen::VectorXd& x;
/// The inequality constraint slack variables.
const Eigen::VectorXd& s;
/// The gradient of the cost function.
const Eigen::SparseVector<double>& g;
@@ -33,4 +30,4 @@ struct SolverIterationInfo {
const Eigen::SparseMatrix<double>& A_i;
};
} // namespace sleipnir
} // namespace slp

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

@@ -0,0 +1,113 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <functional>
#include <span>
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include "sleipnir/optimization/solver/exit_status.hpp"
#include "sleipnir/optimization/solver/iteration_info.hpp"
#include "sleipnir/optimization/solver/options.hpp"
#include "sleipnir/util/symbol_exports.hpp"
namespace slp {
/**
* Matrix callbacks for the Newton's method solver.
*/
struct SLEIPNIR_DLLEXPORT NewtonMatrixCallbacks {
/// Cost function value f(x) getter.
///
/// <table>
/// <tr>
/// <th>Variable</th>
/// <th>Rows</th>
/// <th>Columns</th>
/// </tr>
/// <tr>
/// <td>x</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>f(x)</td>
/// <td>1</td>
/// <td>1</td>
/// </tr>
/// </table>
std::function<double(const Eigen::VectorXd& x)> f;
/// Cost function gradient ∇f(x) getter.
///
/// <table>
/// <tr>
/// <th>Variable</th>
/// <th>Rows</th>
/// <th>Columns</th>
/// </tr>
/// <tr>
/// <td>x</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>∇f(x)</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// </table>
std::function<Eigen::SparseVector<double>(const Eigen::VectorXd& x)> g;
/// Lagrangian Hessian ∇ₓₓ²L(x) getter.
///
/// L(xₖ) = f(xₖ)
///
/// <table>
/// <tr>
/// <th>Variable</th>
/// <th>Rows</th>
/// <th>Columns</th>
/// </tr>
/// <tr>
/// <td>x</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>∇ₓₓ²L(x)</td>
/// <td>num_decision_variables</td>
/// <td>num_decision_variables</td>
/// </tr>
/// </table>
std::function<Eigen::SparseMatrix<double>(const Eigen::VectorXd& x)> H;
};
/**
Finds the optimal solution to a nonlinear program using Newton's method.
A nonlinear program has the form:
@verbatim
min_x f(x)
@endverbatim
where f(x) is the cost function.
@param[in] matrix_callbacks Matrix callbacks.
@param[in] iteration_callbacks The list of callbacks to call at the beginning of
each iteration.
@param[in] options Solver options.
@param[in,out] x The initial guess and output location for the decision
variables.
@return The exit status.
*/
SLEIPNIR_DLLEXPORT ExitStatus
newton(const NewtonMatrixCallbacks& matrix_callbacks,
std::span<std::function<bool(const IterationInfo& info)>>
iteration_callbacks,
const Options& options, Eigen::VectorXd& x);
} // namespace slp

94
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/options.hpp vendored Normal file
View File

@@ -0,0 +1,94 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <chrono>
#include <limits>
#include "sleipnir/util/symbol_exports.hpp"
namespace slp {
/**
* Solver options.
*/
struct SLEIPNIR_DLLEXPORT Options {
/// The solver will stop once the error is below this tolerance.
double tolerance = 1e-8;
/// The maximum number of solver iterations before returning a solution.
int max_iterations = 5000;
/// The maximum elapsed wall clock time before returning a solution.
std::chrono::duration<double> timeout{
std::numeric_limits<double>::infinity()};
/// Enables the feasible interior-point method. When the inequality
/// constraints are all feasible, step sizes are reduced when necessary to
/// prevent them becoming infeasible again. This is useful when parts of the
/// problem are ill-conditioned in infeasible regions (e.g., square root of a
/// negative value). This can slow or prevent progress toward a solution
/// though, so only enable it if necessary.
bool feasible_ipm = false;
/// Enables diagnostic prints.
///
/// <table>
/// <tr>
/// <th>Heading</th>
/// <th>Description</th>
/// </tr>
/// <tr>
/// <td>iter</td>
/// <td>Iteration number</td>
/// </tr>
/// <tr>
/// <td>type</td>
/// <td>Iteration type (normal, accepted second-order correction, rejected
/// second-order correction)</td>
/// </tr>
/// <tr>
/// <td>time (ms)</td>
/// <td>Duration of iteration in milliseconds</td>
/// </tr>
/// <tr>
/// <td>error</td>
/// <td>Error estimate</td>
/// </tr>
/// <tr>
/// <td>cost</td>
/// <td>Cost function value at current iterate</td>
/// </tr>
/// <tr>
/// <td>infeas.</td>
/// <td>Constraint infeasibility at current iterate</td>
/// </tr>
/// <tr>
/// <td>complement.</td>
/// <td>Complementary slackness at current iterate (sᵀz)</td>
/// </tr>
/// <tr>
/// <td>μ</td>
/// <td>Barrier parameter</td>
/// </tr>
/// <tr>
/// <td>reg</td>
/// <td>Iteration matrix regularization</td>
/// </tr>
/// <tr>
/// <td>primal α</td>
/// <td>Primal step size</td>
/// </tr>
/// <tr>
/// <td>dual α</td>
/// <td>Dual step size</td>
/// </tr>
/// <tr>
/// <td>↩</td>
/// <td>Number of line search backtracks</td>
/// </tr>
/// </table>
bool diagnostics = false;
};
} // namespace slp

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

@@ -0,0 +1,171 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <functional>
#include <span>
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include "sleipnir/optimization/solver/exit_status.hpp"
#include "sleipnir/optimization/solver/iteration_info.hpp"
#include "sleipnir/optimization/solver/options.hpp"
#include "sleipnir/util/symbol_exports.hpp"
namespace slp {
/**
* Matrix callbacks for the Sequential Quadratic Programming (SQP) solver.
*/
struct SLEIPNIR_DLLEXPORT SQPMatrixCallbacks {
/// Cost function value f(x) getter.
///
/// <table>
/// <tr>
/// <th>Variable</th>
/// <th>Rows</th>
/// <th>Columns</th>
/// </tr>
/// <tr>
/// <td>x</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>f(x)</td>
/// <td>1</td>
/// <td>1</td>
/// </tr>
/// </table>
std::function<double(const Eigen::VectorXd& x)> f;
/// Cost function gradient ∇f(x) getter.
///
/// <table>
/// <tr>
/// <th>Variable</th>
/// <th>Rows</th>
/// <th>Columns</th>
/// </tr>
/// <tr>
/// <td>x</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>∇f(x)</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// </table>
std::function<Eigen::SparseVector<double>(const Eigen::VectorXd& x)> g;
/// Lagrangian Hessian ∇ₓₓ²L(x, y) getter.
///
/// L(xₖ, yₖ) = f(xₖ) − yₖᵀcₑ(xₖ)
///
/// <table>
/// <tr>
/// <th>Variable</th>
/// <th>Rows</th>
/// <th>Columns</th>
/// </tr>
/// <tr>
/// <td>x</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>y</td>
/// <td>num_equality_constraints</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>∇ₓₓ²L(x, y)</td>
/// <td>num_decision_variables</td>
/// <td>num_decision_variables</td>
/// </tr>
/// </table>
std::function<Eigen::SparseMatrix<double>(const Eigen::VectorXd& x,
const Eigen::VectorXd& y)>
H;
/// Equality constraint value cₑ(x) getter.
///
/// <table>
/// <tr>
/// <th>Variable</th>
/// <th>Rows</th>
/// <th>Columns</th>
/// </tr>
/// <tr>
/// <td>x</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>cₑ(x)</td>
/// <td>num_equality_constraints</td>
/// <td>1</td>
/// </tr>
/// </table>
std::function<Eigen::VectorXd(const Eigen::VectorXd& x)> c_e;
/// Equality constraint Jacobian ∂cₑ/∂x getter.
///
/// @verbatim
/// [∇ᵀcₑ₁(xₖ)]
/// Aₑ(x) = [∇ᵀcₑ₂(xₖ)]
/// [ ⋮ ]
/// [∇ᵀcₑₘ(xₖ)]
/// @endverbatim
///
/// <table>
/// <tr>
/// <th>Variable</th>
/// <th>Rows</th>
/// <th>Columns</th>
/// </tr>
/// <tr>
/// <td>x</td>
/// <td>num_decision_variables</td>
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>Aₑ(x)</td>
/// <td>num_equality_constraints</td>
/// <td>num_decision_variables</td>
/// </tr>
/// </table>
std::function<Eigen::SparseMatrix<double>(const Eigen::VectorXd& x)> A_e;
};
/**
Finds the optimal solution to a nonlinear program using Sequential Quadratic
Programming (SQP).
A nonlinear program has the form:
@verbatim
min_x f(x)
subject to cₑ(x) = 0
@endverbatim
where f(x) is the cost function and cₑ(x) are the equality constraints.
@param[in] matrix_callbacks Matrix callbacks.
@param[in] iteration_callbacks The list of callbacks to call at the beginning of
each iteration.
@param[in] options Solver options.
@param[in,out] x The initial guess and output location for the decision
variables.
@return The exit status.
*/
SLEIPNIR_DLLEXPORT ExitStatus
sqp(const SQPMatrixCallbacks& matrix_callbacks,
std::span<std::function<bool(const IterationInfo& info)>>
iteration_callbacks,
const Options& options, Eigen::VectorXd& x);
} // namespace slp

25
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Assert.hpp vendored
View File

@@ -1,25 +0,0 @@
// Copyright (c) Sleipnir contributors
#pragma once
#ifdef JORMUNGANDR
#include <format>
#include <stdexcept>
/**
* Throw an exception in Python.
*/
#define Assert(condition) \
do { \
if (!(condition)) { \
throw std::invalid_argument( \
std::format("{}:{}: {}: Assertion `{}' failed.", __FILE__, __LINE__, \
__func__, #condition)); \
} \
} while (0);
#else
#include <cassert>
/**
* Abort in C++.
*/
#define Assert(condition) assert(condition)
#endif

33
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Concepts.hpp vendored
View File

@@ -1,33 +0,0 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <concepts>
#include <Eigen/Core>
namespace sleipnir {
template <typename T>
concept ScalarLike = requires(T t) {
t + 1.0;
t = 1.0;
};
template <typename T>
concept SleipnirMatrixLike = requires(T t, int rows, int cols) {
t.Rows();
t.Cols();
t(rows, cols);
};
template <typename T>
concept EigenMatrixLike = std::derived_from<T, Eigen::MatrixBase<T>>;
template <typename T>
concept MatrixLike = SleipnirMatrixLike<T> || EigenMatrixLike<T>;
template <typename T>
concept EigenSolver = requires(T t) { t.solve(Eigen::VectorXd{}); };
} // namespace sleipnir

89
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Spy.hpp vendored
View File

@@ -1,89 +0,0 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <fstream>
#include <string>
#include <string_view>
#include <Eigen/SparseCore>
#include <wpi/SmallVector.h>
#include "sleipnir/util/SymbolExports.hpp"
namespace sleipnir {
/**
* Write the sparsity pattern of a sparse matrix to a file.
*
* Each character represents an element with '.' representing zero, '+'
* representing positive, and '-' representing negative. Here's an example for a
* 3x3 identity matrix.
*
* "+.."
* ".+."
* "..+"
*
* @param[out] file A file stream.
* @param[in] mat The sparse matrix.
*/
SLEIPNIR_DLLEXPORT inline void Spy(std::ostream& file,
const Eigen::SparseMatrix<double>& mat) {
const int cells_width = mat.cols() + 1;
const int cells_height = mat.rows();
wpi::SmallVector<uint8_t> cells;
// Allocate space for matrix of characters plus trailing newlines
cells.reserve(cells_width * cells_height);
// Initialize cell array
for (int row = 0; row < mat.rows(); ++row) {
for (int col = 0; col < mat.cols(); ++col) {
cells.emplace_back('.');
}
cells.emplace_back('\n');
}
// Fill in non-sparse entries
for (int k = 0; k < mat.outerSize(); ++k) {
for (Eigen::SparseMatrix<double>::InnerIterator it{mat, k}; it; ++it) {
if (it.value() < 0.0) {
cells[it.row() * cells_width + it.col()] = '-';
} else if (it.value() > 0.0) {
cells[it.row() * cells_width + it.col()] = '+';
}
}
}
// Write cell array to file
for (const auto& c : cells) {
file << c;
}
}
/**
* Write the sparsity pattern of a sparse matrix to a file.
*
* Each character represents an element with "." representing zero, "+"
* representing positive, and "-" representing negative. Here's an example for a
* 3x3 identity matrix.
*
* "+.."
* ".+."
* "..+"
*
* @param[in] filename The filename.
* @param[in] mat The sparse matrix.
*/
SLEIPNIR_DLLEXPORT inline void Spy(std::string_view filename,
const Eigen::SparseMatrix<double>& mat) {
std::ofstream file{std::string{filename}};
if (!file.is_open()) {
return;
}
Spy(file, mat);
}
} // namespace sleipnir

27
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/assert.hpp vendored Normal file
View File

@@ -0,0 +1,27 @@
// Copyright (c) Sleipnir contributors
#pragma once
#ifdef JORMUNGANDR
#include <format>
#include <source_location>
#include <stdexcept>
/**
* Throw an exception in Python.
*/
#define slp_assert(condition) \
do { \
if (!(condition)) { \
auto location = std::source_location::current(); \
throw std::invalid_argument(std::format( \
"{}:{}: {}: Assertion `{}' failed.", location.file_name(), \
location.line(), location.function_name(), #condition)); \
} \
} while (0);
#else
#include <cassert>
/**
* Abort in C++.
*/
#define slp_assert(condition) assert(condition)
#endif

42
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/concepts.hpp vendored Normal file
View File

@@ -0,0 +1,42 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <concepts>
#include <type_traits>
#include <Eigen/Core>
namespace slp {
template <typename T>
concept ScalarLike = requires(std::decay_t<T> t) {
t + 1.0;
t = 1.0;
};
template <typename T>
concept SleipnirScalarLike = requires(std::decay_t<T> t) {
t + 1.0;
t = 1.0;
{ t.value() } -> std::same_as<double>;
};
template <typename T>
concept EigenMatrixLike =
std::derived_from<std::decay_t<T>, Eigen::MatrixBase<std::decay_t<T>>>;
template <typename T>
concept SleipnirMatrixLike = requires(std::decay_t<T> t, int rows, int cols) {
t.rows();
t.cols();
{ t.value() } -> std::same_as<Eigen::MatrixXd>;
} && !EigenMatrixLike<T>;
template <typename T>
concept SleipnirType = SleipnirScalarLike<T> || SleipnirMatrixLike<T>;
template <typename T>
concept MatrixLike = SleipnirMatrixLike<T> || EigenMatrixLike<T>;
} // namespace slp

26
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/FunctionRef.hpp → wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/function_ref.hpp vendored
View File

@@ -7,15 +7,15 @@
#include <type_traits>
#include <utility>
namespace sleipnir {
namespace slp {
template <class F>
class function_ref;
/**
* An implementation of std::function_ref, a lightweight non-owning reference to
* a callable.
*/
template <class F>
class function_ref;
template <class R, class... Args>
class function_ref<R(Args...)> {
public:
@@ -23,12 +23,17 @@ class function_ref<R(Args...)> {
/**
* Creates a `function_ref` which refers to the same callable as `rhs`.
*
* @param rhs Other `function_ref`.
*/
constexpr function_ref(const function_ref<R(Args...)>& rhs) noexcept =
default;
/**
* Constructs a `function_ref` referring to `f`.
*
* @tparam F Callable type.
* @param f Callable to which to refer.
*/
template <typename F>
requires(!std::is_same_v<std::decay_t<F>, function_ref> &&
@@ -45,12 +50,18 @@ class function_ref<R(Args...)> {
/**
* Makes `*this` refer to the same callable as `rhs`.
*
* @param rhs Other `function_ref`.
* @return `*this`
*/
constexpr function_ref<R(Args...)>& operator=(
const function_ref<R(Args...)>& rhs) noexcept = default;
/**
* Makes `*this` refer to `f`.
*
* @param f Callable to which to refer.
* @return `*this`
*/
template <typename F>
requires std::is_invocable_r_v<R, F&&, Args...>
@@ -67,6 +78,8 @@ class function_ref<R(Args...)> {
/**
* Swaps the referred callables of `*this` and `rhs`.
*
* @param rhs Other `function_ref`.
*/
constexpr void swap(function_ref<R(Args...)>& rhs) noexcept {
std::swap(obj_, rhs.obj_);
@@ -75,6 +88,9 @@ class function_ref<R(Args...)> {
/**
* Call the stored callable with the given arguments.
*
* @param args The arguments.
* @return The return value of the callable.
*/
R operator()(Args... args) const {
return callback_(obj_, std::forward<Args>(args)...);
@@ -97,4 +113,4 @@ constexpr void swap(function_ref<R(Args...)>& lhs,
template <typename R, typename... Args>
function_ref(R (*)(Args...)) -> function_ref<R(Args...)>;
} // namespace sleipnir
} // namespace slp

142
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/IntrusiveSharedPtr.hpp → wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/intrusive_shared_ptr.hpp vendored
View File

@@ -2,11 +2,12 @@
#pragma once
#include <concepts>
#include <cstddef>
#include <memory>
#include <utility>
namespace sleipnir {
namespace slp {
/**
* A custom intrusive shared pointer implementation without thread
@@ -16,17 +17,20 @@ namespace sleipnir {
*
* 1. A zero-initialized public counter variable that serves as the shared
* pointer's reference count.
* 2. A free function `void IntrusiveSharedPtrIncRefCount(T*)` that increments
* the reference count.
* 3. A free function `void IntrusiveSharedPtrDecRefCount(T*)` that decrements
* the reference count and deallocates the pointed to object if the reference
* count reaches zero.
* 2. A free function `void inc_ref_count(T*)` that increments the reference
* count.
* 3. A free function `void dec_ref_count(T*)` that decrements the reference
* count and deallocates the pointed to object if the reference count reaches
* zero.
*
* @tparam T The type of the object to be reference counted.
*/
template <typename T>
class IntrusiveSharedPtr {
public:
template <typename>
friend class IntrusiveSharedPtr;
/**
* Constructs an empty intrusive shared pointer.
*/
@@ -40,31 +44,53 @@ class IntrusiveSharedPtr {
/**
* Constructs an intrusive shared pointer from the given pointer and takes
* ownership.
*
* @param ptr The pointer of which to take ownership.
*/
explicit constexpr IntrusiveSharedPtr(T* ptr) noexcept : m_ptr{ptr} {
if (m_ptr != nullptr) {
IntrusiveSharedPtrIncRefCount(m_ptr);
inc_ref_count(m_ptr);
}
}
constexpr ~IntrusiveSharedPtr() {
if (m_ptr != nullptr) {
IntrusiveSharedPtrDecRefCount(m_ptr);
dec_ref_count(m_ptr);
}
}
/**
* Copy constructs from the given intrusive shared pointer.
*
* @param rhs The other intrusive shared pointer.
*/
constexpr IntrusiveSharedPtr(const IntrusiveSharedPtr<T>& rhs) noexcept
: m_ptr{rhs.m_ptr} {
if (m_ptr != nullptr) {
IntrusiveSharedPtrIncRefCount(m_ptr);
inc_ref_count(m_ptr);
}
}
/**
* Copy constructs from the given intrusive shared pointer.
*
* @param rhs The other intrusive shared pointer.
*/
template <typename U>
requires(!std::same_as<T, U> && std::convertible_to<U*, T*>)
constexpr IntrusiveSharedPtr( // NOLINT
const IntrusiveSharedPtr<U>& rhs) noexcept
: m_ptr{rhs.m_ptr} {
if (m_ptr != nullptr) {
inc_ref_count(m_ptr);
}
}
/**
* Makes a copy of the given intrusive shared pointer.
*
* @param rhs The other intrusive shared pointer.
* @return This intrusive shared pointer.
*/
constexpr IntrusiveSharedPtr<T>& operator=( // NOLINT
const IntrusiveSharedPtr<T>& rhs) noexcept {
@@ -73,13 +99,40 @@ class IntrusiveSharedPtr {
}
if (m_ptr != nullptr) {
IntrusiveSharedPtrDecRefCount(m_ptr);
dec_ref_count(m_ptr);
}
m_ptr = rhs.m_ptr;
if (m_ptr != nullptr) {
IntrusiveSharedPtrIncRefCount(m_ptr);
inc_ref_count(m_ptr);
}
return *this;
}
/**
* Makes a copy of the given intrusive shared pointer.
*
* @param rhs The other intrusive shared pointer.
* @return This intrusive shared pointer.
*/
template <typename U>
requires(!std::same_as<T, U> && std::convertible_to<U*, T*>)
constexpr IntrusiveSharedPtr<T>& operator=( // NOLINT
const IntrusiveSharedPtr<U>& rhs) noexcept {
if (m_ptr == rhs.m_ptr) {
return *this;
}
if (m_ptr != nullptr) {
dec_ref_count(m_ptr);
}
m_ptr = rhs.m_ptr;
if (m_ptr != nullptr) {
inc_ref_count(m_ptr);
}
return *this;
@@ -87,12 +140,28 @@ class IntrusiveSharedPtr {
/**
* Move constructs from the given intrusive shared pointer.
*
* @param rhs The other intrusive shared pointer.
*/
constexpr IntrusiveSharedPtr(IntrusiveSharedPtr<T>&& rhs) noexcept
: m_ptr{std::exchange(rhs.m_ptr, nullptr)} {}
/**
* Move constructs from the given intrusive shared pointer.
*
* @param rhs The other intrusive shared pointer.
*/
template <typename U>
requires(!std::same_as<T, U> && std::convertible_to<U*, T*>)
constexpr IntrusiveSharedPtr( // NOLINT
IntrusiveSharedPtr<U>&& rhs) noexcept
: m_ptr{std::exchange(rhs.m_ptr, nullptr)} {}
/**
* Move assigns from the given intrusive shared pointer.
*
* @param rhs The other intrusive shared pointer.
* @return This intrusive shared pointer.
*/
constexpr IntrusiveSharedPtr<T>& operator=(
IntrusiveSharedPtr<T>&& rhs) noexcept {
@@ -106,28 +175,58 @@ class IntrusiveSharedPtr {
}
/**
* Returns the internal pointer.
* Move assigns from the given intrusive shared pointer.
*
* @param rhs The other intrusive shared pointer.
* @return This intrusive shared pointer.
*/
constexpr T* Get() const noexcept { return m_ptr; }
template <typename U>
requires(!std::same_as<T, U> && std::convertible_to<U*, T*>)
constexpr IntrusiveSharedPtr<T>& operator=(
IntrusiveSharedPtr<U>&& rhs) noexcept {
if (m_ptr == rhs.m_ptr) {
return *this;
}
std::swap(m_ptr, rhs.m_ptr);
return *this;
}
/**
* Returns the internal pointer.
*
* @return The internal pointer.
*/
constexpr T* get() const noexcept { return m_ptr; }
/**
* Returns the object pointed to by the internal pointer.
*
* @return The object pointed to by the internal pointer.
*/
constexpr T& operator*() const noexcept { return *m_ptr; }
/**
* Returns the internal pointer.
*
* @return The internal pointer.
*/
constexpr T* operator->() const noexcept { return m_ptr; }
/**
* Returns true if the internal pointer isn't nullptr.
*
* @return True if the internal pointer isn't nullptr.
*/
explicit constexpr operator bool() const noexcept { return m_ptr != nullptr; }
/**
* Returns true if the given intrusive shared pointers point to the same
* object.
*
* @param lhs The left-hand side.
* @param rhs The right-hand side.
*/
friend constexpr bool operator==(const IntrusiveSharedPtr<T>& lhs,
const IntrusiveSharedPtr<T>& rhs) noexcept {
@@ -137,6 +236,9 @@ class IntrusiveSharedPtr {
/**
* Returns true if the given intrusive shared pointers point to different
* objects.
*
* @param lhs The left-hand side.
* @param rhs The right-hand side.
*/
friend constexpr bool operator!=(const IntrusiveSharedPtr<T>& lhs,
const IntrusiveSharedPtr<T>& rhs) noexcept {
@@ -145,6 +247,8 @@ class IntrusiveSharedPtr {
/**
* Returns true if the left-hand intrusive shared pointer points to nullptr.
*
* @param lhs The left-hand side.
*/
friend constexpr bool operator==(const IntrusiveSharedPtr<T>& lhs,
std::nullptr_t) noexcept {
@@ -153,6 +257,8 @@ class IntrusiveSharedPtr {
/**
* Returns true if the right-hand intrusive shared pointer points to nullptr.
*
* @param rhs The right-hand side.
*/
friend constexpr bool operator==(std::nullptr_t,
const IntrusiveSharedPtr<T>& rhs) noexcept {
@@ -162,6 +268,8 @@ class IntrusiveSharedPtr {
/**
* Returns true if the left-hand intrusive shared pointer doesn't point to
* nullptr.
*
* @param lhs The left-hand side.
*/
friend constexpr bool operator!=(const IntrusiveSharedPtr<T>& lhs,
std::nullptr_t) noexcept {
@@ -171,6 +279,8 @@ class IntrusiveSharedPtr {
/**
* Returns true if the right-hand intrusive shared pointer doesn't point to
* nullptr.
*
* @param rhs The right-hand side.
*/
friend constexpr bool operator!=(std::nullptr_t,
const IntrusiveSharedPtr<T>& rhs) noexcept {
@@ -190,7 +300,7 @@ class IntrusiveSharedPtr {
* @param args Constructor arguments for T.
*/
template <typename T, typename... Args>
IntrusiveSharedPtr<T> MakeIntrusiveShared(Args&&... args) {
IntrusiveSharedPtr<T> make_intrusive_shared(Args&&... args) {
return IntrusiveSharedPtr<T>{new T(std::forward<Args>(args)...)};
}
@@ -206,11 +316,11 @@ IntrusiveSharedPtr<T> MakeIntrusiveShared(Args&&... args) {
* @param args Constructor arguments for T.
*/
template <typename T, typename Alloc, typename... Args>
IntrusiveSharedPtr<T> AllocateIntrusiveShared(Alloc alloc, Args&&... args) {
IntrusiveSharedPtr<T> allocate_intrusive_shared(Alloc alloc, Args&&... args) {
auto ptr = std::allocator_traits<Alloc>::allocate(alloc, sizeof(T));
std::allocator_traits<Alloc>::construct(alloc, ptr,
std::forward<Args>(args)...);
return IntrusiveSharedPtr<T>{ptr};
}
} // namespace sleipnir
} // namespace slp

93
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Pool.hpp → wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/pool.hpp vendored
View File

@@ -5,11 +5,11 @@
#include <cstddef>
#include <memory>
#include <wpi/SmallVector.h>
#include <gch/small_vector.hpp>
#include "sleipnir/util/SymbolExports.hpp"
#include "sleipnir/util/symbol_exports.hpp"
namespace sleipnir {
namespace slp {
/**
* This class implements a pool memory resource.
@@ -23,14 +23,33 @@ class SLEIPNIR_DLLEXPORT PoolResource {
/**
* Constructs a default PoolResource.
*
* @param blocksPerChunk Number of blocks per chunk of memory.
* @param blocks_per_chunk Number of blocks per chunk of memory.
*/
explicit PoolResource(size_t blocksPerChunk)
: blocksPerChunk{blocksPerChunk} {}
explicit PoolResource(size_t blocks_per_chunk)
: blocks_per_chunk{blocks_per_chunk} {}
/**
* Copy constructor.
*/
PoolResource(const PoolResource&) = delete;
/**
* Copy assignment operator.
*
* @return This pool resource.
*/
PoolResource& operator=(const PoolResource&) = delete;
/**
* Move constructor.
*/
PoolResource(PoolResource&&) = default;
/**
* Move assignment operator.
*
* @return This pool resource.
*/
PoolResource& operator=(PoolResource&&) = default;
/**
@@ -38,16 +57,17 @@ class SLEIPNIR_DLLEXPORT PoolResource {
*
* @param bytes Number of bytes in the block.
* @param alignment Alignment of the block (unused).
* @return A block of memory from the pool.
*/
[[nodiscard]]
void* allocate(size_t bytes, [[maybe_unused]] size_t alignment =
alignof(std::max_align_t)) {
if (m_freeList.empty()) {
AddChunk(bytes);
if (m_free_list.empty()) {
add_chunk(bytes);
}
auto ptr = m_freeList.back();
m_freeList.pop_back();
auto ptr = m_free_list.back();
m_free_list.pop_back();
return ptr;
}
@@ -61,11 +81,14 @@ class SLEIPNIR_DLLEXPORT PoolResource {
void deallocate(
void* p, [[maybe_unused]] size_t bytes,
[[maybe_unused]] size_t alignment = alignof(std::max_align_t)) {
m_freeList.emplace_back(p);
m_free_list.emplace_back(p);
}
/**
* Returns true if this pool resource has the same backing storage as another.
*
* @param other The other pool resource.
* @return True if this pool resource has the same backing storage as another.
*/
bool is_equal(const PoolResource& other) const noexcept {
return this == &other;
@@ -73,26 +96,28 @@ class SLEIPNIR_DLLEXPORT PoolResource {
/**
* Returns the number of blocks from this pool resource that are in use.
*
* @return The number of blocks from this pool resource that are in use.
*/
size_t blocks_in_use() const noexcept {
return m_buffer.size() * blocksPerChunk - m_freeList.size();
return m_buffer.size() * blocks_per_chunk - m_free_list.size();
}
private:
wpi::SmallVector<std::unique_ptr<std::byte[]>> m_buffer;
wpi::SmallVector<void*> m_freeList;
size_t blocksPerChunk;
gch::small_vector<std::unique_ptr<std::byte[]>> m_buffer;
gch::small_vector<void*> m_free_list;
size_t blocks_per_chunk;
/**
* Adds a memory chunk to the pool, partitions it into blocks with the given
* number of bytes, and appends pointers to them to the free list.
*
* @param bytesPerBlock Number of bytes in the block.
* @param bytes_per_block Number of bytes in the block.
*/
void AddChunk(size_t bytesPerBlock) {
m_buffer.emplace_back(new std::byte[bytesPerBlock * blocksPerChunk]);
for (int i = blocksPerChunk - 1; i >= 0; --i) {
m_freeList.emplace_back(m_buffer.back().get() + bytesPerBlock * i);
void add_chunk(size_t bytes_per_block) {
m_buffer.emplace_back(new std::byte[bytes_per_block * blocks_per_chunk]);
for (int i = blocks_per_chunk - 1; i >= 0; --i) {
m_free_list.emplace_back(m_buffer.back().get() + bytes_per_block * i);
}
}
};
@@ -115,19 +140,29 @@ class PoolAllocator {
*
* @param r The pool resource.
*/
explicit constexpr PoolAllocator(PoolResource* r) : m_memoryResource{r} {}
explicit constexpr PoolAllocator(PoolResource* r) : m_memory_resource{r} {}
constexpr PoolAllocator(const PoolAllocator<T>& other) = default;
/**
* Copy constructor.
*/
constexpr PoolAllocator(const PoolAllocator<T>&) = default;
/**
* Copy assignment operator.
*
* @return This pool allocator.
*/
constexpr PoolAllocator<T>& operator=(const PoolAllocator<T>&) = default;
/**
* Returns a block of memory from the pool.
*
* @param n Number of bytes in the block.
* @return A block of memory from the pool.
*/
[[nodiscard]]
constexpr T* allocate(size_t n) {
return static_cast<T*>(m_memoryResource->allocate(n));
return static_cast<T*>(m_memory_resource->allocate(n));
}
/**
@@ -137,17 +172,17 @@ class PoolAllocator {
* @param n Number of bytes in the block.
*/
constexpr void deallocate(T* p, size_t n) {
m_memoryResource->deallocate(p, n);
m_memory_resource->deallocate(p, n);
}
private:
PoolResource* m_memoryResource;
PoolResource* m_memory_resource;
};
/**
* Returns a global pool memory resource.
*/
SLEIPNIR_DLLEXPORT PoolResource& GlobalPoolResource();
SLEIPNIR_DLLEXPORT PoolResource& global_pool_resource();
/**
* Returns an allocator for a global pool memory resource.
@@ -155,8 +190,8 @@ SLEIPNIR_DLLEXPORT PoolResource& GlobalPoolResource();
* @tparam T The type of object in the pool.
*/
template <typename T>
PoolAllocator<T> GlobalPoolAllocator() {
return PoolAllocator<T>{&GlobalPoolResource()};
PoolAllocator<T> global_pool_allocator() {
return PoolAllocator<T>{&global_pool_resource()};
}
} // namespace sleipnir
} // namespace slp

19
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Print.hpp → wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/print.hpp vendored
View File

@@ -2,6 +2,7 @@
#pragma once
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
#include <cstdio>
#include <system_error>
#include <utility>
@@ -12,7 +13,11 @@
#include <fmt/core.h>
#endif
namespace sleipnir {
#endif
namespace slp {
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
/**
* Wrapper around fmt::print() that squelches write failure exceptions.
@@ -58,4 +63,14 @@ inline void println(std::FILE* f, fmt::format_string<T...> fmt, T&&... args) {
}
}
} // namespace sleipnir
#else
template <typename... Args>
inline void print([[maybe_unused]] Args&&... args) {}
template <typename... Args>
inline void println([[maybe_unused]] Args&&... args) {}
#endif
} // namespace slp

127
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/spy.hpp vendored Normal file
View File

@@ -0,0 +1,127 @@
// Copyright (c) Sleipnir contributors
#pragma once
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
#include <stdint.h>
#include <bit>
#include <fstream>
#include <string>
#include <string_view>
#include <Eigen/SparseCore>
#include <wpi/bit.h>
#include "sleipnir/util/symbol_exports.hpp"
namespace slp {
/**
* Writes the sparsity pattern of a sparse matrix to a file.
*
* Each file represents the sparsity pattern of one matrix over time. <a
* href="https://github.com/SleipnirGroup/Sleipnir/blob/main/tools/spy.py">spy.py</a>
* can display it as an animation.
*
* The file starts with the following header:
* <ol>
* <li>Plot title (length as a little-endian int32, then characters)</li>
* <li>Row label (length as a little-endian int32, then characters)</li>
* <li>Column label (length as a little-endian int32, then characters)</li>
* </ol>
*
* Then, each sparsity pattern starts with:
* <ol>
* <li>Number of coordinates as a little-endian int32</li>
* </ol>
*
* followed by that many coordinates in the following format:
* <ol>
* <li>Row index as a little-endian int32</li>
* <li>Column index as a little-endian int32</li>
* <li>Sign as a character ('+' for positive, '-' for negative, or '0' for
* zero)</li>
* </ol>
*
* @param[out] file A file stream.
* @param[in] mat The sparse matrix.
*/
class SLEIPNIR_DLLEXPORT Spy {
public:
/**
* Constructs a Spy instance.
*
* @param filename The filename.
* @param title Plot title.
* @param row_label Row label.
* @param col_label Column label.
* @param rows The sparse matrix's number of rows.
* @param cols The sparse matrix's number of columns.
*/
Spy(std::string_view filename, std::string_view title,
std::string_view row_label, std::string_view col_label, int rows,
int cols)
: m_file{std::string{filename}, std::ios::binary} {
// Write title
write32le(title.size());
m_file.write(title.data(), title.size());
// Write row label
write32le(row_label.size());
m_file.write(row_label.data(), row_label.size());
// Write column label
write32le(col_label.size());
m_file.write(col_label.data(), col_label.size());
// Write row and column counts
write32le(rows);
write32le(cols);
}
/**
* Adds a matrix to the file.
*
* @param mat The matrix.
*/
void add(const Eigen::SparseMatrix<double>& mat) {
// Write number of coordinates
write32le(mat.nonZeros());
// Write coordinates
for (int k = 0; k < mat.outerSize(); ++k) {
for (Eigen::SparseMatrix<double>::InnerIterator it{mat, k}; it; ++it) {
write32le(it.row());
write32le(it.col());
if (it.value() > 0.0) {
m_file << '+';
} else if (it.value() < 0.0) {
m_file << '-';
} else {
m_file << '0';
}
}
}
}
private:
std::ofstream m_file;
/**
* Writes a 32-bit signed integer to the file as little-endian.
*
* @param num A 32-bit signed integer.
*/
void write32le(int32_t num) {
if constexpr (std::endian::native != std::endian::little) {
num = wpi::byteswap(num);
}
m_file.write(reinterpret_cast<char*>(&num), sizeof(num));
}
};
} // namespace slp
#endif

0
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/SymbolExports.hpp → wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/symbol_exports.hpp vendored
View File

2
wpimath/src/main/native/thirdparty/sleipnir/src/.styleguide vendored
View File

@@ -8,5 +8,5 @@ cppSrcFileInclude {
includeOtherLibs {
^Eigen/
^wpi/
^gch/
}

91
wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/VariableMatrix.cpp vendored
View File

@@ -1,91 +0,0 @@
// Copyright (c) Sleipnir contributors
#include "sleipnir/autodiff/VariableMatrix.hpp"
#include <Eigen/QR>
namespace sleipnir {
VariableMatrix Solve(const VariableMatrix& A, const VariableMatrix& B) {
// m x n * n x p = m x p
Assert(A.Rows() == B.Rows());
if (A.Rows() == 1 && A.Cols() == 1) {
// Compute optimal inverse instead of using Eigen's general solver
return B(0, 0) / A(0, 0);
} else if (A.Rows() == 2 && A.Cols() == 2) {
// Compute optimal inverse instead of using Eigen's general solver
//
// [a b]⁻¹ ___1___ [ d −b]
// [c d] = ad − bc [−c a]
const auto& a = A(0, 0);
const auto& b = A(0, 1);
const auto& c = A(1, 0);
const auto& d = A(1, 1);
sleipnir::VariableMatrix Ainv{{d, -b}, {-c, a}};
auto detA = a * d - b * c;
Ainv /= detA;
return Ainv * B;
} else if (A.Rows() == 3 && A.Cols() == 3) {
// Compute optimal inverse instead of using Eigen's general solver
//
// [a b c]⁻¹
// [d e f]
// [g h i]
// 1 [ei − fh ch − bi bf − ce]
// = --------------------------------- [fg − di ai − cg cd − af]
// aei − afh − bdi + bfg + cdh − ceg [dh − eg bg − ah ae − bd]
const auto& a = A(0, 0);
const auto& b = A(0, 1);
const auto& c = A(0, 2);
const auto& d = A(1, 0);
const auto& e = A(1, 1);
const auto& f = A(1, 2);
const auto& g = A(2, 0);
const auto& h = A(2, 1);
const auto& i = A(2, 2);
sleipnir::VariableMatrix Ainv{
{e * i - f * h, c * h - b * i, b * f - c * e},
{f * g - d * i, a * i - c * g, c * d - a * f},
{d * h - e * g, b * g - a * h, a * e - b * d}};
auto detA =
a * e * i - a * f * h - b * d * i + b * f * g + c * d * h - c * e * g;
Ainv /= detA;
return Ainv * B;
} else {
using MatrixXv = Eigen::Matrix<Variable, Eigen::Dynamic, Eigen::Dynamic>;
MatrixXv eigenA{A.Rows(), A.Cols()};
for (int row = 0; row < A.Rows(); ++row) {
for (int col = 0; col < A.Cols(); ++col) {
eigenA(row, col) = A(row, col);
}
}
MatrixXv eigenB{B.Rows(), B.Cols()};
for (int row = 0; row < B.Rows(); ++row) {
for (int col = 0; col < B.Cols(); ++col) {
eigenB(row, col) = B(row, col);
}
}
MatrixXv eigenX = eigenA.householderQr().solve(eigenB);
VariableMatrix X{A.Cols(), B.Cols()};
for (int row = 0; row < X.Rows(); ++row) {
for (int col = 0; col < X.Cols(); ++col) {
X(row, col) = eigenX(row, col);
}
}
return X;
}
}
} // namespace sleipnir

259
wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/variable_matrix.cpp vendored Normal file
View File

@@ -0,0 +1,259 @@
// Copyright (c) Sleipnir contributors
#include "sleipnir/autodiff/variable_matrix.hpp"
#include <Eigen/QR>
namespace slp {
VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
// m x n * n x p = m x p
slp_assert(A.rows() == B.rows());
if (A.rows() == 1 && A.cols() == 1) {
// Compute optimal inverse instead of using Eigen's general solver
return B(0, 0) / A(0, 0);
} else if (A.rows() == 2 && A.cols() == 2) {
// Compute optimal inverse instead of using Eigen's general solver
//
// [a b]⁻¹ ___1___ [ d −b]
// [c d] = ad − bc [−c a]
const auto& a = A(0, 0);
const auto& b = A(0, 1);
const auto& c = A(1, 0);
const auto& d = A(1, 1);
slp::VariableMatrix adj_A{{d, -b}, {-c, a}};
auto det_A = a * d - b * c;
return adj_A / det_A * B;
} else if (A.rows() == 3 && A.cols() == 3) {
// Compute optimal inverse instead of using Eigen's general solver
//
// [a b c]⁻¹
// [d e f]
// [g h i]
// 1 [ei − fh ch − bi bf − ce]
// = ------------------------------------ [fg − di ai − cg cd − af]
// a(ei − fh) + b(fg − di) + c(dh − eg) [dh − eg bg − ah ae − bd]
//
// https://www.wolframalpha.com/input?i=inverse+%7B%7Ba%2C+b%2C+c%7D%2C+%7Bd%2C+e%2C+f%7D%2C+%7Bg%2C+h%2C+i%7D%7D
const auto& a = A(0, 0);
const auto& b = A(0, 1);
const auto& c = A(0, 2);
const auto& d = A(1, 0);
const auto& e = A(1, 1);
const auto& f = A(1, 2);
const auto& g = A(2, 0);
const auto& h = A(2, 1);
const auto& i = A(2, 2);
auto ae = a * e;
auto af = a * f;
auto ah = a * h;
auto ai = a * i;
auto bd = b * d;
auto bf = b * f;
auto bg = b * g;
auto bi = b * i;
auto cd = c * d;
auto ce = c * e;
auto cg = c * g;
auto ch = c * h;
auto dh = d * h;
auto di = d * i;
auto eg = e * g;
auto ei = e * i;
auto fg = f * g;
auto fh = f * h;
auto adj_A00 = ei - fh;
auto adj_A10 = fg - di;
auto adj_A20 = dh - eg;
slp::VariableMatrix adj_A{{adj_A00, ch - bi, bf - ce},
{adj_A10, ai - cg, cd - af},
{adj_A20, bg - ah, ae - bd}};
auto det_A = a * adj_A00 + b * adj_A10 + c * adj_A20;
return adj_A / det_A * B;
} else if (A.rows() == 4 && A.cols() == 4) {
// Compute optimal inverse instead of using Eigen's general solver
//
// [a b c d]⁻¹
// [e f g h]
// [i j k l]
// [m n o p]
//
// https://www.wolframalpha.com/input?i=inverse+%7B%7Ba%2C+b%2C+c%2C+d%7D%2C+%7Be%2C+f%2C+g%2C+h%7D%2C+%7Bi%2C+j%2C+k%2C+l%7D%2C+%7Bm%2C+n%2C+o%2C+p%7D%7D
const auto& a = A(0, 0);
const auto& b = A(0, 1);
const auto& c = A(0, 2);
const auto& d = A(0, 3);
const auto& e = A(1, 0);
const auto& f = A(1, 1);
const auto& g = A(1, 2);
const auto& h = A(1, 3);
const auto& i = A(2, 0);
const auto& j = A(2, 1);
const auto& k = A(2, 2);
const auto& l = A(2, 3);
const auto& m = A(3, 0);
const auto& n = A(3, 1);
const auto& o = A(3, 2);
const auto& p = A(3, 3);
auto afk = a * f * k;
auto afl = a * f * l;
auto afo = a * f * o;
auto afp = a * f * p;
auto agj = a * g * j;
auto agl = a * g * l;
auto agn = a * g * n;
auto agp = a * g * p;
auto ahj = a * h * j;
auto ahk = a * h * k;
auto ahn = a * h * n;
auto aho = a * h * o;
auto ajo = a * j * o;
auto ajp = a * j * p;
auto akn = a * k * n;
auto akp = a * k * p;
auto aln = a * l * n;
auto alo = a * l * o;
auto bek = b * e * k;
auto bel = b * e * l;
auto beo = b * e * o;
auto bep = b * e * p;
auto bgi = b * g * i;
auto bgl = b * g * l;
auto bgm = b * g * m;
auto bgp = b * g * p;
auto bhi = b * h * i;
auto bhk = b * h * k;
auto bhm = b * h * m;
auto bho = b * h * o;
auto bio = b * i * o;
auto bip = b * i * p;
auto bjp = b * j * p;
auto bkm = b * k * m;
auto bkp = b * k * p;
auto blm = b * l * m;
auto blo = b * l * o;
auto cej = c * e * j;
auto cel = c * e * l;
auto cen = c * e * n;
auto cep = c * e * p;
auto cfi = c * f * i;
auto cfl = c * f * l;
auto cfm = c * f * m;
auto cfp = c * f * p;
auto chi = c * h * i;
auto chj = c * h * j;
auto chm = c * h * m;
auto chn = c * h * n;
auto cin = c * i * n;
auto cip = c * i * p;
auto cjm = c * j * m;
auto cjp = c * j * p;
auto clm = c * l * m;
auto cln = c * l * n;
auto dej = d * e * j;
auto dek = d * e * k;
auto den = d * e * n;
auto deo = d * e * o;
auto dfi = d * f * i;
auto dfk = d * f * k;
auto dfm = d * f * m;
auto dfo = d * f * o;
auto dgi = d * g * i;
auto dgj = d * g * j;
auto dgm = d * g * m;
auto dgn = d * g * n;
auto din = d * i * n;
auto dio = d * i * o;
auto djm = d * j * m;
auto djo = d * j * o;
auto dkm = d * k * m;
auto dkn = d * k * n;
auto ejo = e * j * o;
auto ejp = e * j * p;
auto ekn = e * k * n;
auto ekp = e * k * p;
auto eln = e * l * n;
auto elo = e * l * o;
auto fio = f * i * o;
auto fip = f * i * p;
auto fkm = f * k * m;
auto fkp = f * k * p;
auto flm = f * l * m;
auto flo = f * l * o;
auto gin = g * i * n;
auto gip = g * i * p;
auto gjm = g * j * m;
auto gjp = g * j * p;
auto glm = g * l * m;
auto gln = g * l * n;
auto hin = h * i * n;
auto hio = h * i * o;
auto hjm = h * j * m;
auto hjo = h * j * o;
auto hkm = h * k * m;
auto hkn = h * k * n;
auto adj_A00 = fkp - flo - gjp + gln + hjo - hkn;
auto adj_A01 = -bkp + blo + cjp - cln - djo + dkn;
auto adj_A02 = bgp - bho - cfp + chn + dfo - dgn;
auto adj_A03 = -bgl + bhk + cfl - chj - dfk + dgj;
auto adj_A10 = -ekp + elo + gip - glm - hio + hkm;
auto adj_A11 = akp - alo - cip + clm + dio - dkm;
auto adj_A12 = -agp + aho + cep - chm - deo + dgm;
auto adj_A13 = agl - ahk - cel + chi + dek - dgi;
auto adj_A20 = ejp - eln - fip + flm + hin - hjm;
auto adj_A21 = -ajp + aln + bip - blm - din + djm;
auto adj_A22 = afp - ahn - bep + bhm + den - dfm;
auto adj_A23 = -afl + ahj + bel - bhi - dej + dfi;
auto adj_A30 = -ejo + ekn + fio - fkm - gin + gjm;
// NOLINTNEXTLINE
auto adj_A31 = ajo - akn - bio + bkm + cin - cjm;
auto adj_A32 = -afo + agn + beo - bgm - cen + cfm;
auto adj_A33 = afk - agj - bek + bgi + cej - cfi;
slp::VariableMatrix adj_A{{adj_A00, adj_A01, adj_A02, adj_A03},
{adj_A10, adj_A11, adj_A12, adj_A13},
{adj_A20, adj_A21, adj_A22, adj_A23},
{adj_A30, adj_A31, adj_A32, adj_A33}};
auto det_A = a * adj_A00 + b * adj_A10 + c * adj_A20 + d * adj_A30;
return adj_A / det_A * B;
} else {
using MatrixXv = Eigen::Matrix<Variable, Eigen::Dynamic, Eigen::Dynamic>;
MatrixXv eigen_A{A.rows(), A.cols()};
for (int row = 0; row < A.rows(); ++row) {
for (int col = 0; col < A.cols(); ++col) {
eigen_A(row, col) = A(row, col);
}
}
MatrixXv eigen_B{B.rows(), B.cols()};
for (int row = 0; row < B.rows(); ++row) {
for (int col = 0; col < B.cols(); ++col) {
eigen_B(row, col) = B(row, col);
}
}
MatrixXv eigen_X = eigen_A.householderQr().solve(eigen_B);
VariableMatrix X{A.cols(), B.cols()};
for (int row = 0; row < X.rows(); ++row) {
for (int col = 0; col < X.cols(); ++col) {
X(row, col) = eigen_X(row, col);
}
}
return X;
}
}
} // namespace slp

58
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/Inertia.hpp vendored
View File

@@ -1,58 +0,0 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <cstddef>
#include "sleipnir/util/Concepts.hpp"
namespace sleipnir {
/**
* Represents the inertia of a matrix (the number of positive, negative, and
* zero eigenvalues).
*/
class Inertia {
public:
size_t positive = 0;
size_t negative = 0;
size_t zero = 0;
constexpr Inertia() = default;
/**
* Constructs the Inertia type with the given number of positive, negative,
* and zero eigenvalues.
*
* @param positive The number of positive eigenvalues.
* @param negative The number of negative eigenvalues.
* @param zero The number of zero eigenvalues.
*/
constexpr Inertia(size_t positive, size_t negative, size_t zero)
: positive{positive}, negative{negative}, zero{zero} {}
/**
* Constructs the Inertia type with the inertia of the given LDLT
* decomposition.
*
* @tparam Solver Eigen sparse linear system solver.
* @param solver The LDLT decomposition of which to compute the inertia.
*/
template <EigenSolver Solver>
explicit Inertia(const Solver& solver) {
const auto& D = solver.vectorD();
for (int row = 0; row < D.rows(); ++row) {
if (D(row) > 0.0) {
++positive;
} else if (D(row) < 0.0) {
++negative;
} else {
++zero;
}
}
}
bool operator==(const Inertia&) const = default;
};
} // namespace sleipnir

179
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/RegularizedLDLT.hpp vendored
View File

@@ -1,179 +0,0 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <cmath>
#include <cstddef>
#include <Eigen/Core>
#include <Eigen/SparseCholesky>
#include <Eigen/SparseCore>
#include "optimization/Inertia.hpp"
// See docs/algorithms.md#Works_cited for citation definitions
namespace sleipnir {
/**
* Solves systems of linear equations using a regularized LDLT factorization.
*/
class RegularizedLDLT {
public:
using Solver = Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>,
Eigen::Lower, Eigen::AMDOrdering<int>>;
/**
* Constructs a RegularizedLDLT instance.
*/
RegularizedLDLT() = default;
/**
* Reports whether previous computation was successful.
*/
Eigen::ComputationInfo Info() { return m_info; }
/**
* Computes the regularized LDLT factorization of a matrix.
*
* @param lhs Left-hand side of the system.
* @param numEqualityConstraints The number of equality constraints in the
* system.
* @param μ The barrier parameter for the current interior-point iteration.
*/
void Compute(const Eigen::SparseMatrix<double>& lhs,
size_t numEqualityConstraints, double μ) {
// The regularization procedure is based on algorithm B.1 of [1]
m_numDecisionVariables = lhs.rows() - numEqualityConstraints;
m_numEqualityConstraints = numEqualityConstraints;
const Inertia idealInertia{m_numDecisionVariables, m_numEqualityConstraints,
0};
Inertia inertia;
double δ = 0.0;
double γ = 0.0;
AnalyzePattern(lhs);
m_solver.factorize(lhs);
if (m_solver.info() == Eigen::Success) {
inertia = Inertia{m_solver};
// If the inertia is ideal, don't regularize the system
if (inertia == idealInertia) {
m_info = Eigen::Success;
return;
}
}
// If the decomposition succeeded and the inertia has some zero eigenvalues,
// or the decomposition failed, regularize the equality constraints
if ((m_solver.info() == Eigen::Success && inertia.zero > 0) ||
m_solver.info() != Eigen::Success) {
γ = 1e-8 * std::pow(μ, 0.25);
}
// Also regularize the Hessian. If the Hessian wasn't regularized in a
// previous run of Compute(), start at a small value of δ. Otherwise,
// attempt a δ half as big as the previous run so δ can trend downwards over
// time.
if (m_δOld == 0.0) {
δ = 1e-4;
} else {
δ = m_δOld / 2.0;
}
while (true) {
// Regularize lhs by adding a multiple of the identity matrix
//
// lhs = [H + AᵢᵀΣAᵢ + δI Aₑᵀ]
// [ Aₑ −γI ]
Eigen::SparseMatrix<double> lhsReg = lhs + Regularization(δ, γ);
AnalyzePattern(lhsReg);
m_solver.factorize(lhsReg);
inertia = Inertia{m_solver};
// If the inertia is ideal, store that value of δ and return.
// Otherwise, increase δ by an order of magnitude and try again.
if (inertia == idealInertia) {
m_δOld = δ;
m_info = Eigen::Success;
return;
} else {
δ *= 10.0;
// If the Hessian perturbation is too high, report failure. This can
// happen due to a rank-deficient equality constraint Jacobian with
// linearly dependent constraints.
if (δ > 1e20) {
m_info = Eigen::NumericalIssue;
return;
}
}
}
}
/**
* Solve the system of equations using a regularized LDLT factorization.
*
* @param rhs Right-hand side of the system.
*/
template <typename Rhs>
auto Solve(const Eigen::MatrixBase<Rhs>& rhs) {
return m_solver.solve(rhs);
}
private:
Solver m_solver;
Eigen::ComputationInfo m_info = Eigen::Success;
/// The number of decision variables in the system.
size_t m_numDecisionVariables = 0;
/// The number of equality constraints in the system.
size_t m_numEqualityConstraints = 0;
/// The value of δ from the previous run of Compute().
double m_δOld = 0.0;
// Number of non-zeros in LHS.
int m_nonZeros = -1;
/**
* Reanalize LHS matrix's sparsity pattern if it changed.
*
* @param lhs Matrix to analyze.
*/
void AnalyzePattern(const Eigen::SparseMatrix<double>& lhs) {
int nonZeros = lhs.nonZeros();
if (m_nonZeros != nonZeros) {
m_solver.analyzePattern(lhs);
m_nonZeros = nonZeros;
}
}
/**
* Returns regularization matrix.
*
* @param δ The Hessian regularization factor.
* @param γ The equality constraint Jacobian regularization factor.
*/
Eigen::SparseMatrix<double> Regularization(double δ, double γ) {
Eigen::VectorXd vec{m_numDecisionVariables + m_numEqualityConstraints};
size_t row = 0;
while (row < m_numDecisionVariables) {
vec(row) = δ;
++row;
}
while (row < m_numDecisionVariables + m_numEqualityConstraints) {
vec(row) = -γ;
++row;
}
return Eigen::SparseMatrix<double>{vec.asDiagonal()};
}
};
} // namespace sleipnir

220
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/bounds.hpp vendored Normal file
View File

@@ -0,0 +1,220 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <algorithm>
#include <limits>
#include <span>
#include <utility>
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <gch/small_vector.hpp>
#include "sleipnir/autodiff/expression_type.hpp"
#include "sleipnir/autodiff/variable.hpp"
#include "sleipnir/util/assert.hpp"
// See docs/algorithms.md#Works_cited for citation definitions
namespace slp {
struct Bounds {
/// Which constraints, if any, are bound constraints.
Eigen::ArrayX<bool> bound_constraint_mask;
/// The tightest bounds on each decision variable.
gch::small_vector<std::pair<double, double>> bounds;
/// Whether or not the constraints are feasible (given previously encountered
/// bounds).
gch::small_vector<std::pair<Eigen::Index, Eigen::Index>>
conflicting_bound_indices;
};
/**
* A "bound constraint" is any linear constraint in one scalar variable.
*
* Computes which constraints, if any, are bound constraints, the tightest
* bounds on each decision variable, and whether or not they're feasible (given
* previously encountered bounds),
*
* @param decision_variables Decision variables corresponding to each column of
* A_i.
* @param inequality_constraints Variables representing the left-hand side of
* cᵢ(decision_variables) ≥ 0.
* @param A_i The Jacobian of inequality_constraints wrt decision_variables,
* evaluated anywhere, in *row-major* storage; in practice, since we typically
* store Jacobians column-major, the user of this function must perform a
* transpose.
*/
inline Bounds get_bounds(
std::span<Variable> decision_variables,
std::span<Variable> inequality_constraints,
const Eigen::SparseMatrix<double, Eigen::RowMajor>& A_i) {
// TODO: A blocked, out-of-place transpose should be much faster than
// traversing row major on a column major matrix unless we have few linear
// constraints (using a heuristic to choose between this and staying column
// major based on the number of constraints would be an easy performance
// improvement.)
// NB: Casting to long is unspecified if the size of decision_variable.size()
// is greater than the max long value, but then we wouldn't be able to fill
// A_i correctly anyway.
slp_assert(static_cast<Eigen::Index>(decision_variables.size()) ==
A_i.innerSize());
slp_assert(static_cast<Eigen::Index>(inequality_constraints.size()) ==
A_i.outerSize());
// Maps each decision variable's index to the indices of its upper and lower
// bounds if they exist, or NO_BOUND if they do not; used only for bookkeeping
// to compute conflicting bounds
constexpr Eigen::Index NO_BOUND = -1;
gch::small_vector<std::pair<Eigen::Index, Eigen::Index>>
decision_var_indices_to_constraint_indices{decision_variables.size(),
{NO_BOUND, NO_BOUND}};
// Lists pairs of indices of bound constraints in the inequality constraint
// list that conflict with each other
gch::small_vector<std::pair<Eigen::Index, Eigen::Index>>
conflicting_bound_indices;
// Maps each decision variable's index to its upper and lower bounds
gch::small_vector<std::pair<double, double>> decision_var_indices_to_bounds{
decision_variables.size(),
{-std::numeric_limits<double>::infinity(),
std::numeric_limits<double>::infinity()}};
// Vector corresponding to the inequality constraints where the i-th element
// is 1 if the i-th inequality constraint is a bound and 0 otherwise.
Eigen::ArrayX<bool> bound_constraint_mask{inequality_constraints.size()};
bound_constraint_mask.fill(false);
for (decltype(inequality_constraints)::size_type constraint_index = 0;
constraint_index < inequality_constraints.size(); ++constraint_index) {
// A constraint is a bound iff it is linear and its gradient has a
// single nonzero value.
if (inequality_constraints[constraint_index].type() !=
ExpressionType::LINEAR) {
continue;
}
const Eigen::SparseVector<double>& row_A_i =
A_i.innerVector(constraint_index);
const auto non_zeros = row_A_i.nonZeros();
slp_assert(non_zeros != 0);
if (non_zeros > 1) {
// Constraint is in more than one variable and therefore not a bound.
continue;
}
// Claim: The bound given by a bound constraint is the constraint evaluated
// at zero divided by the nonzero element of the constraint's gradient.
//
// Proof: If c(x) is a bound constraint, then by definition c is a linear
// function in one variable, hence there exist a, b ∈ ℝ s.t. c(x) = axᵢ + b
// and a ≠ 0. The gradient of c is then aeᵢ (where eᵢ denotes the i-th basis
// element), and c(0) = b. If c(x) ≥ 0, then since either a < 0 or a > 0, we
// have either x ≤ -b/a or x ≥ -b/a, respectively. ∎
Eigen::SparseVector<double>::InnerIterator row_iter(row_A_i);
const auto constraint_coefficient =
row_iter
.value(); // The nonzero value of the j-th constraint's gradient.
const auto decision_variable_index = row_iter.index();
const auto decision_variable_value =
decision_variables[decision_variable_index].value();
double constraint_constant;
// We need to evaluate this constraint *exactly* at zero.
if (decision_variable_value != 0.0) {
decision_variables[decision_variable_index].set_value(0.0);
constraint_constant = inequality_constraints[constraint_index].value();
decision_variables[decision_variable_index].set_value(
decision_variable_value);
} else {
constraint_constant = inequality_constraints[constraint_index].value();
}
// Shouldn't happen since the constraint is supposed to be linear and not a
// constant.
slp_assert(constraint_coefficient != 0.0);
// We should always get a finite value when evaluating the constraint at
// x = 0 since the constraint is linear.
slp_assert(std::isfinite(constraint_constant));
// This is possible if the user has provided a starting point at which their
// problem is ill-defined.
slp_assert(std::isfinite(constraint_coefficient));
// Update bounds; we assume constraints of the form c(x) ≥ 0.
auto& [lower_bound, upper_bound] =
decision_var_indices_to_bounds[decision_variable_index];
auto& [lower_index, upper_index] =
decision_var_indices_to_constraint_indices[decision_variable_index];
const auto detected_bound = -constraint_constant / constraint_coefficient;
if (constraint_coefficient < 0.0 && detected_bound < upper_bound) {
upper_bound = detected_bound;
upper_index = constraint_index;
} else if (constraint_coefficient > 0.0 && detected_bound > lower_bound) {
lower_bound = detected_bound;
lower_index = constraint_index;
}
// Update conflicting bounds
if (lower_bound > upper_bound) {
conflicting_bound_indices.emplace_back(lower_index, upper_index);
}
// Set the bound constraint mask appropriately.
bound_constraint_mask[constraint_index] = true;
}
return {bound_constraint_mask, decision_var_indices_to_bounds,
conflicting_bound_indices};
}
/**
* Projects the decision variables onto the given bounds, while ensuring some
* configurable distance from the boundary if possible. This is designed to
* match the algorithm given in section 3.6 of [2].
*
* @param x A vector of decision variables.
* @param decision_variable_indices_to_bounds An array of bounds (stored [lower,
* upper]) for each decision variable in x.
* @param κ_1 A constant controlling distance from the lower or upper bound when
* the difference between the upper and lower bound is small.
* @param κ_2 A constant controlling distance from the lower or upper bound when
* the difference between the upper and lower bound is large (including when
* one of the bounds is ±∞).
*/
template <typename Derived>
requires(static_cast<bool>(Eigen::DenseBase<Derived>::IsVectorAtCompileTime))
inline void project_onto_bounds(
Eigen::DenseBase<Derived>& x,
std::span<const std::pair<typename Eigen::DenseBase<Derived>::Scalar,
typename Eigen::DenseBase<Derived>::Scalar>>
decision_variable_indices_to_bounds,
const typename Eigen::DenseBase<Derived>::Scalar κ_1 = 1e-2,
const typename Eigen::DenseBase<Derived>::Scalar κ_2 = 1e-2) {
slp_assert(κ_1 > 0 && κ_2 > 0 && κ_2 < 0.5);
Eigen::Index decision_variable_idx = 0;
for (const auto& [lower, upper] : decision_variable_indices_to_bounds) {
typename Eigen::DenseBase<Derived>::Scalar& x_i =
x[decision_variable_idx++];
// We assume that bound infeasibility is handled elsewhere.
slp_assert(lower <= upper);
// See B.2 in [5] and section 3.6 in [2]
if (std::isfinite(lower) && std::isfinite(upper)) {
auto p_L =
std::min(κ_1 * std::max(1.0, std::abs(lower)), κ_2 * (upper - lower));
auto p_U =
std::min(κ_1 * std::max(1.0, std::abs(upper)), κ_2 * (upper - lower));
x_i = std::min(std::max(lower + p_L, x_i), upper - p_U);
} else if (std::isfinite(lower)) {
x_i = std::max(x_i, lower + κ_1 * std::max(1.0, std::abs(lower)));
} else if (std::isfinite(upper)) {
x_i = std::min(x_i, upper - κ_1 * std::max(1.0, std::abs(upper)));
}
}
}
} // namespace slp

53
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/inertia.hpp vendored Normal file
View File

@@ -0,0 +1,53 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <Eigen/Core>
namespace slp {
/**
* Represents the inertia of a matrix (the number of positive, negative, and
* zero eigenvalues).
*/
class Inertia {
public:
int positive = 0;
int negative = 0;
int zero = 0;
constexpr Inertia() = default;
/**
* Constructs Inertia with the given number of positive, negative, and zero
* eigenvalues.
*
* @param positive The number of positive eigenvalues.
* @param negative The number of negative eigenvalues.
* @param zero The number of zero eigenvalues.
*/
constexpr Inertia(int positive, int negative, int zero)
: positive{positive}, negative{negative}, zero{zero} {}
/**
* Constructs Inertia from the D matrix of an LDLT decomposition
* (see https://en.wikipedia.org/wiki/Sylvester's_law_of_inertia).
*
* @param D The D matrix of an LDLT decomposition in vector form.
*/
explicit Inertia(const Eigen::VectorXd& D) {
for (const auto& elem : D) {
if (elem > 0.0) {
++positive;
} else if (elem < 0.0) {
++negative;
} else {
++zero;
}
}
}
bool operator==(const Inertia&) const = default;
};
} // namespace slp

391
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/problem.cpp vendored Normal file
View File

@@ -0,0 +1,391 @@
// Copyright (c) Sleipnir contributors
#include "sleipnir/optimization/problem.hpp"
#include <array>
#include <cmath>
#include <memory>
#include <optional>
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <gch/small_vector.hpp>
#include "optimization/bounds.hpp"
#include "sleipnir/autodiff/expression_type.hpp"
#include "sleipnir/autodiff/gradient.hpp"
#include "sleipnir/autodiff/hessian.hpp"
#include "sleipnir/autodiff/jacobian.hpp"
#include "sleipnir/autodiff/variable.hpp"
#include "sleipnir/autodiff/variable_matrix.hpp"
#include "sleipnir/optimization/solver/exit_status.hpp"
#include "sleipnir/optimization/solver/interior_point.hpp"
#include "sleipnir/optimization/solver/newton.hpp"
#include "sleipnir/optimization/solver/options.hpp"
#include "sleipnir/optimization/solver/sqp.hpp"
#include "sleipnir/util/print.hpp"
#include "sleipnir/util/spy.hpp"
#include "util/print_diagnostics.hpp"
#include "util/setup_profiler.hpp"
namespace slp {
ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) {
// Create the initial value column vector
Eigen::VectorXd x{m_decision_variables.size()};
for (size_t i = 0; i < m_decision_variables.size(); ++i) {
x[i] = m_decision_variables[i].value();
}
if (options.diagnostics) {
print_exit_conditions(options);
print_problem_analysis();
}
// Get the highest order constraint expression types
auto f_type = cost_function_type();
auto c_e_type = equality_constraint_type();
auto c_i_type = inequality_constraint_type();
// If the problem is empty or constant, there's nothing to do
if (f_type <= ExpressionType::CONSTANT &&
c_e_type <= ExpressionType::CONSTANT &&
c_i_type <= ExpressionType::CONSTANT) {
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
if (options.diagnostics) {
slp::println("\nInvoking no-op solver...\n");
}
#endif
return ExitStatus::SUCCESS;
}
gch::small_vector<SetupProfiler> ad_setup_profilers;
ad_setup_profilers.emplace_back("setup").start();
VariableMatrix x_ad{m_decision_variables};
// Set up cost function
Variable f = m_f.value_or(0.0);
// Set up gradient autodiff
ad_setup_profilers.emplace_back(" ↳ ∇f(x)").start();
Gradient g{f, x_ad};
ad_setup_profilers.back().stop();
[[maybe_unused]]
int num_decision_variables = m_decision_variables.size();
[[maybe_unused]]
int num_equality_constraints = m_equality_constraints.size();
[[maybe_unused]]
int num_inequality_constraints = m_inequality_constraints.size();
// Solve the optimization problem
ExitStatus status;
if (m_equality_constraints.empty() && m_inequality_constraints.empty()) {
if (options.diagnostics) {
slp::println("\nInvoking Newton solver...\n");
}
// Set up Lagrangian Hessian autodiff
ad_setup_profilers.emplace_back(" ↳ ∇²ₓₓL").start();
Hessian<Eigen::Lower> H{f, x_ad};
ad_setup_profilers.back().stop();
ad_setup_profilers[0].stop();
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
// Sparsity pattern files written when spy flag is set
std::unique_ptr<Spy> H_spy;
if (spy) {
H_spy = std::make_unique<Spy>(
"H.spy", "Hessian", "Decision variables", "Decision variables",
num_decision_variables, num_decision_variables);
m_iteration_callbacks.push_back([&](const IterationInfo& info) -> bool {
H_spy->add(info.H);
return false;
});
}
#endif
// Invoke Newton solver
status = newton(
NewtonMatrixCallbacks{
[&](const Eigen::VectorXd& x) -> double {
x_ad.set_value(x);
return f.value();
},
[&](const Eigen::VectorXd& x) -> Eigen::SparseVector<double> {
x_ad.set_value(x);
return g.value();
},
[&](const Eigen::VectorXd& x) -> Eigen::SparseMatrix<double> {
x_ad.set_value(x);
return H.value();
}},
m_iteration_callbacks, options, x);
} else if (m_inequality_constraints.empty()) {
if (options.diagnostics) {
slp::println("\nInvoking SQP solver\n");
}
VariableMatrix c_e_ad{m_equality_constraints};
// Set up equality constraint Jacobian autodiff
ad_setup_profilers.emplace_back(" ↳ ∂cₑ/∂x").start();
Jacobian A_e{c_e_ad, x_ad};
ad_setup_profilers.back().stop();
// Set up Lagrangian
VariableMatrix y_ad(num_equality_constraints);
Variable L = f - (y_ad.T() * c_e_ad)[0];
// Set up Lagrangian Hessian autodiff
ad_setup_profilers.emplace_back(" ↳ ∇²ₓₓL").start();
Hessian<Eigen::Lower> H{L, x_ad};
ad_setup_profilers.back().stop();
ad_setup_profilers[0].stop();
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
// Sparsity pattern files written when spy flag is set
std::unique_ptr<Spy> H_spy;
std::unique_ptr<Spy> A_e_spy;
if (spy) {
H_spy = std::make_unique<Spy>(
"H.spy", "Hessian", "Decision variables", "Decision variables",
num_decision_variables, num_decision_variables);
A_e_spy = std::make_unique<Spy>("A_e.spy", "Equality constraint Jacobian",
"Constraints", "Decision variables",
num_equality_constraints,
num_decision_variables);
m_iteration_callbacks.push_back([&](const IterationInfo& info) -> bool {
H_spy->add(info.H);
A_e_spy->add(info.A_e);
return false;
});
}
#endif
// Invoke SQP solver
status =
sqp(SQPMatrixCallbacks{
[&](const Eigen::VectorXd& x) -> double {
x_ad.set_value(x);
return f.value();
},
[&](const Eigen::VectorXd& x) -> Eigen::SparseVector<double> {
x_ad.set_value(x);
return g.value();
},
[&](const Eigen::VectorXd& x,
const Eigen::VectorXd& y) -> Eigen::SparseMatrix<double> {
x_ad.set_value(x);
y_ad.set_value(y);
return H.value();
},
[&](const Eigen::VectorXd& x) -> Eigen::VectorXd {
x_ad.set_value(x);
return c_e_ad.value();
},
[&](const Eigen::VectorXd& x) -> Eigen::SparseMatrix<double> {
x_ad.set_value(x);
return A_e.value();
}},
m_iteration_callbacks, options, x);
} else {
if (options.diagnostics) {
slp::println("\nInvoking IPM solver...\n");
}
VariableMatrix c_e_ad{m_equality_constraints};
VariableMatrix c_i_ad{m_inequality_constraints};
// Set up equality constraint Jacobian autodiff
ad_setup_profilers.emplace_back(" ↳ ∂cₑ/∂x").start();
Jacobian A_e{c_e_ad, x_ad};
ad_setup_profilers.back().stop();
// Set up inequality constraint Jacobian autodiff
ad_setup_profilers.emplace_back(" ↳ ∂cᵢ/∂x").start();
Jacobian A_i{c_i_ad, x_ad};
ad_setup_profilers.back().stop();
// Set up Lagrangian
VariableMatrix y_ad(num_equality_constraints);
VariableMatrix z_ad(num_inequality_constraints);
Variable L = f - (y_ad.T() * c_e_ad)[0] - (z_ad.T() * c_i_ad)[0];
// Set up Lagrangian Hessian autodiff
ad_setup_profilers.emplace_back(" ↳ ∇²ₓₓL").start();
Hessian<Eigen::Lower> H{L, x_ad};
ad_setup_profilers.back().stop();
ad_setup_profilers[0].stop();
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
// Sparsity pattern files written when spy flag is set
std::unique_ptr<Spy> H_spy;
std::unique_ptr<Spy> A_e_spy;
std::unique_ptr<Spy> A_i_spy;
if (spy) {
H_spy = std::make_unique<Spy>(
"H.spy", "Hessian", "Decision variables", "Decision variables",
num_decision_variables, num_decision_variables);
A_e_spy = std::make_unique<Spy>("A_e.spy", "Equality constraint Jacobian",
"Constraints", "Decision variables",
num_equality_constraints,
num_decision_variables);
A_i_spy = std::make_unique<Spy>(
"A_i.spy", "Inequality constraint Jacobian", "Constraints",
"Decision variables", num_inequality_constraints,
num_decision_variables);
m_iteration_callbacks.push_back([&](const IterationInfo& info) -> bool {
H_spy->add(info.H);
A_e_spy->add(info.A_e);
A_i_spy->add(info.A_i);
return false;
});
}
#endif
const auto [bound_constraint_mask, bounds, conflicting_bound_indices] =
get_bounds(m_decision_variables, m_inequality_constraints, A_i.value());
if (!conflicting_bound_indices.empty()) {
if (options.diagnostics) {
print_bound_constraint_global_infeasibility_error(
conflicting_bound_indices);
}
return ExitStatus::GLOBALLY_INFEASIBLE;
}
#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
project_onto_bounds(x, bounds);
#endif
// Invoke interior-point method solver
status = interior_point(
InteriorPointMatrixCallbacks{
[&](const Eigen::VectorXd& x) -> double {
x_ad.set_value(x);
return f.value();
},
[&](const Eigen::VectorXd& x) -> Eigen::SparseVector<double> {
x_ad.set_value(x);
return g.value();
},
[&](const Eigen::VectorXd& x, const Eigen::VectorXd& y,
const Eigen::VectorXd& z) -> Eigen::SparseMatrix<double> {
x_ad.set_value(x);
y_ad.set_value(y);
z_ad.set_value(z);
return H.value();
},
[&](const Eigen::VectorXd& x) -> Eigen::VectorXd {
x_ad.set_value(x);
return c_e_ad.value();
},
[&](const Eigen::VectorXd& x) -> Eigen::SparseMatrix<double> {
x_ad.set_value(x);
return A_e.value();
},
[&](const Eigen::VectorXd& x) -> Eigen::VectorXd {
x_ad.set_value(x);
return c_i_ad.value();
},
[&](const Eigen::VectorXd& x) -> Eigen::SparseMatrix<double> {
x_ad.set_value(x);
return A_i.value();
}},
m_iteration_callbacks, options,
#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
bound_constraint_mask,
#endif
x);
}
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
if (spy) {
m_iteration_callbacks.pop_back();
}
#endif
if (options.diagnostics) {
print_autodiff_diagnostics(ad_setup_profilers);
slp::println("\nExit: {}", to_message(status));
}
// Assign the solution to the original Variable instances
VariableMatrix{m_decision_variables}.set_value(x);
return status;
}
void Problem::print_exit_conditions([[maybe_unused]] const Options& options) {
// Print possible exit conditions
slp::println("User-configured exit conditions:");
slp::println(" ↳ error below {}", options.tolerance);
if (!m_iteration_callbacks.empty()) {
slp::println(" ↳ iteration callback requested stop");
}
if (std::isfinite(options.max_iterations)) {
slp::println(" ↳ executed {} iterations", options.max_iterations);
}
if (std::isfinite(options.timeout.count())) {
slp::println(" ↳ {} elapsed", options.timeout.count());
}
}
void Problem::print_problem_analysis() {
constexpr std::array types{"no", "constant", "linear", "quadratic",
"nonlinear"};
// Print problem structure
slp::println("\nProblem structure:");
slp::println(" ↳ {} cost function",
types[static_cast<uint8_t>(cost_function_type())]);
slp::println(" ↳ {} equality constraints",
types[static_cast<uint8_t>(equality_constraint_type())]);
slp::println(" ↳ {} inequality constraints",
types[static_cast<uint8_t>(inequality_constraint_type())]);
if (m_decision_variables.size() == 1) {
slp::print("\n1 decision variable\n");
} else {
slp::print("\n{} decision variables\n", m_decision_variables.size());
}
auto print_constraint_types =
[](const gch::small_vector<Variable>& constraints) {
std::array<size_t, 5> counts{};
for (const auto& constraint : constraints) {
++counts[static_cast<uint8_t>(constraint.type())];
}
for (size_t i = 0; i < counts.size(); ++i) {
constexpr std::array names{"empty", "constant", "linear", "quadratic",
"nonlinear"};
const auto& count = counts[i];
const auto& name = names[i];
if (count > 0) {
slp::println(" ↳ {} {}", count, name);
}
}
};
// Print constraint types
if (m_equality_constraints.size() == 1) {
slp::println("1 equality constraint");
} else {
slp::println("{} equality constraints", m_equality_constraints.size());
}
print_constraint_types(m_equality_constraints);
if (m_inequality_constraints.size() == 1) {
slp::println("1 inequality constraint");
} else {
slp::println("{} inequality constraints", m_inequality_constraints.size());
}
print_constraint_types(m_inequality_constraints);
}
} // namespace slp

227
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/regularized_ldlt.hpp vendored Normal file
View File

@@ -0,0 +1,227 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <Eigen/Cholesky>
#include <Eigen/Core>
#include <Eigen/SparseCholesky>
#include <Eigen/SparseCore>
#include "optimization/inertia.hpp"
// See docs/algorithms.md#Works_cited for citation definitions
namespace slp {
/**
* Solves systems of linear equations using a regularized LDLT factorization.
*/
class RegularizedLDLT {
public:
/**
* Constructs a RegularizedLDLT 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.
*/
RegularizedLDLT(int num_decision_variables, int num_equality_constraints)
: m_num_decision_variables{num_decision_variables},
m_num_equality_constraints{num_equality_constraints} {}
/**
* 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.
*/
RegularizedLDLT& compute(const Eigen::SparseMatrix<double>& 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();
Inertia inertia;
if (m_info == Eigen::Success) {
inertia = m_is_sparse ? Inertia{m_sparse_solver.vectorD()}
: Inertia{m_dense_solver.vectorD()};
// If the inertia is ideal, don't regularize the system
if (inertia == ideal_inertia) {
m_prev_δ = 0.0;
return *this;
}
}
// 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.
double δ = m_prev_δ == 0.0 ? 1e-4 : m_prev_δ / 2.0;
double γ = 1e-10;
while (true) {
// 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
δ *= 10.0;
γ *= 10.0;
} else if (inertia.negative > ideal_inertia.negative) {
// If there's too many negative eigenvalues, increase δ by an order of
// magnitude and try again
δ *= 10.0;
} else if (inertia.positive > ideal_inertia.positive) {
// If there's too many positive eigenvalues, increase γ by an order of
// magnitude and try again
γ *= 10.0;
}
} else {
// If the decomposition failed, increase δ and γ by an order of
// magnitude and try again
δ *= 10.0;
γ *= 10.0;
}
// If the Hessian perturbation is too high, report failure. This can be
// caused by ill-conditioning.
if (δ > 1e20 || γ > 1e20) {
m_info = Eigen::NumericalIssue;
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>
Eigen::VectorXd solve(const Eigen::MatrixBase<Rhs>& rhs) {
if (m_is_sparse) {
return m_sparse_solver.solve(rhs);
} else {
return m_dense_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>
Eigen::VectorXd solve(const Eigen::SparseMatrixBase<Rhs>& rhs) {
if (m_is_sparse) {
return m_sparse_solver.solve(rhs);
} else {
return m_dense_solver.solve(rhs.toDense());
}
}
/**
* Returns the Hessian regularization factor.
*
* @return Hessian regularization factor.
*/
double hessian_regularization() const { return m_prev_δ; }
private:
using SparseSolver = Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>>;
using DenseSolver = Eigen::LDLT<Eigen::MatrixXd>;
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 ideal system inertia.
Inertia ideal_inertia{m_num_decision_variables, m_num_equality_constraints,
0};
/// The value of δ from the previous run of compute().
double m_prev_δ = 0.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 Eigen::SparseMatrix<double>& 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.
*/
Eigen::SparseMatrix<double> regularization(double δ, double γ) {
Eigen::VectorXd 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 Eigen::SparseMatrix<double>{vec.asDiagonal()};
}
};
} // namespace slp

837
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp vendored
View File

@@ -1,837 +0,0 @@
// Copyright (c) Sleipnir contributors
#include "sleipnir/optimization/solver/InteriorPoint.hpp"
#include <algorithm>
#include <chrono>
#include <cmath>
#include <fstream>
#include <limits>
#include <Eigen/SparseCholesky>
#include <wpi/SmallVector.h>
#include "optimization/RegularizedLDLT.hpp"
#include "optimization/solver/util/ErrorEstimate.hpp"
#include "optimization/solver/util/FeasibilityRestoration.hpp"
#include "optimization/solver/util/Filter.hpp"
#include "optimization/solver/util/FractionToTheBoundaryRule.hpp"
#include "optimization/solver/util/IsLocallyInfeasible.hpp"
#include "optimization/solver/util/KKTError.hpp"
#include "sleipnir/autodiff/Gradient.hpp"
#include "sleipnir/autodiff/Hessian.hpp"
#include "sleipnir/autodiff/Jacobian.hpp"
#include "sleipnir/optimization/SolverExitCondition.hpp"
#include "sleipnir/util/Print.hpp"
#include "sleipnir/util/Spy.hpp"
#include "util/ScopeExit.hpp"
#include "util/ToMilliseconds.hpp"
// See docs/algorithms.md#Works_cited for citation definitions.
//
// See docs/algorithms.md#Interior-point_method for a derivation of the
// interior-point method formulation being used.
namespace sleipnir {
void InteriorPoint(std::span<Variable> decisionVariables,
std::span<Variable> equalityConstraints,
std::span<Variable> inequalityConstraints, Variable& f,
function_ref<bool(const SolverIterationInfo& info)> callback,
const SolverConfig& config, bool feasibilityRestoration,
Eigen::VectorXd& x, Eigen::VectorXd& s,
SolverStatus* status) {
const auto solveStartTime = std::chrono::system_clock::now();
// Map decision variables and constraints to VariableMatrices for Lagrangian
VariableMatrix xAD{decisionVariables};
xAD.SetValue(x);
VariableMatrix c_eAD{equalityConstraints};
VariableMatrix c_iAD{inequalityConstraints};
// Create autodiff variables for s, y, and z for Lagrangian
VariableMatrix sAD(inequalityConstraints.size());
sAD.SetValue(s);
VariableMatrix yAD(equalityConstraints.size());
for (auto& y : yAD) {
y.SetValue(0.0);
}
VariableMatrix zAD(inequalityConstraints.size());
for (auto& z : zAD) {
z.SetValue(1.0);
}
// Lagrangian L
//
// L(xₖ, sₖ, yₖ, zₖ) = f(xₖ) − yₖᵀcₑ(xₖ) − zₖᵀ(cᵢ(xₖ) − sₖ)
auto L = f - (yAD.T() * c_eAD)(0) - (zAD.T() * (c_iAD - sAD))(0);
// Equality constraint Jacobian Aₑ
//
// [∇ᵀcₑ₁(xₖ)]
// Aₑ(x) = [∇ᵀcₑ₂(xₖ)]
// [ ⋮ ]
// [∇ᵀcₑₘ(xₖ)]
Jacobian jacobianCe{c_eAD, xAD};
Eigen::SparseMatrix<double> A_e = jacobianCe.Value();
// Inequality constraint Jacobian Aᵢ
//
// [∇ᵀcᵢ₁(xₖ)]
// Aᵢ(x) = [∇ᵀcᵢ₂(xₖ)]
// [ ⋮ ]
// [∇ᵀcᵢₘ(xₖ)]
Jacobian jacobianCi{c_iAD, xAD};
Eigen::SparseMatrix<double> A_i = jacobianCi.Value();
// Gradient of f ∇f
Gradient gradientF{f, xAD};
Eigen::SparseVector<double> g = gradientF.Value();
// Hessian of the Lagrangian H
//
// Hₖ = ∇²ₓₓL(xₖ, sₖ, yₖ, zₖ)
Hessian hessianL{L, xAD};
Eigen::SparseMatrix<double> H = hessianL.Value();
Eigen::VectorXd y = yAD.Value();
Eigen::VectorXd z = zAD.Value();
Eigen::VectorXd c_e = c_eAD.Value();
Eigen::VectorXd c_i = c_iAD.Value();
// Check for overconstrained problem
if (equalityConstraints.size() > decisionVariables.size()) {
if (config.diagnostics) {
sleipnir::println("The problem has too few degrees of freedom.");
sleipnir::println(
"Violated constraints (cₑ(x) = 0) in order of declaration:");
for (int row = 0; row < c_e.rows(); ++row) {
if (c_e(row) < 0.0) {
sleipnir::println(" {}/{}: {} = 0", row + 1, c_e.rows(), c_e(row));
}
}
}
status->exitCondition = SolverExitCondition::kTooFewDOFs;
return;
}
// Check whether initial guess has finite f(xₖ), cₑ(xₖ), and cᵢ(xₖ)
if (!std::isfinite(f.Value()) || !c_e.allFinite() || !c_i.allFinite()) {
status->exitCondition =
SolverExitCondition::kNonfiniteInitialCostOrConstraints;
return;
}
// Sparsity pattern files written when spy flag is set in SolverConfig
std::ofstream H_spy;
std::ofstream A_e_spy;
std::ofstream A_i_spy;
if (config.spy) {
A_e_spy.open("A_e.spy");
A_i_spy.open("A_i.spy");
H_spy.open("H.spy");
}
if (config.diagnostics && !feasibilityRestoration) {
sleipnir::println("Error tolerance: {}\n", config.tolerance);
}
std::chrono::system_clock::time_point iterationsStartTime;
int iterations = 0;
// Prints final diagnostics when the solver exits
scope_exit exit{[&] {
status->cost = f.Value();
if (config.diagnostics && !feasibilityRestoration) {
auto solveEndTime = std::chrono::system_clock::now();
sleipnir::println("\nSolve time: {:.3f} ms",
ToMilliseconds(solveEndTime - solveStartTime));
sleipnir::println(" ↳ {:.3f} ms (solver setup)",
ToMilliseconds(iterationsStartTime - solveStartTime));
if (iterations > 0) {
sleipnir::println(
" ↳ {:.3f} ms ({} solver iterations; {:.3f} ms average)",
ToMilliseconds(solveEndTime - iterationsStartTime), iterations,
ToMilliseconds((solveEndTime - iterationsStartTime) / iterations));
}
sleipnir::println("");
sleipnir::println("{:^8} {:^10} {:^14} {:^6}", "autodiff",
"setup (ms)", "avg solve (ms)", "solves");
sleipnir::println("{:=^47}", "");
constexpr auto format = "{:^8} {:10.3f} {:14.3f} {:6}";
sleipnir::println(format, "∇f(x)",
gradientF.GetProfiler().SetupDuration(),
gradientF.GetProfiler().AverageSolveDuration(),
gradientF.GetProfiler().SolveMeasurements());
sleipnir::println(format, "∇²ₓₓL", hessianL.GetProfiler().SetupDuration(),
hessianL.GetProfiler().AverageSolveDuration(),
hessianL.GetProfiler().SolveMeasurements());
sleipnir::println(format, "∂cₑ/∂x",
jacobianCe.GetProfiler().SetupDuration(),
jacobianCe.GetProfiler().AverageSolveDuration(),
jacobianCe.GetProfiler().SolveMeasurements());
sleipnir::println(format, "∂cᵢ/∂x",
jacobianCi.GetProfiler().SetupDuration(),
jacobianCi.GetProfiler().AverageSolveDuration(),
jacobianCi.GetProfiler().SolveMeasurements());
sleipnir::println("");
}
}};
// Barrier parameter minimum
const double μ_min = config.tolerance / 10.0;
// Barrier parameter μ
double μ = 0.1;
// Fraction-to-the-boundary rule scale factor minimum
constexpr double τ_min = 0.99;
// Fraction-to-the-boundary rule scale factor τ
double τ = τ_min;
Filter filter{f};
// This should be run when the error estimate is below a desired threshold for
// the current barrier parameter
auto UpdateBarrierParameterAndResetFilter = [&] {
// Barrier parameter linear decrease power in "κ_μ μ". Range of (0, 1).
constexpr double κ_μ = 0.2;
// Barrier parameter superlinear decrease power in "μ^(θ_μ)". Range of (1,
// 2).
constexpr double θ_μ = 1.5;
// Update the barrier parameter.
//
// μⱼ₊₁ = max(εₜₒₗ/10, min(κ_μ μⱼ, μⱼ^θ_μ))
//
// See equation (7) of [2].
μ = std::max(μ_min, std::min(κ_μ * μ, std::pow(μ, θ_μ)));
// Update the fraction-to-the-boundary rule scaling factor.
//
// τⱼ = max(τₘᵢₙ, 1 − μⱼ)
//
// See equation (8) of [2].
τ = std::max(τ_min, 1.0 - μ);
// Reset the filter when the barrier parameter is updated
filter.Reset();
};
// Kept outside the loop so its storage can be reused
wpi::SmallVector<Eigen::Triplet<double>> triplets;
RegularizedLDLT solver;
// Variables for determining when a step is acceptable
constexpr double α_red_factor = 0.5;
int acceptableIterCounter = 0;
int fullStepRejectedCounter = 0;
int stepTooSmallCounter = 0;
// Error estimate
double E_0 = std::numeric_limits<double>::infinity();
if (config.diagnostics) {
iterationsStartTime = std::chrono::system_clock::now();
}
while (E_0 > config.tolerance &&
acceptableIterCounter < config.maxAcceptableIterations) {
std::chrono::system_clock::time_point innerIterStartTime;
if (config.diagnostics) {
innerIterStartTime = std::chrono::system_clock::now();
}
// Check for local equality constraint infeasibility
if (IsEqualityLocallyInfeasible(A_e, c_e)) {
if (config.diagnostics) {
sleipnir::println(
"The problem is locally infeasible due to violated equality "
"constraints.");
sleipnir::println(
"Violated constraints (cₑ(x) = 0) in order of declaration:");
for (int row = 0; row < c_e.rows(); ++row) {
if (c_e(row) < 0.0) {
sleipnir::println(" {}/{}: {} = 0", row + 1, c_e.rows(), c_e(row));
}
}
}
status->exitCondition = SolverExitCondition::kLocallyInfeasible;
return;
}
// Check for local inequality constraint infeasibility
if (IsInequalityLocallyInfeasible(A_i, c_i)) {
if (config.diagnostics) {
sleipnir::println(
"The problem is infeasible due to violated inequality "
"constraints.");
sleipnir::println(
"Violated constraints (cᵢ(x) ≥ 0) in order of declaration:");
for (int row = 0; row < c_i.rows(); ++row) {
if (c_i(row) < 0.0) {
sleipnir::println(" {}/{}: {} ≥ 0", row + 1, c_i.rows(), c_i(row));
}
}
}
status->exitCondition = SolverExitCondition::kLocallyInfeasible;
return;
}
// Check for diverging iterates
if (x.lpNorm<Eigen::Infinity>() > 1e20 || !x.allFinite() ||
s.lpNorm<Eigen::Infinity>() > 1e20 || !s.allFinite()) {
status->exitCondition = SolverExitCondition::kDivergingIterates;
return;
}
// Write out spy file contents if that's enabled
if (config.spy) {
// Gap between sparsity patterns
if (iterations > 0) {
A_e_spy << "\n";
A_i_spy << "\n";
H_spy << "\n";
}
Spy(H_spy, H);
Spy(A_e_spy, A_e);
Spy(A_i_spy, A_i);
}
// Call user callback
if (callback({iterations, x, s, g, H, A_e, A_i})) {
status->exitCondition = SolverExitCondition::kCallbackRequestedStop;
return;
}
// [s₁ 0 ⋯ 0 ]
// S = [0 ⋱ ⋮ ]
// [⋮ ⋱ 0 ]
// [0 ⋯ 0 sₘ]
const auto S = s.asDiagonal();
Eigen::SparseMatrix<double> Sinv;
Sinv = s.cwiseInverse().asDiagonal();
// [z₁ 0 ⋯ 0 ]
// Z = [0 ⋱ ⋮ ]
// [⋮ ⋱ 0 ]
// [0 ⋯ 0 zₘ]
const auto Z = z.asDiagonal();
Eigen::SparseMatrix<double> Zinv;
Zinv = z.cwiseInverse().asDiagonal();
// Σ = S⁻¹Z
const Eigen::SparseMatrix<double> Σ = Sinv * Z;
// lhs = [H + AᵢᵀΣAᵢ Aₑᵀ]
// [ Aₑ 0 ]
//
// Don't assign upper triangle because solver only uses lower triangle.
const Eigen::SparseMatrix<double> topLeft =
H.triangularView<Eigen::Lower>() +
(A_i.transpose() * Σ * A_i).triangularView<Eigen::Lower>();
triplets.clear();
triplets.reserve(topLeft.nonZeros() + A_e.nonZeros());
for (int col = 0; col < H.cols(); ++col) {
// Append column of H + AᵢᵀΣAᵢ lower triangle in top-left quadrant
for (Eigen::SparseMatrix<double>::InnerIterator it{topLeft, col}; it;
++it) {
triplets.emplace_back(it.row(), it.col(), it.value());
}
// Append column of Aₑ in bottom-left quadrant
for (Eigen::SparseMatrix<double>::InnerIterator it{A_e, col}; it; ++it) {
triplets.emplace_back(H.rows() + it.row(), it.col(), it.value());
}
}
Eigen::SparseMatrix<double> lhs(
decisionVariables.size() + equalityConstraints.size(),
decisionVariables.size() + equalityConstraints.size());
lhs.setFromSortedTriplets(triplets.begin(), triplets.end(),
[](const auto&, const auto& b) { return b; });
const Eigen::VectorXd e = Eigen::VectorXd::Ones(s.rows());
// rhs = −[∇f − Aₑᵀy + Aᵢᵀ(S⁻¹(Zcᵢ − μe) − z)]
// [ cₑ ]
Eigen::VectorXd rhs{x.rows() + y.rows()};
rhs.segment(0, x.rows()) =
-(g - A_e.transpose() * y +
A_i.transpose() * (Sinv * (Z * c_i - μ * e) - z));
rhs.segment(x.rows(), y.rows()) = -c_e;
// Solve the Newton-KKT system
//
// [H + AᵢᵀΣAᵢ Aₑᵀ][ pₖˣ] = −[∇f − Aₑᵀy + Aᵢᵀ(S⁻¹(Zcᵢ − μe) − z)]
// [ Aₑ 0 ][−pₖʸ] [ cₑ ]
solver.Compute(lhs, equalityConstraints.size(), μ);
Eigen::VectorXd step{x.rows() + y.rows()};
if (solver.Info() == Eigen::Success) {
step = solver.Solve(rhs);
} else {
// The regularization procedure failed due to a rank-deficient equality
// constraint Jacobian with linearly dependent constraints. Set the step
// length to zero and let second-order corrections attempt to restore
// feasibility.
step.setZero();
}
// step = [ pₖˣ]
// [−pₖʸ]
Eigen::VectorXd p_x = step.segment(0, x.rows());
Eigen::VectorXd p_y = -step.segment(x.rows(), y.rows());
// pₖᶻ = −Σcᵢ + μS⁻¹e − ΣAᵢpₖˣ
Eigen::VectorXd p_z = -Σ * c_i + μ * Sinv * e - Σ * A_i * p_x;
// pₖˢ = μZ⁻¹e − s − Z⁻¹Spₖᶻ
Eigen::VectorXd p_s = μ * Zinv * e - s - Zinv * S * p_z;
// αᵐᵃˣ = max(α ∈ (0, 1] : sₖ + αpₖˢ ≥ (1−τⱼ)sₖ)
const double α_max = FractionToTheBoundaryRule(s, p_s, τ);
double α = α_max;
// αₖᶻ = max(α ∈ (0, 1] : zₖ + αpₖᶻ ≥ (1−τⱼ)zₖ)
double α_z = FractionToTheBoundaryRule(z, p_z, τ);
// Loop until a step is accepted. If a step becomes acceptable, the loop
// will exit early.
while (1) {
Eigen::VectorXd trial_x = x + α * p_x;
Eigen::VectorXd trial_y = y + α_z * p_y;
Eigen::VectorXd trial_z = z + α_z * p_z;
xAD.SetValue(trial_x);
Eigen::VectorXd trial_c_e = c_eAD.Value();
Eigen::VectorXd trial_c_i = c_iAD.Value();
// If f(xₖ + αpₖˣ), cₑ(xₖ + αpₖˣ), or cᵢ(xₖ + αpₖˣ) aren't finite, reduce
// step size immediately
if (!std::isfinite(f.Value()) || !trial_c_e.allFinite() ||
!trial_c_i.allFinite()) {
// Reduce step size
α *= α_red_factor;
continue;
}
Eigen::VectorXd trial_s;
if (config.feasibleIPM && c_i.cwiseGreater(0.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 + α * p_s;
}
// Check whether filter accepts trial iterate
auto entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i, μ);
if (filter.TryAdd(entry)) {
// Accept step
break;
}
double prevConstraintViolation = c_e.lpNorm<1>() + (c_i - s).lpNorm<1>();
double nextConstraintViolation =
trial_c_e.lpNorm<1>() + (trial_c_i - trial_s).lpNorm<1>();
// Second-order corrections
//
// If first trial point was rejected and constraint violation stayed the
// same or went up, apply second-order corrections
if (nextConstraintViolation >= prevConstraintViolation) {
// Apply second-order corrections. See section 2.4 of [2].
Eigen::VectorXd p_x_cor = p_x;
Eigen::VectorXd p_y_soc = p_y;
Eigen::VectorXd p_z_soc = p_z;
Eigen::VectorXd p_s_soc = p_s;
double α_soc = α;
Eigen::VectorXd c_e_soc = c_e;
bool stepAcceptable = false;
for (int soc_iteration = 0; soc_iteration < 5 && !stepAcceptable;
++soc_iteration) {
// Rebuild Newton-KKT rhs with updated constraint values.
//
// rhs = −[∇f − Aₑᵀy + Aᵢᵀ(S⁻¹(Zcᵢ − μe) − z)]
// [ cₑˢᵒᶜ ]
//
// where cₑˢᵒᶜ = αc(xₖ) + c(xₖ + αpₖˣ)
c_e_soc = α_soc * c_e_soc + trial_c_e;
rhs.bottomRows(y.rows()) = -c_e_soc;
// Solve the Newton-KKT system
step = solver.Solve(rhs);
p_x_cor = step.segment(0, x.rows());
p_y_soc = -step.segment(x.rows(), y.rows());
// pₖᶻ = −Σcᵢ + μS⁻¹e − ΣAᵢpₖˣ
p_z_soc = -Σ * c_i + μ * Sinv * e - Σ * A_i * p_x_cor;
// pₖˢ = μZ⁻¹e − s − Z⁻¹Spₖᶻ
p_s_soc = μ * Zinv * e - s - Zinv * S * p_z_soc;
// αˢᵒᶜ = max(α ∈ (0, 1] : sₖ + αpₖˢ ≥ (1−τⱼ)sₖ)
α_soc = FractionToTheBoundaryRule(s, p_s_soc, τ);
trial_x = x + α_soc * p_x_cor;
trial_s = s + α_soc * p_s_soc;
// αₖᶻ = max(α ∈ (0, 1] : zₖ + αpₖᶻ ≥ (1−τⱼ)zₖ)
double α_z_soc = FractionToTheBoundaryRule(z, p_z_soc, τ);
trial_y = y + α_z_soc * p_y_soc;
trial_z = z + α_z_soc * p_z_soc;
xAD.SetValue(trial_x);
trial_c_e = c_eAD.Value();
trial_c_i = c_iAD.Value();
// Check whether filter accepts trial iterate
entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i, μ);
if (filter.TryAdd(entry)) {
p_x = p_x_cor;
p_y = p_y_soc;
p_z = p_z_soc;
p_s = p_s_soc;
α = α_soc;
α_z = α_z_soc;
stepAcceptable = true;
}
}
if (stepAcceptable) {
// Accept step
break;
}
}
// If we got here and α is the full step, the full step was rejected.
// Increment the full-step rejected counter to keep track of how many full
// steps have been rejected in a row.
if (α == α_max) {
++fullStepRejectedCounter;
}
// If the full step was rejected enough times in a row, reset the filter
// because it may be impeding progress.
//
// See section 3.2 case I of [2].
if (fullStepRejectedCounter >= 4 &&
filter.maxConstraintViolation > entry.constraintViolation / 10.0) {
filter.maxConstraintViolation *= 0.1;
filter.Reset();
continue;
}
// Reduce step size
α *= α_red_factor;
// Safety factor for the minimal step size
constexpr double α_min_frac = 0.05;
// If step size hit a minimum, check if the KKT error was reduced. If it
// wasn't, invoke feasibility restoration.
if (α < α_min_frac * Filter::γConstraint) {
double currentKKTError = KKTError(g, A_e, c_e, A_i, c_i, s, y, z, μ);
Eigen::VectorXd trial_x = x + α_max * p_x;
Eigen::VectorXd trial_s = s + α_max * p_s;
Eigen::VectorXd trial_y = y + α_z * p_y;
Eigen::VectorXd trial_z = z + α_z * p_z;
// Upate autodiff
xAD.SetValue(trial_x);
sAD.SetValue(trial_s);
yAD.SetValue(trial_y);
zAD.SetValue(trial_z);
Eigen::VectorXd trial_c_e = c_eAD.Value();
Eigen::VectorXd trial_c_i = c_iAD.Value();
double nextKKTError = KKTError(gradientF.Value(), jacobianCe.Value(),
trial_c_e, jacobianCi.Value(), trial_c_i,
trial_s, trial_y, trial_z, μ);
// If the step using αᵐᵃˣ reduced the KKT error, accept it anyway
if (nextKKTError <= 0.999 * currentKKTError) {
α = α_max;
// Accept step
break;
}
// If the step direction was bad and feasibility restoration is
// already running, running it again won't help
if (feasibilityRestoration) {
status->exitCondition = SolverExitCondition::kLocallyInfeasible;
return;
}
auto initialEntry = filter.MakeEntry(s, c_e, c_i, μ);
// Feasibility restoration phase
Eigen::VectorXd fr_x = x;
Eigen::VectorXd fr_s = s;
SolverStatus fr_status;
FeasibilityRestoration(
decisionVariables, equalityConstraints, inequalityConstraints, μ,
[&](const SolverIterationInfo& info) {
Eigen::VectorXd trial_x =
info.x.segment(0, decisionVariables.size());
xAD.SetValue(trial_x);
Eigen::VectorXd trial_s =
info.s.segment(0, inequalityConstraints.size());
sAD.SetValue(trial_s);
Eigen::VectorXd trial_c_e = c_eAD.Value();
Eigen::VectorXd trial_c_i = c_iAD.Value();
// If current iterate is acceptable to normal filter and
// constraint violation has sufficiently reduced, stop
// feasibility restoration
auto entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i, μ);
if (filter.IsAcceptable(entry) &&
entry.constraintViolation <
0.9 * initialEntry.constraintViolation) {
return true;
}
return false;
},
config, fr_x, fr_s, &fr_status);
if (fr_status.exitCondition ==
SolverExitCondition::kCallbackRequestedStop) {
p_x = fr_x - x;
p_s = fr_s - s;
// Lagrange mutliplier estimates
//
// [y] = (ÂÂᵀ)⁻¹Â[ ∇f]
// [z] [−μe]
//
// where  = [Aₑ 0]
// [Aᵢ −S]
//
// See equation (19.37) of [1].
{
xAD.SetValue(fr_x);
sAD.SetValue(c_iAD.Value());
A_e = jacobianCe.Value();
A_i = jacobianCi.Value();
g = gradientF.Value();
// Â = [Aₑ 0]
// [Aᵢ −S]
triplets.clear();
triplets.reserve(A_e.nonZeros() + A_i.nonZeros() + s.rows());
for (int col = 0; col < A_e.cols(); ++col) {
// Append column of Aₑ in top-left quadrant
for (Eigen::SparseMatrix<double>::InnerIterator it{A_e, col}; it;
++it) {
triplets.emplace_back(it.row(), it.col(), it.value());
}
// Append column of Aᵢ in bottom-left quadrant
for (Eigen::SparseMatrix<double>::InnerIterator it{A_i, col}; it;
++it) {
triplets.emplace_back(A_e.rows() + it.row(), it.col(),
it.value());
}
}
// Append −S in bottom-right quadrant
for (int i = 0; i < s.rows(); ++i) {
triplets.emplace_back(A_e.rows() + i, A_e.cols() + i, -s(i));
}
Eigen::SparseMatrix<double> Ahat{A_e.rows() + A_i.rows(),
A_e.cols() + s.rows()};
Ahat.setFromSortedTriplets(
triplets.begin(), triplets.end(),
[](const auto&, const auto& b) { return b; });
// lhs = ÂÂᵀ
Eigen::SparseMatrix<double> lhs = Ahat * Ahat.transpose();
// rhs = Â[ ∇f]
// [−μe]
Eigen::VectorXd rhsTemp{g.rows() + e.rows()};
rhsTemp.block(0, 0, g.rows(), 1) = g;
rhsTemp.block(g.rows(), 0, s.rows(), 1) = -μ * e;
Eigen::VectorXd rhs = Ahat * rhsTemp;
Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> yzEstimator{lhs};
Eigen::VectorXd sol = yzEstimator.solve(rhs);
p_y = y - sol.block(0, 0, y.rows(), 1);
p_z = z - sol.block(y.rows(), 0, z.rows(), 1);
}
α = 1.0;
α_z = 1.0;
// Accept step
break;
} else if (fr_status.exitCondition == SolverExitCondition::kSuccess) {
status->exitCondition = SolverExitCondition::kLocallyInfeasible;
x = fr_x;
return;
} else {
status->exitCondition =
SolverExitCondition::kFeasibilityRestorationFailed;
x = fr_x;
return;
}
}
}
// If full step was accepted, reset full-step rejected counter
if (α == α_max) {
fullStepRejectedCounter = 0;
}
// Handle very small search directions by letting αₖ = αₖᵐᵃˣ when
// max(|pₖˣ(i)|/(1 + |xₖ(i)|)) < 10ε_mach.
//
// See section 3.9 of [2].
double maxStepScaled = 0.0;
for (int row = 0; row < x.rows(); ++row) {
maxStepScaled = std::max(maxStepScaled,
std::abs(p_x(row)) / (1.0 + std::abs(x(row))));
}
if (maxStepScaled < 10.0 * std::numeric_limits<double>::epsilon()) {
α = α_max;
++stepTooSmallCounter;
} else {
stepTooSmallCounter = 0;
}
// xₖ₊₁ = xₖ + αₖpₖˣ
// sₖ₊₁ = sₖ + αₖpₖˢ
// yₖ₊₁ = yₖ + αₖᶻpₖʸ
// zₖ₊₁ = zₖ + αₖᶻpₖᶻ
x += α * p_x;
s += α * p_s;
y += α_z * p_y;
z += α_z * p_z;
// A requirement for the convergence proof is that the "primal-dual barrier
// term Hessian" Σₖ does not deviate arbitrarily much from the "primal
// Hessian" μⱼSₖ⁻². We ensure this by resetting
//
// zₖ₊₁⁽ⁱ⁾ = max(min(zₖ₊₁⁽ⁱ⁾, κ_Σ μⱼ/sₖ₊₁⁽ⁱ⁾), μⱼ/(κ_Σ sₖ₊₁⁽ⁱ⁾))
//
// for some fixed κ_Σ ≥ 1 after each step. See equation (16) of [2].
{
// Barrier parameter scale factor for inequality constraint Lagrange
// multiplier safeguard
constexpr double κ_Σ = 1e10;
for (int row = 0; row < z.rows(); ++row) {
z(row) =
std::max(std::min(z(row), κ_Σ * μ / s(row)), μ / (κ_Σ * s(row)));
}
}
// Update autodiff for Jacobians and Hessian
xAD.SetValue(x);
sAD.SetValue(s);
yAD.SetValue(y);
zAD.SetValue(z);
A_e = jacobianCe.Value();
A_i = jacobianCi.Value();
g = gradientF.Value();
H = hessianL.Value();
c_e = c_eAD.Value();
c_i = c_iAD.Value();
// Update the error estimate
E_0 = ErrorEstimate(g, A_e, c_e, A_i, c_i, s, y, z, 0.0);
if (E_0 < config.acceptableTolerance) {
++acceptableIterCounter;
} else {
acceptableIterCounter = 0;
}
// Update the barrier parameter if necessary
if (E_0 > config.tolerance) {
// Barrier parameter scale factor for tolerance checks
constexpr double κ_ε = 10.0;
// While the error estimate is below the desired threshold for this
// barrier parameter value, decrease the barrier parameter further
double E_μ = ErrorEstimate(g, A_e, c_e, A_i, c_i, s, y, z, μ);
while (μ > μ_min && E_μ <= κ_ε * μ) {
UpdateBarrierParameterAndResetFilter();
E_μ = ErrorEstimate(g, A_e, c_e, A_i, c_i, s, y, z, μ);
}
}
const auto innerIterEndTime = std::chrono::system_clock::now();
// Diagnostics for current iteration
if (config.diagnostics) {
if (iterations % 20 == 0) {
sleipnir::println("{:^4} {:^9} {:^13} {:^13} {:^13}", "iter",
"time (ms)", "error", "cost", "infeasibility");
sleipnir::println("{:=^61}", "");
}
sleipnir::println("{:4}{} {:9.3f} {:13e} {:13e} {:13e}", iterations,
feasibilityRestoration ? "r" : " ",
ToMilliseconds(innerIterEndTime - innerIterStartTime),
E_0, f.Value(),
c_e.lpNorm<1>() + (c_i - s).lpNorm<1>());
}
++iterations;
// Check for max iterations
if (iterations >= config.maxIterations) {
status->exitCondition = SolverExitCondition::kMaxIterationsExceeded;
return;
}
// Check for max wall clock time
if (innerIterEndTime - solveStartTime > config.timeout) {
status->exitCondition = SolverExitCondition::kTimeout;
return;
}
// Check for solve to acceptable tolerance
if (E_0 > config.tolerance &&
acceptableIterCounter == config.maxAcceptableIterations) {
status->exitCondition = SolverExitCondition::kSolvedToAcceptableTolerance;
return;
}
// The search direction has been very small twice, so assume the problem has
// been solved as well as possible given finite precision and reduce the
// barrier parameter.
//
// See section 3.9 of [2].
if (stepTooSmallCounter >= 2 && μ > μ_min) {
UpdateBarrierParameterAndResetFilter();
continue;
}
}
} // NOLINT(readability/fn_size)
} // namespace sleipnir

559
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/SQP.cpp vendored
View File

@@ -1,559 +0,0 @@
// Copyright (c) Sleipnir contributors
#include "sleipnir/optimization/solver/SQP.hpp"
#include <algorithm>
#include <chrono>
#include <cmath>
#include <fstream>
#include <limits>
#include <Eigen/SparseCholesky>
#include <wpi/SmallVector.h>
#include "optimization/RegularizedLDLT.hpp"
#include "optimization/solver/util/ErrorEstimate.hpp"
#include "optimization/solver/util/FeasibilityRestoration.hpp"
#include "optimization/solver/util/Filter.hpp"
#include "optimization/solver/util/IsLocallyInfeasible.hpp"
#include "optimization/solver/util/KKTError.hpp"
#include "sleipnir/autodiff/Gradient.hpp"
#include "sleipnir/autodiff/Hessian.hpp"
#include "sleipnir/autodiff/Jacobian.hpp"
#include "sleipnir/optimization/SolverExitCondition.hpp"
#include "sleipnir/util/Print.hpp"
#include "sleipnir/util/Spy.hpp"
#include "util/ScopeExit.hpp"
#include "util/ToMilliseconds.hpp"
// See docs/algorithms.md#Works_cited for citation definitions.
namespace sleipnir {
void SQP(std::span<Variable> decisionVariables,
std::span<Variable> equalityConstraints, Variable& f,
function_ref<bool(const SolverIterationInfo& info)> callback,
const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status) {
const auto solveStartTime = std::chrono::system_clock::now();
// Map decision variables and constraints to VariableMatrices for Lagrangian
VariableMatrix xAD{decisionVariables};
xAD.SetValue(x);
VariableMatrix c_eAD{equalityConstraints};
// Create autodiff variables for y for Lagrangian
VariableMatrix yAD(equalityConstraints.size());
for (auto& y : yAD) {
y.SetValue(0.0);
}
// Lagrangian L
//
// L(xₖ, yₖ) = f(xₖ) − yₖᵀcₑ(xₖ)
auto L = f - (yAD.T() * c_eAD)(0);
// Equality constraint Jacobian Aₑ
//
// [∇ᵀcₑ₁(xₖ)]
// Aₑ(x) = [∇ᵀcₑ₂(xₖ)]
// [ ⋮ ]
// [∇ᵀcₑₘ(xₖ)]
Jacobian jacobianCe{c_eAD, xAD};
Eigen::SparseMatrix<double> A_e = jacobianCe.Value();
// Gradient of f ∇f
Gradient gradientF{f, xAD};
Eigen::SparseVector<double> g = gradientF.Value();
// Hessian of the Lagrangian H
//
// Hₖ = ∇²ₓₓL(xₖ, yₖ)
Hessian hessianL{L, xAD};
Eigen::SparseMatrix<double> H = hessianL.Value();
Eigen::VectorXd y = yAD.Value();
Eigen::VectorXd c_e = c_eAD.Value();
// Check for overconstrained problem
if (equalityConstraints.size() > decisionVariables.size()) {
if (config.diagnostics) {
sleipnir::println("The problem has too few degrees of freedom.");
sleipnir::println(
"Violated constraints (cₑ(x) = 0) in order of declaration:");
for (int row = 0; row < c_e.rows(); ++row) {
if (c_e(row) < 0.0) {
sleipnir::println(" {}/{}: {} = 0", row + 1, c_e.rows(), c_e(row));
}
}
}
status->exitCondition = SolverExitCondition::kTooFewDOFs;
return;
}
// Check whether initial guess has finite f(xₖ) and cₑ(xₖ)
if (!std::isfinite(f.Value()) || !c_e.allFinite()) {
status->exitCondition =
SolverExitCondition::kNonfiniteInitialCostOrConstraints;
return;
}
// Sparsity pattern files written when spy flag is set in SolverConfig
std::ofstream H_spy;
std::ofstream A_e_spy;
if (config.spy) {
A_e_spy.open("A_e.spy");
H_spy.open("H.spy");
}
if (config.diagnostics) {
sleipnir::println("Error tolerance: {}\n", config.tolerance);
}
std::chrono::system_clock::time_point iterationsStartTime;
int iterations = 0;
// Prints final diagnostics when the solver exits
scope_exit exit{[&] {
status->cost = f.Value();
if (config.diagnostics) {
auto solveEndTime = std::chrono::system_clock::now();
sleipnir::println("\nSolve time: {:.3f} ms",
ToMilliseconds(solveEndTime - solveStartTime));
sleipnir::println(" ↳ {:.3f} ms (solver setup)",
ToMilliseconds(iterationsStartTime - solveStartTime));
if (iterations > 0) {
sleipnir::println(
" ↳ {:.3f} ms ({} solver iterations; {:.3f} ms average)",
ToMilliseconds(solveEndTime - iterationsStartTime), iterations,
ToMilliseconds((solveEndTime - iterationsStartTime) / iterations));
}
sleipnir::println("");
sleipnir::println("{:^8} {:^10} {:^14} {:^6}", "autodiff",
"setup (ms)", "avg solve (ms)", "solves");
sleipnir::println("{:=^47}", "");
constexpr auto format = "{:^8} {:10.3f} {:14.3f} {:6}";
sleipnir::println(format, "∇f(x)",
gradientF.GetProfiler().SetupDuration(),
gradientF.GetProfiler().AverageSolveDuration(),
gradientF.GetProfiler().SolveMeasurements());
sleipnir::println(format, "∇²ₓₓL", hessianL.GetProfiler().SetupDuration(),
hessianL.GetProfiler().AverageSolveDuration(),
hessianL.GetProfiler().SolveMeasurements());
sleipnir::println(format, "∂cₑ/∂x",
jacobianCe.GetProfiler().SetupDuration(),
jacobianCe.GetProfiler().AverageSolveDuration(),
jacobianCe.GetProfiler().SolveMeasurements());
sleipnir::println("");
}
}};
Filter filter{f};
// Kept outside the loop so its storage can be reused
wpi::SmallVector<Eigen::Triplet<double>> triplets;
RegularizedLDLT solver;
// Variables for determining when a step is acceptable
constexpr double α_red_factor = 0.5;
int acceptableIterCounter = 0;
int fullStepRejectedCounter = 0;
// Error estimate
double E_0 = std::numeric_limits<double>::infinity();
if (config.diagnostics) {
iterationsStartTime = std::chrono::system_clock::now();
}
while (E_0 > config.tolerance &&
acceptableIterCounter < config.maxAcceptableIterations) {
std::chrono::system_clock::time_point innerIterStartTime;
if (config.diagnostics) {
innerIterStartTime = std::chrono::system_clock::now();
}
// Check for local equality constraint infeasibility
if (IsEqualityLocallyInfeasible(A_e, c_e)) {
if (config.diagnostics) {
sleipnir::println(
"The problem is locally infeasible due to violated equality "
"constraints.");
sleipnir::println(
"Violated constraints (cₑ(x) = 0) in order of declaration:");
for (int row = 0; row < c_e.rows(); ++row) {
if (c_e(row) < 0.0) {
sleipnir::println(" {}/{}: {} = 0", row + 1, c_e.rows(), c_e(row));
}
}
}
status->exitCondition = SolverExitCondition::kLocallyInfeasible;
return;
}
// Check for diverging iterates
if (x.lpNorm<Eigen::Infinity>() > 1e20 || !x.allFinite()) {
status->exitCondition = SolverExitCondition::kDivergingIterates;
return;
}
// Write out spy file contents if that's enabled
if (config.spy) {
// Gap between sparsity patterns
if (iterations > 0) {
A_e_spy << "\n";
H_spy << "\n";
}
Spy(H_spy, H);
Spy(A_e_spy, A_e);
}
// Call user callback
if (callback({iterations, x, Eigen::VectorXd::Zero(0), g, H, A_e,
Eigen::SparseMatrix<double>{}})) {
status->exitCondition = SolverExitCondition::kCallbackRequestedStop;
return;
}
// lhs = [H Aₑᵀ]
// [Aₑ 0 ]
//
// Don't assign upper triangle because solver only uses lower triangle.
const Eigen::SparseMatrix<double> topLeft =
H.triangularView<Eigen::Lower>();
triplets.clear();
triplets.reserve(topLeft.nonZeros() + A_e.nonZeros());
for (int col = 0; col < H.cols(); ++col) {
// Append column of H + AᵢᵀΣAᵢ lower triangle in top-left quadrant
for (Eigen::SparseMatrix<double>::InnerIterator it{topLeft, col}; it;
++it) {
triplets.emplace_back(it.row(), it.col(), it.value());
}
// Append column of Aₑ in bottom-left quadrant
for (Eigen::SparseMatrix<double>::InnerIterator it{A_e, col}; it; ++it) {
triplets.emplace_back(H.rows() + it.row(), it.col(), it.value());
}
}
Eigen::SparseMatrix<double> lhs(
decisionVariables.size() + equalityConstraints.size(),
decisionVariables.size() + equalityConstraints.size());
lhs.setFromSortedTriplets(triplets.begin(), triplets.end(),
[](const auto&, const auto& b) { return b; });
// rhs = −[∇f − Aₑᵀy]
// [ cₑ ]
Eigen::VectorXd rhs{x.rows() + y.rows()};
rhs.segment(0, x.rows()) = -(g - A_e.transpose() * y);
rhs.segment(x.rows(), y.rows()) = -c_e;
// Solve the Newton-KKT system
//
// [H Aₑᵀ][ pₖˣ] = −[∇f − Aₑᵀy]
// [Aₑ 0 ][−pₖʸ] [ cₑ ]
solver.Compute(lhs, equalityConstraints.size(), config.tolerance / 10.0);
Eigen::VectorXd step{x.rows() + y.rows()};
if (solver.Info() == Eigen::Success) {
step = solver.Solve(rhs);
} else {
// The regularization procedure failed due to a rank-deficient equality
// constraint Jacobian with linearly dependent constraints
status->exitCondition = SolverExitCondition::kLocallyInfeasible;
return;
}
// step = [ pₖˣ]
// [−pₖʸ]
Eigen::VectorXd p_x = step.segment(0, x.rows());
Eigen::VectorXd p_y = -step.segment(x.rows(), y.rows());
constexpr double α_max = 1.0;
double α = α_max;
// Loop until a step is accepted. If a step becomes acceptable, the loop
// will exit early.
while (1) {
Eigen::VectorXd trial_x = x + α * p_x;
Eigen::VectorXd trial_y = y + α * p_y;
xAD.SetValue(trial_x);
Eigen::VectorXd trial_c_e = c_eAD.Value();
// If f(xₖ + αpₖˣ) or cₑ(xₖ + αpₖˣ) aren't finite, reduce step size
// immediately
if (!std::isfinite(f.Value()) || !trial_c_e.allFinite()) {
// Reduce step size
α *= α_red_factor;
continue;
}
// Check whether filter accepts trial iterate
auto entry = filter.MakeEntry(trial_c_e);
if (filter.TryAdd(entry)) {
// Accept step
break;
}
double prevConstraintViolation = c_e.lpNorm<1>();
double nextConstraintViolation = trial_c_e.lpNorm<1>();
// Second-order corrections
//
// If first trial point was rejected and constraint violation stayed the
// same or went up, apply second-order corrections
if (nextConstraintViolation >= prevConstraintViolation) {
// Apply second-order corrections. See section 2.4 of [2].
Eigen::VectorXd p_x_cor = p_x;
Eigen::VectorXd p_y_soc = p_y;
double α_soc = α;
Eigen::VectorXd c_e_soc = c_e;
bool stepAcceptable = false;
for (int soc_iteration = 0; soc_iteration < 5 && !stepAcceptable;
++soc_iteration) {
// Rebuild Newton-KKT rhs with updated constraint values.
//
// rhs = −[∇f − Aₑᵀy]
// [ cₑˢᵒᶜ ]
//
// where cₑˢᵒᶜ = αc(xₖ) + c(xₖ + αpₖˣ)
c_e_soc = α_soc * c_e_soc + trial_c_e;
rhs.bottomRows(y.rows()) = -c_e_soc;
// Solve the Newton-KKT system
step = solver.Solve(rhs);
p_x_cor = step.segment(0, x.rows());
p_y_soc = -step.segment(x.rows(), y.rows());
trial_x = x + α_soc * p_x_cor;
trial_y = y + α_soc * p_y_soc;
xAD.SetValue(trial_x);
trial_c_e = c_eAD.Value();
// Check whether filter accepts trial iterate
entry = filter.MakeEntry(trial_c_e);
if (filter.TryAdd(entry)) {
p_x = p_x_cor;
p_y = p_y_soc;
α = α_soc;
stepAcceptable = true;
}
}
if (stepAcceptable) {
// Accept step
break;
}
}
// If we got here and α is the full step, the full step was rejected.
// Increment the full-step rejected counter to keep track of how many full
// steps have been rejected in a row.
if (α == α_max) {
++fullStepRejectedCounter;
}
// If the full step was rejected enough times in a row, reset the filter
// because it may be impeding progress.
//
// See section 3.2 case I of [2].
if (fullStepRejectedCounter >= 4 &&
filter.maxConstraintViolation > entry.constraintViolation / 10.0) {
filter.maxConstraintViolation *= 0.1;
filter.Reset();
continue;
}
// Reduce step size
α *= α_red_factor;
// Safety factor for the minimal step size
constexpr double α_min_frac = 0.05;
// If step size hit a minimum, check if the KKT error was reduced. If it
// wasn't, report infeasible.
if (α < α_min_frac * Filter::γConstraint) {
double currentKKTError = KKTError(g, A_e, c_e, y);
Eigen::VectorXd trial_x = x + α_max * p_x;
Eigen::VectorXd trial_y = y + α_max * p_y;
// Upate autodiff
xAD.SetValue(trial_x);
yAD.SetValue(trial_y);
Eigen::VectorXd trial_c_e = c_eAD.Value();
double nextKKTError =
KKTError(gradientF.Value(), jacobianCe.Value(), trial_c_e, trial_y);
// If the step using αᵐᵃˣ reduced the KKT error, accept it anyway
if (nextKKTError <= 0.999 * currentKKTError) {
α = α_max;
// Accept step
break;
}
auto initialEntry = filter.MakeEntry(c_e);
// Feasibility restoration phase
Eigen::VectorXd fr_x = x;
SolverStatus fr_status;
FeasibilityRestoration(
decisionVariables, equalityConstraints,
[&](const SolverIterationInfo& info) {
trial_x = info.x.segment(0, decisionVariables.size());
xAD.SetValue(trial_x);
trial_c_e = c_eAD.Value();
// If current iterate is acceptable to normal filter and
// constraint violation has sufficiently reduced, stop
// feasibility restoration
entry = filter.MakeEntry(trial_c_e);
if (filter.IsAcceptable(entry) &&
entry.constraintViolation <
0.9 * initialEntry.constraintViolation) {
return true;
}
return false;
},
config, fr_x, &fr_status);
if (fr_status.exitCondition ==
SolverExitCondition::kCallbackRequestedStop) {
p_x = fr_x - x;
// Lagrange mutliplier estimates
//
// y = (AₑAₑᵀ)⁻¹Aₑ∇f
//
// See equation (19.37) of [1].
{
xAD.SetValue(fr_x);
A_e = jacobianCe.Value();
g = gradientF.Value();
// lhs = AₑAₑᵀ
Eigen::SparseMatrix<double> lhs = A_e * A_e.transpose();
// rhs = Aₑ∇f
Eigen::VectorXd rhs = A_e * g;
Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> yEstimator{lhs};
Eigen::VectorXd sol = yEstimator.solve(rhs);
p_y = y - sol.block(0, 0, y.rows(), 1);
}
α = 1.0;
// Accept step
break;
} else if (fr_status.exitCondition == SolverExitCondition::kSuccess) {
status->exitCondition = SolverExitCondition::kLocallyInfeasible;
x = fr_x;
return;
} else {
status->exitCondition =
SolverExitCondition::kFeasibilityRestorationFailed;
x = fr_x;
return;
}
}
}
// If full step was accepted, reset full-step rejected counter
if (α == α_max) {
fullStepRejectedCounter = 0;
}
// Handle very small search directions by letting αₖ = αₖᵐᵃˣ when
// max(|pₖˣ(i)|/(1 + |xₖ(i)|)) < 10ε_mach.
//
// See section 3.9 of [2].
double maxStepScaled = 0.0;
for (int row = 0; row < x.rows(); ++row) {
maxStepScaled = std::max(maxStepScaled,
std::abs(p_x(row)) / (1.0 + std::abs(x(row))));
}
if (maxStepScaled < 10.0 * std::numeric_limits<double>::epsilon()) {
α = α_max;
}
// xₖ₊₁ = xₖ + αₖpₖˣ
// yₖ₊₁ = yₖ + αₖpₖʸ
x += α * p_x;
y += α * p_y;
// Update autodiff for Jacobians and Hessian
xAD.SetValue(x);
yAD.SetValue(y);
A_e = jacobianCe.Value();
g = gradientF.Value();
H = hessianL.Value();
c_e = c_eAD.Value();
// Update the error estimate
E_0 = ErrorEstimate(g, A_e, c_e, y);
if (E_0 < config.acceptableTolerance) {
++acceptableIterCounter;
} else {
acceptableIterCounter = 0;
}
const auto innerIterEndTime = std::chrono::system_clock::now();
// Diagnostics for current iteration
if (config.diagnostics) {
if (iterations % 20 == 0) {
sleipnir::println("{:^4} {:^9} {:^13} {:^13} {:^13}", "iter",
"time (ms)", "error", "cost", "infeasibility");
sleipnir::println("{:=^61}", "");
}
sleipnir::println("{:4} {:9.3f} {:13e} {:13e} {:13e}", iterations,
ToMilliseconds(innerIterEndTime - innerIterStartTime),
E_0, f.Value(), c_e.lpNorm<1>());
}
++iterations;
// Check for max iterations
if (iterations >= config.maxIterations) {
status->exitCondition = SolverExitCondition::kMaxIterationsExceeded;
return;
}
// Check for max wall clock time
if (innerIterEndTime - solveStartTime > config.timeout) {
status->exitCondition = SolverExitCondition::kTimeout;
return;
}
// Check for solve to acceptable tolerance
if (E_0 > config.tolerance &&
acceptableIterCounter == config.maxAcceptableIterations) {
status->exitCondition = SolverExitCondition::kSolvedToAcceptableTolerance;
return;
}
}
}
} // namespace sleipnir

664
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/interior_point.cpp vendored Normal file
View File

@@ -0,0 +1,664 @@
// Copyright (c) Sleipnir contributors
#include "sleipnir/optimization/solver/interior_point.hpp"
#include <algorithm>
#include <chrono>
#include <cmath>
#include <functional>
#include <limits>
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <gch/small_vector.hpp>
#include "optimization/regularized_ldlt.hpp"
#include "optimization/solver/util/error_estimate.hpp"
#include "optimization/solver/util/filter.hpp"
#include "optimization/solver/util/fraction_to_the_boundary_rule.hpp"
#include "optimization/solver/util/is_locally_infeasible.hpp"
#include "optimization/solver/util/kkt_error.hpp"
#include "sleipnir/optimization/solver/exit_status.hpp"
#include "sleipnir/optimization/solver/iteration_info.hpp"
#include "sleipnir/optimization/solver/options.hpp"
#include "sleipnir/util/assert.hpp"
#include "util/print_diagnostics.hpp"
#include "util/scope_exit.hpp"
#include "util/scoped_profiler.hpp"
#include "util/solve_profiler.hpp"
// See docs/algorithms.md#Works_cited for citation definitions.
//
// See docs/algorithms.md#Interior-point_method for a derivation of the
// interior-point method formulation being used.
namespace {
/**
* Interior-point method step direction.
*/
struct Step {
/// Primal step.
Eigen::VectorXd p_x;
/// Equality constraint dual step.
Eigen::VectorXd p_y;
/// Inequality constraint slack variable step.
Eigen::VectorXd p_s;
/// Inequality constraint dual step.
Eigen::VectorXd p_z;
};
} // namespace
namespace slp {
ExitStatus interior_point(
const InteriorPointMatrixCallbacks& matrix_callbacks,
std::span<std::function<bool(const IterationInfo& info)>>
iteration_callbacks,
const Options& options,
#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
const Eigen::ArrayX<bool>& bound_constraint_mask,
#endif
Eigen::VectorXd& x) {
const auto solve_start_time = std::chrono::steady_clock::now();
gch::small_vector<SolveProfiler> solve_profilers;
solve_profilers.emplace_back("solver");
solve_profilers.emplace_back(" ↳ setup");
solve_profilers.emplace_back(" ↳ iteration");
solve_profilers.emplace_back(" ↳ feasibility ✓");
solve_profilers.emplace_back(" ↳ iteration callbacks");
solve_profilers.emplace_back(" ↳ iter matrix build");
solve_profilers.emplace_back(" ↳ iter matrix compute");
solve_profilers.emplace_back(" ↳ iter matrix solve");
solve_profilers.emplace_back(" ↳ line search");
solve_profilers.emplace_back(" ↳ SOC");
solve_profilers.emplace_back(" ↳ next iter prep");
solve_profilers.emplace_back(" ↳ f(x)");
solve_profilers.emplace_back(" ↳ ∇f(x)");
solve_profilers.emplace_back(" ↳ ∇²ₓₓL");
solve_profilers.emplace_back(" ↳ cₑ(x)");
solve_profilers.emplace_back(" ↳ ∂cₑ/∂x");
solve_profilers.emplace_back(" ↳ cᵢ(x)");
solve_profilers.emplace_back(" ↳ ∂cᵢ/∂x");
auto& solver_prof = solve_profilers[0];
auto& setup_prof = solve_profilers[1];
auto& inner_iter_prof = solve_profilers[2];
auto& feasibility_check_prof = solve_profilers[3];
auto& iteration_callbacks_prof = solve_profilers[4];
auto& linear_system_build_prof = solve_profilers[5];
auto& linear_system_compute_prof = solve_profilers[6];
auto& linear_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];
// Set up profiled matrix callbacks
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
auto& f_prof = solve_profilers[11];
auto& g_prof = solve_profilers[12];
auto& H_prof = solve_profilers[13];
auto& c_e_prof = solve_profilers[14];
auto& A_e_prof = solve_profilers[15];
auto& c_i_prof = solve_profilers[16];
auto& A_i_prof = solve_profilers[17];
InteriorPointMatrixCallbacks matrices{
[&](const Eigen::VectorXd& x) -> double {
ScopedProfiler prof{f_prof};
return matrix_callbacks.f(x);
},
[&](const Eigen::VectorXd& x) -> Eigen::SparseVector<double> {
ScopedProfiler prof{g_prof};
return matrix_callbacks.g(x);
},
[&](const Eigen::VectorXd& x, const Eigen::VectorXd& y,
const Eigen::VectorXd& z) -> Eigen::SparseMatrix<double> {
ScopedProfiler prof{H_prof};
return matrix_callbacks.H(x, y, z);
},
[&](const Eigen::VectorXd& x) -> Eigen::VectorXd {
ScopedProfiler prof{c_e_prof};
return matrix_callbacks.c_e(x);
},
[&](const Eigen::VectorXd& x) -> Eigen::SparseMatrix<double> {
ScopedProfiler prof{A_e_prof};
return matrix_callbacks.A_e(x);
},
[&](const Eigen::VectorXd& x) -> Eigen::VectorXd {
ScopedProfiler prof{c_i_prof};
return matrix_callbacks.c_i(x);
},
[&](const Eigen::VectorXd& x) -> Eigen::SparseMatrix<double> {
ScopedProfiler prof{A_i_prof};
return matrix_callbacks.A_i(x);
}};
#else
const auto& matrices = matrix_callbacks;
#endif
solver_prof.start();
setup_prof.start();
double f = matrices.f(x);
Eigen::VectorXd c_e = matrices.c_e(x);
Eigen::VectorXd c_i = matrices.c_i(x);
int num_decision_variables = x.rows();
int num_equality_constraints = c_e.rows();
int num_inequality_constraints = c_i.rows();
// Check for overconstrained problem
if (num_equality_constraints > num_decision_variables) {
if (options.diagnostics) {
print_too_few_dofs_error(c_e);
}
return ExitStatus::TOO_FEW_DOFS;
}
Eigen::SparseVector<double> g = matrices.g(x);
Eigen::SparseMatrix<double> A_e = matrices.A_e(x);
Eigen::SparseMatrix<double> A_i = matrices.A_i(x);
Eigen::VectorXd s = Eigen::VectorXd::Ones(num_inequality_constraints);
#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
// We set sʲ = cᵢʲ(x) for each bound inequality constraint index j
s = bound_constraint_mask.select(c_i, s);
#endif
Eigen::VectorXd y = Eigen::VectorXd::Zero(num_equality_constraints);
Eigen::VectorXd z = Eigen::VectorXd::Ones(num_inequality_constraints);
Eigen::SparseMatrix<double> H = matrices.H(x, y, z);
// Ensure matrix callback dimensions are consistent
slp_assert(g.rows() == num_decision_variables);
slp_assert(A_e.rows() == num_equality_constraints);
slp_assert(A_e.cols() == num_decision_variables);
slp_assert(A_i.rows() == num_inequality_constraints);
slp_assert(A_i.cols() == num_decision_variables);
slp_assert(H.rows() == num_decision_variables);
slp_assert(H.cols() == num_decision_variables);
// Check whether initial guess has finite f(xₖ), cₑ(xₖ), and cᵢ(xₖ)
if (!std::isfinite(f) || !c_e.allFinite() || !c_i.allFinite()) {
return ExitStatus::NONFINITE_INITIAL_COST_OR_CONSTRAINTS;
}
int iterations = 0;
// Barrier parameter minimum
const double μ_min = options.tolerance / 10.0;
// Barrier parameter μ
double μ = 0.1;
// Fraction-to-the-boundary rule scale factor minimum
constexpr double τ_min = 0.99;
// Fraction-to-the-boundary rule scale factor τ
double τ = τ_min;
Filter filter;
// This should be run when the error estimate is below a desired threshold for
// the current barrier parameter
auto update_barrier_parameter_and_reset_filter = [&] {
// Barrier parameter linear decrease power in "κ_μ μ". Range of (0, 1).
constexpr double κ_μ = 0.2;
// Barrier parameter superlinear decrease power in "μ^(θ_μ)". Range of (1,
// 2).
constexpr double θ_μ = 1.5;
// Update the barrier parameter.
//
// μⱼ₊₁ = max(εₜₒₗ/10, min(κ_μ μⱼ, μⱼ^θ_μ))
//
// See equation (7) of [2].
μ = std::max(μ_min, std::min(κ_μ * μ, std::pow(μ, θ_μ)));
// Update the fraction-to-the-boundary rule scaling factor.
//
// τⱼ = max(τₘᵢₙ, 1 − μⱼ)
//
// See equation (8) of [2].
τ = std::max(τ_min, 1.0 - μ);
// Reset the filter when the barrier parameter is updated
filter.reset();
};
// Kept outside the loop so its storage can be reused
gch::small_vector<Eigen::Triplet<double>> triplets;
RegularizedLDLT solver{num_decision_variables, num_equality_constraints};
// Variables for determining when a step is acceptable
constexpr double α_reduction_factor = 0.5;
constexpr double α_min = 1e-7;
int full_step_rejected_counter = 0;
// Error estimate
double E_0 = std::numeric_limits<double>::infinity();
setup_prof.stop();
// Prints final solver diagnostics when the solver exits
scope_exit exit{[&] {
if (options.diagnostics) {
solver_prof.stop();
if (iterations > 0) {
print_bottom_iteration_diagnostics();
}
print_solver_diagnostics(solve_profilers);
}
}};
while (E_0 > options.tolerance) {
ScopedProfiler inner_iter_profiler{inner_iter_prof};
ScopedProfiler feasibility_check_profiler{feasibility_check_prof};
// Check for local equality constraint infeasibility
if (is_equality_locally_infeasible(A_e, c_e)) {
if (options.diagnostics) {
print_c_e_local_infeasibility_error(c_e);
}
return ExitStatus::LOCALLY_INFEASIBLE;
}
// Check for local inequality constraint infeasibility
if (is_inequality_locally_infeasible(A_i, c_i)) {
if (options.diagnostics) {
print_c_i_local_infeasibility_error(c_i);
}
return ExitStatus::LOCALLY_INFEASIBLE;
}
// Check for diverging iterates
if (x.lpNorm<Eigen::Infinity>() > 1e10 || !x.allFinite() ||
s.lpNorm<Eigen::Infinity>() > 1e10 || !s.allFinite()) {
return ExitStatus::DIVERGING_ITERATES;
}
feasibility_check_profiler.stop();
ScopedProfiler iteration_callbacks_profiler{iteration_callbacks_prof};
// Call iteration callbacks
for (const auto& callback : iteration_callbacks) {
if (callback({iterations, x, g, H, A_e, A_i})) {
return ExitStatus::CALLBACK_REQUESTED_STOP;
}
}
iteration_callbacks_profiler.stop();
ScopedProfiler linear_system_build_profiler{linear_system_build_prof};
// S = diag(s)
// Z = diag(z)
// Σ = S⁻¹Z
const Eigen::SparseMatrix<double> Σ{s.cwiseInverse().asDiagonal() *
z.asDiagonal()};
// lhs = [H + AᵢᵀΣAᵢ Aₑᵀ]
// [ Aₑ 0 ]
//
// Don't assign upper triangle because solver only uses lower triangle.
const Eigen::SparseMatrix<double> top_left =
H + (A_i.transpose() * Σ * A_i).triangularView<Eigen::Lower>();
triplets.clear();
triplets.reserve(top_left.nonZeros() + A_e.nonZeros());
for (int col = 0; col < H.cols(); ++col) {
// Append column of H + AᵢᵀΣAᵢ lower triangle in top-left quadrant
for (Eigen::SparseMatrix<double>::InnerIterator it{top_left, col}; it;
++it) {
triplets.emplace_back(it.row(), it.col(), it.value());
}
// Append column of Aₑ in bottom-left quadrant
for (Eigen::SparseMatrix<double>::InnerIterator it{A_e, col}; it; ++it) {
triplets.emplace_back(H.rows() + it.row(), it.col(), it.value());
}
}
Eigen::SparseMatrix<double> lhs(
num_decision_variables + num_equality_constraints,
num_decision_variables + num_equality_constraints);
lhs.setFromSortedTriplets(triplets.begin(), triplets.end(),
[](const auto&, const auto& b) { return b; });
// rhs = −[∇f − Aₑᵀy − Aᵢᵀ(−Σcᵢ + μS⁻¹e + z)]
// [ cₑ ]
Eigen::VectorXd rhs{x.rows() + y.rows()};
rhs.segment(0, x.rows()) =
-g + A_e.transpose() * y +
A_i.transpose() * (-Σ * c_i + μ * s.cwiseInverse() + z);
rhs.segment(x.rows(), y.rows()) = -c_e;
linear_system_build_profiler.stop();
ScopedProfiler linear_system_compute_profiler{linear_system_compute_prof};
Step step;
double α_max = 1.0;
double α = 1.0;
double α_z = 1.0;
// Solve the Newton-KKT system
//
// [H + AᵢᵀΣAᵢ Aₑᵀ][ pˣ] = −[∇f − Aₑᵀy − Aᵢᵀ(−Σcᵢ + μS⁻¹e + z)]
// [ Aₑ 0 ][−pʸ] [ cₑ ]
if (solver.compute(lhs).info() != Eigen::Success) [[unlikely]] {
return ExitStatus::FACTORIZATION_FAILED;
}
linear_system_compute_profiler.stop();
ScopedProfiler linear_system_solve_profiler{linear_system_solve_prof};
auto compute_step = [&](Step& step) {
// p = [ pˣ]
// [−pʸ]
Eigen::VectorXd p = solver.solve(rhs);
step.p_x = p.segment(0, x.rows());
step.p_y = -p.segment(x.rows(), y.rows());
// pˢ = cᵢ − s + Aᵢpˣ
// pᶻ = −Σcᵢ + μS⁻¹e − ΣAᵢpˣ
step.p_s = c_i - s + A_i * step.p_x;
step.p_z = -Σ * c_i + μ * s.cwiseInverse() - Σ * A_i * step.p_x;
};
compute_step(step);
linear_system_solve_profiler.stop();
ScopedProfiler line_search_profiler{line_search_prof};
// αᵐᵃˣ = max(α ∈ (0, 1] : sₖ + αpₖˢ ≥ (1−τⱼ)sₖ)
α_max = fraction_to_the_boundary_rule(s, step.p_s, τ);
α = α_max;
// If maximum step size is below minimum, report line search failure
if (α < α_min) {
return ExitStatus::LINE_SEARCH_FAILED;
}
// αₖᶻ = max(α ∈ (0, 1] : zₖ + αpₖᶻ ≥ (1−τⱼ)zₖ)
α_z = fraction_to_the_boundary_rule(z, step.p_z, τ);
// Loop until a step is accepted
while (1) {
Eigen::VectorXd trial_x = x + α * step.p_x;
Eigen::VectorXd trial_y = y + α_z * step.p_y;
Eigen::VectorXd trial_z = z + α_z * step.p_z;
double trial_f = matrices.f(trial_x);
Eigen::VectorXd trial_c_e = matrices.c_e(trial_x);
Eigen::VectorXd 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
if (!std::isfinite(trial_f) || !trial_c_e.allFinite() ||
!trial_c_i.allFinite()) {
// Reduce step size
α *= α_reduction_factor;
if (α < α_min) {
return ExitStatus::LINE_SEARCH_FAILED;
}
continue;
}
Eigen::VectorXd trial_s;
if (options.feasible_ipm && c_i.cwiseGreater(0.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
if (filter.try_add(FilterEntry{trial_f, trial_s, trial_c_e, trial_c_i, μ},
α)) {
// Accept step
break;
}
double prev_constraint_violation =
c_e.lpNorm<1>() + (c_i - s).lpNorm<1>();
double next_constraint_violation =
trial_c_e.lpNorm<1>() + (trial_c_i - trial_s).lpNorm<1>();
// Second-order corrections
//
// If first trial point was rejected and constraint violation stayed the
// same or went up, apply second-order corrections
if (α == α_max &&
next_constraint_violation >= prev_constraint_violation) {
// Apply second-order corrections. See section 2.4 of [2].
auto soc_step = step;
double α_soc = α;
double α_z_soc = α_z;
Eigen::VectorXd c_e_soc = c_e;
bool step_acceptable = false;
for (int soc_iteration = 0; soc_iteration < 5 && !step_acceptable;
++soc_iteration) {
ScopedProfiler soc_profiler{soc_prof};
scope_exit soc_exit{[&] {
soc_profiler.stop();
if (options.diagnostics) {
print_iteration_diagnostics(
iterations,
step_acceptable ? IterationType::ACCEPTED_SOC
: IterationType::REJECTED_SOC,
soc_profiler.current_duration(),
error_estimate(g, A_e, trial_c_e, trial_y), trial_f,
trial_c_e.lpNorm<1>() + (trial_c_i - trial_s).lpNorm<1>(),
trial_s.dot(trial_z), μ, solver.hessian_regularization(),
α_soc, 1.0, α_reduction_factor, α_z_soc);
}
}};
// Rebuild Newton-KKT rhs with updated constraint values.
//
// rhs = −[∇f − Aₑᵀy − Aᵢᵀ(−Σcᵢ + μS⁻¹e + z)]
// [ cₑˢᵒᶜ ]
//
// where cₑˢᵒᶜ = αc(xₖ) + c(xₖ + αpₖˣ)
c_e_soc = α_soc * c_e_soc + trial_c_e;
rhs.bottomRows(y.rows()) = -c_e_soc;
// Solve the Newton-KKT system
compute_step(soc_step);
// αˢᵒᶜ = max(α ∈ (0, 1] : sₖ + αpₖˢ ≥ (1−τⱼ)sₖ)
α_soc = fraction_to_the_boundary_rule(s, soc_step.p_s, τ);
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(z, soc_step.p_z, τ);
trial_y = y + α_z_soc * soc_step.p_y;
trial_z = z + α_z_soc * soc_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);
// Constraint violation scale factor for second-order corrections
constexpr double κ_soc = 0.99;
// If constraint violation hasn't been sufficiently reduced, stop
// making second-order corrections
next_constraint_violation =
trial_c_e.lpNorm<1>() + (trial_c_i - trial_s).lpNorm<1>();
if (next_constraint_violation > κ_soc * prev_constraint_violation) {
break;
}
// Check whether filter accepts trial iterate
if (filter.try_add(
FilterEntry{trial_f, trial_s, trial_c_e, trial_c_i, μ}, α)) {
step = soc_step;
α = α_soc;
α_z = α_z_soc;
step_acceptable = true;
}
}
if (step_acceptable) {
// Accept step
break;
}
}
// If we got here and α is the full step, the full step was rejected.
// Increment the full-step rejected counter to keep track of how many full
// steps have been rejected in a row.
if (α == α_max) {
++full_step_rejected_counter;
}
// If the full step was rejected enough times in a row, reset the filter
// because it may be impeding progress.
//
// See section 3.2 case I of [2].
if (full_step_rejected_counter >= 4 &&
filter.max_constraint_violation >
filter.back().constraint_violation / 10.0) {
filter.max_constraint_violation *= 0.1;
filter.reset();
continue;
}
// Reduce step size
α *= α_reduction_factor;
// If step size hit a minimum, check if the KKT error was reduced. If it
// wasn't, report line search failure.
if (α < α_min) {
double current_kkt_error = kkt_error(g, A_e, c_e, A_i, c_i, s, y, z, μ);
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_c_e = matrices.c_e(trial_x);
trial_c_i = matrices.c_i(trial_x);
double next_kkt_error = kkt_error(
matrices.g(trial_x), matrices.A_e(trial_x), matrices.c_e(trial_x),
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 <= 0.999 * current_kkt_error) {
α = α_max;
// Accept step
break;
}
return ExitStatus::LINE_SEARCH_FAILED;
}
}
line_search_profiler.stop();
// 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;
// A requirement for the convergence proof is that the primal-dual barrier
// term Hessian Σₖ₊₁ does not deviate arbitrarily much from the primal
// barrier term Hessian μSₖ₊₁⁻².
//
// Σₖ₊₁ = μSₖ₊₁⁻²
// Sₖ₊₁⁻¹Zₖ₊₁ = μSₖ₊₁⁻²
// Zₖ₊₁ = μSₖ₊₁⁻¹
//
// We ensure this by resetting
//
// zₖ₊₁ = clamp(zₖ₊₁, 1/κ_Σ μ/sₖ₊₁, κ_Σ μ/sₖ₊₁)
//
// for some fixed κ_Σ ≥ 1 after each step. See equation (16) of [2].
for (int row = 0; row < z.rows(); ++row) {
constexpr double κ_Σ = 1e10;
z[row] = std::clamp(z[row], 1.0 / κ_Σ * μ / s[row], κ_Σ * μ / s[row]);
}
// 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 estimate
E_0 = error_estimate(g, A_e, c_e, A_i, c_i, s, y, z, 0.0);
// Update the barrier parameter if necessary
if (E_0 > options.tolerance) {
// Barrier parameter scale factor for tolerance checks
constexpr double κ_ε = 10.0;
// While the error estimate is below the desired threshold for this
// barrier parameter value, decrease the barrier parameter further
double E_μ = error_estimate(g, A_e, c_e, A_i, c_i, s, y, z, μ);
while (μ > μ_min && E_μ <= κ_ε * μ) {
update_barrier_parameter_and_reset_filter();
E_μ = error_estimate(g, A_e, c_e, A_i, c_i, s, y, z, μ);
}
}
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.lpNorm<1>() + (c_i - s).lpNorm<1>(),
s.dot(z), μ, solver.hessian_regularization(),
α, α_max, α_reduction_factor, α_z);
}
++iterations;
// Check for max iterations
if (iterations >= options.max_iterations) {
return ExitStatus::MAX_ITERATIONS_EXCEEDED;
}
// Check for max wall clock time
if (std::chrono::steady_clock::now() - solve_start_time > options.timeout) {
return ExitStatus::TIMEOUT;
}
}
return ExitStatus::SUCCESS;
}
} // namespace slp

259
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/newton.cpp vendored Normal file
View File

@@ -0,0 +1,259 @@
// Copyright (c) Sleipnir contributors
#include "sleipnir/optimization/solver/newton.hpp"
#include <algorithm>
#include <chrono>
#include <cmath>
#include <functional>
#include <limits>
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <gch/small_vector.hpp>
#include "optimization/regularized_ldlt.hpp"
#include "optimization/solver/util/error_estimate.hpp"
#include "optimization/solver/util/filter.hpp"
#include "optimization/solver/util/kkt_error.hpp"
#include "sleipnir/optimization/solver/exit_status.hpp"
#include "sleipnir/optimization/solver/iteration_info.hpp"
#include "sleipnir/optimization/solver/options.hpp"
#include "sleipnir/util/assert.hpp"
#include "util/print_diagnostics.hpp"
#include "util/scope_exit.hpp"
#include "util/scoped_profiler.hpp"
#include "util/solve_profiler.hpp"
// See docs/algorithms.md#Works_cited for citation definitions.
namespace slp {
ExitStatus newton(const NewtonMatrixCallbacks& matrix_callbacks,
std::span<std::function<bool(const IterationInfo& info)>>
iteration_callbacks,
const Options& options, Eigen::VectorXd& x) {
const auto solve_start_time = std::chrono::steady_clock::now();
gch::small_vector<SolveProfiler> solve_profilers;
solve_profilers.emplace_back("solver");
solve_profilers.emplace_back(" ↳ setup");
solve_profilers.emplace_back(" ↳ iteration");
solve_profilers.emplace_back(" ↳ feasibility ✓");
solve_profilers.emplace_back(" ↳ iteration callbacks");
solve_profilers.emplace_back(" ↳ iter matrix compute");
solve_profilers.emplace_back(" ↳ iter matrix 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");
auto& solver_prof = solve_profilers[0];
auto& setup_prof = solve_profilers[1];
auto& inner_iter_prof = solve_profilers[2];
auto& feasibility_check_prof = solve_profilers[3];
auto& iteration_callbacks_prof = solve_profilers[4];
auto& linear_system_compute_prof = solve_profilers[5];
auto& linear_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];
NewtonMatrixCallbacks matrices{
[&](const Eigen::VectorXd& x) -> double {
ScopedProfiler prof{f_prof};
return matrix_callbacks.f(x);
},
[&](const Eigen::VectorXd& x) -> Eigen::SparseVector<double> {
ScopedProfiler prof{g_prof};
return matrix_callbacks.g(x);
},
[&](const Eigen::VectorXd& x) -> Eigen::SparseMatrix<double> {
ScopedProfiler prof{H_prof};
return matrix_callbacks.H(x);
}};
#else
const auto& matrices = matrix_callbacks;
#endif
solver_prof.start();
setup_prof.start();
double f = matrices.f(x);
int num_decision_variables = x.rows();
Eigen::SparseVector<double> g = matrices.g(x);
Eigen::SparseMatrix<double> H = matrices.H(x);
// Ensure matrix callback dimensions are consistent
slp_assert(g.rows() == num_decision_variables);
slp_assert(H.rows() == num_decision_variables);
slp_assert(H.cols() == num_decision_variables);
// Check whether initial guess has finite f(xₖ)
if (!std::isfinite(f)) {
return ExitStatus::NONFINITE_INITIAL_COST_OR_CONSTRAINTS;
}
int iterations = 0;
Filter filter;
RegularizedLDLT solver{num_decision_variables, 0};
// Variables for determining when a step is acceptable
constexpr double α_reduction_factor = 0.5;
constexpr double α_min = 1e-20;
// Error estimate
double E_0 = std::numeric_limits<double>::infinity();
setup_prof.stop();
// Prints final solver diagnostics when the solver exits
scope_exit exit{[&] {
if (options.diagnostics) {
solver_prof.stop();
if (iterations > 0) {
print_bottom_iteration_diagnostics();
}
print_solver_diagnostics(solve_profilers);
}
}};
while (E_0 > options.tolerance) {
ScopedProfiler inner_iter_profiler{inner_iter_prof};
ScopedProfiler feasibility_check_profiler{feasibility_check_prof};
// Check for diverging iterates
if (x.lpNorm<Eigen::Infinity>() > 1e10 || !x.allFinite()) {
return ExitStatus::DIVERGING_ITERATES;
}
feasibility_check_profiler.stop();
ScopedProfiler iteration_callbacks_profiler{iteration_callbacks_prof};
// Call iteration callbacks
for (const auto& callback : iteration_callbacks) {
if (callback({iterations, x, g, H, Eigen::SparseMatrix<double>{},
Eigen::SparseMatrix<double>{}})) {
return ExitStatus::CALLBACK_REQUESTED_STOP;
}
}
iteration_callbacks_profiler.stop();
ScopedProfiler linear_system_compute_profiler{linear_system_compute_prof};
// Solve the Newton-KKT system
//
// Hpˣ = −∇f
solver.compute(H);
linear_system_compute_profiler.stop();
ScopedProfiler linear_system_solve_profiler{linear_system_solve_prof};
Eigen::VectorXd p_x = solver.solve(-g);
linear_system_solve_profiler.stop();
ScopedProfiler line_search_profiler{line_search_prof};
constexpr double α_max = 1.0;
double α = α_max;
// Loop until a step is accepted. If a step becomes acceptable, the loop
// will exit early.
while (1) {
Eigen::VectorXd trial_x = x + α * p_x;
double trial_f = matrices.f(trial_x);
// If f(xₖ + αpₖˣ) isn't finite, reduce step size immediately
if (!std::isfinite(trial_f)) {
// Reduce step size
α *= α_reduction_factor;
if (α < α_min) {
return ExitStatus::LINE_SEARCH_FAILED;
}
continue;
}
// Check whether filter accepts trial iterate
if (filter.try_add(FilterEntry{trial_f}, α)) {
// Accept step
break;
}
// Reduce step size
α *= α_reduction_factor;
// If step size hit a minimum, check if the KKT error was reduced. If it
// wasn't, report bad line search.
if (α < α_min) {
double current_kkt_error = kkt_error(g);
Eigen::VectorXd trial_x = x + α_max * p_x;
double next_kkt_error = kkt_error(matrices.g(trial_x));
// If the step using αᵐᵃˣ reduced the KKT error, accept it anyway
if (next_kkt_error <= 0.999 * current_kkt_error) {
α = α_max;
// Accept step
break;
}
return ExitStatus::LINE_SEARCH_FAILED;
}
}
line_search_profiler.stop();
// xₖ₊₁ = xₖ + αₖpₖˣ
x += α * p_x;
// 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 estimate
E_0 = error_estimate(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, 0.0, 0.0, 0.0,
solver.hessian_regularization(), α, α_max, α_reduction_factor, 1.0);
}
++iterations;
// Check for max iterations
if (iterations >= options.max_iterations) {
return ExitStatus::MAX_ITERATIONS_EXCEEDED;
}
// Check for max wall clock time
if (std::chrono::steady_clock::now() - solve_start_time > options.timeout) {
return ExitStatus::TIMEOUT;
}
}
return ExitStatus::SUCCESS;
}
} // namespace slp

480
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/sqp.cpp vendored Normal file
View File

@@ -0,0 +1,480 @@
// Copyright (c) Sleipnir contributors
#include "sleipnir/optimization/solver/sqp.hpp"
#include <algorithm>
#include <chrono>
#include <cmath>
#include <functional>
#include <limits>
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <gch/small_vector.hpp>
#include "optimization/regularized_ldlt.hpp"
#include "optimization/solver/util/error_estimate.hpp"
#include "optimization/solver/util/filter.hpp"
#include "optimization/solver/util/is_locally_infeasible.hpp"
#include "optimization/solver/util/kkt_error.hpp"
#include "sleipnir/optimization/solver/exit_status.hpp"
#include "sleipnir/optimization/solver/iteration_info.hpp"
#include "sleipnir/optimization/solver/options.hpp"
#include "sleipnir/util/assert.hpp"
#include "util/print_diagnostics.hpp"
#include "util/scope_exit.hpp"
#include "util/scoped_profiler.hpp"
#include "util/solve_profiler.hpp"
// See docs/algorithms.md#Works_cited for citation definitions.
namespace {
/**
* SQP step direction.
*/
struct Step {
/// Primal step.
Eigen::VectorXd p_x;
/// Dual step.
Eigen::VectorXd p_y;
};
} // namespace
namespace slp {
ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks,
std::span<std::function<bool(const IterationInfo& info)>>
iteration_callbacks,
const Options& options, Eigen::VectorXd& x) {
const auto solve_start_time = std::chrono::steady_clock::now();
gch::small_vector<SolveProfiler> solve_profilers;
solve_profilers.emplace_back("solver");
solve_profilers.emplace_back(" ↳ setup");
solve_profilers.emplace_back(" ↳ iteration");
solve_profilers.emplace_back(" ↳ feasibility ✓");
solve_profilers.emplace_back(" ↳ iteration callbacks");
solve_profilers.emplace_back(" ↳ iter matrix build");
solve_profilers.emplace_back(" ↳ iter matrix compute");
solve_profilers.emplace_back(" ↳ iter matrix solve");
solve_profilers.emplace_back(" ↳ line search");
solve_profilers.emplace_back(" ↳ SOC");
solve_profilers.emplace_back(" ↳ next iter prep");
solve_profilers.emplace_back(" ↳ f(x)");
solve_profilers.emplace_back(" ↳ ∇f(x)");
solve_profilers.emplace_back(" ↳ ∇²ₓₓL");
solve_profilers.emplace_back(" ↳ cₑ(x)");
solve_profilers.emplace_back(" ↳ ∂cₑ/∂x");
auto& solver_prof = solve_profilers[0];
auto& setup_prof = solve_profilers[1];
auto& inner_iter_prof = solve_profilers[2];
auto& feasibility_check_prof = solve_profilers[3];
auto& iteration_callbacks_prof = solve_profilers[4];
auto& linear_system_build_prof = solve_profilers[5];
auto& linear_system_compute_prof = solve_profilers[6];
auto& linear_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];
// Set up profiled matrix callbacks
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
auto& f_prof = solve_profilers[11];
auto& g_prof = solve_profilers[12];
auto& H_prof = solve_profilers[13];
auto& c_e_prof = solve_profilers[14];
auto& A_e_prof = solve_profilers[15];
SQPMatrixCallbacks matrices{
[&](const Eigen::VectorXd& x) -> double {
ScopedProfiler prof{f_prof};
return matrix_callbacks.f(x);
},
[&](const Eigen::VectorXd& x) -> Eigen::SparseVector<double> {
ScopedProfiler prof{g_prof};
return matrix_callbacks.g(x);
},
[&](const Eigen::VectorXd& x,
const Eigen::VectorXd& y) -> Eigen::SparseMatrix<double> {
ScopedProfiler prof{H_prof};
return matrix_callbacks.H(x, y);
},
[&](const Eigen::VectorXd& x) -> Eigen::VectorXd {
ScopedProfiler prof{c_e_prof};
return matrix_callbacks.c_e(x);
},
[&](const Eigen::VectorXd& x) -> Eigen::SparseMatrix<double> {
ScopedProfiler prof{A_e_prof};
return matrix_callbacks.A_e(x);
}};
#else
const auto& matrices = matrix_callbacks;
#endif
solver_prof.start();
setup_prof.start();
double f = matrices.f(x);
Eigen::VectorXd c_e = matrices.c_e(x);
int num_decision_variables = x.rows();
int num_equality_constraints = c_e.rows();
// Check for overconstrained problem
if (num_equality_constraints > num_decision_variables) {
if (options.diagnostics) {
print_too_few_dofs_error(c_e);
}
return ExitStatus::TOO_FEW_DOFS;
}
Eigen::SparseVector<double> g = matrices.g(x);
Eigen::SparseMatrix<double> A_e = matrices.A_e(x);
Eigen::VectorXd y = Eigen::VectorXd::Zero(num_equality_constraints);
Eigen::SparseMatrix<double> H = matrices.H(x, y);
// Ensure matrix callback dimensions are consistent
slp_assert(g.rows() == num_decision_variables);
slp_assert(A_e.rows() == num_equality_constraints);
slp_assert(A_e.cols() == num_decision_variables);
slp_assert(H.rows() == num_decision_variables);
slp_assert(H.cols() == num_decision_variables);
// Check whether initial guess has finite f(xₖ) and cₑ(xₖ)
if (!std::isfinite(f) || !c_e.allFinite()) {
return ExitStatus::NONFINITE_INITIAL_COST_OR_CONSTRAINTS;
}
int iterations = 0;
Filter filter;
// Kept outside the loop so its storage can be reused
gch::small_vector<Eigen::Triplet<double>> triplets;
RegularizedLDLT solver{num_decision_variables, num_equality_constraints};
// Variables for determining when a step is acceptable
constexpr double α_reduction_factor = 0.5;
constexpr double α_min = 1e-7;
int full_step_rejected_counter = 0;
// Error estimate
double E_0 = std::numeric_limits<double>::infinity();
setup_prof.stop();
// Prints final solver diagnostics when the solver exits
scope_exit exit{[&] {
if (options.diagnostics) {
solver_prof.stop();
if (iterations > 0) {
print_bottom_iteration_diagnostics();
}
print_solver_diagnostics(solve_profilers);
}
}};
while (E_0 > options.tolerance) {
ScopedProfiler inner_iter_profiler{inner_iter_prof};
ScopedProfiler feasibility_check_profiler{feasibility_check_prof};
// Check for local equality constraint infeasibility
if (is_equality_locally_infeasible(A_e, c_e)) {
if (options.diagnostics) {
print_c_e_local_infeasibility_error(c_e);
}
return ExitStatus::LOCALLY_INFEASIBLE;
}
// Check for diverging iterates
if (x.lpNorm<Eigen::Infinity>() > 1e10 || !x.allFinite()) {
return ExitStatus::DIVERGING_ITERATES;
}
feasibility_check_profiler.stop();
ScopedProfiler iteration_callbacks_profiler{iteration_callbacks_prof};
// Call iteration callbacks
for (const auto& callback : iteration_callbacks) {
if (callback({iterations, x, g, H, A_e, Eigen::SparseMatrix<double>{}})) {
return ExitStatus::CALLBACK_REQUESTED_STOP;
}
}
iteration_callbacks_profiler.stop();
ScopedProfiler linear_system_build_profiler{linear_system_build_prof};
// lhs = [H Aₑᵀ]
// [Aₑ 0 ]
//
// Don't assign upper triangle because solver only uses lower triangle.
triplets.clear();
triplets.reserve(H.nonZeros() + A_e.nonZeros());
for (int col = 0; col < H.cols(); ++col) {
// Append column of H lower triangle in top-left quadrant
for (Eigen::SparseMatrix<double>::InnerIterator it{H, col}; it; ++it) {
triplets.emplace_back(it.row(), it.col(), it.value());
}
// Append column of Aₑ in bottom-left quadrant
for (Eigen::SparseMatrix<double>::InnerIterator it{A_e, col}; it; ++it) {
triplets.emplace_back(H.rows() + it.row(), it.col(), it.value());
}
}
Eigen::SparseMatrix<double> lhs(
num_decision_variables + num_equality_constraints,
num_decision_variables + num_equality_constraints);
lhs.setFromSortedTriplets(triplets.begin(), triplets.end(),
[](const auto&, const auto& b) { return b; });
// rhs = −[∇f − Aₑᵀy]
// [ cₑ ]
Eigen::VectorXd rhs{x.rows() + y.rows()};
rhs.segment(0, x.rows()) = -g + A_e.transpose() * y;
rhs.segment(x.rows(), y.rows()) = -c_e;
linear_system_build_profiler.stop();
ScopedProfiler linear_system_compute_profiler{linear_system_compute_prof};
Step step;
constexpr double α_max = 1.0;
double α = 1.0;
// Solve the Newton-KKT system
//
// [H Aₑᵀ][ pˣ] = −[∇f − Aₑᵀy]
// [Aₑ 0 ][−pʸ] [ cₑ ]
if (solver.compute(lhs).info() != Eigen::Success) [[unlikely]] {
return ExitStatus::FACTORIZATION_FAILED;
}
linear_system_compute_profiler.stop();
ScopedProfiler linear_system_solve_profiler{linear_system_solve_prof};
auto compute_step = [&](Step& step) {
// p = [ pˣ]
// [−pʸ]
Eigen::VectorXd p = solver.solve(rhs);
step.p_x = p.segment(0, x.rows());
step.p_y = -p.segment(x.rows(), y.rows());
};
compute_step(step);
linear_system_solve_profiler.stop();
ScopedProfiler line_search_profiler{line_search_prof};
α = α_max;
// Loop until a step is accepted
while (1) {
Eigen::VectorXd trial_x = x + α * step.p_x;
Eigen::VectorXd trial_y = y + α * step.p_y;
double trial_f = matrices.f(trial_x);
Eigen::VectorXd trial_c_e = matrices.c_e(trial_x);
// If f(xₖ + αpₖˣ) or cₑ(xₖ + αpₖˣ) aren't finite, reduce step size
// immediately
if (!std::isfinite(trial_f) || !trial_c_e.allFinite()) {
// Reduce step size
α *= α_reduction_factor;
if (α < α_min) {
return ExitStatus::LINE_SEARCH_FAILED;
}
continue;
}
// Check whether filter accepts trial iterate
if (filter.try_add(FilterEntry{trial_f, trial_c_e}, α)) {
// Accept step
break;
}
double prev_constraint_violation = c_e.lpNorm<1>();
double next_constraint_violation = trial_c_e.lpNorm<1>();
// Second-order corrections
//
// If first trial point was rejected and constraint violation stayed the
// same or went up, apply second-order corrections
if (α == α_max &&
next_constraint_violation >= prev_constraint_violation) {
// Apply second-order corrections. See section 2.4 of [2].
auto soc_step = step;
double α_soc = α;
Eigen::VectorXd c_e_soc = c_e;
bool step_acceptable = false;
for (int soc_iteration = 0; soc_iteration < 5 && !step_acceptable;
++soc_iteration) {
ScopedProfiler soc_profiler{soc_prof};
scope_exit soc_exit{[&] {
soc_profiler.stop();
if (options.diagnostics) {
print_iteration_diagnostics(
iterations,
step_acceptable ? IterationType::ACCEPTED_SOC
: IterationType::REJECTED_SOC,
soc_profiler.current_duration(),
error_estimate(g, A_e, trial_c_e, trial_y), trial_f,
trial_c_e.lpNorm<1>(), 0.0, 0.0,
solver.hessian_regularization(), α_soc, 1.0,
α_reduction_factor, 1.0);
}
}};
// Rebuild Newton-KKT rhs with updated constraint values.
//
// rhs = −[∇f − Aₑᵀy]
// [ cₑˢᵒᶜ ]
//
// where cₑˢᵒᶜ = αc(xₖ) + c(xₖ + αpₖˣ)
c_e_soc = α_soc * c_e_soc + trial_c_e;
rhs.bottomRows(y.rows()) = -c_e_soc;
// Solve the Newton-KKT system
compute_step(soc_step);
trial_x = x + α_soc * soc_step.p_x;
trial_y = y + α_soc * soc_step.p_y;
trial_f = matrices.f(trial_x);
trial_c_e = matrices.c_e(trial_x);
// Constraint violation scale factor for second-order corrections
constexpr double κ_soc = 0.99;
// If constraint violation hasn't been sufficiently reduced, stop
// making second-order corrections
next_constraint_violation = trial_c_e.lpNorm<1>();
if (next_constraint_violation > κ_soc * prev_constraint_violation) {
break;
}
// Check whether filter accepts trial iterate
if (filter.try_add(FilterEntry{trial_f, trial_c_e}, α)) {
step = soc_step;
α = α_soc;
step_acceptable = true;
}
}
if (step_acceptable) {
// Accept step
break;
}
}
// If we got here and α is the full step, the full step was rejected.
// Increment the full-step rejected counter to keep track of how many full
// steps have been rejected in a row.
if (α == α_max) {
++full_step_rejected_counter;
}
// If the full step was rejected enough times in a row, reset the filter
// because it may be impeding progress.
//
// See section 3.2 case I of [2].
if (full_step_rejected_counter >= 4 &&
filter.max_constraint_violation >
filter.back().constraint_violation / 10.0) {
filter.max_constraint_violation *= 0.1;
filter.reset();
continue;
}
// Reduce step size
α *= α_reduction_factor;
// If step size hit a minimum, check if the KKT error was reduced. If it
// wasn't, report line search failure.
if (α < α_min) {
double current_kkt_error = kkt_error(g, A_e, c_e, y);
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);
double next_kkt_error = kkt_error(
matrices.g(trial_x), matrices.A_e(trial_x), trial_c_e, trial_y);
// If the step using αᵐᵃˣ reduced the KKT error, accept it anyway
if (next_kkt_error <= 0.999 * current_kkt_error) {
α = α_max;
// Accept step
break;
}
return ExitStatus::LINE_SEARCH_FAILED;
}
}
line_search_profiler.stop();
// 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 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 estimate
E_0 = error_estimate(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.lpNorm<1>(), 0.0, 0.0,
solver.hessian_regularization(), α, α_max,
α_reduction_factor, α);
}
++iterations;
// Check for max iterations
if (iterations >= options.max_iterations) {
return ExitStatus::MAX_ITERATIONS_EXCEEDED;
}
// Check for max wall clock time
if (std::chrono::steady_clock::now() - solve_start_time > options.timeout) {
return ExitStatus::TIMEOUT;
}
}
return ExitStatus::SUCCESS;
}
} // namespace slp

400
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FeasibilityRestoration.hpp vendored
View File

@@ -1,400 +0,0 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <algorithm>
#include <cmath>
#include <iterator>
#include <span>
#include <Eigen/Core>
#include <wpi/SmallVector.h>
#include "sleipnir/autodiff/Variable.hpp"
#include "sleipnir/autodiff/VariableMatrix.hpp"
#include "sleipnir/optimization/SolverConfig.hpp"
#include "sleipnir/optimization/SolverIterationInfo.hpp"
#include "sleipnir/optimization/SolverStatus.hpp"
#include "sleipnir/optimization/solver/InteriorPoint.hpp"
#include "sleipnir/util/FunctionRef.hpp"
namespace sleipnir {
/**
* Finds the iterate that minimizes the constraint violation while not deviating
* too far from the starting point. This is a fallback procedure when the normal
* Sequential Quadratic Programming method fails to converge to a feasible
* point.
*
* @param[in] decisionVariables The list of decision variables.
* @param[in] equalityConstraints The list of equality constraints.
* @param[in] callback The user callback.
* @param[in] config Configuration options for the solver.
* @param[in,out] x The current iterate from the normal solve.
* @param[out] status The solver status.
*/
inline void FeasibilityRestoration(
std::span<Variable> decisionVariables,
std::span<Variable> equalityConstraints,
function_ref<bool(const SolverIterationInfo& info)> callback,
const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status) {
// Feasibility restoration
//
// min ρ Σ (pₑ + nₑ) + ζ/2 (x - x_R)ᵀD_R(x - x_R)
// x
// pₑ,nₑ
//
// s.t. cₑ(x) - pₑ + nₑ = 0
// pₑ ≥ 0
// nₑ ≥ 0
//
// where ρ = 1000, ζ = √μ where μ is the barrier parameter, x_R is original
// iterate before feasibility restoration, and D_R is a scaling matrix defined
// by
//
// D_R = diag(min(1, 1/|x_R⁽¹⁾|), …, min(1, 1/|x_R|⁽ⁿ⁾)
constexpr double ρ = 1000.0;
double μ = config.tolerance / 10.0;
wpi::SmallVector<Variable> fr_decisionVariables;
fr_decisionVariables.reserve(decisionVariables.size() +
2 * equalityConstraints.size());
// Assign x
fr_decisionVariables.assign(decisionVariables.begin(),
decisionVariables.end());
// Allocate pₑ and nₑ
for (size_t row = 0; row < 2 * equalityConstraints.size(); ++row) {
fr_decisionVariables.emplace_back();
}
auto it = fr_decisionVariables.begin();
VariableMatrix xAD{std::span{it, it + decisionVariables.size()}};
it += decisionVariables.size();
VariableMatrix p_e{std::span{it, it + equalityConstraints.size()}};
it += equalityConstraints.size();
VariableMatrix n_e{std::span{it, it + equalityConstraints.size()}};
it += equalityConstraints.size();
// Set initial values for pₑ and nₑ.
//
//
// From equation (33) of [2]:
// ______________________
// μ − ρ c(x) /(μ − ρ c(x))² μ c(x)
// n = −−−−−−−−−− + / (−−−−−−−−−−) + −−−−−− (1)
// 2ρ √ ( 2ρ ) 2ρ
//
// The quadratic formula:
// ________
// -b + √b² - 4ac
// x = −−−−−−−−−−−−−− (2)
// 2a
//
// Rearrange (1) to fit the quadratic formula better:
// _________________________
// μ - ρ c(x) + √(μ - ρ c(x))² + 2ρ μ c(x)
// n = −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
// 2ρ
//
// Solve for coefficients:
//
// a = ρ (3)
// b = ρ c(x) - μ (4)
//
// -4ac = μ c(x) 2ρ
// -4(ρ)c = 2ρ μ c(x)
// -4c = 2μ c(x)
// c = -μ c(x)/2 (5)
//
// p = c(x) + n (6)
for (int row = 0; row < p_e.Rows(); ++row) {
double c_e = equalityConstraints[row].Value();
constexpr double a = 2 * ρ;
double b = ρ * c_e - μ;
double c = -μ * c_e / 2.0;
double n = -b * std::sqrt(b * b - 4.0 * a * c) / (2.0 * a);
double p = c_e + n;
p_e(row).SetValue(p);
n_e(row).SetValue(n);
}
// cₑ(x) - pₑ + nₑ = 0
wpi::SmallVector<Variable> fr_equalityConstraints;
fr_equalityConstraints.assign(equalityConstraints.begin(),
equalityConstraints.end());
for (size_t row = 0; row < fr_equalityConstraints.size(); ++row) {
auto& constraint = fr_equalityConstraints[row];
constraint = constraint - p_e(row) + n_e(row);
}
// cᵢ(x) - s - pᵢ + nᵢ = 0
wpi::SmallVector<Variable> fr_inequalityConstraints;
// pₑ ≥ 0
std::copy(p_e.begin(), p_e.end(),
std::back_inserter(fr_inequalityConstraints));
// nₑ ≥ 0
std::copy(n_e.begin(), n_e.end(),
std::back_inserter(fr_inequalityConstraints));
Variable J = 0.0;
// J += ρ Σ (pₑ + nₑ)
for (auto& elem : p_e) {
J += elem;
}
for (auto& elem : n_e) {
J += elem;
}
J *= ρ;
// D_R = diag(min(1, 1/|x_R⁽¹⁾|), …, min(1, 1/|x_R|⁽ⁿ⁾)
Eigen::VectorXd D_R{x.rows()};
for (int row = 0; row < D_R.rows(); ++row) {
D_R(row) = std::min(1.0, 1.0 / std::abs(x(row)));
}
// J += ζ/2 (x - x_R)ᵀD_R(x - x_R)
for (int row = 0; row < x.rows(); ++row) {
J += std::sqrt(μ) / 2.0 * D_R(row) * sleipnir::pow(xAD(row) - x(row), 2);
}
Eigen::VectorXd fr_x = VariableMatrix{fr_decisionVariables}.Value();
// Set up initial value for inequality constraint slack variables
Eigen::VectorXd fr_s{fr_inequalityConstraints.size()};
fr_s.setOnes();
InteriorPoint(fr_decisionVariables, fr_equalityConstraints,
fr_inequalityConstraints, J, callback, config, true, fr_x, fr_s,
status);
x = fr_x.segment(0, decisionVariables.size());
}
/**
* Finds the iterate that minimizes the constraint violation while not deviating
* too far from the starting point. This is a fallback procedure when the normal
* interior-point method fails to converge to a feasible point.
*
* @param[in] decisionVariables The list of decision variables.
* @param[in] equalityConstraints The list of equality constraints.
* @param[in] inequalityConstraints The list of inequality constraints.
* @param[in] μ Barrier parameter.
* @param[in] callback The user callback.
* @param[in] config Configuration options for the solver.
* @param[in,out] x The current iterate from the normal solve.
* @param[in,out] s The current inequality constraint slack variables from the
* normal solve.
* @param[out] status The solver status.
*/
inline void FeasibilityRestoration(
std::span<Variable> decisionVariables,
std::span<Variable> equalityConstraints,
std::span<Variable> inequalityConstraints, double μ,
function_ref<bool(const SolverIterationInfo& info)> callback,
const SolverConfig& config, Eigen::VectorXd& x, Eigen::VectorXd& s,
SolverStatus* status) {
// Feasibility restoration
//
// min ρ Σ (pₑ + nₑ + pᵢ + nᵢ) + ζ/2 (x - x_R)ᵀD_R(x - x_R)
// x
// pₑ,nₑ
// pᵢ,nᵢ
//
// s.t. cₑ(x) - pₑ + nₑ = 0
// cᵢ(x) - s - pᵢ + nᵢ = 0
// pₑ ≥ 0
// nₑ ≥ 0
// pᵢ ≥ 0
// nᵢ ≥ 0
//
// where ρ = 1000, ζ = √μ where μ is the barrier parameter, x_R is original
// iterate before feasibility restoration, and D_R is a scaling matrix defined
// by
//
// D_R = diag(min(1, 1/|x_R⁽¹⁾|), …, min(1, 1/|x_R|⁽ⁿ⁾)
constexpr double ρ = 1000.0;
wpi::SmallVector<Variable> fr_decisionVariables;
fr_decisionVariables.reserve(decisionVariables.size() +
2 * equalityConstraints.size() +
2 * inequalityConstraints.size());
// Assign x
fr_decisionVariables.assign(decisionVariables.begin(),
decisionVariables.end());
// Allocate pₑ, nₑ, pᵢ, and nᵢ
for (size_t row = 0;
row < 2 * equalityConstraints.size() + 2 * inequalityConstraints.size();
++row) {
fr_decisionVariables.emplace_back();
}
auto it = fr_decisionVariables.begin();
VariableMatrix xAD{std::span{it, it + decisionVariables.size()}};
it += decisionVariables.size();
VariableMatrix p_e{std::span{it, it + equalityConstraints.size()}};
it += equalityConstraints.size();
VariableMatrix n_e{std::span{it, it + equalityConstraints.size()}};
it += equalityConstraints.size();
VariableMatrix p_i{std::span{it, it + inequalityConstraints.size()}};
it += inequalityConstraints.size();
VariableMatrix n_i{std::span{it, it + inequalityConstraints.size()}};
// Set initial values for pₑ, nₑ, pᵢ, and nᵢ.
//
//
// From equation (33) of [2]:
// ______________________
// μ − ρ c(x) /(μ − ρ c(x))² μ c(x)
// n = −−−−−−−−−− + / (−−−−−−−−−−) + −−−−−− (1)
// 2ρ √ ( 2ρ ) 2ρ
//
// The quadratic formula:
// ________
// -b + √b² - 4ac
// x = −−−−−−−−−−−−−− (2)
// 2a
//
// Rearrange (1) to fit the quadratic formula better:
// _________________________
// μ - ρ c(x) + √(μ - ρ c(x))² + 2ρ μ c(x)
// n = −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
// 2ρ
//
// Solve for coefficients:
//
// a = ρ (3)
// b = ρ c(x) - μ (4)
//
// -4ac = μ c(x) 2ρ
// -4(ρ)c = 2ρ μ c(x)
// -4c = 2μ c(x)
// c = -μ c(x)/2 (5)
//
// p = c(x) + n (6)
for (int row = 0; row < p_e.Rows(); ++row) {
double c_e = equalityConstraints[row].Value();
constexpr double a = 2 * ρ;
double b = ρ * c_e - μ;
double c = -μ * c_e / 2.0;
double n = -b * std::sqrt(b * b - 4.0 * a * c) / (2.0 * a);
double p = c_e + n;
p_e(row).SetValue(p);
n_e(row).SetValue(n);
}
for (int row = 0; row < p_i.Rows(); ++row) {
double c_i = inequalityConstraints[row].Value() - s(row);
constexpr double a = 2 * ρ;
double b = ρ * c_i - μ;
double c = -μ * c_i / 2.0;
double n = -b * std::sqrt(b * b - 4.0 * a * c) / (2.0 * a);
double p = c_i + n;
p_i(row).SetValue(p);
n_i(row).SetValue(n);
}
// cₑ(x) - pₑ + nₑ = 0
wpi::SmallVector<Variable> fr_equalityConstraints;
fr_equalityConstraints.assign(equalityConstraints.begin(),
equalityConstraints.end());
for (size_t row = 0; row < fr_equalityConstraints.size(); ++row) {
auto& constraint = fr_equalityConstraints[row];
constraint = constraint - p_e(row) + n_e(row);
}
// cᵢ(x) - s - pᵢ + nᵢ = 0
wpi::SmallVector<Variable> fr_inequalityConstraints;
fr_inequalityConstraints.assign(inequalityConstraints.begin(),
inequalityConstraints.end());
for (size_t row = 0; row < fr_inequalityConstraints.size(); ++row) {
auto& constraint = fr_inequalityConstraints[row];
constraint = constraint - s(row) - p_i(row) + n_i(row);
}
// pₑ ≥ 0
std::copy(p_e.begin(), p_e.end(),
std::back_inserter(fr_inequalityConstraints));
// pᵢ ≥ 0
std::copy(p_i.begin(), p_i.end(),
std::back_inserter(fr_inequalityConstraints));
// nₑ ≥ 0
std::copy(n_e.begin(), n_e.end(),
std::back_inserter(fr_inequalityConstraints));
// nᵢ ≥ 0
std::copy(n_i.begin(), n_i.end(),
std::back_inserter(fr_inequalityConstraints));
Variable J = 0.0;
// J += ρ Σ (pₑ + nₑ + pᵢ + nᵢ)
for (auto& elem : p_e) {
J += elem;
}
for (auto& elem : p_i) {
J += elem;
}
for (auto& elem : n_e) {
J += elem;
}
for (auto& elem : n_i) {
J += elem;
}
J *= ρ;
// D_R = diag(min(1, 1/|x_R⁽¹⁾|), …, min(1, 1/|x_R|⁽ⁿ⁾)
Eigen::VectorXd D_R{x.rows()};
for (int row = 0; row < D_R.rows(); ++row) {
D_R(row) = std::min(1.0, 1.0 / std::abs(x(row)));
}
// J += ζ/2 (x - x_R)ᵀD_R(x - x_R)
for (int row = 0; row < x.rows(); ++row) {
J += std::sqrt(μ) / 2.0 * D_R(row) * sleipnir::pow(xAD(row) - x(row), 2);
}
Eigen::VectorXd fr_x = VariableMatrix{fr_decisionVariables}.Value();
// Set up initial value for inequality constraint slack variables
Eigen::VectorXd fr_s{fr_inequalityConstraints.size()};
fr_s.segment(0, inequalityConstraints.size()) = s;
fr_s.segment(inequalityConstraints.size(),
fr_s.size() - inequalityConstraints.size())
.setOnes();
InteriorPoint(fr_decisionVariables, fr_equalityConstraints,
fr_inequalityConstraints, J, callback, config, true, fr_x, fr_s,
status);
x = fr_x.segment(0, decisionVariables.size());
s = fr_s.segment(0, inequalityConstraints.size());
}
} // namespace sleipnir

183
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/Filter.hpp vendored
View File

@@ -1,183 +0,0 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <algorithm>
#include <cmath>
#include <limits>
#include <utility>
#include <Eigen/Core>
#include <wpi/SmallVector.h>
#include "sleipnir/autodiff/Variable.hpp"
// See docs/algorithms.md#Works_cited for citation definitions.
namespace sleipnir {
/**
* Filter entry consisting of cost and constraint violation.
*/
struct FilterEntry {
/// The cost function's value
double cost = 0.0;
/// The constraint violation
double constraintViolation = 0.0;
constexpr FilterEntry() = default;
/**
* Constructs a FilterEntry.
*
* @param cost The cost function's value.
* @param constraintViolation The constraint violation.
*/
constexpr FilterEntry(double cost, double constraintViolation)
: cost{cost}, constraintViolation{constraintViolation} {}
};
/**
* Step filter.
*
* See the section on filters in chapter 15 of [1].
*/
class Filter {
public:
static constexpr double γCost = 1e-8;
static constexpr double γConstraint = 1e-5;
double maxConstraintViolation = 1e4;
/**
* Construct an empty filter.
*
* @param f The cost function.
*/
explicit Filter(Variable& f) {
m_f = &f;
// Initial filter entry rejects constraint violations above max
m_filter.emplace_back(std::numeric_limits<double>::infinity(),
maxConstraintViolation);
}
/**
* Reset the filter.
*/
void Reset() {
m_filter.clear();
// Initial filter entry rejects constraint violations above max
m_filter.emplace_back(std::numeric_limits<double>::infinity(),
maxConstraintViolation);
}
/**
* Creates a new Sequential Quadratic Programming filter entry.
*
* @param c_e The equality constraint values (nonzero means violation).
*/
FilterEntry MakeEntry(const Eigen::VectorXd& c_e) {
return FilterEntry{m_f->Value(), c_e.lpNorm<1>()};
}
/**
* Creates a new interior-point method filter entry.
*
* @param s The inequality constraint slack variables.
* @param c_e The equality constraint values (nonzero means violation).
* @param c_i The inequality constraint values (negative means violation).
* @param μ The barrier parameter.
*/
FilterEntry MakeEntry(Eigen::VectorXd& s, const Eigen::VectorXd& c_e,
const Eigen::VectorXd& c_i, double μ) {
return FilterEntry{m_f->Value() - μ * s.array().log().sum(),
c_e.lpNorm<1>() + (c_i - s).lpNorm<1>()};
}
/**
* Add a new entry to the filter.
*
* @param entry The entry to add to the filter.
*/
void Add(const FilterEntry& entry) {
// Remove dominated entries
erase_if(m_filter, [&](const auto& elem) {
return entry.cost <= elem.cost &&
entry.constraintViolation <= elem.constraintViolation;
});
m_filter.push_back(entry);
}
/**
* Add a new entry to the filter.
*
* @param entry The entry to add to the filter.
*/
void Add(FilterEntry&& entry) {
// Remove dominated entries
erase_if(m_filter, [&](const auto& elem) {
return entry.cost <= elem.cost &&
entry.constraintViolation <= elem.constraintViolation;
});
m_filter.push_back(entry);
}
/**
* Returns true if the given iterate is accepted by the filter.
*
* @param entry The entry to attempt adding to the filter.
*/
bool TryAdd(const FilterEntry& entry) {
if (IsAcceptable(entry)) {
Add(entry);
return true;
} else {
return false;
}
}
/**
* Returns true if the given iterate is accepted by the filter.
*
* @param entry The entry to attempt adding to the filter.
*/
bool TryAdd(FilterEntry&& entry) {
if (IsAcceptable(entry)) {
Add(std::move(entry));
return true;
} else {
return false;
}
}
/**
* Returns true if the given entry is acceptable to the filter.
*
* @param entry The entry to check.
*/
bool IsAcceptable(const FilterEntry& entry) {
if (!std::isfinite(entry.cost) ||
!std::isfinite(entry.constraintViolation)) {
return false;
}
// If current filter entry is better than all prior ones in some respect,
// accept it
return std::all_of(m_filter.begin(), m_filter.end(), [&](const auto& elem) {
return entry.cost <= elem.cost - γCost * elem.constraintViolation ||
entry.constraintViolation <=
(1.0 - γConstraint) * elem.constraintViolation;
});
}
private:
Variable* m_f = nullptr;
wpi::SmallVector<FilterEntry> m_filter;
};
} // namespace sleipnir

57
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/ErrorEstimate.hpp → wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/error_estimate.hpp vendored
View File

@@ -9,7 +9,21 @@
// See docs/algorithms.md#Works_cited for citation definitions
namespace sleipnir {
namespace slp {
/**
* Returns the error estimate using the KKT conditions for Newton's method.
*
* @param g Gradient of the cost function ∇f.
*/
inline double error_estimate(const Eigen::VectorXd& g) {
// Update the error estimate using the KKT conditions from equations (19.5a)
// through (19.5d) of [1].
//
// ∇f = 0
return g.lpNorm<Eigen::Infinity>();
}
/**
* Returns the error estimate using the KKT conditions for SQP.
@@ -21,12 +35,10 @@ namespace sleipnir {
* iterate.
* @param y Equality constraint dual variables.
*/
inline double ErrorEstimate(const Eigen::VectorXd& g,
const Eigen::SparseMatrix<double>& A_e,
const Eigen::VectorXd& c_e,
const Eigen::VectorXd& y) {
int numEqualityConstraints = A_e.rows();
inline double error_estimate(const Eigen::VectorXd& g,
const Eigen::SparseMatrix<double>& A_e,
const Eigen::VectorXd& c_e,
const Eigen::VectorXd& y) {
// Update the error estimate using the KKT conditions from equations (19.5a)
// through (19.5d) of [1].
//
@@ -41,7 +53,7 @@ inline double ErrorEstimate(const Eigen::VectorXd& g,
// s_d = max(sₘₐₓ, ‖y‖₁ / m) / sₘₐₓ
constexpr double s_max = 100.0;
double s_d = std::max(s_max, y.lpNorm<1>() / numEqualityConstraints) / s_max;
double s_d = std::max(s_max, y.lpNorm<1>() / y.rows()) / s_max;
return std::max({(g - A_e.transpose() * y).lpNorm<Eigen::Infinity>() / s_d,
c_e.lpNorm<Eigen::Infinity>()});
@@ -65,16 +77,13 @@ inline double ErrorEstimate(const Eigen::VectorXd& g,
* @param z Inequality constraint dual variables.
* @param μ Barrier parameter.
*/
inline double ErrorEstimate(const Eigen::VectorXd& g,
const Eigen::SparseMatrix<double>& A_e,
const Eigen::VectorXd& c_e,
const Eigen::SparseMatrix<double>& A_i,
const Eigen::VectorXd& c_i,
const Eigen::VectorXd& s, const Eigen::VectorXd& y,
const Eigen::VectorXd& z, double μ) {
int numEqualityConstraints = A_e.rows();
int numInequalityConstraints = A_i.rows();
inline double error_estimate(const Eigen::VectorXd& g,
const Eigen::SparseMatrix<double>& A_e,
const Eigen::VectorXd& c_e,
const Eigen::SparseMatrix<double>& A_i,
const Eigen::VectorXd& c_i,
const Eigen::VectorXd& s, const Eigen::VectorXd& y,
const Eigen::VectorXd& z, double μ) {
// Update the error estimate using the KKT conditions from equations (19.5a)
// through (19.5d) of [1].
//
@@ -94,23 +103,21 @@ inline double ErrorEstimate(const Eigen::VectorXd& g,
// s_d = max(sₘₐₓ, (‖y‖₁ + ‖z‖₁) / (m + n)) / sₘₐₓ
constexpr double s_max = 100.0;
double s_d =
std::max(s_max, (y.lpNorm<1>() + z.lpNorm<1>()) /
(numEqualityConstraints + numInequalityConstraints)) /
std::max(s_max, (y.lpNorm<1>() + z.lpNorm<1>()) / (y.rows() + z.rows())) /
s_max;
// s_c = max(sₘₐₓ, ‖z‖₁ / n) / sₘₐₓ
double s_c =
std::max(s_max, z.lpNorm<1>() / numInequalityConstraints) / s_max;
double s_c = std::max(s_max, z.lpNorm<1>() / z.rows()) / s_max;
const auto S = s.asDiagonal();
const Eigen::VectorXd e = Eigen::VectorXd::Ones(s.rows());
const Eigen::VectorXd μe = Eigen::VectorXd::Constant(s.rows(), μ);
return std::max({(g - A_e.transpose() * y - A_i.transpose() * z)
.lpNorm<Eigen::Infinity>() /
s_d,
(S * z - μ * e).lpNorm<Eigen::Infinity>() / s_c,
(S * z - μe).lpNorm<Eigen::Infinity>() / s_c,
c_e.lpNorm<Eigen::Infinity>(),
(c_i - s).lpNorm<Eigen::Infinity>()});
}
} // namespace sleipnir
} // namespace slp

197
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/filter.hpp vendored Normal file
View File

@@ -0,0 +1,197 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <algorithm>
#include <cmath>
#include <limits>
#include <utility>
#include <Eigen/Core>
#include <gch/small_vector.hpp>
// See docs/algorithms.md#Works_cited for citation definitions.
namespace slp {
/**
* Filter entry consisting of cost and constraint violation.
*/
struct FilterEntry {
/// The cost function's value
double cost = 0.0;
/// The constraint violation
double constraint_violation = 0.0;
constexpr FilterEntry() = default;
/**
* Constructs a FilterEntry.
*
* @param cost The cost function's value.
* @param constraint_violation The constraint violation.
*/
constexpr FilterEntry(double cost, double constraint_violation)
: cost{cost}, constraint_violation{constraint_violation} {}
/**
* Constructs a Newton's method filter entry.
*
* @param f The cost function value.
*/
explicit FilterEntry(double f) : FilterEntry{f, 0.0} {}
/**
* Constructs a Sequential Quadratic Programming filter entry.
*
* @param f The cost function value.
* @param c_e The equality constraint values (nonzero means violation).
*/
FilterEntry(double f, const Eigen::VectorXd& c_e)
: FilterEntry{f, c_e.lpNorm<1>()} {}
/**
* Constructs an interior-point method filter entry.
*
* @param f The cost function value.
* @param s The inequality constraint slack variables.
* @param c_e The equality constraint values (nonzero means violation).
* @param c_i The inequality constraint values (negative means violation).
* @param μ The barrier parameter.
*/
FilterEntry(double f, Eigen::VectorXd& s, const Eigen::VectorXd& c_e,
const Eigen::VectorXd& c_i, double μ)
: FilterEntry{f - μ * s.array().log().sum(),
c_e.lpNorm<1>() + (c_i - s).lpNorm<1>()} {}
};
/**
* Step filter.
*
* See the section on filters in chapter 15 of [1].
*/
class Filter {
public:
double max_constraint_violation = 1e4;
/**
* Constructs an empty filter.
*/
Filter() {
// Initial filter entry rejects constraint violations above max
m_filter.emplace_back(std::numeric_limits<double>::infinity(),
max_constraint_violation);
}
/**
* Resets the filter.
*/
void reset() {
m_filter.clear();
// Initial filter entry rejects constraint violations above max
m_filter.emplace_back(std::numeric_limits<double>::infinity(),
max_constraint_violation);
}
/**
* Adds a new entry to the filter.
*
* @param entry The entry to add to the filter.
*/
void add(const FilterEntry& entry) {
// Remove dominated entries
erase_if(m_filter, [&](const auto& elem) {
return entry.cost <= elem.cost &&
entry.constraint_violation <= elem.constraint_violation;
});
m_filter.push_back(entry);
}
/**
* Adds a new entry to the filter.
*
* @param entry The entry to add to the filter.
*/
void add(FilterEntry&& entry) {
// Remove dominated entries
erase_if(m_filter, [&](const auto& elem) {
return entry.cost <= elem.cost &&
entry.constraint_violation <= elem.constraint_violation;
});
m_filter.push_back(entry);
}
/**
* Returns true if the given iterate is accepted by the filter.
*
* @param entry The entry to attempt adding to the filter.
* @param α The step size (0, 1].
*/
bool try_add(const FilterEntry& entry, double α) {
if (is_acceptable(entry, α)) {
add(entry);
return true;
} else {
return false;
}
}
/**
* Returns true if the given iterate is accepted by the filter.
*
* @param entry The entry to attempt adding to the filter.
* @param α The step size (0, 1].
*/
bool try_add(FilterEntry&& entry, double α) {
if (is_acceptable(entry, α)) {
add(std::move(entry));
return true;
} else {
return false;
}
}
/**
* Returns true if the given entry is acceptable to the filter.
*
* @param entry The entry to check.
* @param α The step size (0, 1].
*/
bool is_acceptable(const FilterEntry& entry, double α) {
if (!std::isfinite(entry.cost) ||
!std::isfinite(entry.constraint_violation)) {
return false;
}
double ϕ = std::pow(α, 1.5);
// If current filter entry is better than all prior ones in some respect,
// accept it.
//
// See equation (2.13) of [4].
return std::ranges::all_of(m_filter, [&](const auto& elem) {
return entry.cost <= elem.cost - ϕ * γ_cost * elem.constraint_violation ||
entry.constraint_violation <=
(1.0 - ϕ * γ_constraint) * elem.constraint_violation;
});
}
/**
* Returns the most recently added filter entry.
*
* @return The most recently added filter entry.
*/
const FilterEntry& back() const { return m_filter.back(); }
private:
static constexpr double γ_cost = 1e-8;
static constexpr double γ_constraint = 1e-5;
gch::small_vector<FilterEntry> m_filter;
};
} // namespace slp

6
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FractionToTheBoundaryRule.hpp → wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/fraction_to_the_boundary_rule.hpp vendored
View File

@@ -6,7 +6,7 @@
// See docs/algorithms.md#Works_cited for citation definitions
namespace sleipnir {
namespace slp {
/**
* Applies fraction-to-the-boundary rule to a variable and its iterate, then
@@ -17,7 +17,7 @@ namespace sleipnir {
* @param τ Fraction-to-the-boundary rule scaling factor within (0, 1].
* @return Fraction of the iterate step size within (0, 1].
*/
inline double FractionToTheBoundaryRule(
inline double fraction_to_the_boundary_rule(
const Eigen::Ref<const Eigen::VectorXd>& x,
const Eigen::Ref<const Eigen::VectorXd>& p, double τ) {
// α = max(α ∈ (0, 1] : x + αp ≥ (1 − τ)x)
@@ -42,4 +42,4 @@ inline double FractionToTheBoundaryRule(
return α;
}
} // namespace sleipnir
} // namespace slp

10
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/IsLocallyInfeasible.hpp → wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/is_locally_infeasible.hpp vendored
View File

@@ -7,7 +7,7 @@
// See docs/algorithms.md#Works_cited for citation definitions
namespace sleipnir {
namespace slp {
/**
* Returns true if the problem's equality constraints are locally infeasible.
@@ -17,8 +17,8 @@ namespace sleipnir {
* @param c_e The problem's equality constraints cₑ(x) evaluated at the current
* iterate.
*/
inline bool IsEqualityLocallyInfeasible(const Eigen::SparseMatrix<double>& A_e,
const Eigen::VectorXd& c_e) {
inline bool is_equality_locally_infeasible(
const Eigen::SparseMatrix<double>& A_e, const Eigen::VectorXd& c_e) {
// The equality constraints are locally infeasible if
//
// Aₑᵀcₑ → 0
@@ -37,7 +37,7 @@ inline bool IsEqualityLocallyInfeasible(const Eigen::SparseMatrix<double>& A_e,
* @param c_i The problem's inequality constraints cᵢ(x) evaluated at the
* current iterate.
*/
inline bool IsInequalityLocallyInfeasible(
inline bool is_inequality_locally_infeasible(
const Eigen::SparseMatrix<double>& A_i, const Eigen::VectorXd& c_i) {
// The inequality constraints are locally infeasible if
//
@@ -60,4 +60,4 @@ inline bool IsInequalityLocallyInfeasible(
return false;
}
} // namespace sleipnir
} // namespace slp

42
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/KKTError.hpp → wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/kkt_error.hpp vendored
View File

@@ -7,7 +7,21 @@
// See docs/algorithms.md#Works_cited for citation definitions
namespace sleipnir {
namespace slp {
/**
* Returns the KKT error for Newton's method.
*
* @param g Gradient of the cost function ∇f.
*/
inline double kkt_error(const Eigen::VectorXd& g) {
// Compute the KKT error as the 1-norm of the KKT conditions from equations
// (19.5a) through (19.5d) of [1].
//
// ∇f = 0
return g.lpNorm<1>();
}
/**
* Returns the KKT error for Sequential Quadratic Programming.
@@ -19,9 +33,9 @@ namespace sleipnir {
* iterate.
* @param y Equality constraint dual variables.
*/
inline double KKTError(const Eigen::VectorXd& g,
const Eigen::SparseMatrix<double>& A_e,
const Eigen::VectorXd& c_e, const Eigen::VectorXd& y) {
inline double kkt_error(const Eigen::VectorXd& g,
const Eigen::SparseMatrix<double>& A_e,
const Eigen::VectorXd& c_e, const Eigen::VectorXd& y) {
// Compute the KKT error as the 1-norm of the KKT conditions from equations
// (19.5a) through (19.5d) of [1].
//
@@ -48,13 +62,13 @@ inline double KKTError(const Eigen::VectorXd& g,
* @param z Inequality constraint dual variables.
* @param μ Barrier parameter.
*/
inline double KKTError(const Eigen::VectorXd& g,
const Eigen::SparseMatrix<double>& A_e,
const Eigen::VectorXd& c_e,
const Eigen::SparseMatrix<double>& A_i,
const Eigen::VectorXd& c_i, const Eigen::VectorXd& s,
const Eigen::VectorXd& y, const Eigen::VectorXd& z,
double μ) {
inline double kkt_error(const Eigen::VectorXd& g,
const Eigen::SparseMatrix<double>& A_e,
const Eigen::VectorXd& c_e,
const Eigen::SparseMatrix<double>& A_i,
const Eigen::VectorXd& c_i, const Eigen::VectorXd& s,
const Eigen::VectorXd& y, const Eigen::VectorXd& z,
double μ) {
// Compute the KKT error as the 1-norm of the KKT conditions from equations
// (19.5a) through (19.5d) of [1].
//
@@ -64,10 +78,10 @@ inline double KKTError(const Eigen::VectorXd& g,
// cᵢ − s = 0
const auto S = s.asDiagonal();
const Eigen::VectorXd e = Eigen::VectorXd::Ones(s.rows());
const Eigen::VectorXd μe = Eigen::VectorXd::Constant(s.rows(), μ);
return (g - A_e.transpose() * y - A_i.transpose() * z).lpNorm<1>() +
(S * z - μ * e).lpNorm<1>() + c_e.lpNorm<1>() + (c_i - s).lpNorm<1>();
(S * z - μe).lpNorm<1>() + c_e.lpNorm<1>() + (c_i - s).lpNorm<1>();
}
} // namespace sleipnir
} // namespace slp

12
wpimath/src/main/native/thirdparty/sleipnir/src/util/Pool.cpp vendored
View File

@@ -1,12 +0,0 @@
// Copyright (c) Sleipnir contributors
#include "sleipnir/util/Pool.hpp"
namespace sleipnir {
PoolResource& GlobalPoolResource() {
thread_local PoolResource pool{16384};
return pool;
}
} // namespace sleipnir

21
wpimath/src/main/native/thirdparty/sleipnir/src/util/ToMilliseconds.hpp vendored
View File

@@ -1,21 +0,0 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <chrono>
namespace sleipnir {
/**
* Converts std::chrono::duration to a number of milliseconds rounded to three
* decimals.
*/
template <typename Rep, typename Period = std::ratio<1>>
constexpr double ToMilliseconds(
const std::chrono::duration<Rep, Period>& duration) {
using std::chrono::duration_cast;
using std::chrono::microseconds;
return duration_cast<microseconds>(duration).count() / 1e3;
}
} // namespace sleipnir

12
wpimath/src/main/native/thirdparty/sleipnir/src/util/pool.cpp vendored Normal file
View File

@@ -0,0 +1,12 @@
// Copyright (c) Sleipnir contributors
#include "sleipnir/util/pool.hpp"
namespace slp {
PoolResource& global_pool_resource() {
thread_local PoolResource pool{16384};
return pool;
}
} // namespace slp

357
wpimath/src/main/native/thirdparty/sleipnir/src/util/print_diagnostics.hpp vendored Normal file
View File

@@ -0,0 +1,357 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <stdint.h>
#include <algorithm>
#include <array>
#include <chrono>
#include <cmath>
#include <ranges>
#include <string>
#include <utility>
#include <Eigen/Core>
#include <gch/small_vector.hpp>
#include "sleipnir/util/print.hpp"
#include "util/setup_profiler.hpp"
#include "util/solve_profiler.hpp"
namespace slp {
/**
* Iteration type.
*/
enum class IterationType : uint8_t {
/// Normal iteration.
NORMAL,
/// Accepted second-order correction iteration.
ACCEPTED_SOC,
/// Rejected second-order correction iteration.
REJECTED_SOC
};
/**
* Converts std::chrono::duration to a number of milliseconds rounded to three
* decimals.
*/
template <typename Rep, typename Period = std::ratio<1>>
constexpr double to_ms(const std::chrono::duration<Rep, Period>& duration) {
using std::chrono::duration_cast;
using std::chrono::microseconds;
return duration_cast<microseconds>(duration).count() / 1e3;
}
/**
* Renders value as power of 10.
*
* @param value Value.
*/
inline std::string power_of_10(double value) {
if (value == 0.0) {
return " 0";
} else {
int exponent = std::log10(value);
if (exponent == 0) {
return " 1";
} else if (exponent == 1) {
return "10";
} else {
// Gather exponent digits
int n = std::abs(exponent);
gch::small_vector<int> digits;
do {
digits.emplace_back(n % 10);
n /= 10;
} while (n > 0);
std::string output = "10";
// Append exponent
if (exponent < 0) {
output += "⁻";
}
constexpr std::array strs = {"⁰", "¹", "²", "³", "⁴",
"⁵", "⁶", "⁷", "⁸", "⁹"};
for (const auto& digit : digits | std::views::reverse) {
output += strs[digit];
}
return output;
}
}
}
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
/**
* Prints error for too few degrees of freedom.
*
* @param c_e The problem's equality constraints cₑ(x) evaluated at the current
* iterate.
*/
inline void print_too_few_dofs_error(const Eigen::VectorXd& c_e) {
slp::println("The problem has too few degrees of freedom.");
slp::println("Violated constraints (cₑ(x) = 0) in order of declaration:");
for (int row = 0; row < c_e.rows(); ++row) {
if (c_e[row] < 0.0) {
slp::println(" {}/{}: {} = 0", row + 1, c_e.rows(), c_e[row]);
}
}
}
#else
#define print_too_few_dofs_error(...)
#endif
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
/**
* Prints equality constraint local infeasibility error.
*
* @param c_e The problem's equality constraints cₑ(x) evaluated at the current
* iterate.
*/
inline void print_c_e_local_infeasibility_error(const Eigen::VectorXd& c_e) {
slp::println(
"The problem is locally infeasible due to violated equality "
"constraints.");
slp::println("Violated constraints (cₑ(x) = 0) in order of declaration:");
for (int row = 0; row < c_e.rows(); ++row) {
if (c_e[row] < 0.0) {
slp::println(" {}/{}: {} = 0", row + 1, c_e.rows(), c_e[row]);
}
}
}
#else
#define print_c_e_local_infeasibility_error(...)
#endif
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
/**
* Prints inequality constraint local infeasibility error.
*
* @param c_i The problem's inequality constraints cᵢ(x) evaluated at the
* current iterate.
*/
inline void print_c_i_local_infeasibility_error(const Eigen::VectorXd& c_i) {
slp::println(
"The problem is locally infeasible due to violated inequality "
"constraints.");
slp::println("Violated constraints (cᵢ(x) ≥ 0) in order of declaration:");
for (int row = 0; row < c_i.rows(); ++row) {
if (c_i[row] < 0.0) {
slp::println(" {}/{}: {} ≥ 0", row + 1, c_i.rows(), c_i[row]);
}
}
}
#else
#define print_c_i_local_infeasibility_error(...)
#endif
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
inline void print_bound_constraint_global_infeasibility_error(
const std::span<const std::pair<Eigen::Index, Eigen::Index>>
conflicting_lower_upper_bound_indices) {
slp::println(
"The problem is globally infeasible due to conflicting bound "
"constraints:");
for (const auto& [lower_bound_idx, upper_bound_idx] :
conflicting_lower_upper_bound_indices) {
slp::println(
" Inequality constraint {} gives a lower bound that is greater than "
"the upper bound given by inequality constraint {}",
lower_bound_idx, upper_bound_idx);
}
}
#else
#define print_bound_constraint_global_infeasibility_error(...)
#endif
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
/**
* Prints diagnostics for the current iteration.
*
* @param iterations Number of iterations.
* @param type The iteration's type.
* @param time The iteration duration.
* @param error The error.
* @param cost The cost.
* @param infeasibility The infeasibility.
* @param complementarity The complementarity.
* @param μ The barrier parameter.
* @param δ The Hessian regularization factor.
* @param primal_α The primal step size.
* @param primal_α_max The max primal step size.
* @param α_reduction_factor Factor by which primal_α is reduced during
* backtracking.
* @param dual_α The dual step size.
*/
template <typename Rep, typename Period = std::ratio<1>>
void print_iteration_diagnostics(int iterations, IterationType type,
const std::chrono::duration<Rep, Period>& time,
double error, double cost,
double infeasibility, double complementarity,
double μ, double δ, double primal_α,
double primal_α_max, double α_reduction_factor,
double dual_α) {
if (iterations % 20 == 0) {
if (iterations == 0) {
slp::print("┏");
} 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(
"┃{:^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}┩",
"", "", "", "", "", "", "", "", "", "", "", "");
}
// For the number of backtracks, we want x such that:
//
// αᵐᵃˣrˣ = α
//
// where r ∈ (0, 1) is the reduction factor.
//
// rˣ = α/αᵐᵃˣ
// ln(rˣ) = ln(α/αᵐᵃˣ)
// x ln(r) = ln(α/αᵐᵃˣ)
// x = ln(α/αᵐᵃˣ)/ln(r)
int backtracks = static_cast<int>(std::log(primal_α / primal_α_max) /
std::log(α_reduction_factor));
constexpr std::array ITERATION_TYPES = {"norm", "✓SOC", "XSOC"};
slp::println(
"│{:4} {:4} {:9.3f} {:12e} {:13e} {:12e} {:12e} {:.2e} {:<5} {:.2e} "
"{:.2e} {:2d}│",
iterations, ITERATION_TYPES[static_cast<uint8_t>(type)], to_ms(time),
error, cost, infeasibility, complementarity, μ, power_of_10(δ), primal_α,
dual_α, backtracks);
}
#else
#define print_iteration_diagnostics(...)
#endif
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
/**
* Prints bottom of iteration diagnostics table.
*/
inline void print_bottom_iteration_diagnostics() {
slp::println("└{:─^108}┘", "");
}
#else
#define print_bottom_iteration_diagnostics(...)
#endif
/**
* Renders histogram of the given normalized value.
*
* @tparam Width Width of the histogram in characters.
* @param value Normalized value from 0 to 1.
*/
template <int Width>
requires(Width > 0)
std::string histogram(double value) {
value = std::clamp(value, 0.0, 1.0);
double ipart;
int fpart = static_cast<int>(std::modf(value * Width, &ipart) * 8);
constexpr std::array strs = {" ", "▏", "▎", "▍", "▌", "▋", "▊", "▉", "█"};
std::string hist;
int index = 0;
while (index < ipart) {
hist += strs[8];
++index;
}
if (fpart > 0) {
hist += strs[fpart];
++index;
}
while (index < Width) {
hist += strs[0];
++index;
}
return hist;
}
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
/**
* Prints solver diagnostics.
*
* @param solve_profilers Solve profilers.
*/
inline void print_solver_diagnostics(
const gch::small_vector<SolveProfiler>& solve_profilers) {
auto solve_duration = to_ms(solve_profilers[0].total_duration());
slp::println("┏{:━^25}┯{:━^18}┯{:━^10}┯{:━^9}┯{:━^4}┓", "", "", "", "", "");
slp::println("┃{:^25}│{:^18}│{:^10}│{:^9}│{:^4}┃", "solver trace", "percent",
"total (ms)", "each (ms)", "runs");
slp::println("┡{:━^25}┷{:━^18}┷{:━^10}┷{:━^9}┷{:━^4}┩", "", "", "", "", "");
for (auto& profiler : solve_profilers) {
double norm = solve_duration == 0.0
? (&profiler == &solve_profilers[0] ? 1.0 : 0.0)
: to_ms(profiler.total_duration()) / solve_duration;
slp::println("│{:<25} {:>6.2f}%▕{}▏ {:>10.3f} {:>9.3f} {:>4}│",
profiler.name(), norm * 100.0, histogram<9>(norm),
to_ms(profiler.total_duration()),
to_ms(profiler.average_duration()), profiler.num_solves());
}
slp::println("└{:─^70}┘", "");
}
#else
#define print_solver_diagnostics(...)
#endif
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
/**
* Prints autodiff diagnostics.
*
* @param setup_profilers Autodiff setup profilers.
*/
inline void print_autodiff_diagnostics(
const gch::small_vector<SetupProfiler>& setup_profilers) {
auto setup_duration = to_ms(setup_profilers[0].duration());
// Print heading
slp::println("┏{:━^25}┯{:━^18}┯{:━^10}┯{:━^9}┯{:━^4}┓", "", "", "", "", "");
slp::println("┃{:^25}│{:^18}│{:^10}│{:^9}│{:^4}┃", "autodiff trace",
"percent", "total (ms)", "each (ms)", "runs");
slp::println("┡{:━^25}┷{:━^18}┷{:━^10}┷{:━^9}┷{:━^4}┩", "", "", "", "", "");
// 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("│{:<25} {:>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("└{:─^70}┘", "");
}
#else
#define print_autodiff_diagnostics(...)
#endif
} // namespace slp

4
wpimath/src/main/native/thirdparty/sleipnir/src/util/ScopeExit.hpp → wpimath/src/main/native/thirdparty/sleipnir/src/util/scope_exit.hpp vendored
View File

@@ -4,7 +4,7 @@
#include <utility>
namespace sleipnir {
namespace slp {
template <typename F>
class scope_exit {
@@ -32,4 +32,4 @@ class scope_exit {
bool m_active = true;
};
} // namespace sleipnir
} // namespace slp

78
wpimath/src/main/native/thirdparty/sleipnir/src/util/scoped_profiler.hpp vendored Normal file
View File

@@ -0,0 +1,78 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <concepts>
#include <utility>
#include "util/setup_profiler.hpp"
#include "util/solve_profiler.hpp"
namespace slp {
/**
* Starts a profiler in the constructor and stops it in the destructor.
*/
template <typename Profiler>
requires std::same_as<Profiler, SetupProfiler> ||
std::same_as<Profiler, SolveProfiler>
class ScopedProfiler {
public:
/**
* Starts a profiler.
*
* @param profiler The profiler.
*/
explicit ScopedProfiler(Profiler& profiler) noexcept : m_profiler{&profiler} {
m_profiler->start();
}
/**
* Stops a profiler.
*/
~ScopedProfiler() {
if (m_active) {
m_profiler->stop();
}
}
/**
* Move constructor.
*
* @param rhs The other ScopedProfiler.
*/
ScopedProfiler(ScopedProfiler&& rhs) noexcept
: m_profiler{std::move(rhs.m_profiler)}, m_active{rhs.m_active} {
rhs.m_active = false;
}
ScopedProfiler(const ScopedProfiler&) = delete;
ScopedProfiler& operator=(const ScopedProfiler&) = delete;
/**
* Stops the profiler.
*
* If this is called, the destructor is a no-op.
*/
void stop() {
if (m_active) {
m_profiler->stop();
m_active = false;
}
}
/**
* Returns the most recent solve duration in milliseconds as a double.
*
* @return The most recent solve duration in milliseconds as a double.
*/
const std::chrono::duration<double>& current_duration() const {
return m_profiler->current_duration();
}
private:
Profiler* m_profiler;
bool m_active = true;
};
} // namespace slp

68
wpimath/src/main/native/thirdparty/sleipnir/src/util/setup_profiler.hpp vendored Normal file
View File

@@ -0,0 +1,68 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <chrono>
#include <string>
#include <string_view>
namespace slp {
/**
* Records the number of profiler measurements (start/stop pairs) and the
* average duration between each start and stop call.
*/
class SetupProfiler {
public:
/**
* Constructs a SetupProfiler.
*
* @param name Name of measurement to show in diagnostics.
*/
explicit SetupProfiler(std::string_view name) : m_name{name} {}
/**
* Tell the profiler to start measuring setup time.
*/
void start() {
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
m_setup_start_time = std::chrono::steady_clock::now();
#endif
}
/**
* Tell the profiler to stop measuring setup time.
*/
void stop() {
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
m_setup_stop_time = std::chrono::steady_clock::now();
m_setup_duration = m_setup_stop_time - m_setup_start_time;
#endif
}
/**
* Returns name of measurement to show in diagnostics.
*
* @return Name of measurement to show in diagnostics.
*/
std::string_view name() const { return m_name; }
/**
* Returns the setup duration in milliseconds as a double.
*
* @return The setup duration in milliseconds as a double.
*/
const std::chrono::duration<double>& duration() const {
return m_setup_duration;
}
private:
/// Name of measurement to show in diagnostics.
std::string m_name;
std::chrono::steady_clock::time_point m_setup_start_time;
std::chrono::steady_clock::time_point m_setup_stop_time;
std::chrono::duration<double> m_setup_duration{0.0};
};
} // namespace slp

105
wpimath/src/main/native/thirdparty/sleipnir/src/util/solve_profiler.hpp vendored Normal file
View File

@@ -0,0 +1,105 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <chrono>
#include <string>
#include <string_view>
namespace slp {
/**
* Records the number of profiler measurements (start/stop pairs) and the
* average duration between each start and stop call.
*/
class SolveProfiler {
public:
/**
* Constructs a SolveProfiler.
*
* @param name Name of measurement to show in diagnostics.
*/
explicit SolveProfiler(std::string_view name) : m_name{name} {}
/**
* Tell the profiler to start measuring solve time.
*/
void start() {
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
m_current_solve_start_time = std::chrono::steady_clock::now();
#endif
}
/**
* Tell the profiler to stop measuring solve time, increment the number of
* averages, and incorporate the latest measurement into the average.
*/
void stop() {
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
m_current_solve_stop_time = std::chrono::steady_clock::now();
m_current_solve_duration =
m_current_solve_stop_time - m_current_solve_start_time;
m_total_solve_duration += m_current_solve_duration;
++m_num_solves;
m_average_solve_duration =
(m_num_solves - 1.0) / m_num_solves * m_average_solve_duration +
1.0 / m_num_solves * m_current_solve_duration;
#endif
}
/**
* Returns name of measurement to show in diagnostics.
*
* @return Name of measurement to show in diagnostics.
*/
std::string_view name() const { return m_name; }
/**
* Returns the number of solves.
*
* @return The number of solves.
*/
int num_solves() const { return m_num_solves; }
/**
* Returns the most recent solve duration in seconds.
*
* @return The most recent solve duration in seconds.
*/
const std::chrono::duration<double>& current_duration() const {
return m_current_solve_duration;
}
/**
* Returns the average solve duration in seconds.
*
* @return The average solve duration in seconds.
*/
const std::chrono::duration<double>& average_duration() const {
return m_average_solve_duration;
}
/**
* Returns the sum of all solve durations in seconds.
*
* @return The sum of all solve durations in seconds.
*/
const std::chrono::duration<double>& total_duration() const {
return m_total_solve_duration;
}
private:
/// Name of measurement to show in diagnostics.
std::string m_name;
std::chrono::steady_clock::time_point m_current_solve_start_time;
std::chrono::steady_clock::time_point m_current_solve_stop_time;
std::chrono::duration<double> m_current_solve_duration{0.0};
std::chrono::duration<double> m_total_solve_duration{0.0};
int m_num_solves = 0;
std::chrono::duration<double> m_average_solve_duration{0.0};
};
} // namespace slp

25
wpimath/src/test/native/cpp/SleipnirTest.cpp
View File

@@ -3,24 +3,25 @@
// the WPILib BSD license file in the root directory of this project.
#include <gtest/gtest.h>
#include <sleipnir/optimization/OptimizationProblem.hpp>
#include <sleipnir/optimization/problem.hpp>
TEST(SleipnirTest, Quartic) {
sleipnir::OptimizationProblem problem;
slp::Problem problem;
auto x = problem.DecisionVariable();
x.SetValue(20.0);
auto x = problem.decision_variable();
x.set_value(20.0);
problem.Minimize(sleipnir::pow(x, 4));
problem.minimize(slp::pow(x, 4));
problem.SubjectTo(x >= 1);
problem.subject_to(x >= 1);
auto status = problem.Solve({.diagnostics = true});
EXPECT_EQ(problem.cost_function_type(), slp::ExpressionType::NONLINEAR);
EXPECT_EQ(problem.equality_constraint_type(), slp::ExpressionType::NONE);
EXPECT_EQ(problem.inequality_constraint_type(), slp::ExpressionType::LINEAR);
EXPECT_EQ(status.costFunctionType, sleipnir::ExpressionType::kNonlinear);
EXPECT_EQ(status.equalityConstraintType, sleipnir::ExpressionType::kNone);
EXPECT_EQ(status.inequalityConstraintType, sleipnir::ExpressionType::kLinear);
EXPECT_EQ(status.exitCondition, sleipnir::SolverExitCondition::kSuccess);
auto status = problem.solve({.diagnostics = true});
EXPECT_NEAR(x.Value(), 1.0, 1e-6);
EXPECT_EQ(status, slp::ExitStatus::SUCCESS);
EXPECT_NEAR(x.value(), 1.0, 1e-6);
}