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

[upstream_utils] Upgrade to Sleipnir 0.3.2 (#8323)

Browse Source 
Also includes a C++ benchmark, which has a Java counterpart in #8236.
This commit is contained in:
Tyler Veness
2025-12-01 12:51:28 -08:00
committed by GitHub
parent feea24251f
commit 08784dc2d1
65 changed files with 5699 additions and 4446 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
benchmark/src/main/native/cpp/CartPoleBenchmark.hpp
View File

@@ -11,8 +11,9 @@
#include "wpi/math/system/NumericalIntegration.hpp"
inline slp::VariableMatrix CartPoleDynamics(const slp::VariableMatrix& x,
const slp::VariableMatrix& u) {
inline slp::VariableMatrix<double> CartPoleDynamics(
const slp::VariableMatrix<double>& x,
const slp::VariableMatrix<double>& u) {
constexpr double m_c = 5.0; // Cart mass (kg)
constexpr double m_p = 0.5; // Pole mass (kg)
constexpr double l = 0.5; // Pole length (m)
@@ -25,23 +26,23 @@ inline slp::VariableMatrix CartPoleDynamics(const slp::VariableMatrix& x,
// [ m_c + m_p m_p l cosθ]
// M(q) = [m_p l cosθ m_p l² ]
slp::VariableMatrix M{{m_c + m_p, m_p * l * cos(theta)},
{m_p * l * cos(theta), m_p * std::pow(l, 2)}};
slp::VariableMatrix<double> M{{m_c + m_p, m_p * l * cos(theta)},
{m_p * l * cos(theta), m_p * std::pow(l, 2)}};
// [0 −m_p lθ̇ sinθ]
// C(q, q̇) = [0 0 ]
slp::VariableMatrix C{{0, -m_p * l * thetadot * sin(theta)}, {0, 0}};
slp::VariableMatrix<double> C{{0, -m_p * l * thetadot * sin(theta)}, {0, 0}};
// [ 0 ]
// τ_g(q) = [-m_p gl sinθ]
slp::VariableMatrix tau_g{{0}, {-m_p * g * l * sin(theta)}};
slp::VariableMatrix<double> tau_g{{0}, {-m_p * g * l * sin(theta)}};
// [1]
// B = [0]
constexpr Eigen::Matrix<double, 2, 1> B{{1}, {0}};
// q̈ = M⁻¹(q)(τ_g(q) − C(q, q̇)q̇ + Bu)
slp::VariableMatrix qddot{4};
slp::VariableMatrix<double> qddot{4};
qddot.segment(0, 2) = qdot;
qddot.segment(2, 2) = solve(M, tau_g - C * qdot + B * u);
return qddot;
@@ -63,7 +64,7 @@ inline void BM_CartPole(benchmark::State& state) {
constexpr Eigen::Vector<double, 4> x_final{
{1.0, std::numbers::pi, 0.0, 0.0}};
slp::Problem problem;
slp::Problem<double> problem;
// x = [q, q̇]ᵀ = [x, θ, ẋ, θ̇]ᵀ
auto X = problem.decision_variable(4, N + 1);
@@ -95,11 +96,11 @@ inline void BM_CartPole(benchmark::State& state) {
// Dynamics constraints - RK4 integration
for (int k = 0; k < N; ++k) {
problem.subject_to(
X.col(k + 1) ==
wpi::math::RK4<decltype(CartPoleDynamics), slp::VariableMatrix,
slp::VariableMatrix>(CartPoleDynamics, X.col(k),
U.col(k), dt));
problem.subject_to(X.col(k + 1) ==
wpi::math::RK4<decltype(CartPoleDynamics),
slp::VariableMatrix<double>,
slp::VariableMatrix<double>>(
CartPoleDynamics, X.col(k), U.col(k), dt));
}
// Minimize sum squared inputs

5
docs/build.gradle
View File

@@ -97,6 +97,11 @@ doxygen.sourceSets.main {
exclude 'wpi/util/bit.hpp'
exclude 'wpi/util/raw_ostream.hpp'
// Sleipnir
exclude 'sleipnir/optimization/solver/interior_point.hpp'
exclude 'sleipnir/optimization/solver/newton.hpp'
exclude 'sleipnir/optimization/solver/sqp.hpp'
// apriltag
exclude 'apriltag_pose.h'

6
upstream_utils/sleipnir.py
View File

@@ -18,8 +18,7 @@ def copy_upstream_src(wpilib_root: Path):
# Copy Sleipnir files into allwpilib
walk_cwd_and_copy_if(
lambda dp, f: (has_prefix(dp, Path("include")) or has_prefix(dp, Path("src")))
and f not in [".styleguide", ".styleguide-license"],
lambda dp, f: (has_prefix(dp, Path("include")) or has_prefix(dp, Path("src"))),
wpimath / "src/main/native/thirdparty/sleipnir",
)
@@ -49,8 +48,7 @@ using small_vector = wpi::util::SmallVector<T>;
def main():
name = "sleipnir"
url = "https://github.com/SleipnirGroup/Sleipnir"
# main on 2025-09-19
tag = "7f89d5547702a09e3617bc31fe5bafe6add04fab"
tag = "v0.3.2"
sleipnir = Lib(name, url, tag, copy_upstream_src)
sleipnir.main()

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

@@ -4,41 +4,41 @@ Date: Wed, 29 May 2024 16:29:55 -0700
Subject: [PATCH 1/8] Use fmtlib
---
include/.styleguide | 1 +
include/sleipnir/util/assert.hpp | 5 +++--
include/sleipnir/util/print.hpp | 31 ++++++++++++++++++-------------
src/.styleguide | 1 +
src/optimization/problem.cpp | 1 +
5 files changed, 24 insertions(+), 15 deletions(-)
include/sleipnir/optimization/problem.hpp | 1 +
include/sleipnir/util/assert.hpp | 5 ++--
include/sleipnir/util/print.hpp | 31 +++++++++++++----------
3 files changed, 22 insertions(+), 15 deletions(-)
diff --git a/include/.styleguide b/include/.styleguide
index 1b6652d3d5886cf8c9eca0d855c21031775bad7c..4f4c76204071f90bf49eddb8c2aceb583b5e09ba 100644
--- a/include/.styleguide
+++ b/include/.styleguide
@@ -8,5 +8,6 @@ cppSrcFileInclude {
diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp
index 3185466605b6604068e2807e461d07d8c856c505..95a33952a5a368c7c81491dbe849a8096357dc38 100644
--- a/include/sleipnir/optimization/problem.hpp
+++ b/include/sleipnir/optimization/problem.hpp
@@ -15,6 +15,7 @@
includeOtherLibs {
^Eigen/
+ ^fmt/
^gch/
}
#include <Eigen/Core>
#include <Eigen/SparseCore>
+#include <fmt/chrono.h>
#include <gch/small_vector.hpp>
#include "sleipnir/autodiff/expression_type.hpp"
diff --git a/include/sleipnir/util/assert.hpp b/include/sleipnir/util/assert.hpp
index 75d8ffca32accbf66ffce30f073de1db2f42469b..53de01928b929793fa77885ec4a6d1a928bdc5a9 100644
index 0846928c3da7a6047a3c271dd2d377a3b755eeab..5d432608def05b6dee6b7cbdb9a0b91a6ab5e1c2 100644
--- a/include/sleipnir/util/assert.hpp
+++ b/include/sleipnir/util/assert.hpp
@@ -3,9 +3,10 @@
#pragma once
@@ -4,10 +4,11 @@
#ifdef SLEIPNIR_PYTHON
#ifdef JORMUNGANDR
-#include <format>
#include <source_location>
#include <stdexcept>
+
+#include <fmt/format.h>
+
/**
* Throw an exception in Python.
*/
@@ -13,7 +14,7 @@
@@ -15,7 +16,7 @@
do { \
if (!(condition)) { \
auto location = std::source_location::current(); \
@@ -48,7 +48,7 @@ index 75d8ffca32accbf66ffce30f073de1db2f42469b..53de01928b929793fa77885ec4a6d1a9
location.line(), location.function_name(), #condition)); \
} \
diff --git a/include/sleipnir/util/print.hpp b/include/sleipnir/util/print.hpp
index fe430352dabf4cd6a890dc8007237c7a261dfd4b..055d5c9fa246201f1d8ae7ddca00b1159aeb2a57 100644
index 797df849f63d960cf10eaf847415595961868ab0..a89b7d4f9864965443405a8e79cddd8dbfc54ad3 100644
--- a/include/sleipnir/util/print.hpp
+++ b/include/sleipnir/util/print.hpp
@@ -4,10 +4,15 @@
@@ -76,8 +76,8 @@ index fe430352dabf4cd6a890dc8007237c7a261dfd4b..055d5c9fa246201f1d8ae7ddca00b115
+ * Wrapper around fmt::print() that squelches write failure exceptions.
*/
template <typename... T>
-inline void print(std::format_string<T...> fmt, T&&... args) {
+inline void print(fmt::format_string<T...> fmt, T&&... args) {
-void print(std::format_string<T...> fmt, T&&... args) {
+void print(fmt::format_string<T...> fmt, T&&... args) {
try {
- std::print(fmt, std::forward<T>(args)...);
+ fmt::print(fmt, std::forward<T>(args)...);
@@ -90,8 +90,8 @@ index fe430352dabf4cd6a890dc8007237c7a261dfd4b..055d5c9fa246201f1d8ae7ddca00b115
+ * Wrapper around fmt::print() that squelches write failure exceptions.
*/
template <typename... T>
-inline void print(std::FILE* f, std::format_string<T...> fmt, T&&... args) {
+inline void print(std::FILE* f, fmt::format_string<T...> fmt, T&&... args) {
-void print(std::FILE* f, std::format_string<T...> fmt, T&&... args) {
+void print(std::FILE* f, fmt::format_string<T...> fmt, T&&... args) {
try {
- std::print(f, fmt, std::forward<T>(args)...);
+ fmt::print(f, fmt, std::forward<T>(args)...);
@@ -104,8 +104,8 @@ index fe430352dabf4cd6a890dc8007237c7a261dfd4b..055d5c9fa246201f1d8ae7ddca00b115
+ * Wrapper around fmt::println() that squelches write failure exceptions.
*/
template <typename... T>
-inline void println(std::format_string<T...> fmt, T&&... args) {
+inline void println(fmt::format_string<T...> fmt, T&&... args) {
-void println(std::format_string<T...> fmt, T&&... args) {
+void println(fmt::format_string<T...> fmt, T&&... args) {
try {
- std::println(fmt, std::forward<T>(args)...);
+ fmt::println(fmt, std::forward<T>(args)...);
@@ -118,34 +118,11 @@ index fe430352dabf4cd6a890dc8007237c7a261dfd4b..055d5c9fa246201f1d8ae7ddca00b115
+ * Wrapper around fmt::println() that squelches write failure exceptions.
*/
template <typename... T>
-inline void println(std::FILE* f, std::format_string<T...> fmt, T&&... args) {
+inline void println(std::FILE* f, fmt::format_string<T...> fmt, T&&... args) {
-void println(std::FILE* f, std::format_string<T...> fmt, T&&... args) {
+void println(std::FILE* f, fmt::format_string<T...> fmt, T&&... args) {
try {
- std::println(f, fmt, std::forward<T>(args)...);
+ fmt::println(f, fmt, std::forward<T>(args)...);
} catch (const std::system_error&) {
}
}
diff --git a/src/.styleguide b/src/.styleguide
index 1b6652d3d5886cf8c9eca0d855c21031775bad7c..4f4c76204071f90bf49eddb8c2aceb583b5e09ba 100644
--- a/src/.styleguide
+++ b/src/.styleguide
@@ -8,5 +8,6 @@ cppSrcFileInclude {
includeOtherLibs {
^Eigen/
+ ^fmt/
^gch/
}
diff --git a/src/optimization/problem.cpp b/src/optimization/problem.cpp
index c3331197e2365934273f57422b79fa18c2b78a5b..09828cdb6d7cddff692b9d17603dc0c11cd5a3ec 100644
--- a/src/optimization/problem.cpp
+++ b/src/optimization/problem.cpp
@@ -11,6 +11,7 @@
#include <Eigen/Core>
#include <Eigen/SparseCore>
+#include <fmt/chrono.h>
#include <gch/small_vector.hpp>
#include "optimization/bounds.hpp"

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

@@ -5,37 +5,37 @@ Subject: [PATCH 2/8] Use wpi::SmallVector
---
include/sleipnir/autodiff/expression.hpp | 4 ++--
include/sleipnir/autodiff/variable.hpp | 5 ++---
include/sleipnir/autodiff/variable.hpp | 4 ++--
include/sleipnir/autodiff/variable_matrix.hpp | 4 ++--
3 files changed, 6 insertions(+), 7 deletions(-)
3 files changed, 6 insertions(+), 6 deletions(-)
diff --git a/include/sleipnir/autodiff/expression.hpp b/include/sleipnir/autodiff/expression.hpp
index bb4d8c5641a5b3d633d372674e0a35f857889cd4..53a5f6d68d3153537840c4ff45fe5e5d8b0076b7 100644
index f5919de6c9c0be044335ce7764ded545215f0486..46814576a3db9f472329b880b94b1ab98d218867 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;
inline constexpr void inc_ref_count(Expression* expr);
-inline constexpr void dec_ref_count(Expression* expr);
+inline void dec_ref_count(Expression* expr);
@@ -33,7 +33,7 @@ struct Expression;
template <typename Scalar>
constexpr void inc_ref_count(Expression<Scalar>* expr);
template <typename Scalar>
-constexpr void dec_ref_count(Expression<Scalar>* expr);
+void dec_ref_count(Expression<Scalar>* expr);
/**
* Typedef for intrusive shared pointer to Expression.
@@ -733,7 +733,7 @@ inline constexpr void inc_ref_count(Expression* expr) {
*
@@ -801,7 +801,7 @@ constexpr void inc_ref_count(Expression<Scalar>* expr) {
* @param expr The shared pointer's managed object.
*/
-inline constexpr void dec_ref_count(Expression* expr) {
+inline void dec_ref_count(Expression* expr) {
template <typename Scalar>
-constexpr void dec_ref_count(Expression<Scalar>* expr) {
+void dec_ref_count(Expression<Scalar>* 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
diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp
index f60236811eba45c67a9638e90d5101d877ecc2d0..264f0950f293c67d6e6c7e729887090c050e40e2 100644
index c78af7224b2ef93ad50b238117583e01940c53ce..0a55b906130d7506c80eb150644ac44c222d1368 100644
--- a/include/sleipnir/autodiff/variable.hpp
+++ b/include/sleipnir/autodiff/variable.hpp
@@ -47,7 +47,7 @@ class SLEIPNIR_DLLEXPORT Variable {
@@ -61,7 +61,7 @@ class Variable : public SleipnirBase {
/**
* Constructs an empty Variable.
*/
@@ -43,35 +43,34 @@ index f60236811eba45c67a9638e90d5101d877ecc2d0..264f0950f293c67d6e6c7e729887090c
+ explicit Variable(std::nullptr_t) : expr{nullptr} {}
/**
* Constructs a Variable from a floating point type.
@@ -77,8 +77,7 @@ class SLEIPNIR_DLLEXPORT Variable {
* Constructs a Variable from a scalar type.
@@ -116,7 +116,7 @@ class Variable : public SleipnirBase {
*
* @param expr The autodiff variable.
*/
- explicit constexpr Variable(detail::ExpressionPtr&& expr)
- : expr{std::move(expr)} {}
+ explicit Variable(detail::ExpressionPtr&& expr) : expr{std::move(expr)} {}
- explicit constexpr Variable(detail::ExpressionPtr<Scalar>&& expr)
+ explicit Variable(detail::ExpressionPtr<Scalar>&& expr)
: expr{std::move(expr)} {}
/**
* Assignment operator for double.
diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp
index e1a419ca5356660b3c1c27230d1cb2a86977fb65..349a1550235516f9853609b61feded834ef2894b 100644
index bb66bebc01413a291242886366ce329bb5f4b70a..7ddf02c0e2f66aff8da422b874cbe9772f9fd00d 100644
--- a/include/sleipnir/autodiff/variable_matrix.hpp
+++ b/include/sleipnir/autodiff/variable_matrix.hpp
@@ -1120,14 +1120,14 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
@@ -1281,14 +1281,14 @@ class VariableMatrix : public SleipnirBase {
*
* @return Begin iterator.
* @return Const begin iterator.
*/
- const_iterator cbegin() const { return const_iterator{m_storage.cbegin()}; }
+ const_iterator cbegin() const { return const_iterator{m_storage.begin()}; }
/**
* Returns end iterator.
* Returns const end iterator.
*
* @return End iterator.
* @return Const end iterator.
*/
- const_iterator cend() const { return const_iterator{m_storage.cend()}; }
+ const_iterator cend() const { return const_iterator{m_storage.end()}; }
/**
* Returns number of elements in matrix.
* Returns reverse begin iterator.

19
upstream_utils/sleipnir_patches/0003-Use-wpi-byteswap.patch
View File

@@ -4,22 +4,11 @@ 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(-)
1 file changed, 2 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..74dd7990b03783ce805a186920d5142caeb178c6 100644
index f9143f2b925064e9df5c763823dcf3d435e7aa28..4b810e54a8038162e03cf08fc8eab52b67b2cdd5 100644
--- a/include/sleipnir/util/spy.hpp
+++ b/include/sleipnir/util/spy.hpp
@@ -12,6 +12,7 @@
@@ -28,9 +17,9 @@ index a2f94803e3744cee771669210d1af883160e9896..74dd7990b03783ce805a186920d5142c
#include <Eigen/SparseCore>
+#include <wpi/util/bit.hpp>
#include "sleipnir/util/symbol_exports.hpp"
namespace slp {
@@ -115,7 +116,7 @@ class SLEIPNIR_DLLEXPORT Spy {
@@ -114,7 +115,7 @@ class Spy {
*/
void write32le(int32_t num) {
if constexpr (std::endian::native != std::endian::little) {

78
upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch
View File

@@ -4,51 +4,43 @@ 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(-)
include/sleipnir/optimization/problem.hpp | 8 ++++----
include/sleipnir/util/print_diagnostics.hpp | 6 +++---
2 files changed, 7 insertions(+), 7 deletions(-)
diff --git a/src/optimization/problem.cpp b/src/optimization/problem.cpp
index 09828cdb6d7cddff692b9d17603dc0c11cd5a3ec..886de24cc0532d31f1e186150da79e925f212556 100644
--- a/src/optimization/problem.cpp
+++ b/src/optimization/problem.cpp
@@ -7,7 +7,6 @@
#include <memory>
#include <optional>
#include <ranges>
-#include <utility>
diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp
index 95a33952a5a368c7c81491dbe849a8096357dc38..d20777a5b1912754dda5504313549197e867d34b 100644
--- a/include/sleipnir/optimization/problem.hpp
+++ b/include/sleipnir/optimization/problem.hpp
@@ -708,11 +708,11 @@ class Problem {
// 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())]);
#include <Eigen/Core>
#include <Eigen/SparseCore>
@@ -350,11 +349,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");
@@ -366,7 +365,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,
if (m_decision_variables.size() == 1) {
slp::print("\n1 decision variable\n");
@@ -724,7 +724,7 @@ class Problem {
[](const gch::small_vector<Variable<Scalar>>& 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/include/sleipnir/util/print_diagnostics.hpp b/include/sleipnir/util/print_diagnostics.hpp
index 9c1f9eb71b9417e138b95fd4d2d678cfb54595d1..032be8fb7b5e4196ff401c77ae9e91f1c966cde6 100644
--- a/include/sleipnir/util/print_diagnostics.hpp
+++ b/include/sleipnir/util/print_diagnostics.hpp
@@ -252,9 +252,9 @@ void print_iteration_diagnostics(int iterations, IterationType type,
slp::println(
"│{:4} {:4} {:9.3f} {:12e} {:13e} {:12e} {:12e} {:.2e} {:<5} {:.2e} "
"{:.2e} {:2d}│",

54
upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch
View File

@@ -5,14 +5,14 @@ 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(-)
include/sleipnir/optimization/problem.hpp | 8 +++++---
2 files changed, 9 insertions(+), 4 deletions(-)
diff --git a/include/sleipnir/autodiff/adjoint_expression_graph.hpp b/include/sleipnir/autodiff/adjoint_expression_graph.hpp
index 33b6eee615141a1d6472f116842d62052ef54dd9..b333aebd3e59fa23eed6046c13d736c3d2eccac7 100644
index 16bea7efeeca78d25b34b0b1242ca19cbd05a482..a77323eee9277fc3c77a11ab57ab5003d9ed4543 100644
--- a/include/sleipnir/autodiff/adjoint_expression_graph.hpp
+++ b/include/sleipnir/autodiff/adjoint_expression_graph.hpp
@@ -158,7 +158,10 @@ class AdjointExpressionGraph {
@@ -171,7 +171,10 @@ class AdjointExpressionGraph {
}
}
} else {
@@ -22,32 +22,24 @@ index 33b6eee615141a1d6472f116842d62052ef54dd9..b333aebd3e59fa23eed6046c13d736c3
+ const auto& node = m_top_list[i];
+
// Append adjoints of wrt to sparse matrix triplets
if (col != -1 && node->adjoint != 0.0) {
if (col != -1 && node->adjoint != Scalar(0)) {
triplets.emplace_back(row, col, node->adjoint);
diff --git a/src/optimization/problem.cpp b/src/optimization/problem.cpp
index 886de24cc0532d31f1e186150da79e925f212556..e32481e9314c9ef472843adb5bedbd993627d5d9 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>
@@ -367,9 +366,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);
diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp
index d20777a5b1912754dda5504313549197e867d34b..5256d08e5f9d8642049d8bb8323d76c7b3bbbef7 100644
--- a/include/sleipnir/optimization/problem.hpp
+++ b/include/sleipnir/optimization/problem.hpp
@@ -726,9 +726,11 @@ class Problem {
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);
}

4
upstream_utils/sleipnir_patches/0006-Suppress-clang-tidy-false-positives.patch
View File

@@ -8,10 +8,10 @@ Subject: [PATCH 6/8] Suppress clang-tidy false positives
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp
index 264f0950f293c67d6e6c7e729887090c050e40e2..62135a5539308ae69f6b45a64d9337c4c3e96d7b 100644
index 0a55b906130d7506c80eb150644ac44c222d1368..30ec62161df75c6948bbf3d65432c852a0d926c2 100644
--- a/include/sleipnir/autodiff/variable.hpp
+++ b/include/sleipnir/autodiff/variable.hpp
@@ -633,7 +633,7 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints {
@@ -862,7 +862,7 @@ struct InequalityConstraints {
* @param inequality_constraints The list of InequalityConstraints to
* concatenate.
*/

10
upstream_utils/sleipnir_patches/0007-Suppress-GCC-12-warning-false-positive.patch
View File

@@ -8,12 +8,12 @@ Subject: [PATCH 7/8] Suppress GCC 12 warning false positive
1 file changed, 7 insertions(+)
diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp
index 349a1550235516f9853609b61feded834ef2894b..70bccf4fc078a49e22b6699db1228c765430a121 100644
index 7ddf02c0e2f66aff8da422b874cbe9772f9fd00d..351030b4041027ba63a2e6ec08f2077b3c35b5db 100644
--- a/include/sleipnir/autodiff/variable_matrix.hpp
+++ b/include/sleipnir/autodiff/variable_matrix.hpp
@@ -573,6 +573,10 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
@@ -578,6 +578,10 @@ class VariableMatrix : public SleipnirBase {
VariableMatrix result(VariableMatrix::empty, lhs.rows(), rhs.cols());
VariableMatrix<Scalar> result(detail::empty, lhs.rows(), rhs.cols());
+#if __GNUC__ >= 12
+#pragma GCC diagnostic push
@@ -21,8 +21,8 @@ index 349a1550235516f9853609b61feded834ef2894b..70bccf4fc078a49e22b6699db1228c76
+#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 {
Variable sum{Scalar(0)};
@@ -637,6 +641,9 @@ class VariableMatrix : public SleipnirBase {
result[i, j] = sum;
}
}

724
upstream_utils/sleipnir_patches/0008-Revert-Use-multidimensional-array-subscript-operator.patch → upstream_utils/sleipnir_patches/0008-Use-operator-instead-of-multidimensional-array-subsc.patch
View File

File diff suppressed because it is too large Load Diff

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

@@ -19,7 +19,7 @@ wpi::units::volt_t ArmFeedforward::Calculate(
wpi::units::unit_t<Angle> currentAngle,
wpi::units::unit_t<Velocity> currentVelocity,
wpi::units::unit_t<Velocity> nextVelocity) const {
using VarMat = slp::VariableMatrix;
using VarMat = slp::VariableMatrix<double>;
// Small kₐ values make the solver ill-conditioned
if (kA < wpi::units::unit_t<ka_unit>{1e-1}) {
@@ -40,7 +40,7 @@ wpi::units::volt_t ArmFeedforward::Calculate(
Vectord<2> r_k{currentAngle.value(), currentVelocity.value()};
slp::Variable u_k;
slp::Variable<double> u_k;
// Initial guess
auto acceleration = (nextVelocity - currentVelocity) / m_dt;

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

@@ -20,7 +20,7 @@ Translation2d Ellipse2d::Nearest(const Translation2d& point) const {
// Find nearest point
{
slp::Problem problem;
slp::Problem<double> problem;
// Point on ellipse
auto x = problem.decision_variable();

45
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/adjoint_expression_graph.hpp vendored
View File

@@ -12,13 +12,17 @@
#include "sleipnir/autodiff/variable.hpp"
#include "sleipnir/autodiff/variable_matrix.hpp"
#include "sleipnir/util/assert.hpp"
#include "sleipnir/util/empty.hpp"
namespace slp::detail {
/**
* This class is an adaptor type that performs value updates of an expression's
* adjoint graph.
*
* @tparam Scalar Scalar type.
*/
template <typename Scalar>
class AdjointExpressionGraph {
public:
/**
@@ -26,7 +30,7 @@ class AdjointExpressionGraph {
*
* @param root The root node of the expression.
*/
explicit AdjointExpressionGraph(const Variable& root)
explicit AdjointExpressionGraph(const Variable<Scalar>& root)
: m_top_list{topological_sort(root.expr)} {
for (const auto& node : m_top_list) {
m_col_list.emplace_back(node->col);
@@ -50,18 +54,19 @@ class AdjointExpressionGraph {
* @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 {
VariableMatrix<Scalar> generate_gradient_tree(
const VariableMatrix<Scalar>& wrt) const {
slp_assert(wrt.cols() == 1);
// 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};
return VariableMatrix<Scalar>{detail::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);
m_top_list[0]->adjoint_expr = constant_ptr(Scalar(1));
// 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
@@ -72,15 +77,19 @@ class AdjointExpressionGraph {
auto& rhs = node->args[1];
if (lhs != nullptr) {
lhs->adjoint_expr += node->grad_expr_l(lhs, rhs, node->adjoint_expr);
if (rhs != nullptr) {
// Binary operator
lhs->adjoint_expr += node->grad_expr_l(lhs, rhs, node->adjoint_expr);
rhs->adjoint_expr += node->grad_expr_r(lhs, rhs, node->adjoint_expr);
} else {
// Unary operator
lhs->adjoint_expr += node->grad_expr_l(lhs, rhs, node->adjoint_expr);
}
}
}
// Move gradient tree to return value
VariableMatrix grad{VariableMatrix::empty, wrt.rows(), 1};
VariableMatrix<Scalar> grad{detail::empty, wrt.rows(), 1};
for (int row = 0; row < grad.rows(); ++row) {
grad[row] = Variable{std::move(wrt[row].expr->adjoint_expr)};
}
@@ -104,16 +113,18 @@ class AdjointExpressionGraph {
* @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 {
void append_gradient_triplets(
gch::small_vector<Eigen::Triplet<Scalar>>& triplets, int row,
const VariableMatrix<Scalar>& wrt) const {
slp_assert(wrt.cols() == 1);
// 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;
elem.expr->adjoint = Scalar(0);
}
}
@@ -122,11 +133,11 @@ class AdjointExpressionGraph {
}
// Set root node's adjoint to 1 since df/df is 1
m_top_list[0]->adjoint = 1.0;
m_top_list[0]->adjoint = Scalar(1);
// Zero the rest of the adjoints
for (auto& node : m_top_list | std::views::drop(1)) {
node->adjoint = 0.0;
node->adjoint = Scalar(0);
}
// df/dx = (df/dy)(dy/dx). The adjoint of x is equal to the adjoint of y
@@ -139,10 +150,12 @@ class AdjointExpressionGraph {
if (lhs != nullptr) {
if (rhs != nullptr) {
// Binary operator
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);
// Unary operator
lhs->adjoint += node->grad_l(lhs->val, Scalar(0), node->adjoint);
}
}
}
@@ -153,7 +166,7 @@ class AdjointExpressionGraph {
const auto& node = wrt[col].expr;
// Append adjoints of wrt to sparse matrix triplets
if (node->adjoint != 0.0) {
if (node->adjoint != Scalar(0)) {
triplets.emplace_back(row, col, node->adjoint);
}
}
@@ -163,7 +176,7 @@ class AdjointExpressionGraph {
const auto& node = m_top_list[i];
// Append adjoints of wrt to sparse matrix triplets
if (col != -1 && node->adjoint != 0.0) {
if (col != -1 && node->adjoint != Scalar(0)) {
triplets.emplace_back(row, col, node->adjoint);
}
}
@@ -172,7 +185,7 @@ class AdjointExpressionGraph {
private:
// Topological sort of graph from parent to child
gch::small_vector<Expression*> m_top_list;
gch::small_vector<Expression<Scalar>*> m_top_list;
// List that maps nodes to their respective column
gch::small_vector<int> m_col_list;

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

File diff suppressed because it is too large Load Diff

20
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_graph.hpp vendored
View File

@@ -15,11 +15,13 @@ namespace slp::detail {
*
* https://en.wikipedia.org/wiki/Topological_sorting
*
* @tparam Scalar Scalar type.
* @param root The root node of the expression.
*/
inline gch::small_vector<Expression*> topological_sort(
const ExpressionPtr& root) {
gch::small_vector<Expression*> list;
template <typename Scalar>
gch::small_vector<Expression<Scalar>*> topological_sort(
const ExpressionPtr<Scalar>& root) {
gch::small_vector<Expression<Scalar>*> list;
// If the root type is constant, updates are a no-op, so return an empty list
if (root == nullptr || root->type() == ExpressionType::CONSTANT) {
@@ -27,7 +29,7 @@ inline gch::small_vector<Expression*> topological_sort(
}
// Stack of nodes to explore
gch::small_vector<Expression*> stack;
gch::small_vector<Expression<Scalar>*> stack;
// Enumerate incoming edges for each node via depth-first search
stack.emplace_back(root.get());
@@ -72,20 +74,18 @@ inline gch::small_vector<Expression*> topological_sort(
* Update the values of all nodes in this graph based on the values of
* their dependent nodes.
*
* @tparam Scalar Scalar type.
* @param list Topological sort of graph from parent to child.
*/
inline void update_values(const gch::small_vector<Expression*>& list) {
template <typename Scalar>
void update_values(const gch::small_vector<Expression<Scalar>*>& 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);
}
node->val = node->value(lhs->val, rhs ? rhs->val : Scalar(0));
}
}
}

20
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient.hpp vendored
View File

@@ -20,8 +20,11 @@ namespace slp {
*
* The gradient is only recomputed if the variable expression is quadratic or
* higher order.
*
* @tparam Scalar Scalar type.
*/
class SLEIPNIR_DLLEXPORT Gradient {
template <typename Scalar>
class Gradient {
public:
/**
* Constructs a Gradient object.
@@ -29,7 +32,7 @@ class SLEIPNIR_DLLEXPORT Gradient {
* @param variable Variable of which to compute the gradient.
* @param wrt Variable with respect to which to compute the gradient.
*/
Gradient(Variable variable, Variable wrt)
Gradient(Variable<Scalar> variable, Variable<Scalar> wrt)
: m_jacobian{std::move(variable), std::move(wrt)} {}
/**
@@ -39,7 +42,7 @@ class SLEIPNIR_DLLEXPORT Gradient {
* @param wrt Vector of variables with respect to which to compute the
* gradient.
*/
Gradient(Variable variable, SleipnirMatrixLike auto wrt)
Gradient(Variable<Scalar> variable, SleipnirMatrixLike<Scalar> auto wrt)
: m_jacobian{VariableMatrix{std::move(variable)}, std::move(wrt)} {}
/**
@@ -50,23 +53,26 @@ class SLEIPNIR_DLLEXPORT Gradient {
*
* @return The gradient as a VariableMatrix.
*/
VariableMatrix get() const { return m_jacobian.get().T(); }
VariableMatrix<Scalar> 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() {
const Eigen::SparseVector<Scalar>& value() {
m_g = m_jacobian.value().transpose();
return m_g;
}
private:
Eigen::SparseVector<double> m_g;
Eigen::SparseVector<Scalar> m_g;
Jacobian m_jacobian;
Jacobian<Scalar> m_jacobian;
};
extern template class EXPORT_TEMPLATE_DECLARE(SLEIPNIR_DLLEXPORT)
Gradient<double>;
} // namespace slp

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

@@ -12,7 +12,6 @@
#include "sleipnir/autodiff/variable_matrix.hpp"
#include "sleipnir/util/assert.hpp"
#include "sleipnir/util/concepts.hpp"
#include "sleipnir/util/symbol_exports.hpp"
namespace slp {
@@ -23,11 +22,12 @@ namespace slp {
* The gradient tree is cached so subsequent Hessian calculations are faster,
* and the Hessian is only recomputed if the variable expression is nonlinear.
*
* @tparam Scalar Scalar type.
* @tparam UpLo Which part of the Hessian to compute (Lower or Lower | Upper).
*/
template <int UpLo>
template <typename Scalar, int UpLo>
requires(UpLo == Eigen::Lower) || (UpLo == (Eigen::Lower | Eigen::Upper))
class SLEIPNIR_DLLEXPORT Hessian {
class Hessian {
public:
/**
* Constructs a Hessian object.
@@ -35,8 +35,8 @@ class SLEIPNIR_DLLEXPORT Hessian {
* @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)
: Hessian{std::move(variable), VariableMatrix{std::move(wrt)}} {}
Hessian(Variable<Scalar> variable, Variable<Scalar> wrt)
: Hessian{std::move(variable), VariableMatrix<Scalar>{std::move(wrt)}} {}
/**
* Constructs a Hessian object.
@@ -45,8 +45,8 @@ class SLEIPNIR_DLLEXPORT Hessian {
* @param wrt Vector of variables with respect to which to compute the
* Hessian.
*/
Hessian(Variable variable, SleipnirMatrixLike auto wrt)
: m_variables{detail::AdjointExpressionGraph{variable}
Hessian(Variable<Scalar> variable, SleipnirMatrixLike<Scalar> auto wrt)
: m_variables{detail::AdjointExpressionGraph<Scalar>{variable}
.generate_gradient_tree(wrt)},
m_wrt{wrt} {
slp_assert(m_wrt.cols() == 1);
@@ -74,7 +74,7 @@ class SLEIPNIR_DLLEXPORT Hessian {
// If the row is linear, compute its gradient once here and cache its
// triplets. Constant rows are ignored because their gradients have no
// nonzero triplets.
m_graphs[row].append_adjoint_triplets(m_cached_triplets, row, m_wrt);
m_graphs[row].append_gradient_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().
@@ -85,7 +85,7 @@ class SLEIPNIR_DLLEXPORT Hessian {
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>();
m_H = m_H.template triangularView<Eigen::Lower>();
}
}
}
@@ -98,9 +98,9 @@ class SLEIPNIR_DLLEXPORT Hessian {
*
* @return The Hessian as a VariableMatrix.
*/
VariableMatrix get() const {
VariableMatrix result{VariableMatrix::empty, m_variables.rows(),
m_wrt.rows()};
VariableMatrix<Scalar> get() const {
VariableMatrix<Scalar> result{detail::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);
@@ -108,7 +108,7 @@ class SLEIPNIR_DLLEXPORT Hessian {
if (grad[col].expr != nullptr) {
result(row, col) = std::move(grad[col]);
} else {
result(row, col) = Variable{0.0};
result(row, col) = Variable{Scalar(0)};
}
}
}
@@ -121,7 +121,7 @@ class SLEIPNIR_DLLEXPORT Hessian {
*
* @return The Hessian at wrt's value.
*/
const Eigen::SparseMatrix<double>& value() {
const Eigen::SparseMatrix<Scalar>& value() {
if (m_nonlinear_rows.empty()) {
return m_H;
}
@@ -136,31 +136,36 @@ class SLEIPNIR_DLLEXPORT Hessian {
// Compute each nonlinear row of the Hessian
for (int row : m_nonlinear_rows) {
m_graphs[row].append_adjoint_triplets(triplets, row, m_wrt);
m_graphs[row].append_gradient_triplets(triplets, row, m_wrt);
}
m_H.setFromTriplets(triplets.begin(), triplets.end());
if constexpr (UpLo == Eigen::Lower) {
m_H = m_H.triangularView<Eigen::Lower>();
m_H = m_H.template triangularView<Eigen::Lower>();
}
return m_H;
}
private:
VariableMatrix m_variables;
VariableMatrix m_wrt;
VariableMatrix<Scalar> m_variables;
VariableMatrix<Scalar> m_wrt;
gch::small_vector<detail::AdjointExpressionGraph> m_graphs;
gch::small_vector<detail::AdjointExpressionGraph<Scalar>> m_graphs;
Eigen::SparseMatrix<double> m_H{m_variables.rows(), m_wrt.rows()};
Eigen::SparseMatrix<Scalar> m_H{m_variables.rows(), m_wrt.rows()};
// Cached triplets for gradients of linear rows
gch::small_vector<Eigen::Triplet<double>> m_cached_triplets;
gch::small_vector<Eigen::Triplet<Scalar>> m_cached_triplets;
// List of row indices for nonlinear rows whose graients will be computed in
// Value()
gch::small_vector<int> m_nonlinear_rows;
};
// @cond Suppress Doxygen
extern template class EXPORT_TEMPLATE_DECLARE(SLEIPNIR_DLLEXPORT)
Hessian<double, Eigen::Lower | Eigen::Upper>;
// @endcond
} // namespace slp

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

@@ -12,6 +12,7 @@
#include "sleipnir/autodiff/variable_matrix.hpp"
#include "sleipnir/util/assert.hpp"
#include "sleipnir/util/concepts.hpp"
#include "sleipnir/util/empty.hpp"
#include "sleipnir/util/symbol_exports.hpp"
namespace slp {
@@ -22,8 +23,11 @@ namespace slp {
*
* The Jacobian is only recomputed if the variable expression is quadratic or
* higher order.
*
* @tparam Scalar Scalar type.
*/
class SLEIPNIR_DLLEXPORT Jacobian {
template <typename Scalar>
class Jacobian {
public:
/**
* Constructs a Jacobian object.
@@ -31,9 +35,19 @@ class SLEIPNIR_DLLEXPORT Jacobian {
* @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)
: Jacobian{VariableMatrix{std::move(variable)},
VariableMatrix{std::move(wrt)}} {}
Jacobian(Variable<Scalar> variable, Variable<Scalar> wrt)
: Jacobian{VariableMatrix<Scalar>{std::move(variable)},
VariableMatrix<Scalar>{std::move(wrt)}} {}
/**
* Constructs a Jacobian object.
*
* @param variable Variable of which to compute the Jacobian.
* @param wrt Vector of variables with respect to which to compute the
* Jacobian.
*/
Jacobian(Variable<Scalar> variable, SleipnirMatrixLike<Scalar> auto wrt)
: Jacobian{VariableMatrix<Scalar>{std::move(variable)}, std::move(wrt)} {}
/**
* Constructs a Jacobian object.
@@ -42,7 +56,8 @@ class SLEIPNIR_DLLEXPORT Jacobian {
* @param wrt Vector of variables with respect to which to compute the
* Jacobian.
*/
Jacobian(VariableMatrix variables, SleipnirMatrixLike auto wrt)
Jacobian(VariableMatrix<Scalar> variables,
SleipnirMatrixLike<Scalar> auto wrt)
: m_variables{std::move(variables)}, m_wrt{std::move(wrt)} {
slp_assert(m_variables.cols() == 1);
slp_assert(m_wrt.cols() == 1);
@@ -70,7 +85,7 @@ class SLEIPNIR_DLLEXPORT Jacobian {
// If the row is linear, compute its gradient once here and cache its
// triplets. Constant rows are ignored because their gradients have no
// nonzero triplets.
m_graphs[row].append_adjoint_triplets(m_cached_triplets, row, m_wrt);
m_graphs[row].append_gradient_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().
@@ -91,9 +106,9 @@ class SLEIPNIR_DLLEXPORT Jacobian {
*
* @return The Jacobian as a VariableMatrix.
*/
VariableMatrix get() const {
VariableMatrix result{VariableMatrix::empty, m_variables.rows(),
m_wrt.rows()};
VariableMatrix<Scalar> get() const {
VariableMatrix<Scalar> result{detail::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);
@@ -101,7 +116,7 @@ class SLEIPNIR_DLLEXPORT Jacobian {
if (grad[col].expr != nullptr) {
result(row, col) = std::move(grad[col]);
} else {
result(row, col) = Variable{0.0};
result(row, col) = Variable{Scalar(0)};
}
}
}
@@ -114,7 +129,7 @@ class SLEIPNIR_DLLEXPORT Jacobian {
*
* @return The Jacobian at wrt's value.
*/
const Eigen::SparseMatrix<double>& value() {
const Eigen::SparseMatrix<Scalar>& value() {
if (m_nonlinear_rows.empty()) {
return m_J;
}
@@ -129,7 +144,7 @@ class SLEIPNIR_DLLEXPORT Jacobian {
// Compute each nonlinear row of the Jacobian
for (int row : m_nonlinear_rows) {
m_graphs[row].append_adjoint_triplets(triplets, row, m_wrt);
m_graphs[row].append_gradient_triplets(triplets, row, m_wrt);
}
m_J.setFromTriplets(triplets.begin(), triplets.end());
@@ -138,19 +153,22 @@ class SLEIPNIR_DLLEXPORT Jacobian {
}
private:
VariableMatrix m_variables;
VariableMatrix m_wrt;
VariableMatrix<Scalar> m_variables;
VariableMatrix<Scalar> m_wrt;
gch::small_vector<detail::AdjointExpressionGraph> m_graphs;
gch::small_vector<detail::AdjointExpressionGraph<Scalar>> m_graphs;
Eigen::SparseMatrix<double> m_J{m_variables.rows(), m_wrt.rows()};
Eigen::SparseMatrix<Scalar> m_J{m_variables.rows(), m_wrt.rows()};
// Cached triplets for gradients of linear rows
gch::small_vector<Eigen::Triplet<double>> m_cached_triplets;
gch::small_vector<Eigen::Triplet<Scalar>> m_cached_triplets;
// List of row indices for nonlinear rows whose graients will be computed in
// Value()
gch::small_vector<int> m_nonlinear_rows;
};
extern template class EXPORT_TEMPLATE_DECLARE(SLEIPNIR_DLLEXPORT)
Jacobian<double>;
} // namespace slp

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

@@ -0,0 +1,13 @@
// Copyright (c) Sleipnir contributors
#pragma once
namespace slp {
/**
* Marker interface for concepts to determine whether a given scalar or matrix
* type belongs to Sleipnir.
*/
class SleipnirBase {};
} // namespace slp

6
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/slice.hpp vendored
View File

@@ -47,7 +47,8 @@ class SLEIPNIR_DLLEXPORT Slice {
/**
* Constructs a slice.
*/
constexpr Slice(slicing::none_t) // NOLINT
// NOLINTNEXTLINE (google-explicit-constructor)
constexpr Slice(slicing::none_t)
: Slice(0, std::numeric_limits<int>::max(), 1) {}
/**
@@ -55,7 +56,8 @@ class SLEIPNIR_DLLEXPORT Slice {
*
* @param start Slice start index (inclusive).
*/
constexpr Slice(int start) { // NOLINT
// NOLINTNEXTLINE (google-explicit-constructor)
constexpr Slice(int start) {
this->start = start;
this->stop = (start == -1) ? std::numeric_limits<int>::max() : start + 1;
this->step = 1;

730
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable.hpp vendored
View File

File diff suppressed because it is too large Load Diff

223
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_block.hpp vendored
View File

@@ -8,9 +8,11 @@
#include <Eigen/Core>
#include "sleipnir/autodiff/sleipnir_base.hpp"
#include "sleipnir/autodiff/slice.hpp"
#include "sleipnir/autodiff/variable.hpp"
#include "sleipnir/util/assert.hpp"
#include "sleipnir/util/empty.hpp"
#include "sleipnir/util/function_ref.hpp"
namespace slp {
@@ -21,8 +23,13 @@ namespace slp {
* @tparam Mat The type of the matrix whose storage this class points to.
*/
template <typename Mat>
class VariableBlock {
class VariableBlock : public SleipnirBase {
public:
/**
* Scalar type alias.
*/
using Scalar = typename Mat::Scalar;
/**
* Copy constructor.
*/
@@ -98,12 +105,8 @@ class VariableBlock {
*
* @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())} {}
// NOLINTNEXTLINE (google-explicit-constructor)
VariableBlock(Mat& mat) : VariableBlock{mat, 0, 0, mat.rows(), mat.cols()} {}
/**
* Constructs a Variable block pointing to a subset of the given matrix.
@@ -142,7 +145,7 @@ class VariableBlock {
m_col_slice_length{col_slice_length} {}
/**
* Assigns a double to the block.
* Assigns a scalar to the block.
*
* This only works for blocks with one row and one column.
*
@@ -158,13 +161,13 @@ class VariableBlock {
}
/**
* Assigns a double to the block.
* Assigns a scalar to the block.
*
* This only works for blocks with one row and one column.
*
* @param value Value to assign.
*/
void set_value(double value) {
void set_value(Scalar value) {
slp_assert(rows() == 1 && cols() == 1);
(*this)(0, 0).set_value(value);
@@ -195,7 +198,7 @@ class VariableBlock {
* @param values Eigen matrix of values.
*/
template <typename Derived>
requires std::same_as<typename Derived::Scalar, double>
requires std::same_as<typename Derived::Scalar, Scalar>
void set_value(const Eigen::MatrixBase<Derived>& values) {
slp_assert(rows() == values.rows() && cols() == values.cols());
@@ -247,7 +250,7 @@ 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<Scalar>& operator()(int row, int col)
requires(!std::is_const_v<Mat>)
{
slp_assert(row >= 0 && row < rows());
@@ -263,7 +266,7 @@ 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<Scalar>& 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,
@@ -271,27 +274,27 @@ class VariableBlock {
}
/**
* Returns a scalar subblock at the given row.
* Returns a scalar subblock at the given index.
*
* @param row The scalar subblock's row.
* @return A scalar subblock at the given row.
* @param index The scalar subblock's index.
* @return A scalar subblock at the given index.
*/
Variable& operator[](int row)
Variable<Scalar>& operator[](int index)
requires(!std::is_const_v<Mat>)
{
slp_assert(row >= 0 && row < rows() * cols());
return (*this)(row / cols(), row % cols());
slp_assert(index >= 0 && index < rows() * cols());
return (*this)(index / cols(), index % cols());
}
/**
* Returns a scalar subblock at the given row.
* Returns a scalar subblock at the given index.
*
* @param row The scalar subblock's row.
* @return A scalar subblock at the given row.
* @param index The scalar subblock's index.
* @return A scalar subblock at the given index.
*/
const Variable& operator[](int row) const {
slp_assert(row >= 0 && row < rows() * cols());
return (*this)(row / cols(), row % cols());
const Variable<Scalar>& operator[](int index) const {
slp_assert(index >= 0 && index < rows() * cols());
return (*this)(index / cols(), index % cols());
}
/**
@@ -342,14 +345,7 @@ class VariableBlock {
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};
return (*this)(row_slice, row_slice_length, col_slice, col_slice_length);
}
/**
@@ -363,14 +359,7 @@ class VariableBlock {
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};
return (*this)(row_slice, row_slice_length, col_slice, col_slice_length);
}
/**
@@ -390,10 +379,12 @@ class VariableBlock {
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},
m_row_slice.start + row_slice.stop * m_row_slice.step,
row_slice.step * m_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},
m_col_slice.start + col_slice.stop * m_col_slice.step,
col_slice.step * m_col_slice.step},
col_slice_length};
}
@@ -416,10 +407,12 @@ class VariableBlock {
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},
m_row_slice.start + row_slice.stop * m_row_slice.step,
row_slice.step * m_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},
m_col_slice.start + col_slice.stop * m_col_slice.step,
col_slice.step * m_col_slice.step},
col_slice_length};
}
@@ -431,8 +424,9 @@ class VariableBlock {
* @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);
slp_assert(cols() == 1);
slp_assert(offset >= 0 && offset < rows());
slp_assert(length >= 0 && length <= rows() - offset);
return block(offset, 0, length, 1);
}
@@ -444,8 +438,9 @@ class VariableBlock {
* @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);
slp_assert(cols() == 1);
slp_assert(offset >= 0 && offset < rows());
slp_assert(length >= 0 && length <= rows() - offset);
return block(offset, 0, length, 1);
}
@@ -504,7 +499,7 @@ class VariableBlock {
for (int i = 0; i < rows(); ++i) {
for (int j = 0; j < rhs.cols(); ++j) {
Variable sum;
Variable sum{Scalar(0)};
for (int k = 0; k < cols(); ++k) {
sum += (*this)(i, k) * rhs(k, j);
}
@@ -640,7 +635,8 @@ class VariableBlock {
/**
* Implicit conversion operator from 1x1 VariableBlock to Variable.
*/
operator Variable() const { // NOLINT
// NOLINTNEXTLINE (google-explicit-constructor)
operator Variable<Scalar>() const {
slp_assert(rows() == 1 && cols() == 1);
return (*this)(0, 0);
}
@@ -651,7 +647,7 @@ class VariableBlock {
* @return The transpose of the variable matrix.
*/
std::remove_cv_t<Mat> T() const {
std::remove_cv_t<Mat> result{Mat::empty, cols(), rows()};
std::remove_cv_t<Mat> result{detail::empty, cols(), rows()};
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
@@ -683,21 +679,15 @@ class VariableBlock {
* @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();
}
Scalar value(int row, int col) { return (*this)(row, col).value(); }
/**
* Returns a row of the variable column vector.
* Returns an element of the variable block.
*
* @param index The index of the element to return.
* @return A row of the variable column vector.
* @return An element of the variable block.
*/
double value(int index) {
Scalar value(int index) {
slp_assert(index >= 0 && index < rows() * cols());
return value(index / cols(), index % cols());
}
@@ -707,8 +697,9 @@ class VariableBlock {
*
* @return The contents of the variable matrix.
*/
Eigen::MatrixXd value() {
Eigen::MatrixXd result{rows(), cols()};
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> value() {
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> result{rows(),
cols()};
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
@@ -726,8 +717,9 @@ class VariableBlock {
* @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()};
function_ref<Variable<Scalar>(const Variable<Scalar>& x)> unary_op)
const {
std::remove_cv_t<Mat> result{detail::empty, rows(), cols()};
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
@@ -742,11 +734,11 @@ class VariableBlock {
class iterator {
public:
using iterator_category = std::forward_iterator_tag;
using value_type = Variable;
using iterator_category = std::bidirectional_iterator_tag;
using value_type = Variable<Scalar>;
using difference_type = std::ptrdiff_t;
using pointer = Variable*;
using reference = Variable&;
using pointer = Variable<Scalar>*;
using reference = Variable<Scalar>&;
constexpr iterator() noexcept = default;
@@ -764,6 +756,17 @@ class VariableBlock {
return retval;
}
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]; }
@@ -775,11 +778,11 @@ class VariableBlock {
class const_iterator {
public:
using iterator_category = std::forward_iterator_tag;
using value_type = Variable;
using iterator_category = std::bidirectional_iterator_tag;
using value_type = Variable<Scalar>;
using difference_type = std::ptrdiff_t;
using pointer = Variable*;
using const_reference = const Variable&;
using pointer = Variable<Scalar>*;
using const_reference = const Variable<Scalar>&;
constexpr const_iterator() noexcept = default;
@@ -797,6 +800,17 @@ class VariableBlock {
return retval;
}
constexpr const_iterator& operator--() noexcept {
--m_index;
return *this;
}
constexpr const_iterator operator--(int) noexcept {
iterator retval = *this;
--(*this);
return retval;
}
constexpr bool operator==(const const_iterator&) const noexcept = default;
constexpr const_reference operator*() const noexcept {
@@ -808,6 +822,9 @@ class VariableBlock {
int m_index = 0;
};
using reverse_iterator = std::reverse_iterator<iterator>;
using const_reverse_iterator = std::reverse_iterator<const_iterator>;
#endif // DOXYGEN_SHOULD_SKIP_THIS
/**
@@ -839,19 +856,69 @@ class VariableBlock {
const_iterator end() const { return const_iterator(this, rows() * cols()); }
/**
* Returns begin iterator.
* Returns const begin iterator.
*
* @return Begin iterator.
* @return Const begin iterator.
*/
const_iterator cbegin() const { return const_iterator(this, 0); }
/**
* Returns end iterator.
* Returns const end iterator.
*
* @return End iterator.
* @return Const end iterator.
*/
const_iterator cend() const { return const_iterator(this, rows() * cols()); }
/**
* Returns reverse begin iterator.
*
* @return Reverse begin iterator.
*/
reverse_iterator rbegin() { return reverse_iterator{end()}; }
/**
* Returns reverse end iterator.
*
* @return Reverse end iterator.
*/
reverse_iterator rend() { return reverse_iterator{begin()}; }
/**
* Returns const reverse begin iterator.
*
* @return Const reverse begin iterator.
*/
const_reverse_iterator rbegin() const {
return const_reverse_iterator{end()};
}
/**
* Returns const reverse end iterator.
*
* @return Const reverse end iterator.
*/
const_reverse_iterator rend() const {
return const_reverse_iterator{begin()};
}
/**
* Returns const reverse begin iterator.
*
* @return Const reverse begin iterator.
*/
const_reverse_iterator crbegin() const {
return const_reverse_iterator{cend()};
}
/**
* Returns const reverse end iterator.
*
* @return Const reverse end iterator.
*/
const_reverse_iterator crend() const {
return const_reverse_iterator{cbegin()};
}
/**
* Returns number of elements in matrix.
*

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

File diff suppressed because it is too large Load Diff

17
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/multistart.hpp vendored
View File

@@ -16,15 +16,16 @@ namespace slp {
/**
* The result of a multistart solve.
*
* @tparam Scalar Scalar type.
* @tparam DecisionVariables The type containing the decision variable initial
* guess.
*/
template <typename DecisionVariables>
template <typename Scalar, typename DecisionVariables>
struct MultistartResult {
/// The solver exit status.
ExitStatus status;
/// The solution's cost.
double cost;
Scalar cost;
/// The decision variables.
DecisionVariables variables;
};
@@ -37,26 +38,28 @@ struct MultistartResult {
* solves are always preferred over solutions from unsuccessful solves, and cost
* (lower is better) is the tiebreaker between successful solves.
*
* @tparam Scalar Scalar type.
* @tparam DecisionVariables The type containing the decision variable initial
* guess.
* @param solve A user-provided function that takes a decision variable initial
* guess and returns a MultistartResult.
* @param initial_guesses A list of decision variable initial guesses to try.
*/
template <typename DecisionVariables>
MultistartResult<DecisionVariables> Multistart(
function_ref<MultistartResult<DecisionVariables>(
template <typename Scalar, typename DecisionVariables>
MultistartResult<Scalar, DecisionVariables> Multistart(
function_ref<MultistartResult<Scalar, DecisionVariables>(
const DecisionVariables& initial_guess)>
solve,
std::span<const DecisionVariables> initial_guesses) {
gch::small_vector<std::future<MultistartResult<DecisionVariables>>> futures;
gch::small_vector<std::future<MultistartResult<Scalar, DecisionVariables>>>
futures;
futures.reserve(initial_guesses.size());
for (const auto& initial_guess : initial_guesses) {
futures.emplace_back(std::async(std::launch::async, solve, initial_guess));
}
gch::small_vector<MultistartResult<DecisionVariables>> results;
gch::small_vector<MultistartResult<Scalar, DecisionVariables>> results;
results.reserve(futures.size());
for (auto& future : futures) {

134
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp.hpp vendored
View File

@@ -44,8 +44,11 @@ namespace slp {
*
* https://underactuated.mit.edu/trajopt.html goes into more detail on each
* transcription method.
*
* @tparam Scalar Scalar type.
*/
class SLEIPNIR_DLLEXPORT OCP : public Problem {
template <typename Scalar>
class OCP : public Problem<Scalar> {
public:
/**
* Build an optimization problem using a system evolution function (explicit
@@ -64,10 +67,10 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
* @param timestep_method The timestep method.
* @param transcription_method The transcription method.
*/
OCP(int num_states, int num_inputs, std::chrono::duration<double> dt,
OCP(int num_states, int num_inputs, std::chrono::duration<Scalar> dt,
int num_steps,
function_ref<VariableMatrix(const VariableMatrix& x,
const VariableMatrix& u)>
function_ref<VariableMatrix<Scalar>(const VariableMatrix<Scalar>& x,
const VariableMatrix<Scalar>& u)>
dynamics,
DynamicsType dynamics_type = DynamicsType::EXPLICIT_ODE,
TimestepMethod timestep_method = TimestepMethod::FIXED,
@@ -77,12 +80,11 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
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);
},
[=]([[maybe_unused]] const VariableMatrix<Scalar>& t,
const VariableMatrix<Scalar>& x,
const VariableMatrix<Scalar>& u,
[[maybe_unused]] const VariableMatrix<Scalar>& dt)
-> VariableMatrix<Scalar> { return dynamics(x, u); },
dynamics_type,
timestep_method,
transcription_method} {}
@@ -104,10 +106,11 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
* @param timestep_method The timestep method.
* @param transcription_method The transcription method.
*/
OCP(int num_states, int num_inputs, std::chrono::duration<double> dt,
OCP(int num_states, int num_inputs, std::chrono::duration<Scalar> dt,
int num_steps,
function_ref<VariableMatrix(const Variable& t, const VariableMatrix& x,
const VariableMatrix& u, const Variable& dt)>
function_ref<VariableMatrix<Scalar>(
const Variable<Scalar>& t, const VariableMatrix<Scalar>& x,
const VariableMatrix<Scalar>& u, const Variable<Scalar>& dt)>
dynamics,
DynamicsType dynamics_type = DynamicsType::EXPLICIT_ODE,
TimestepMethod timestep_method = TimestepMethod::FIXED,
@@ -117,40 +120,40 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
m_dynamics{std::move(dynamics)},
m_dynamics_type{dynamics_type} {
// u is num_steps + 1 so that the final constraint function evaluation works
m_U = decision_variable(num_inputs, m_num_steps + 1);
m_U = this->decision_variable(num_inputs, m_num_steps + 1);
if (timestep_method == TimestepMethod::FIXED) {
m_DT = VariableMatrix{1, m_num_steps + 1};
m_DT = VariableMatrix<Scalar>{1, m_num_steps + 1};
for (int i = 0; i < num_steps + 1; ++i) {
m_DT(0, i) = dt.count();
}
} else if (timestep_method == TimestepMethod::VARIABLE_SINGLE) {
Variable single_dt = decision_variable();
Variable single_dt = this->decision_variable();
single_dt.set_value(dt.count());
// Set the member variable matrix to track the decision variable
m_DT = VariableMatrix{1, m_num_steps + 1};
m_DT = VariableMatrix<Scalar>{1, m_num_steps + 1};
for (int i = 0; i < num_steps + 1; ++i) {
m_DT(0, i) = single_dt;
}
} else if (timestep_method == TimestepMethod::VARIABLE) {
m_DT = decision_variable(1, m_num_steps + 1);
m_DT = this->decision_variable(1, m_num_steps + 1);
for (int i = 0; i < num_steps + 1; ++i) {
m_DT(0, i).set_value(dt.count());
}
}
if (transcription_method == TranscriptionMethod::DIRECT_TRANSCRIPTION) {
m_X = decision_variable(num_states, m_num_steps + 1);
m_X = this->decision_variable(num_states, m_num_steps + 1);
constrain_direct_transcription();
} else if (transcription_method ==
TranscriptionMethod::DIRECT_COLLOCATION) {
m_X = decision_variable(num_states, m_num_steps + 1);
m_X = this->decision_variable(num_states, m_num_steps + 1);
constrain_direct_collocation();
} else if (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{num_states, m_num_steps + 1};
m_X = VariableMatrix<Scalar>{num_states, m_num_steps + 1};
constrain_single_shooting();
}
}
@@ -163,7 +166,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
template <typename T>
requires ScalarLike<T> || MatrixLike<T>
void constrain_initial_state(const T& initial_state) {
subject_to(this->initial_state() == initial_state);
this->subject_to(this->initial_state() == initial_state);
}
/**
@@ -174,7 +177,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
template <typename T>
requires ScalarLike<T> || MatrixLike<T>
void constrain_final_state(const T& final_state) {
subject_to(this->final_state() == final_state);
this->subject_to(this->final_state() == final_state);
}
/**
@@ -185,9 +188,9 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
* @param callback The callback f(x, u) where x is the state and u is the
* input vector.
*/
void for_each_step(
const function_ref<void(const VariableMatrix& x, const VariableMatrix& u)>
callback) {
void for_each_step(const function_ref<void(const VariableMatrix<Scalar>& x,
const VariableMatrix<Scalar>& u)>
callback) {
for (int i = 0; i < m_num_steps + 1; ++i) {
auto x = X().col(i);
auto u = U().col(i);
@@ -204,10 +207,11 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
* vector, u is the input vector, and dt is the timestep duration.
*/
void for_each_step(
const function_ref<void(const Variable& t, const VariableMatrix& x,
const VariableMatrix& u, const Variable& dt)>
const function_ref<
void(const Variable<Scalar>& t, const VariableMatrix<Scalar>& x,
const VariableMatrix<Scalar>& u, const Variable<Scalar>& dt)>
callback) {
Variable time = 0.0;
Variable<Scalar> time{0};
for (int i = 0; i < m_num_steps + 1; ++i) {
auto x = X().col(i);
@@ -229,7 +233,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
requires ScalarLike<T> || MatrixLike<T>
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);
this->subject_to(U().col(i) >= lower_bound);
}
}
@@ -243,7 +247,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
requires ScalarLike<T> || MatrixLike<T>
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);
this->subject_to(U().col(i) <= upper_bound);
}
}
@@ -252,8 +256,8 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
*
* @param min_timestep The minimum timestep.
*/
void set_min_timestep(std::chrono::duration<double> min_timestep) {
subject_to(dt() >= min_timestep.count());
void set_min_timestep(std::chrono::duration<Scalar> min_timestep) {
this->subject_to(dt() >= min_timestep.count());
}
/**
@@ -261,8 +265,8 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
*
* @param max_timestep The maximum timestep.
*/
void set_max_timestep(std::chrono::duration<double> max_timestep) {
subject_to(dt() <= max_timestep.count());
void set_max_timestep(std::chrono::duration<Scalar> max_timestep) {
this->subject_to(dt() <= max_timestep.count());
}
/**
@@ -273,7 +277,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
*
* @return The state variable matrix.
*/
VariableMatrix& X() { return m_X; }
VariableMatrix<Scalar>& X() { return m_X; }
/**
* Get the input variables. After the problem is solved, this will contain the
@@ -284,7 +288,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
*
* @return The input variable matrix.
*/
VariableMatrix& U() { return m_U; }
VariableMatrix<Scalar>& U() { return m_U; }
/**
* Get the timestep variables. After the problem is solved, this will contain
@@ -295,33 +299,34 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
*
* @return The timestep variable matrix.
*/
VariableMatrix& dt() { return m_DT; }
VariableMatrix<Scalar>& dt() { return m_DT; }
/**
* Convenience function to get the initial state in the trajectory.
*
* @return The initial state of the trajectory.
*/
VariableMatrix initial_state() { return m_X.col(0); }
VariableMatrix<Scalar> initial_state() { return m_X.col(0); }
/**
* Convenience function to get the final state in the trajectory.
*
* @return The final state of the trajectory.
*/
VariableMatrix final_state() { return m_X.col(m_num_steps); }
VariableMatrix<Scalar> final_state() { return m_X.col(m_num_steps); }
private:
int m_num_steps;
function_ref<VariableMatrix(const Variable& t, const VariableMatrix& x,
const VariableMatrix& u, const Variable& dt)>
function_ref<VariableMatrix<Scalar>(
const Variable<Scalar>& t, const VariableMatrix<Scalar>& x,
const VariableMatrix<Scalar>& u, const Variable<Scalar>& dt)>
m_dynamics;
DynamicsType m_dynamics_type;
VariableMatrix m_X;
VariableMatrix m_U;
VariableMatrix m_DT;
VariableMatrix<Scalar> m_X;
VariableMatrix<Scalar> m_U;
VariableMatrix<Scalar> m_DT;
/**
* Performs 4th order Runge-Kutta integration of dx/dt = f(t, x, u) for dt.
@@ -334,13 +339,13 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
*/
template <typename F, typename State, typename Input, typename Time>
State rk4(F&& f, State x, Input u, Time t0, Time dt) {
auto halfdt = dt * 0.5;
auto halfdt = dt * Scalar(0.5);
State k1 = f(t0, x, u, dt);
State k2 = f(t0 + halfdt, x + k1 * halfdt, u, dt);
State k3 = f(t0 + halfdt, x + k2 * halfdt, u, dt);
State k4 = f(t0 + dt, x + k3 * dt, u, dt);
return x + (k1 + k2 * 2.0 + k3 * 2.0 + k4) * (dt / 6.0);
return x + (k1 + k2 * Scalar(2) + k3 * Scalar(2) + k4) * (dt / Scalar(6));
}
/**
@@ -349,7 +354,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
void constrain_direct_collocation() {
slp_assert(m_dynamics_type == DynamicsType::EXPLICIT_ODE);
Variable time = 0.0;
Variable<Scalar> time{0};
// Derivation at https://mec560sbu.github.io/2016/09/30/direct_collocation/
for (int i = 0; i < m_num_steps; ++i) {
@@ -368,14 +373,15 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
auto xdot_begin = f(t_begin, x_begin, u_begin, h);
auto xdot_end = f(t_end, x_end, u_end, h);
auto xdot_c =
-3 / (2 * h) * (x_begin - x_end) - 0.25 * (xdot_begin + xdot_end);
auto xdot_c = Scalar(-3) / (Scalar(2) * h) * (x_begin - x_end) -
Scalar(0.25) * (xdot_begin + xdot_end);
auto t_c = t_begin + 0.5 * h;
auto x_c = 0.5 * (x_begin + x_end) + h / 8 * (xdot_begin - xdot_end);
auto u_c = 0.5 * (u_begin + u_end);
auto t_c = t_begin + Scalar(0.5) * h;
auto x_c = Scalar(0.5) * (x_begin + x_end) +
h / Scalar(8) * (xdot_begin - xdot_end);
auto u_c = Scalar(0.5) * (u_begin + u_end);
subject_to(xdot_c == f(t_c, x_c, u_c, h));
this->subject_to(xdot_c == f(t_c, x_c, u_c, h));
time += h;
}
@@ -385,7 +391,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
* Apply direct transcription dynamics constraints.
*/
void constrain_direct_transcription() {
Variable time = 0.0;
Variable<Scalar> time{0};
for (int i = 0; i < m_num_steps; ++i) {
auto x_begin = X().col(i);
@@ -394,11 +400,12 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
Variable dt = this->dt()(0, i);
if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) {
subject_to(x_end == rk4<const decltype(m_dynamics)&, VariableMatrix,
VariableMatrix, Variable>(m_dynamics, x_begin,
u, time, dt));
this->subject_to(
x_end == rk4<const decltype(m_dynamics)&, VariableMatrix<Scalar>,
VariableMatrix<Scalar>, Variable<Scalar>>(
m_dynamics, x_begin, u, time, dt));
} else if (m_dynamics_type == DynamicsType::DISCRETE) {
subject_to(x_end == m_dynamics(time, x_begin, u, dt));
this->subject_to(x_end == m_dynamics(time, x_begin, u, dt));
}
time += dt;
@@ -409,7 +416,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
* Apply single shooting dynamics constraints.
*/
void constrain_single_shooting() {
Variable time = 0.0;
Variable<Scalar> time{0};
for (int i = 0; i < m_num_steps; ++i) {
auto x_begin = X().col(i);
@@ -418,8 +425,9 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
Variable dt = this->dt()(0, i);
if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) {
x_end = rk4<const decltype(m_dynamics)&, VariableMatrix, VariableMatrix,
Variable>(m_dynamics, x_begin, u, time, dt);
x_end = rk4<const decltype(m_dynamics)&, VariableMatrix<Scalar>,
VariableMatrix<Scalar>, Variable<Scalar>>(
m_dynamics, x_begin, u, time, dt);
} else if (m_dynamics_type == DynamicsType::DISCRETE) {
x_end = m_dynamics(time, x_begin, u, dt);
}
@@ -429,4 +437,6 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
}
};
extern template class EXPORT_TEMPLATE_DECLARE(SLEIPNIR_DLLEXPORT) OCP<double>;
} // namespace slp

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

@@ -3,22 +3,39 @@
#pragma once
#include <algorithm>
#include <array>
#include <cmath>
#include <concepts>
#include <functional>
#include <iterator>
#include <memory>
#include <optional>
#include <ranges>
#include <utility>
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <fmt/chrono.h>
#include <gch/small_vector.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/iteration_info.hpp"
#include "sleipnir/optimization/solver/newton.hpp"
#include "sleipnir/optimization/solver/options.hpp"
#include "sleipnir/optimization/solver/sqp.hpp"
#include "sleipnir/optimization/solver/util/bounds.hpp"
#include "sleipnir/util/empty.hpp"
#include "sleipnir/util/print.hpp"
#include "sleipnir/util/print_diagnostics.hpp"
#include "sleipnir/util/setup_profiler.hpp"
#include "sleipnir/util/spy.hpp"
#include "sleipnir/util/symbol_exports.hpp"
namespace slp {
@@ -43,8 +60,11 @@ subject to cₑ(x) = 0
* 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.
*
* @tparam Scalar Scalar type.
*/
class SLEIPNIR_DLLEXPORT Problem {
template <typename Scalar>
class Problem {
public:
/**
* Construct the optimization problem.
@@ -57,7 +77,7 @@ class SLEIPNIR_DLLEXPORT Problem {
* @return A decision variable in the optimization problem.
*/
[[nodiscard]]
Variable decision_variable() {
Variable<Scalar> decision_variable() {
m_decision_variables.emplace_back();
return m_decision_variables.back();
}
@@ -70,10 +90,10 @@ class SLEIPNIR_DLLEXPORT Problem {
* @return A matrix of decision variables in the optimization problem.
*/
[[nodiscard]]
VariableMatrix decision_variable(int rows, int cols = 1) {
VariableMatrix<Scalar> decision_variable(int rows, int cols = 1) {
m_decision_variables.reserve(m_decision_variables.size() + rows * cols);
VariableMatrix vars{VariableMatrix::empty, rows, cols};
VariableMatrix<Scalar> vars{detail::empty, rows, cols};
for (int row = 0; row < rows; ++row) {
for (int col = 0; col < cols; ++col) {
@@ -97,7 +117,7 @@ class SLEIPNIR_DLLEXPORT Problem {
* problem.
*/
[[nodiscard]]
VariableMatrix symmetric_decision_variable(int rows) {
VariableMatrix<Scalar> 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.
@@ -108,7 +128,7 @@ class SLEIPNIR_DLLEXPORT Problem {
m_decision_variables.reserve(m_decision_variables.size() +
(rows * rows + rows) / 2);
VariableMatrix vars{VariableMatrix::empty, rows, rows};
VariableMatrix<Scalar> vars{detail::empty, rows, rows};
for (int row = 0; row < rows; ++row) {
for (int col = 0; col <= row; ++col) {
@@ -130,7 +150,7 @@ class SLEIPNIR_DLLEXPORT Problem {
*
* @param cost The cost function to minimize.
*/
void minimize(const Variable& cost) { m_f = cost; }
void minimize(const Variable<Scalar>& cost) { m_f = cost; }
/**
* Tells the solver to minimize the output of the given cost function.
@@ -141,7 +161,7 @@ class SLEIPNIR_DLLEXPORT Problem {
*
* @param cost The cost function to minimize.
*/
void minimize(Variable&& cost) { m_f = std::move(cost); }
void minimize(Variable<Scalar>&& cost) { m_f = std::move(cost); }
/**
* Tells the solver to maximize the output of the given objective function.
@@ -152,7 +172,7 @@ class SLEIPNIR_DLLEXPORT Problem {
*
* @param objective The objective function to maximize.
*/
void maximize(const Variable& objective) {
void maximize(const Variable<Scalar>& objective) {
// Maximizing a cost function is the same as minimizing its negative
m_f = -objective;
}
@@ -166,7 +186,7 @@ class SLEIPNIR_DLLEXPORT Problem {
*
* @param objective The objective function to maximize.
*/
void maximize(Variable&& objective) {
void maximize(Variable<Scalar>&& objective) {
// Maximizing a cost function is the same as minimizing its negative
m_f = -std::move(objective);
}
@@ -177,7 +197,7 @@ class SLEIPNIR_DLLEXPORT Problem {
*
* @param constraint The constraint to satisfy.
*/
void subject_to(const EqualityConstraints& constraint) {
void subject_to(const EqualityConstraints<Scalar>& constraint) {
m_equality_constraints.reserve(m_equality_constraints.size() +
constraint.constraints.size());
std::ranges::copy(constraint.constraints,
@@ -190,7 +210,7 @@ class SLEIPNIR_DLLEXPORT Problem {
*
* @param constraint The constraint to satisfy.
*/
void subject_to(EqualityConstraints&& constraint) {
void subject_to(EqualityConstraints<Scalar>&& constraint) {
m_equality_constraints.reserve(m_equality_constraints.size() +
constraint.constraints.size());
std::ranges::copy(constraint.constraints,
@@ -203,7 +223,7 @@ class SLEIPNIR_DLLEXPORT Problem {
*
* @param constraint The constraint to satisfy.
*/
void subject_to(const InequalityConstraints& constraint) {
void subject_to(const InequalityConstraints<Scalar>& constraint) {
m_inequality_constraints.reserve(m_inequality_constraints.size() +
constraint.constraints.size());
std::ranges::copy(constraint.constraints,
@@ -216,7 +236,7 @@ class SLEIPNIR_DLLEXPORT Problem {
*
* @param constraint The constraint to satisfy.
*/
void subject_to(InequalityConstraints&& constraint) {
void subject_to(InequalityConstraints<Scalar>&& constraint) {
m_inequality_constraints.reserve(m_inequality_constraints.size() +
constraint.constraints.size());
std::ranges::copy(constraint.constraints,
@@ -243,7 +263,8 @@ class SLEIPNIR_DLLEXPORT Problem {
*/
ExpressionType equality_constraint_type() const {
if (!m_equality_constraints.empty()) {
return std::ranges::max(m_equality_constraints, {}, &Variable::type)
return std::ranges::max(m_equality_constraints, {},
&Variable<Scalar>::type)
.type();
} else {
return ExpressionType::NONE;
@@ -257,7 +278,8 @@ class SLEIPNIR_DLLEXPORT Problem {
*/
ExpressionType inequality_constraint_type() const {
if (!m_inequality_constraints.empty()) {
return std::ranges::max(m_inequality_constraints, {}, &Variable::type)
return std::ranges::max(m_inequality_constraints, {},
&Variable<Scalar>::type)
.type();
} else {
return ExpressionType::NONE;
@@ -275,7 +297,314 @@ class SLEIPNIR_DLLEXPORT Problem {
* @return The solver status.
*/
ExitStatus solve(const Options& options = Options{},
[[maybe_unused]] bool spy = false);
[[maybe_unused]] bool spy = false) {
// Create the initial value column vector
Eigen::Vector<Scalar, Eigen::Dynamic> 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<Scalar> x_ad{m_decision_variables};
// Set up cost function
Variable f = m_f.value_or(Scalar(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();
gch::small_vector<std::function<bool(const IterationInfo<Scalar>& info)>>
callbacks;
for (const auto& callback : m_iteration_callbacks) {
callbacks.emplace_back(callback);
}
for (const auto& callback : m_persistent_iteration_callbacks) {
callbacks.emplace_back(callback);
}
// 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<Scalar, 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<Scalar>> H_spy;
if (spy) {
H_spy = std::make_unique<Spy<Scalar>>(
"H.spy", "Hessian", "Decision variables", "Decision variables",
num_decision_variables, num_decision_variables);
callbacks.push_back([&](const IterationInfo<Scalar>& info) -> bool {
H_spy->add(info.H);
return false;
});
}
#endif
// Invoke Newton solver
status = newton<Scalar>(
NewtonMatrixCallbacks<Scalar>{
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x) -> Scalar {
x_ad.set_value(x);
return f.value();
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseVector<Scalar> {
x_ad.set_value(x);
return g.value();
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseMatrix<Scalar> {
x_ad.set_value(x);
return H.value();
}},
callbacks, options, x);
} else if (m_inequality_constraints.empty()) {
if (options.diagnostics) {
slp::println("\nInvoking SQP solver\n");
}
VariableMatrix<Scalar> 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<Scalar> y_ad(num_equality_constraints);
Variable L = f - y_ad.T() * c_e_ad;
// Set up Lagrangian Hessian autodiff
ad_setup_profilers.emplace_back(" ↳ ∇²ₓₓL").start();
Hessian<Scalar, 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<Scalar>> H_spy;
std::unique_ptr<Spy<Scalar>> A_e_spy;
if (spy) {
H_spy = std::make_unique<Spy<Scalar>>(
"H.spy", "Hessian", "Decision variables", "Decision variables",
num_decision_variables, num_decision_variables);
A_e_spy = std::make_unique<Spy<Scalar>>(
"A_e.spy", "Equality constraint Jacobian", "Constraints",
"Decision variables", num_equality_constraints,
num_decision_variables);
callbacks.push_back([&](const IterationInfo<Scalar>& info) -> bool {
H_spy->add(info.H);
A_e_spy->add(info.A_e);
return false;
});
}
#endif
// Invoke SQP solver
status = sqp<Scalar>(
SQPMatrixCallbacks<Scalar>{
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x) -> Scalar {
x_ad.set_value(x);
return f.value();
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseVector<Scalar> {
x_ad.set_value(x);
return g.value();
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x,
const Eigen::Vector<Scalar, Eigen::Dynamic>& y)
-> Eigen::SparseMatrix<Scalar> {
x_ad.set_value(x);
y_ad.set_value(y);
return H.value();
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::Vector<Scalar, Eigen::Dynamic> {
x_ad.set_value(x);
return c_e_ad.value();
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseMatrix<Scalar> {
x_ad.set_value(x);
return A_e.value();
}},
callbacks, options, x);
} else {
if (options.diagnostics) {
slp::println("\nInvoking IPM solver...\n");
}
VariableMatrix<Scalar> c_e_ad{m_equality_constraints};
VariableMatrix<Scalar> 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<Scalar> y_ad(num_equality_constraints);
VariableMatrix<Scalar> z_ad(num_inequality_constraints);
Variable L = f - y_ad.T() * c_e_ad - z_ad.T() * c_i_ad;
// Set up Lagrangian Hessian autodiff
ad_setup_profilers.emplace_back(" ↳ ∇²ₓₓL").start();
Hessian<Scalar, 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<Scalar>> H_spy;
std::unique_ptr<Spy<Scalar>> A_e_spy;
std::unique_ptr<Spy<Scalar>> A_i_spy;
if (spy) {
H_spy = std::make_unique<Spy<Scalar>>(
"H.spy", "Hessian", "Decision variables", "Decision variables",
num_decision_variables, num_decision_variables);
A_e_spy = std::make_unique<Spy<Scalar>>(
"A_e.spy", "Equality constraint Jacobian", "Constraints",
"Decision variables", num_equality_constraints,
num_decision_variables);
A_i_spy = std::make_unique<Spy<Scalar>>(
"A_i.spy", "Inequality constraint Jacobian", "Constraints",
"Decision variables", num_inequality_constraints,
num_decision_variables);
callbacks.push_back([&](const IterationInfo<Scalar>& 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<Scalar>(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<Scalar>(
InteriorPointMatrixCallbacks<Scalar>{
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x) -> Scalar {
x_ad.set_value(x);
return f.value();
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseVector<Scalar> {
x_ad.set_value(x);
return g.value();
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x,
const Eigen::Vector<Scalar, Eigen::Dynamic>& y,
const Eigen::Vector<Scalar, Eigen::Dynamic>& z)
-> Eigen::SparseMatrix<Scalar> {
x_ad.set_value(x);
y_ad.set_value(y);
z_ad.set_value(z);
return H.value();
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::Vector<Scalar, Eigen::Dynamic> {
x_ad.set_value(x);
return c_e_ad.value();
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseMatrix<Scalar> {
x_ad.set_value(x);
return A_e.value();
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::Vector<Scalar, Eigen::Dynamic> {
x_ad.set_value(x);
return c_i_ad.value();
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseMatrix<Scalar> {
x_ad.set_value(x);
return A_i.value();
}},
callbacks, options,
#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
bound_constraint_mask,
#endif
x);
}
if (options.diagnostics) {
print_autodiff_diagnostics(ad_setup_profilers);
slp::println("\nExit: {}", to_message(status));
}
// Assign the solution to the original Variable instances
VariableMatrix<Scalar>{m_decision_variables}.set_value(x);
return status;
}
/**
* Adds a callback to be called at the beginning of each solver iteration.
@@ -285,12 +614,13 @@ class SLEIPNIR_DLLEXPORT Problem {
* @param callback The callback.
*/
template <typename F>
requires requires(F callback, const IterationInfo& info) {
requires requires(F callback, const IterationInfo<Scalar>& 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 =
std::forward<F>(callback)](const IterationInfo<Scalar>& info) {
callback(info);
return false;
});
@@ -305,7 +635,7 @@ class SLEIPNIR_DLLEXPORT Problem {
* solver to exit early with the solution it has so far.
*/
template <typename F>
requires requires(F callback, const IterationInfo& info) {
requires requires(F callback, const IterationInfo<Scalar>& info) {
{ callback(info) } -> std::same_as<bool>;
}
void add_callback(F&& callback) {
@@ -328,7 +658,7 @@ class SLEIPNIR_DLLEXPORT Problem {
* solver to exit early with the solution it has so far.
*/
template <typename F>
requires requires(F callback, const IterationInfo& info) {
requires requires(F callback, const IterationInfo<Scalar>& info) {
{ callback(info) } -> std::same_as<bool>;
}
void add_persistent_callback(F&& callback) {
@@ -338,25 +668,93 @@ class SLEIPNIR_DLLEXPORT Problem {
private:
// The list of decision variables, which are the root of the problem's
// expression tree
gch::small_vector<Variable> m_decision_variables;
gch::small_vector<Variable<Scalar>> m_decision_variables;
// The cost function: f(x)
std::optional<Variable> m_f;
std::optional<Variable<Scalar>> m_f;
// The list of equality constraints: cₑ(x) = 0
gch::small_vector<Variable> m_equality_constraints;
gch::small_vector<Variable<Scalar>> m_equality_constraints;
// The list of inequality constraints: cᵢ(x) ≥ 0
gch::small_vector<Variable> m_inequality_constraints;
gch::small_vector<Variable<Scalar>> m_inequality_constraints;
// The iteration callbacks
gch::small_vector<std::function<bool(const IterationInfo& info)>>
gch::small_vector<std::function<bool(const IterationInfo<Scalar>& info)>>
m_iteration_callbacks;
gch::small_vector<std::function<bool(const IterationInfo& info)>>
gch::small_vector<std::function<bool(const IterationInfo<Scalar>& info)>>
m_persistent_iteration_callbacks;
void print_exit_conditions([[maybe_unused]] const Options& options);
void print_problem_analysis();
void 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() ||
!m_persistent_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);
}
}
void 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<Scalar>>& 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);
}
};
extern template class EXPORT_TEMPLATE_DECLARE(SLEIPNIR_DLLEXPORT)
Problem<double>;
} // namespace slp

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

@@ -2,200 +2,41 @@
#pragma once
#include <algorithm>
#include <chrono>
#include <cmath>
#include <functional>
#include <limits>
#include <span>
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <gch/small_vector.hpp>
#include "sleipnir/optimization/solver/exit_status.hpp"
#include "sleipnir/optimization/solver/interior_point_matrix_callbacks.hpp"
#include "sleipnir/optimization/solver/iteration_info.hpp"
#include "sleipnir/optimization/solver/options.hpp"
#include "sleipnir/optimization/solver/util/error_estimate.hpp"
#include "sleipnir/optimization/solver/util/filter.hpp"
#include "sleipnir/optimization/solver/util/fraction_to_the_boundary_rule.hpp"
#include "sleipnir/optimization/solver/util/is_locally_infeasible.hpp"
#include "sleipnir/optimization/solver/util/kkt_error.hpp"
#include "sleipnir/optimization/solver/util/regularized_ldlt.hpp"
#include "sleipnir/util/assert.hpp"
#include "sleipnir/util/print_diagnostics.hpp"
#include "sleipnir/util/scope_exit.hpp"
#include "sleipnir/util/scoped_profiler.hpp"
#include "sleipnir/util/solve_profiler.hpp"
#include "sleipnir/util/symbol_exports.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 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.
@@ -211,6 +52,7 @@ subject to cₑ(x) = 0
where f(x) is the cost function, cₑ(x) are the equality constraints, and cᵢ(x)
are the inequality constraints.
@tparam Scalar Scalar type.
@param[in] matrix_callbacks Matrix callbacks.
@param[in] iteration_callbacks The list of callbacks to call at the beginning of
each iteration.
@@ -219,14 +61,659 @@ are the inequality constraints.
variables.
@return The exit status.
*/
SLEIPNIR_DLLEXPORT ExitStatus
interior_point(const InteriorPointMatrixCallbacks& matrix_callbacks,
std::span<std::function<bool(const IterationInfo& info)>>
template <typename Scalar>
ExitStatus interior_point(
const InteriorPointMatrixCallbacks<Scalar>& matrix_callbacks,
std::span<std::function<bool(const IterationInfo<Scalar>& info)>>
iteration_callbacks,
const Options& options,
#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
const Eigen::ArrayX<bool>& bound_constraint_mask,
#endif
Eigen::Vector<Scalar, Eigen::Dynamic>& x) {
/**
* Interior-point method step direction.
*/
struct Step {
/// Primal step.
Eigen::Vector<Scalar, Eigen::Dynamic> p_x;
/// Equality constraint dual step.
Eigen::Vector<Scalar, Eigen::Dynamic> p_y;
/// Inequality constraint slack variable step.
Eigen::Vector<Scalar, Eigen::Dynamic> p_s;
/// Inequality constraint dual step.
Eigen::Vector<Scalar, Eigen::Dynamic> p_z;
};
using std::isfinite;
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(" ↳ iter callbacks");
solve_profilers.emplace_back(" ↳ KKT matrix build");
solve_profilers.emplace_back(" ↳ KKT matrix decomp");
solve_profilers.emplace_back(" ↳ KKT system solve");
solve_profilers.emplace_back(" ↳ line search");
solve_profilers.emplace_back(" ↳ SOC");
solve_profilers.emplace_back(" ↳ next iter prep");
solve_profilers.emplace_back(" ↳ 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& iter_callbacks_prof = solve_profilers[4];
auto& kkt_matrix_build_prof = solve_profilers[5];
auto& kkt_matrix_decomp_prof = solve_profilers[6];
auto& kkt_system_solve_prof = solve_profilers[7];
auto& line_search_prof = solve_profilers[8];
auto& soc_prof = solve_profilers[9];
auto& next_iter_prep_prof = solve_profilers[10];
// 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<Scalar> matrices{
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x) -> Scalar {
ScopedProfiler prof{f_prof};
return matrix_callbacks.f(x);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseVector<Scalar> {
ScopedProfiler prof{g_prof};
return matrix_callbacks.g(x);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x,
const Eigen::Vector<Scalar, Eigen::Dynamic>& y,
const Eigen::Vector<Scalar, Eigen::Dynamic>& z)
-> Eigen::SparseMatrix<Scalar> {
ScopedProfiler prof{H_prof};
return matrix_callbacks.H(x, y, z);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::Vector<Scalar, Eigen::Dynamic> {
ScopedProfiler prof{c_e_prof};
return matrix_callbacks.c_e(x);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseMatrix<Scalar> {
ScopedProfiler prof{A_e_prof};
return matrix_callbacks.A_e(x);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::Vector<Scalar, Eigen::Dynamic> {
ScopedProfiler prof{c_i_prof};
return matrix_callbacks.c_i(x);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseMatrix<Scalar> {
ScopedProfiler prof{A_i_prof};
return matrix_callbacks.A_i(x);
}};
#else
const auto& matrices = matrix_callbacks;
#endif
solver_prof.start();
setup_prof.start();
Scalar f = matrices.f(x);
Eigen::Vector<Scalar, Eigen::Dynamic> c_e = matrices.c_e(x);
Eigen::Vector<Scalar, Eigen::Dynamic> 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<Scalar> g = matrices.g(x);
Eigen::SparseMatrix<Scalar> A_e = matrices.A_e(x);
Eigen::SparseMatrix<Scalar> A_i = matrices.A_i(x);
Eigen::Vector<Scalar, Eigen::Dynamic> s =
Eigen::Vector<Scalar, Eigen::Dynamic>::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::Vector<Scalar, Eigen::Dynamic> y =
Eigen::Vector<Scalar, Eigen::Dynamic>::Zero(num_equality_constraints);
Eigen::Vector<Scalar, Eigen::Dynamic> z =
Eigen::Vector<Scalar, Eigen::Dynamic>::Ones(num_inequality_constraints);
Eigen::SparseMatrix<Scalar> 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 (!isfinite(f) || !c_e.allFinite() || !c_i.allFinite()) {
return ExitStatus::NONFINITE_INITIAL_COST_OR_CONSTRAINTS;
}
int iterations = 0;
// Barrier parameter minimum
const Scalar μ_min = Scalar(options.tolerance) / Scalar(10);
// Barrier parameter μ
Scalar μ(0.1);
// Fraction-to-the-boundary rule scale factor minimum
constexpr Scalar τ_min(0.99);
// Fraction-to-the-boundary rule scale factor τ
Scalar τ = τ_min;
Filter<Scalar> 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 Scalar κ_μ(0.2);
// Barrier parameter superlinear decrease power in "μ^(θ_μ)". Range of (1,
// 2).
constexpr Scalar θ_μ(1.5);
// Update the barrier parameter.
//
// μⱼ₊₁ = max(εₜₒₗ/10, min(κ_μ μⱼ, μⱼ^θ_μ))
//
// See equation (7) of [2].
using std::pow;
μ = std::max(μ_min, std::min(κ_μ * μ, pow(μ, θ_μ)));
// Update the fraction-to-the-boundary rule scaling factor.
//
// τⱼ = max(τₘᵢₙ, 1 − μⱼ)
//
// See equation (8) of [2].
τ = std::max(τ_min, Scalar(1) - μ);
// 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<Scalar>> triplets;
RegularizedLDLT<Scalar> solver{num_decision_variables,
num_equality_constraints};
// Variables for determining when a step is acceptable
constexpr Scalar α_reduction_factor(0.5);
constexpr Scalar α_min(1e-7);
int full_step_rejected_counter = 0;
// Error estimate
Scalar E_0 = std::numeric_limits<Scalar>::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 > Scalar(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.template lpNorm<Eigen::Infinity>() > Scalar(1e10) || !x.allFinite() ||
s.template lpNorm<Eigen::Infinity>() > Scalar(1e10) || !s.allFinite()) {
return ExitStatus::DIVERGING_ITERATES;
}
feasibility_check_profiler.stop();
ScopedProfiler iter_callbacks_profiler{iter_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;
}
}
iter_callbacks_profiler.stop();
ScopedProfiler kkt_matrix_build_profiler{kkt_matrix_build_prof};
// S = diag(s)
// Z = diag(z)
// Σ = S⁻¹Z
const Eigen::SparseMatrix<Scalar> Σ{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<Scalar> top_left =
H + (A_i.transpose() * Σ * A_i).template 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 (typename Eigen::SparseMatrix<Scalar>::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 (typename Eigen::SparseMatrix<Scalar>::InnerIterator it{A_e, col}; it;
++it) {
triplets.emplace_back(H.rows() + it.row(), it.col(), it.value());
}
}
Eigen::SparseMatrix<Scalar> lhs(
num_decision_variables + num_equality_constraints,
num_decision_variables + num_equality_constraints);
lhs.setFromSortedTriplets(triplets.begin(), triplets.end());
// rhs = −[∇f − Aₑᵀy − Aᵢᵀ(−Σcᵢ + μS⁻¹e + z)]
// [ cₑ ]
Eigen::Vector<Scalar, Eigen::Dynamic> 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;
kkt_matrix_build_profiler.stop();
ScopedProfiler kkt_matrix_decomp_profiler{kkt_matrix_decomp_prof};
Step step;
Scalar α_max(1);
Scalar α(1);
Scalar α_z(1);
// 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;
}
kkt_matrix_decomp_profiler.stop();
ScopedProfiler kkt_system_solve_profiler{kkt_system_solve_prof};
auto compute_step = [&](Step& step) {
// p = [ pˣ]
// [−pʸ]
Eigen::Vector<Scalar, Eigen::Dynamic> 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);
kkt_system_solve_profiler.stop();
ScopedProfiler line_search_profiler{line_search_prof};
// αᵐᵃˣ = max(α ∈ (0, 1] : sₖ + αpₖˢ ≥ (1−τⱼ)sₖ)
α_max = fraction_to_the_boundary_rule<Scalar>(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<Scalar>(z, step.p_z, τ);
// Loop until a step is accepted
while (1) {
Eigen::Vector<Scalar, Eigen::Dynamic> trial_x = x + α * step.p_x;
Eigen::Vector<Scalar, Eigen::Dynamic> trial_y = y + α_z * step.p_y;
Eigen::Vector<Scalar, Eigen::Dynamic> trial_z = z + α_z * step.p_z;
Scalar trial_f = matrices.f(trial_x);
Eigen::Vector<Scalar, Eigen::Dynamic> trial_c_e = matrices.c_e(trial_x);
Eigen::Vector<Scalar, Eigen::Dynamic> 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 (!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::Vector<Scalar, Eigen::Dynamic> trial_s;
if (options.feasible_ipm && c_i.cwiseGreater(Scalar(0)).all()) {
// If the inequality constraints are all feasible, prevent them from
// becoming infeasible again.
//
// See equation (19.30) in [1].
trial_s = trial_c_i;
} else {
trial_s = s + α * step.p_s;
}
// Check whether filter accepts trial iterate
if (filter.try_add(FilterEntry{trial_f, trial_s, trial_c_e, trial_c_i, μ},
α)) {
// Accept step
break;
}
Scalar prev_constraint_violation =
c_e.template lpNorm<1>() + (c_i - s).template lpNorm<1>();
Scalar next_constraint_violation =
trial_c_e.template lpNorm<1>() +
(trial_c_i - trial_s).template 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;
Scalar α_soc = α;
Scalar α_z_soc = α_z;
Eigen::Vector<Scalar, Eigen::Dynamic> 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<Scalar>(g, A_e, trial_c_e, trial_y), trial_f,
trial_c_e.template lpNorm<1>() +
(trial_c_i - trial_s).template lpNorm<1>(),
trial_s.dot(trial_z), μ, solver.hessian_regularization(),
α_soc, Scalar(1), α_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<Scalar>(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<Scalar>(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 Scalar κ_soc(0.99);
// If constraint violation hasn't been sufficiently reduced, stop
// making second-order corrections
next_constraint_violation =
trial_c_e.template lpNorm<1>() +
(trial_c_i - trial_s).template 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 / Scalar(10)) {
filter.max_constraint_violation *= Scalar(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) {
Scalar current_kkt_error =
kkt_error<Scalar>(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);
Scalar next_kkt_error = kkt_error<Scalar>(
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 <= Scalar(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 Scalar κ_Σ(1e10);
z[row] =
std::clamp(z[row], Scalar(1) / κ_Σ * μ / 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<Scalar>(g, A_e, c_e, A_i, c_i, s, y, z, Scalar(0));
// Update the barrier parameter if necessary
if (E_0 > Scalar(options.tolerance)) {
// Barrier parameter scale factor for tolerance checks
constexpr Scalar κ_ε(10);
// While the error estimate is below the desired threshold for this
// barrier parameter value, decrease the barrier parameter further
Scalar E_μ = error_estimate<Scalar>(g, A_e, c_e, A_i, c_i, s, y, z, μ);
while (μ > μ_min && E_μ <= κ_ε * μ) {
update_barrier_parameter_and_reset_filter();
E_μ = error_estimate<Scalar>(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.template lpNorm<1>() + (c_i - s).template 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;
}
extern template SLEIPNIR_DLLEXPORT ExitStatus
interior_point(const InteriorPointMatrixCallbacks<double>& matrix_callbacks,
std::span<std::function<bool(const IterationInfo<double>& info)>>
iteration_callbacks,
const Options& options,
#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
const Eigen::ArrayX<bool>& bound_constraint_mask,
#endif
Eigen::VectorXd& x);
Eigen::Vector<double, Eigen::Dynamic>& x);
} // namespace slp

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

@@ -0,0 +1,207 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <functional>
#include <Eigen/Core>
#include <Eigen/SparseCore>
namespace slp {
/**
* Matrix callbacks for the interior-point method solver.
*
* @tparam Scalar Scalar type.
*/
template <typename Scalar>
struct 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<Scalar(const Eigen::Vector<Scalar, Eigen::Dynamic>& 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<Scalar>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& 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<Scalar>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& x,
const Eigen::Vector<Scalar, Eigen::Dynamic>& y,
const Eigen::Vector<Scalar, Eigen::Dynamic>& 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::Vector<Scalar, Eigen::Dynamic>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& 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<Scalar>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& 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::Vector<Scalar, Eigen::Dynamic>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& 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<Scalar>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& x)>
A_i;
};
} // namespace slp

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

@@ -9,25 +9,28 @@ namespace slp {
/**
* Solver iteration information exposed to an iteration callback.
*
* @tparam Scalar Scalar type.
*/
template <typename Scalar>
struct IterationInfo {
/// The solver iteration.
int iteration;
/// The decision variables.
const Eigen::VectorXd& x;
const Eigen::Vector<Scalar, Eigen::Dynamic>& x;
/// The gradient of the cost function.
const Eigen::SparseVector<double>& g;
const Eigen::SparseVector<Scalar>& g;
/// The Hessian of the Lagrangian.
const Eigen::SparseMatrix<double>& H;
const Eigen::SparseMatrix<Scalar>& H;
/// The equality constraint Jacobian.
const Eigen::SparseMatrix<double>& A_e;
const Eigen::SparseMatrix<Scalar>& A_e;
/// The inequality constraint Jacobian.
const Eigen::SparseMatrix<double>& A_i;
const Eigen::SparseMatrix<Scalar>& A_i;
};
} // namespace slp

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

@@ -2,89 +2,35 @@
#pragma once
#include <chrono>
#include <cmath>
#include <functional>
#include <limits>
#include <span>
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <gch/small_vector.hpp>
#include "sleipnir/optimization/solver/exit_status.hpp"
#include "sleipnir/optimization/solver/iteration_info.hpp"
#include "sleipnir/optimization/solver/newton_matrix_callbacks.hpp"
#include "sleipnir/optimization/solver/options.hpp"
#include "sleipnir/optimization/solver/util/error_estimate.hpp"
#include "sleipnir/optimization/solver/util/filter.hpp"
#include "sleipnir/optimization/solver/util/kkt_error.hpp"
#include "sleipnir/optimization/solver/util/regularized_ldlt.hpp"
#include "sleipnir/util/assert.hpp"
#include "sleipnir/util/print_diagnostics.hpp"
#include "sleipnir/util/scope_exit.hpp"
#include "sleipnir/util/scoped_profiler.hpp"
#include "sleipnir/util/solve_profiler.hpp"
#include "sleipnir/util/symbol_exports.hpp"
// See docs/algorithms.md#Works_cited for citation definitions.
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.
@@ -96,6 +42,7 @@ A nonlinear program has the form:
where f(x) is the cost function.
@tparam Scalar Scalar type.
@param[in] matrix_callbacks Matrix callbacks.
@param[in] iteration_callbacks The list of callbacks to call at the beginning of
each iteration.
@@ -104,10 +51,244 @@ where f(x) is the cost function.
variables.
@return The exit status.
*/
SLEIPNIR_DLLEXPORT ExitStatus
newton(const NewtonMatrixCallbacks& matrix_callbacks,
std::span<std::function<bool(const IterationInfo& info)>>
template <typename Scalar>
ExitStatus newton(
const NewtonMatrixCallbacks<Scalar>& matrix_callbacks,
std::span<std::function<bool(const IterationInfo<Scalar>& info)>>
iteration_callbacks,
const Options& options, Eigen::Vector<Scalar, Eigen::Dynamic>& x) {
using std::isfinite;
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(" ↳ iter callbacks");
solve_profilers.emplace_back(" ↳ KKT matrix decomp");
solve_profilers.emplace_back(" ↳ KKT system solve");
solve_profilers.emplace_back(" ↳ line search");
solve_profilers.emplace_back(" ↳ next iter prep");
solve_profilers.emplace_back(" ↳ f(x)");
solve_profilers.emplace_back(" ↳ ∇f(x)");
solve_profilers.emplace_back(" ↳ ∇²ₓₓL");
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& iter_callbacks_prof = solve_profilers[4];
auto& kkt_matrix_decomp_prof = solve_profilers[5];
auto& kkt_system_solve_prof = solve_profilers[6];
auto& line_search_prof = solve_profilers[7];
auto& next_iter_prep_prof = solve_profilers[8];
// Set up profiled matrix callbacks
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
auto& f_prof = solve_profilers[9];
auto& g_prof = solve_profilers[10];
auto& H_prof = solve_profilers[11];
NewtonMatrixCallbacks<Scalar> matrices{
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x) -> Scalar {
ScopedProfiler prof{f_prof};
return matrix_callbacks.f(x);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseVector<Scalar> {
ScopedProfiler prof{g_prof};
return matrix_callbacks.g(x);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseMatrix<Scalar> {
ScopedProfiler prof{H_prof};
return matrix_callbacks.H(x);
}};
#else
const auto& matrices = matrix_callbacks;
#endif
solver_prof.start();
setup_prof.start();
Scalar f = matrices.f(x);
int num_decision_variables = x.rows();
Eigen::SparseVector<Scalar> g = matrices.g(x);
Eigen::SparseMatrix<Scalar> 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 (!isfinite(f)) {
return ExitStatus::NONFINITE_INITIAL_COST_OR_CONSTRAINTS;
}
int iterations = 0;
Filter<Scalar> filter;
RegularizedLDLT<Scalar> solver{num_decision_variables, 0};
// Variables for determining when a step is acceptable
constexpr Scalar α_reduction_factor(0.5);
constexpr Scalar α_min(1e-20);
// Error estimate
Scalar E_0 = std::numeric_limits<Scalar>::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 > Scalar(options.tolerance)) {
ScopedProfiler inner_iter_profiler{inner_iter_prof};
ScopedProfiler feasibility_check_profiler{feasibility_check_prof};
// Check for diverging iterates
if (x.template lpNorm<Eigen::Infinity>() > Scalar(1e10) || !x.allFinite()) {
return ExitStatus::DIVERGING_ITERATES;
}
feasibility_check_profiler.stop();
ScopedProfiler iter_callbacks_profiler{iter_callbacks_prof};
// Call iteration callbacks
for (const auto& callback : iteration_callbacks) {
if (callback({iterations, x, g, H, Eigen::SparseMatrix<Scalar>{},
Eigen::SparseMatrix<Scalar>{}})) {
return ExitStatus::CALLBACK_REQUESTED_STOP;
}
}
iter_callbacks_profiler.stop();
ScopedProfiler kkt_matrix_decomp_profiler{kkt_matrix_decomp_prof};
// Solve the Newton-KKT system
//
// Hpˣ = −∇f
solver.compute(H);
kkt_matrix_decomp_profiler.stop();
ScopedProfiler kkt_system_solve_profiler{kkt_system_solve_prof};
Eigen::Vector<Scalar, Eigen::Dynamic> p_x = solver.solve(-g);
kkt_system_solve_profiler.stop();
ScopedProfiler line_search_profiler{line_search_prof};
constexpr Scalar α_max(1);
Scalar α = α_max;
// Loop until a step is accepted. If a step becomes acceptable, the loop
// will exit early.
while (1) {
Eigen::Vector<Scalar, Eigen::Dynamic> trial_x = x + α * p_x;
Scalar trial_f = matrices.f(trial_x);
// If f(xₖ + αpₖˣ) isn't finite, reduce step size immediately
if (!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) {
Scalar current_kkt_error = kkt_error<Scalar>(g);
Eigen::Vector<Scalar, Eigen::Dynamic> trial_x = x + α_max * p_x;
Scalar next_kkt_error = kkt_error<Scalar>(matrices.g(trial_x));
// If the step using αᵐᵃˣ reduced the KKT error, accept it anyway
if (next_kkt_error <= Scalar(0.999) * current_kkt_error) {
α = α_max;
// 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<Scalar>(g);
next_iter_prep_profiler.stop();
inner_iter_profiler.stop();
if (options.diagnostics) {
print_iteration_diagnostics(iterations, IterationType::NORMAL,
inner_iter_profiler.current_duration(), E_0,
f, Scalar(0), Scalar(0), Scalar(0),
solver.hessian_regularization(), α, α_max,
α_reduction_factor, Scalar(1));
}
++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;
}
extern template SLEIPNIR_DLLEXPORT ExitStatus
newton(const NewtonMatrixCallbacks<double>& matrix_callbacks,
std::span<std::function<bool(const IterationInfo<double>& info)>>
iteration_callbacks,
const Options& options, Eigen::VectorXd& x);
const Options& options, Eigen::Vector<double, Eigen::Dynamic>& x);
} // namespace slp

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

@@ -0,0 +1,89 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <functional>
#include <Eigen/Core>
#include <Eigen/SparseCore>
namespace slp {
/**
* Matrix callbacks for the Newton's method solver.
*
* @tparam Scalar Scalar type.
*/
template <typename Scalar>
struct 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<Scalar(const Eigen::Vector<Scalar, Eigen::Dynamic>& 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<Scalar>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& 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<Scalar>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& x)>
H;
};
} // namespace slp

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

@@ -2,145 +2,36 @@
#pragma once
#include <chrono>
#include <cmath>
#include <functional>
#include <limits>
#include <span>
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <gch/small_vector.hpp>
#include "sleipnir/optimization/solver/exit_status.hpp"
#include "sleipnir/optimization/solver/iteration_info.hpp"
#include "sleipnir/optimization/solver/options.hpp"
#include "sleipnir/optimization/solver/sqp_matrix_callbacks.hpp"
#include "sleipnir/optimization/solver/util/error_estimate.hpp"
#include "sleipnir/optimization/solver/util/filter.hpp"
#include "sleipnir/optimization/solver/util/is_locally_infeasible.hpp"
#include "sleipnir/optimization/solver/util/kkt_error.hpp"
#include "sleipnir/optimization/solver/util/regularized_ldlt.hpp"
#include "sleipnir/util/assert.hpp"
#include "sleipnir/util/print_diagnostics.hpp"
#include "sleipnir/util/scope_exit.hpp"
#include "sleipnir/util/scoped_profiler.hpp"
#include "sleipnir/util/solve_profiler.hpp"
#include "sleipnir/util/symbol_exports.hpp"
// See docs/algorithms.md#Works_cited for citation definitions.
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).
@@ -154,6 +45,7 @@ subject to cₑ(x) = 0
where f(x) is the cost function and cₑ(x) are the equality constraints.
@tparam Scalar Scalar type.
@param[in] matrix_callbacks Matrix callbacks.
@param[in] iteration_callbacks The list of callbacks to call at the beginning of
each iteration.
@@ -162,10 +54,463 @@ where f(x) is the cost function and cₑ(x) are the equality constraints.
variables.
@return The exit status.
*/
SLEIPNIR_DLLEXPORT ExitStatus
sqp(const SQPMatrixCallbacks& matrix_callbacks,
std::span<std::function<bool(const IterationInfo& info)>>
template <typename Scalar>
ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
std::span<std::function<bool(const IterationInfo<Scalar>& info)>>
iteration_callbacks,
const Options& options,
Eigen::Vector<Scalar, Eigen::Dynamic>& x) {
/**
* SQP step direction.
*/
struct Step {
/// Primal step.
Eigen::Vector<Scalar, Eigen::Dynamic> p_x;
/// Dual step.
Eigen::Vector<Scalar, Eigen::Dynamic> p_y;
};
using std::isfinite;
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(" ↳ iter callbacks");
solve_profilers.emplace_back(" ↳ KKT matrix build");
solve_profilers.emplace_back(" ↳ KKT matrix decomp");
solve_profilers.emplace_back(" ↳ KKT system solve");
solve_profilers.emplace_back(" ↳ line search");
solve_profilers.emplace_back(" ↳ SOC");
solve_profilers.emplace_back(" ↳ next iter prep");
solve_profilers.emplace_back(" ↳ 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& iter_callbacks_prof = solve_profilers[4];
auto& kkt_matrix_build_prof = solve_profilers[5];
auto& kkt_matrix_decomp_prof = solve_profilers[6];
auto& kkt_system_solve_prof = solve_profilers[7];
auto& line_search_prof = solve_profilers[8];
auto& soc_prof = solve_profilers[9];
auto& next_iter_prep_prof = solve_profilers[10];
// 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<Scalar> matrices{
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x) -> Scalar {
ScopedProfiler prof{f_prof};
return matrix_callbacks.f(x);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseVector<Scalar> {
ScopedProfiler prof{g_prof};
return matrix_callbacks.g(x);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x,
const Eigen::Vector<Scalar, Eigen::Dynamic>& y)
-> Eigen::SparseMatrix<Scalar> {
ScopedProfiler prof{H_prof};
return matrix_callbacks.H(x, y);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::Vector<Scalar, Eigen::Dynamic> {
ScopedProfiler prof{c_e_prof};
return matrix_callbacks.c_e(x);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseMatrix<Scalar> {
ScopedProfiler prof{A_e_prof};
return matrix_callbacks.A_e(x);
}};
#else
const auto& matrices = matrix_callbacks;
#endif
solver_prof.start();
setup_prof.start();
Scalar f = matrices.f(x);
Eigen::Vector<Scalar, Eigen::Dynamic> 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<Scalar> g = matrices.g(x);
Eigen::SparseMatrix<Scalar> A_e = matrices.A_e(x);
Eigen::Vector<Scalar, Eigen::Dynamic> y =
Eigen::Vector<Scalar, Eigen::Dynamic>::Zero(num_equality_constraints);
Eigen::SparseMatrix<Scalar> 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 (!isfinite(f) || !c_e.allFinite()) {
return ExitStatus::NONFINITE_INITIAL_COST_OR_CONSTRAINTS;
}
int iterations = 0;
Filter<Scalar> filter;
// Kept outside the loop so its storage can be reused
gch::small_vector<Eigen::Triplet<Scalar>> triplets;
RegularizedLDLT<Scalar> solver{num_decision_variables,
num_equality_constraints};
// Variables for determining when a step is acceptable
constexpr Scalar α_reduction_factor(0.5);
constexpr Scalar α_min(1e-7);
int full_step_rejected_counter = 0;
// Error estimate
Scalar E_0 = std::numeric_limits<Scalar>::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 > Scalar(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.template lpNorm<Eigen::Infinity>() > Scalar(1e10) || !x.allFinite()) {
return ExitStatus::DIVERGING_ITERATES;
}
feasibility_check_profiler.stop();
ScopedProfiler iter_callbacks_profiler{iter_callbacks_prof};
// Call iteration callbacks
for (const auto& callback : iteration_callbacks) {
if (callback({iterations, x, g, H, A_e, Eigen::SparseMatrix<Scalar>{}})) {
return ExitStatus::CALLBACK_REQUESTED_STOP;
}
}
iter_callbacks_profiler.stop();
ScopedProfiler kkt_matrix_build_profiler{kkt_matrix_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 (typename Eigen::SparseMatrix<Scalar>::InnerIterator it{H, col}; it;
++it) {
triplets.emplace_back(it.row(), it.col(), it.value());
}
// Append column of Aₑ in bottom-left quadrant
for (typename Eigen::SparseMatrix<Scalar>::InnerIterator it{A_e, col}; it;
++it) {
triplets.emplace_back(H.rows() + it.row(), it.col(), it.value());
}
}
Eigen::SparseMatrix<Scalar> lhs(
num_decision_variables + num_equality_constraints,
num_decision_variables + num_equality_constraints);
lhs.setFromSortedTriplets(triplets.begin(), triplets.end());
// rhs = −[∇f − Aₑᵀy]
// [ cₑ ]
Eigen::Vector<Scalar, Eigen::Dynamic> rhs{x.rows() + y.rows()};
rhs.segment(0, x.rows()) = -g + A_e.transpose() * y;
rhs.segment(x.rows(), y.rows()) = -c_e;
kkt_matrix_build_profiler.stop();
ScopedProfiler kkt_matrix_decomp_profiler{kkt_matrix_decomp_prof};
Step step;
constexpr Scalar α_max(1);
Scalar α(1);
// 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;
}
kkt_matrix_decomp_profiler.stop();
ScopedProfiler kkt_system_solve_profiler{kkt_system_solve_prof};
auto compute_step = [&](Step& step) {
// p = [ pˣ]
// [−pʸ]
Eigen::Vector<Scalar, Eigen::Dynamic> p = solver.solve(rhs);
step.p_x = p.segment(0, x.rows());
step.p_y = -p.segment(x.rows(), y.rows());
};
compute_step(step);
kkt_system_solve_profiler.stop();
ScopedProfiler line_search_profiler{line_search_prof};
α = α_max;
// Loop until a step is accepted
while (1) {
Eigen::Vector<Scalar, Eigen::Dynamic> trial_x = x + α * step.p_x;
Eigen::Vector<Scalar, Eigen::Dynamic> trial_y = y + α * step.p_y;
Scalar trial_f = matrices.f(trial_x);
Eigen::Vector<Scalar, Eigen::Dynamic> trial_c_e = matrices.c_e(trial_x);
// If f(xₖ + αpₖˣ) or cₑ(xₖ + αpₖˣ) aren't finite, reduce step size
// immediately
if (!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;
}
Scalar prev_constraint_violation = c_e.template lpNorm<1>();
Scalar next_constraint_violation = trial_c_e.template 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;
Scalar α_soc = α;
Eigen::Vector<Scalar, Eigen::Dynamic> 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<Scalar>(g, A_e, trial_c_e, trial_y), trial_f,
trial_c_e.template lpNorm<1>(), Scalar(0), Scalar(0),
solver.hessian_regularization(), α_soc, Scalar(1),
α_reduction_factor, Scalar(1));
}
}};
// 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 Scalar κ_soc(0.99);
// If constraint violation hasn't been sufficiently reduced, stop
// making second-order corrections
next_constraint_violation = trial_c_e.template lpNorm<1>();
if (next_constraint_violation > κ_soc * prev_constraint_violation) {
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 / Scalar(10)) {
filter.max_constraint_violation *= Scalar(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) {
Scalar current_kkt_error = kkt_error<Scalar>(g, A_e, c_e, y);
trial_x = x + α_max * step.p_x;
trial_y = y + α_max * step.p_y;
trial_c_e = matrices.c_e(trial_x);
Scalar next_kkt_error = kkt_error<Scalar>(
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 <= Scalar(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<Scalar>(g, A_e, c_e, y);
next_iter_prep_profiler.stop();
inner_iter_profiler.stop();
if (options.diagnostics) {
print_iteration_diagnostics(iterations, IterationType::NORMAL,
inner_iter_profiler.current_duration(), E_0,
f, c_e.template lpNorm<1>(), Scalar(0),
Scalar(0), solver.hessian_regularization(), α,
α_max, α_reduction_factor, α);
}
++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;
}
extern template SLEIPNIR_DLLEXPORT ExitStatus
sqp(const SQPMatrixCallbacks<double>& matrix_callbacks,
std::span<std::function<bool(const IterationInfo<double>& info)>>
iteration_callbacks,
const Options& options, Eigen::VectorXd& x);
const Options& options, Eigen::Vector<double, Eigen::Dynamic>& x);
} // namespace slp

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

@@ -0,0 +1,148 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <functional>
#include <Eigen/Core>
#include <Eigen/SparseCore>
namespace slp {
/**
* Matrix callbacks for the Sequential Quadratic Programming (SQP) solver.
*
* @tparam Scalar Scalar type.
*/
template <typename Scalar>
struct 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<Scalar(const Eigen::Vector<Scalar, Eigen::Dynamic>& 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<Scalar>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& 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<Scalar>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& x,
const Eigen::Vector<Scalar, Eigen::Dynamic>& 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::Vector<Scalar, Eigen::Dynamic>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& 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<Scalar>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& x)>
A_e;
};
} // namespace slp

86
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/bounds.hpp → wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/bounds.hpp vendored
View File

@@ -19,12 +19,18 @@
namespace slp {
/**
* Bound constraint metadata.
*
* @tparam Scalar Scalar type.
*/
template <typename Scalar>
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;
gch::small_vector<std::pair<Scalar, Scalar>> bounds;
/// Whether or not the constraints are feasible (given previously encountered
/// bounds).
@@ -39,6 +45,7 @@ struct Bounds {
* bounds on each decision variable, and whether or not they're feasible (given
* previously encountered bounds),
*
* @tparam Scalar Scalar type.
* @param decision_variables Decision variables corresponding to each column of
* A_i.
* @param inequality_constraints Variables representing the left-hand side of
@@ -48,10 +55,11 @@ struct Bounds {
* 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) {
template <typename Scalar>
Bounds<Scalar> get_bounds(
std::span<Variable<Scalar>> decision_variables,
std::span<Variable<Scalar>> inequality_constraints,
const Eigen::SparseMatrix<Scalar, 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
@@ -79,17 +87,18 @@ inline Bounds get_bounds(
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{
gch::small_vector<std::pair<Scalar, Scalar>> decision_var_indices_to_bounds{
decision_variables.size(),
{-std::numeric_limits<double>::infinity(),
std::numeric_limits<double>::infinity()}};
{-std::numeric_limits<Scalar>::infinity(),
std::numeric_limits<Scalar>::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;
for (typename 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.
@@ -97,7 +106,7 @@ inline Bounds get_bounds(
ExpressionType::LINEAR) {
continue;
}
const Eigen::SparseVector<double>& row_A_i =
const Eigen::SparseVector<Scalar>& row_A_i =
A_i.innerVector(constraint_index);
const auto non_zeros = row_A_i.nonZeros();
slp_assert(non_zeros != 0);
@@ -114,17 +123,17 @@ inline Bounds get_bounds(
// 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);
typename Eigen::SparseVector<Scalar>::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;
Scalar 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);
if (decision_variable_value != Scalar(0)) {
decision_variables[decision_variable_index].set_value(Scalar(0));
constraint_constant = inequality_constraints[constraint_index].value();
decision_variables[decision_variable_index].set_value(
decision_variable_value);
@@ -134,15 +143,17 @@ inline Bounds get_bounds(
// Shouldn't happen since the constraint is supposed to be linear and not a
// constant.
slp_assert(constraint_coefficient != 0.0);
slp_assert(constraint_coefficient != Scalar(0));
using std::isfinite;
// 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));
slp_assert(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));
slp_assert(isfinite(constraint_coefficient));
// Update bounds; we assume constraints of the form c(x) ≥ 0.
auto& [lower_bound, upper_bound] =
@@ -150,10 +161,11 @@ inline Bounds get_bounds(
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) {
if (constraint_coefficient < Scalar(0) && detected_bound < upper_bound) {
upper_bound = detected_bound;
upper_index = constraint_index;
} else if (constraint_coefficient > 0.0 && detected_bound > lower_bound) {
} else if (constraint_coefficient > Scalar(0) &&
detected_bound > lower_bound) {
lower_bound = detected_bound;
lower_index = constraint_index;
}
@@ -186,34 +198,40 @@ inline Bounds get_bounds(
*/
template <typename Derived>
requires(static_cast<bool>(Eigen::DenseBase<Derived>::IsVectorAtCompileTime))
inline void project_onto_bounds(
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);
const typename Eigen::DenseBase<Derived>::Scalar κ_1 =
typename Eigen::DenseBase<Derived>::Scalar(1e-2),
const typename Eigen::DenseBase<Derived>::Scalar κ_2 =
typename Eigen::DenseBase<Derived>::Scalar(1e-2)) {
using Scalar = typename Eigen::DenseBase<Derived>::Scalar;
using std::abs;
using std::isfinite;
slp_assert(κ_1 > Scalar(0) && κ_2 > Scalar(0) && κ_2 < Scalar(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++];
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));
if (isfinite(lower) && isfinite(upper)) {
auto p_L = std::min(κ_1 * std::max(Scalar(1), abs(lower)),
κ_2 * (upper - lower));
auto p_U = std::min(κ_1 * std::max(Scalar(1), 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)));
} else if (isfinite(lower)) {
x_i = std::max(x_i, lower + κ_1 * std::max(Scalar(1), abs(lower)));
} else if (isfinite(upper)) {
x_i = std::min(x_i, upper - κ_1 * std::max(Scalar(1), abs(upper)));
}
}
}

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

@@ -14,20 +14,23 @@ namespace slp {
/**
* Returns the error estimate using the KKT conditions for Newton's method.
*
* @tparam Scalar Scalar type.
* @param g Gradient of the cost function ∇f.
*/
inline double error_estimate(const Eigen::VectorXd& g) {
template <typename Scalar>
Scalar error_estimate(const Eigen::Vector<Scalar, Eigen::Dynamic>& 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>();
return g.template lpNorm<Eigen::Infinity>();
}
/**
* Returns the error estimate using the KKT conditions for SQP.
*
* @tparam Scalar Scalar type.
* @param g Gradient of the cost function ∇f.
* @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
* current iterate.
@@ -35,10 +38,11 @@ inline double error_estimate(const Eigen::VectorXd& g) {
* iterate.
* @param y Equality constraint dual variables.
*/
inline double error_estimate(const Eigen::VectorXd& g,
const Eigen::SparseMatrix<double>& A_e,
const Eigen::VectorXd& c_e,
const Eigen::VectorXd& y) {
template <typename Scalar>
Scalar error_estimate(const Eigen::Vector<Scalar, Eigen::Dynamic>& g,
const Eigen::SparseMatrix<Scalar>& A_e,
const Eigen::Vector<Scalar, Eigen::Dynamic>& c_e,
const Eigen::Vector<Scalar, Eigen::Dynamic>& y) {
// Update the error estimate using the KKT conditions from equations (19.5a)
// through (19.5d) of [1].
//
@@ -52,17 +56,20 @@ inline double error_estimate(const Eigen::VectorXd& g,
// ‖cₑ‖_∞
// s_d = max(sₘₐₓ, ‖y‖₁ / m) / sₘₐₓ
constexpr double s_max = 100.0;
double s_d = std::max(s_max, y.lpNorm<1>() / y.rows()) / s_max;
constexpr Scalar s_max(100);
Scalar s_d =
std::max(s_max, y.template lpNorm<1>() / Scalar(y.rows())) / s_max;
return std::max({(g - A_e.transpose() * y).lpNorm<Eigen::Infinity>() / s_d,
c_e.lpNorm<Eigen::Infinity>()});
return std::max(
{(g - A_e.transpose() * y).template lpNorm<Eigen::Infinity>() / s_d,
c_e.template lpNorm<Eigen::Infinity>()});
}
/**
* Returns the error estimate using the KKT conditions for the interior-point
* method.
*
* @tparam Scalar Scalar type.
* @param g Gradient of the cost function ∇f.
* @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
* current iterate.
@@ -77,13 +84,16 @@ inline double error_estimate(const Eigen::VectorXd& g,
* @param z Inequality constraint dual variables.
* @param μ Barrier parameter.
*/
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 μ) {
template <typename Scalar>
Scalar error_estimate(const Eigen::Vector<Scalar, Eigen::Dynamic>& g,
const Eigen::SparseMatrix<Scalar>& A_e,
const Eigen::Vector<Scalar, Eigen::Dynamic>& c_e,
const Eigen::SparseMatrix<Scalar>& A_i,
const Eigen::Vector<Scalar, Eigen::Dynamic>& c_i,
const Eigen::Vector<Scalar, Eigen::Dynamic>& s,
const Eigen::Vector<Scalar, Eigen::Dynamic>& y,
const Eigen::Vector<Scalar, Eigen::Dynamic>& z,
Scalar μ) {
// Update the error estimate using the KKT conditions from equations (19.5a)
// through (19.5d) of [1].
//
@@ -101,23 +111,26 @@ inline double error_estimate(const Eigen::VectorXd& g,
// ‖cᵢ − s‖_∞
// 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>()) / (y.rows() + z.rows())) /
constexpr Scalar s_max(100);
Scalar s_d =
std::max(s_max, (y.template lpNorm<1>() + z.template lpNorm<1>()) /
Scalar(y.rows() + z.rows())) /
s_max;
// s_c = max(sₘₐₓ, ‖z‖₁ / n) / sₘₐₓ
double s_c = std::max(s_max, z.lpNorm<1>() / z.rows()) / s_max;
Scalar s_c =
std::max(s_max, z.template lpNorm<1>() / Scalar(z.rows())) / s_max;
const auto S = s.asDiagonal();
const Eigen::VectorXd μe = Eigen::VectorXd::Constant(s.rows(), μ);
const Eigen::Vector<Scalar, Eigen::Dynamic> μe =
Eigen::Vector<Scalar, Eigen::Dynamic>::Constant(s.rows(), μ);
return std::max({(g - A_e.transpose() * y - A_i.transpose() * z)
.lpNorm<Eigen::Infinity>() /
.template lpNorm<Eigen::Infinity>() /
s_d,
(S * z - μe).lpNorm<Eigen::Infinity>() / s_c,
c_e.lpNorm<Eigen::Infinity>(),
(c_i - s).lpNorm<Eigen::Infinity>()});
(S * z - μe).template lpNorm<Eigen::Infinity>() / s_c,
c_e.template lpNorm<Eigen::Infinity>(),
(c_i - s).template lpNorm<Eigen::Infinity>()});
}
} // namespace slp

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

@@ -16,13 +16,16 @@ namespace slp {
/**
* Filter entry consisting of cost and constraint violation.
*
* @tparam Scalar Scalar type.
*/
template <typename Scalar>
struct FilterEntry {
/// The cost function's value
double cost = 0.0;
Scalar cost{0};
/// The constraint violation
double constraint_violation = 0.0;
Scalar constraint_violation{0};
constexpr FilterEntry() = default;
@@ -32,7 +35,7 @@ struct FilterEntry {
* @param cost The cost function's value.
* @param constraint_violation The constraint violation.
*/
constexpr FilterEntry(double cost, double constraint_violation)
constexpr FilterEntry(Scalar cost, Scalar constraint_violation)
: cost{cost}, constraint_violation{constraint_violation} {}
/**
@@ -40,7 +43,7 @@ struct FilterEntry {
*
* @param f The cost function value.
*/
explicit FilterEntry(double f) : FilterEntry{f, 0.0} {}
explicit FilterEntry(Scalar f) : FilterEntry{f, Scalar(0)} {}
/**
* Constructs a Sequential Quadratic Programming filter entry.
@@ -48,8 +51,8 @@ struct FilterEntry {
* @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>()} {}
FilterEntry(Scalar f, const Eigen::Vector<Scalar, Eigen::Dynamic>& c_e)
: FilterEntry{f, c_e.template lpNorm<1>()} {}
/**
* Constructs an interior-point method filter entry.
@@ -60,27 +63,33 @@ struct FilterEntry {
* @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(Scalar f, Eigen::Vector<Scalar, Eigen::Dynamic>& s,
const Eigen::Vector<Scalar, Eigen::Dynamic>& c_e,
const Eigen::Vector<Scalar, Eigen::Dynamic>& c_i, Scalar μ)
: FilterEntry{f - μ * s.array().log().sum(),
c_e.lpNorm<1>() + (c_i - s).lpNorm<1>()} {}
c_e.template lpNorm<1>() + (c_i - s).template lpNorm<1>()} {
}
};
/**
* Step filter.
*
* See the section on filters in chapter 15 of [1].
*
* @tparam Scalar Scalar type.
*/
template <typename Scalar>
class Filter {
public:
double max_constraint_violation = 1e4;
/// The maximum constraint violation
Scalar 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(),
m_filter.emplace_back(std::numeric_limits<Scalar>::infinity(),
max_constraint_violation);
}
@@ -91,7 +100,7 @@ class Filter {
m_filter.clear();
// Initial filter entry rejects constraint violations above max
m_filter.emplace_back(std::numeric_limits<double>::infinity(),
m_filter.emplace_back(std::numeric_limits<Scalar>::infinity(),
max_constraint_violation);
}
@@ -100,7 +109,7 @@ class Filter {
*
* @param entry The entry to add to the filter.
*/
void add(const FilterEntry& entry) {
void add(const FilterEntry<Scalar>& entry) {
// Remove dominated entries
erase_if(m_filter, [&](const auto& elem) {
return entry.cost <= elem.cost &&
@@ -115,7 +124,7 @@ class Filter {
*
* @param entry The entry to add to the filter.
*/
void add(FilterEntry&& entry) {
void add(FilterEntry<Scalar>&& entry) {
// Remove dominated entries
erase_if(m_filter, [&](const auto& elem) {
return entry.cost <= elem.cost &&
@@ -130,8 +139,9 @@ class Filter {
*
* @param entry The entry to attempt adding to the filter.
* @param α The step size (0, 1].
* @return True if the given iterate is accepted by the filter.
*/
bool try_add(const FilterEntry& entry, double α) {
bool try_add(const FilterEntry<Scalar>& entry, Scalar α) {
if (is_acceptable(entry, α)) {
add(entry);
return true;
@@ -145,8 +155,9 @@ class Filter {
*
* @param entry The entry to attempt adding to the filter.
* @param α The step size (0, 1].
* @return True if the given iterate is accepted by the filter.
*/
bool try_add(FilterEntry&& entry, double α) {
bool try_add(FilterEntry<Scalar>&& entry, Scalar α) {
if (is_acceptable(entry, α)) {
add(std::move(entry));
return true;
@@ -160,14 +171,17 @@ class Filter {
*
* @param entry The entry to check.
* @param α The step size (0, 1].
* @return True if the given entry is acceptable to the filter.
*/
bool is_acceptable(const FilterEntry& entry, double α) {
if (!std::isfinite(entry.cost) ||
!std::isfinite(entry.constraint_violation)) {
bool is_acceptable(const FilterEntry<Scalar>& entry, Scalar α) {
using std::isfinite;
using std::pow;
if (!isfinite(entry.cost) || !isfinite(entry.constraint_violation)) {
return false;
}
double ϕ = std::pow(α, 1.5);
Scalar ϕ = pow(α, Scalar(1.5));
// If current filter entry is better than all prior ones in some respect,
// accept it.
@@ -176,7 +190,7 @@ class Filter {
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;
(Scalar(1) - ϕ * γ_constraint) * elem.constraint_violation;
});
}
@@ -185,13 +199,13 @@ class Filter {
*
* @return The most recently added filter entry.
*/
const FilterEntry& back() const { return m_filter.back(); }
const FilterEntry<Scalar>& back() const { return m_filter.back(); }
private:
static constexpr double γ_cost = 1e-8;
static constexpr double γ_constraint = 1e-5;
static constexpr Scalar γ_cost{1e-8};
static constexpr Scalar γ_constraint{1e-5};
gch::small_vector<FilterEntry> m_filter;
gch::small_vector<FilterEntry<Scalar>> m_filter;
};
} // namespace slp

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

@@ -12,14 +12,17 @@ namespace slp {
* Applies fraction-to-the-boundary rule to a variable and its iterate, then
* returns a fraction of the iterate step size within (0, 1].
*
* @tparam Scalar Scalar type.
* @param x The variable.
* @param p The iterate on the variable.
* @param τ Fraction-to-the-boundary rule scaling factor within (0, 1].
* @return Fraction of the iterate step size within (0, 1].
*/
inline double fraction_to_the_boundary_rule(
const Eigen::Ref<const Eigen::VectorXd>& x,
const Eigen::Ref<const Eigen::VectorXd>& p, double τ) {
template <typename Scalar>
Scalar fraction_to_the_boundary_rule(
const Eigen::Ref<const Eigen::Vector<Scalar, Eigen::Dynamic>>& x,
const Eigen::Ref<const Eigen::Vector<Scalar, Eigen::Dynamic>>& p,
Scalar τ) {
// α = max(α ∈ (0, 1] : x + αp ≥ (1 − τ)x)
//
// where x and τ are positive.
@@ -32,7 +35,7 @@ inline double fraction_to_the_boundary_rule(
// of α that makes the inequality true.
//
// α = −τ/p x
double α = 1.0;
Scalar α(1);
for (int i = 0; i < x.rows(); ++i) {
if (α * p(i) < -τ * x(i)) {
α = -τ / p(i) * x(i);

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

@@ -12,8 +12,11 @@ namespace slp {
*/
class Inertia {
public:
/// The number of positive eigenvalues.
int positive = 0;
/// The number of negative eigenvalues.
int negative = 0;
/// The number of zero eigenvalues.
int zero = 0;
constexpr Inertia() = default;
@@ -33,13 +36,15 @@ class Inertia {
* Constructs Inertia from the D matrix of an LDLT decomposition
* (see https://en.wikipedia.org/wiki/Sylvester's_law_of_inertia).
*
* @tparam Scalar Scalar type.
* @param D The D matrix of an LDLT decomposition in vector form.
*/
explicit Inertia(const Eigen::VectorXd& D) {
template <typename Scalar>
explicit Inertia(const Eigen::Vector<Scalar, Eigen::Dynamic>& D) {
for (const auto& elem : D) {
if (elem > 0.0) {
if (elem > Scalar(0)) {
++positive;
} else if (elem < 0.0) {
} else if (elem < Scalar(0)) {
++negative;
} else {
++zero;
@@ -47,6 +52,33 @@ class Inertia {
}
}
/**
* Constructs Inertia from the D matrix of an LDLT decomposition
* (see https://en.wikipedia.org/wiki/Sylvester's_law_of_inertia).
*
* @tparam Scalar Scalar type.
* @param D The D matrix of an LDLT decomposition in vector form.
*/
template <typename Scalar>
explicit Inertia(
const Eigen::Diagonal<
const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>>& D) {
for (const auto& elem : D) {
if (elem > Scalar(0)) {
++positive;
} else if (elem < Scalar(0)) {
++negative;
} else {
++zero;
}
}
}
/**
* Inertia equality operator.
*
* @return True if Inertia is equal.
*/
bool operator==(const Inertia&) const = default;
};

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

@@ -12,33 +12,39 @@ namespace slp {
/**
* Returns true if the problem's equality constraints are locally infeasible.
*
* @tparam Scalar Scalar type.
* @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
* current iterate.
* @param c_e The problem's equality constraints cₑ(x) evaluated at the current
* iterate.
*/
inline bool is_equality_locally_infeasible(
const Eigen::SparseMatrix<double>& A_e, const Eigen::VectorXd& c_e) {
template <typename Scalar>
bool is_equality_locally_infeasible(
const Eigen::SparseMatrix<Scalar>& A_e,
const Eigen::Vector<Scalar, Eigen::Dynamic>& c_e) {
// The equality constraints are locally infeasible if
//
// Aₑᵀcₑ → 0
// ‖cₑ‖ > ε
//
// See "Infeasibility detection" in section 6 of [3].
return A_e.rows() > 0 && (A_e.transpose() * c_e).norm() < 1e-6 &&
c_e.norm() > 1e-2;
return A_e.rows() > 0 && (A_e.transpose() * c_e).norm() < Scalar(1e-6) &&
c_e.norm() > Scalar(1e-2);
}
/**
* Returns true if the problem's inequality constraints are locally infeasible.
*
* @tparam Scalar Scalar type.
* @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at
* the current iterate.
* @param c_i The problem's inequality constraints cᵢ(x) evaluated at the
* current iterate.
*/
inline bool is_inequality_locally_infeasible(
const Eigen::SparseMatrix<double>& A_i, const Eigen::VectorXd& c_i) {
template <typename Scalar>
bool is_inequality_locally_infeasible(
const Eigen::SparseMatrix<Scalar>& A_i,
const Eigen::Vector<Scalar, Eigen::Dynamic>& c_i) {
// The inequality constraints are locally infeasible if
//
// Aᵢᵀcᵢ⁺ → 0
@@ -51,8 +57,9 @@ inline bool is_inequality_locally_infeasible(
// cᵢ⁺ is used instead of cᵢ⁻ from the paper to follow the convention that
// feasible inequality constraints are ≥ 0.
if (A_i.rows() > 0) {
Eigen::VectorXd c_i_plus = c_i.cwiseMin(0.0);
if ((A_i.transpose() * c_i_plus).norm() < 1e-6 && c_i_plus.norm() > 1e-6) {
Eigen::Vector<Scalar, Eigen::Dynamic> c_i_plus = c_i.cwiseMin(Scalar(0));
if ((A_i.transpose() * c_i_plus).norm() < Scalar(1e-6) &&
c_i_plus.norm() > Scalar(1e-6)) {
return true;
}
}

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

@@ -12,20 +12,23 @@ namespace slp {
/**
* Returns the KKT error for Newton's method.
*
* @tparam Scalar Scalar type.
* @param g Gradient of the cost function ∇f.
*/
inline double kkt_error(const Eigen::VectorXd& g) {
template <typename Scalar>
Scalar kkt_error(const Eigen::Vector<Scalar, Eigen::Dynamic>& 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>();
return g.template lpNorm<1>();
}
/**
* Returns the KKT error for Sequential Quadratic Programming.
*
* @tparam Scalar Scalar type.
* @param g Gradient of the cost function ∇f.
* @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
* current iterate.
@@ -33,21 +36,25 @@ inline double kkt_error(const Eigen::VectorXd& g) {
* iterate.
* @param y Equality constraint dual variables.
*/
inline double kkt_error(const Eigen::VectorXd& g,
const Eigen::SparseMatrix<double>& A_e,
const Eigen::VectorXd& c_e, const Eigen::VectorXd& y) {
template <typename Scalar>
Scalar kkt_error(const Eigen::Vector<Scalar, Eigen::Dynamic>& g,
const Eigen::SparseMatrix<Scalar>& A_e,
const Eigen::Vector<Scalar, Eigen::Dynamic>& c_e,
const Eigen::Vector<Scalar, Eigen::Dynamic>& y) {
// Compute the KKT error as the 1-norm of the KKT conditions from equations
// (19.5a) through (19.5d) of [1].
//
// ∇f − Aₑᵀy = 0
// cₑ = 0
return (g - A_e.transpose() * y).lpNorm<1>() + c_e.lpNorm<1>();
return (g - A_e.transpose() * y).template lpNorm<1>() +
c_e.template lpNorm<1>();
}
/**
* Returns the KKT error for the interior-point method.
*
* @tparam Scalar Scalar type.
* @param g Gradient of the cost function ∇f.
* @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
* current iterate.
@@ -62,13 +69,15 @@ inline double kkt_error(const Eigen::VectorXd& g,
* @param z Inequality constraint dual variables.
* @param μ Barrier parameter.
*/
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 μ) {
template <typename Scalar>
Scalar kkt_error(const Eigen::Vector<Scalar, Eigen::Dynamic>& g,
const Eigen::SparseMatrix<Scalar>& A_e,
const Eigen::Vector<Scalar, Eigen::Dynamic>& c_e,
const Eigen::SparseMatrix<Scalar>& A_i,
const Eigen::Vector<Scalar, Eigen::Dynamic>& c_i,
const Eigen::Vector<Scalar, Eigen::Dynamic>& s,
const Eigen::Vector<Scalar, Eigen::Dynamic>& y,
const Eigen::Vector<Scalar, Eigen::Dynamic>& z, Scalar μ) {
// Compute the KKT error as the 1-norm of the KKT conditions from equations
// (19.5a) through (19.5d) of [1].
//
@@ -78,10 +87,12 @@ inline double kkt_error(const Eigen::VectorXd& g,
// cᵢ − s = 0
const auto S = s.asDiagonal();
const Eigen::VectorXd μe = Eigen::VectorXd::Constant(s.rows(), μ);
const Eigen::Vector<Scalar, Eigen::Dynamic> μe =
Eigen::Vector<Scalar, Eigen::Dynamic>::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>();
return (g - A_e.transpose() * y - A_i.transpose() * z).template lpNorm<1>() +
(S * z - μe).template lpNorm<1>() + c_e.template lpNorm<1>() +
(c_i - s).template lpNorm<1>();
}
} // namespace slp

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

@@ -7,7 +7,7 @@
#include <Eigen/SparseCholesky>
#include <Eigen/SparseCore>
#include "optimization/inertia.hpp"
#include "sleipnir/optimization/solver/util/inertia.hpp"
// See docs/algorithms.md#Works_cited for citation definitions
@@ -15,7 +15,10 @@ namespace slp {
/**
* Solves systems of linear equations using a regularized LDLT factorization.
*
* @tparam Scalar Scalar type.
*/
template <typename Scalar>
class RegularizedLDLT {
public:
/**
@@ -43,7 +46,7 @@ class RegularizedLDLT {
* @param lhs Left-hand side of the system.
* @return The factorization.
*/
RegularizedLDLT& compute(const Eigen::SparseMatrix<double>& lhs) {
RegularizedLDLT& compute(const Eigen::SparseMatrix<Scalar>& 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.
@@ -61,7 +64,7 @@ class RegularizedLDLT {
// If the inertia is ideal, don't regularize the system
if (inertia == ideal_inertia) {
m_prev_δ = 0.0;
m_prev_δ = Scalar(0);
return *this;
}
}
@@ -70,8 +73,8 @@ class RegularizedLDLT {
// 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;
Scalar δ = m_prev_δ == Scalar(0) ? Scalar(1e-4) : m_prev_δ / Scalar(2);
Scalar γ(1e-10);
while (true) {
// Regularize lhs by adding a multiple of the identity matrix
@@ -98,27 +101,27 @@ class RegularizedLDLT {
} else if (inertia.zero > 0) {
// If there's zero eigenvalues, increase δ and γ by an order of
// magnitude and try again
δ *= 10.0;
γ *= 10.0;
δ *= Scalar(10);
γ *= Scalar(10);
} else if (inertia.negative > ideal_inertia.negative) {
// If there's too many negative eigenvalues, increase δ by an order of
// magnitude and try again
δ *= 10.0;
δ *= Scalar(10);
} else if (inertia.positive > ideal_inertia.positive) {
// If there's too many positive eigenvalues, increase γ by an order of
// magnitude and try again
γ *= 10.0;
γ *= Scalar(10);
}
} else {
// If the decomposition failed, increase δ and γ by an order of
// magnitude and try again
δ *= 10.0;
γ *= 10.0;
δ *= Scalar(10);
γ *= Scalar(10);
}
// If the Hessian perturbation is too high, report failure. This can be
// caused by ill-conditioning.
if (δ > 1e20 || γ > 1e20) {
if (δ > Scalar(1e20) || γ > Scalar(1e20)) {
m_info = Eigen::NumericalIssue;
return *this;
}
@@ -132,7 +135,8 @@ class RegularizedLDLT {
* @return The solution.
*/
template <typename Rhs>
Eigen::VectorXd solve(const Eigen::MatrixBase<Rhs>& rhs) {
Eigen::Vector<Scalar, Eigen::Dynamic> solve(
const Eigen::MatrixBase<Rhs>& rhs) {
if (m_is_sparse) {
return m_sparse_solver.solve(rhs);
} else {
@@ -147,7 +151,8 @@ class RegularizedLDLT {
* @return The solution.
*/
template <typename Rhs>
Eigen::VectorXd solve(const Eigen::SparseMatrixBase<Rhs>& rhs) {
Eigen::Vector<Scalar, Eigen::Dynamic> solve(
const Eigen::SparseMatrixBase<Rhs>& rhs) {
if (m_is_sparse) {
return m_sparse_solver.solve(rhs);
} else {
@@ -160,11 +165,12 @@ class RegularizedLDLT {
*
* @return Hessian regularization factor.
*/
double hessian_regularization() const { return m_prev_δ; }
Scalar hessian_regularization() const { return m_prev_δ; }
private:
using SparseSolver = Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>>;
using DenseSolver = Eigen::LDLT<Eigen::MatrixXd>;
using SparseSolver = Eigen::SimplicialLDLT<Eigen::SparseMatrix<Scalar>>;
using DenseSolver =
Eigen::LDLT<Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>>;
SparseSolver m_sparse_solver;
DenseSolver m_dense_solver;
@@ -183,7 +189,7 @@ class RegularizedLDLT {
0};
/// The value of δ from the previous run of compute().
double m_prev_δ = 0.0;
Scalar m_prev_δ{0};
// Number of non-zeros in LHS.
int m_non_zeros = -1;
@@ -194,7 +200,7 @@ class RegularizedLDLT {
* @param lhs Matrix to factorize.
* @return The factorization.
*/
SparseSolver& compute_sparse(const Eigen::SparseMatrix<double>& lhs) {
SparseSolver& compute_sparse(const Eigen::SparseMatrix<Scalar>& lhs) {
// Reanalize lhs's sparsity pattern if it changed
int non_zeros = lhs.nonZeros();
if (m_non_zeros != non_zeros) {
@@ -214,13 +220,14 @@ class RegularizedLDLT {
* @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};
Eigen::SparseMatrix<Scalar> regularization(Scalar δ, Scalar γ) {
Eigen::Vector<Scalar, Eigen::Dynamic> 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()};
return Eigen::SparseMatrix<Scalar>{vec.asDiagonal()};
}
};

10
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/assert.hpp vendored
View File

@@ -2,11 +2,13 @@
#pragma once
#ifdef JORMUNGANDR
#ifdef SLEIPNIR_PYTHON
#include <source_location>
#include <stdexcept>
#include <fmt/format.h>
/**
* Throw an exception in Python.
*/
@@ -18,11 +20,15 @@
"{}:{}: {}: Assertion `{}' failed.", location.file_name(), \
location.line(), location.function_name(), #condition)); \
} \
} while (0);
} while (0)
#else
#include <cassert>
/**
* Abort in C++.
*/
#define slp_assert(condition) assert(condition)
#endif

40
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/concepts.hpp vendored
View File

@@ -7,36 +7,36 @@
#include <Eigen/Core>
#include "sleipnir/autodiff/sleipnir_base.hpp"
namespace slp {
template <typename T>
concept ScalarLike = requires(std::decay_t<T> t) {
t + 1.0;
t = 1.0;
concept SleipnirType = std::derived_from<std::decay_t<T>, SleipnirBase>;
template <typename T>
concept MatrixLike = requires(std::decay_t<T> t) {
t.rows();
t.cols();
};
template <typename T>
concept SleipnirScalarLike = requires(std::decay_t<T> t) {
t + 1.0;
t = 1.0;
{ t.value() } -> std::same_as<double>;
};
concept ScalarLike =
!MatrixLike<T> && std::constructible_from<std::decay_t<T>, int>;
template <typename T>
concept EigenMatrixLike =
std::derived_from<std::decay_t<T>, Eigen::MatrixBase<std::decay_t<T>>>;
std::derived_from<std::decay_t<T>, Eigen::MatrixBase<std::decay_t<T>>> &&
MatrixLike<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, typename Scalar>
concept SleipnirMatrixLike =
SleipnirType<T> && MatrixLike<T> &&
std::same_as<typename std::decay_t<T>::Scalar, Scalar>;
template <typename T>
concept SleipnirType = SleipnirScalarLike<T> || SleipnirMatrixLike<T>;
template <typename T>
concept MatrixLike = SleipnirMatrixLike<T> || EigenMatrixLike<T>;
template <typename T, typename Scalar>
concept SleipnirScalarLike =
SleipnirType<T> && ScalarLike<T> &&
std::same_as<typename std::decay_t<T>::Scalar, Scalar>;
} // namespace slp

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

@@ -0,0 +1,17 @@
// Copyright (c) Sleipnir contributors
#pragma once
namespace slp::detail {
/**
* Type tag used to designate an uninitialized VariableMatrix.
*/
struct empty_t {};
/**
* Designates an uninitialized VariableMatrix.
*/
static constexpr empty_t empty{};
} // namespace slp::detail

3
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/function_ref.hpp vendored
View File

@@ -38,7 +38,8 @@ class function_ref<R(Args...)> {
template <typename F>
requires(!std::is_same_v<std::decay_t<F>, function_ref> &&
std::is_invocable_r_v<R, F &&, Args...>)
constexpr function_ref(F&& f) noexcept // NOLINT(google-explicit-constructor)
// NOLINTNEXTLINE (google-explicit-constructor)
constexpr function_ref(F&& f) noexcept
: obj_(const_cast<void*>(
reinterpret_cast<const void*>(std::addressof(f)))) {
callback_ = [](void* obj, Args... args) -> R {

17
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/intrusive_shared_ptr.hpp vendored
View File

@@ -39,7 +39,8 @@ class IntrusiveSharedPtr {
/**
* Constructs an empty intrusive shared pointer.
*/
constexpr IntrusiveSharedPtr(std::nullptr_t) noexcept {} // NOLINT
// NOLINTNEXTLINE (google-explicit-constructor)
constexpr IntrusiveSharedPtr(std::nullptr_t) noexcept {}
/**
* Constructs an intrusive shared pointer from the given pointer and takes
@@ -78,8 +79,8 @@ class IntrusiveSharedPtr {
*/
template <typename U>
requires(!std::same_as<T, U> && std::convertible_to<U*, T*>)
constexpr IntrusiveSharedPtr( // NOLINT
const IntrusiveSharedPtr<U>& rhs) noexcept
// NOLINTNEXTLINE (google-explicit-constructor)
constexpr IntrusiveSharedPtr(const IntrusiveSharedPtr<U>& rhs) noexcept
: m_ptr{rhs.m_ptr} {
if (m_ptr != nullptr) {
inc_ref_count(m_ptr);
@@ -92,7 +93,8 @@ class IntrusiveSharedPtr {
* @param rhs The other intrusive shared pointer.
* @return This intrusive shared pointer.
*/
constexpr IntrusiveSharedPtr<T>& operator=( // NOLINT
// NOLINTNEXTLINE (google-explicit-constructor)
constexpr IntrusiveSharedPtr<T>& operator=(
const IntrusiveSharedPtr<T>& rhs) noexcept {
if (m_ptr == rhs.m_ptr) {
return *this;
@@ -119,7 +121,8 @@ class IntrusiveSharedPtr {
*/
template <typename U>
requires(!std::same_as<T, U> && std::convertible_to<U*, T*>)
constexpr IntrusiveSharedPtr<T>& operator=( // NOLINT
// NOLINTNEXTLINE (google-explicit-constructor)
constexpr IntrusiveSharedPtr<T>& operator=(
const IntrusiveSharedPtr<U>& rhs) noexcept {
if (m_ptr == rhs.m_ptr) {
return *this;
@@ -153,8 +156,8 @@ class IntrusiveSharedPtr {
*/
template <typename U>
requires(!std::same_as<T, U> && std::convertible_to<U*, T*>)
constexpr IntrusiveSharedPtr( // NOLINT
IntrusiveSharedPtr<U>&& rhs) noexcept
// NOLINTNEXTLINE (google-explicit-constructor)
constexpr IntrusiveSharedPtr(IntrusiveSharedPtr<U>&& rhs) noexcept
: m_ptr{std::exchange(rhs.m_ptr, nullptr)} {}
/**

12
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/print.hpp vendored
View File

@@ -23,7 +23,7 @@ namespace slp {
* Wrapper around fmt::print() that squelches write failure exceptions.
*/
template <typename... T>
inline void print(fmt::format_string<T...> fmt, T&&... args) {
void print(fmt::format_string<T...> fmt, T&&... args) {
try {
fmt::print(fmt, std::forward<T>(args)...);
} catch (const std::system_error&) {
@@ -34,7 +34,7 @@ inline void print(fmt::format_string<T...> fmt, T&&... args) {
* Wrapper around fmt::print() that squelches write failure exceptions.
*/
template <typename... T>
inline void print(std::FILE* f, fmt::format_string<T...> fmt, T&&... args) {
void print(std::FILE* f, fmt::format_string<T...> fmt, T&&... args) {
try {
fmt::print(f, fmt, std::forward<T>(args)...);
} catch (const std::system_error&) {
@@ -45,7 +45,7 @@ inline void print(std::FILE* f, fmt::format_string<T...> fmt, T&&... args) {
* Wrapper around fmt::println() that squelches write failure exceptions.
*/
template <typename... T>
inline void println(fmt::format_string<T...> fmt, T&&... args) {
void println(fmt::format_string<T...> fmt, T&&... args) {
try {
fmt::println(fmt, std::forward<T>(args)...);
} catch (const std::system_error&) {
@@ -56,7 +56,7 @@ inline void println(fmt::format_string<T...> fmt, T&&... args) {
* Wrapper around fmt::println() that squelches write failure exceptions.
*/
template <typename... T>
inline void println(std::FILE* f, fmt::format_string<T...> fmt, T&&... args) {
void println(std::FILE* f, fmt::format_string<T...> fmt, T&&... args) {
try {
fmt::println(f, fmt, std::forward<T>(args)...);
} catch (const std::system_error&) {
@@ -66,10 +66,10 @@ inline void println(std::FILE* f, fmt::format_string<T...> fmt, T&&... args) {
#else
template <typename... Args>
inline void print([[maybe_unused]] Args&&... args) {}
void print([[maybe_unused]] Args&&... args) {}
template <typename... Args>
inline void println([[maybe_unused]] Args&&... args) {}
void println([[maybe_unused]] Args&&... args) {}
#endif

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

@@ -16,8 +16,8 @@
#include <gch/small_vector.hpp>
#include "sleipnir/util/print.hpp"
#include "util/setup_profiler.hpp"
#include "util/solve_profiler.hpp"
#include "sleipnir/util/setup_profiler.hpp"
#include "sleipnir/util/solve_profiler.hpp"
namespace slp {
@@ -47,13 +47,16 @@ constexpr double to_ms(const std::chrono::duration<Rep, Period>& duration) {
/**
* Renders value as power of 10.
*
* @tparam Scalar Scalar type.
* @param value Value.
*/
inline std::string power_of_10(double value) {
if (value == 0.0) {
template <typename Scalar>
std::string power_of_10(Scalar value) {
if (value == Scalar(0)) {
return " 0";
} else {
int exponent = std::log10(value);
using std::log10;
int exponent = static_cast<int>(log10(value));
if (exponent == 0) {
return " 1";
@@ -89,14 +92,17 @@ inline std::string power_of_10(double value) {
/**
* Prints error for too few degrees of freedom.
*
* @tparam Scalar Scalar type.
* @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) {
template <typename Scalar>
void print_too_few_dofs_error(
const Eigen::Vector<Scalar, Eigen::Dynamic>& 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) {
if (c_e[row] < Scalar(0)) {
slp::println(" {}/{}: {} = 0", row + 1, c_e.rows(), c_e[row]);
}
}
@@ -109,16 +115,19 @@ inline void print_too_few_dofs_error(const Eigen::VectorXd& c_e) {
/**
* Prints equality constraint local infeasibility error.
*
* @tparam Scalar Scalar type.
* @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) {
template <typename Scalar>
void print_c_e_local_infeasibility_error(
const Eigen::Vector<Scalar, Eigen::Dynamic>& 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) {
if (c_e[row] < Scalar(0)) {
slp::println(" {}/{}: {} = 0", row + 1, c_e.rows(), c_e[row]);
}
}
@@ -131,16 +140,19 @@ inline void print_c_e_local_infeasibility_error(const Eigen::VectorXd& c_e) {
/**
* Prints inequality constraint local infeasibility error.
*
* @tparam Scalar Scalar type.
* @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) {
template <typename Scalar>
void print_c_i_local_infeasibility_error(
const Eigen::Vector<Scalar, Eigen::Dynamic>& 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) {
if (c_i[row] < Scalar(0)) {
slp::println(" {}/{}: {} ≥ 0", row + 1, c_i.rows(), c_i[row]);
}
}
@@ -172,6 +184,7 @@ inline void print_bound_constraint_global_infeasibility_error(
/**
* Prints diagnostics for the current iteration.
*
* @tparam Scalar Scalar type.
* @param iterations Number of iterations.
* @param type The iteration's type.
* @param time The iteration duration.
@@ -187,14 +200,14 @@ inline void print_bound_constraint_global_infeasibility_error(
* backtracking.
* @param dual_α The dual step size.
*/
template <typename Rep, typename Period = std::ratio<1>>
template <typename Scalar, 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_α) {
Scalar error, Scalar cost,
Scalar infeasibility, Scalar complementarity,
Scalar μ, Scalar δ, Scalar primal_α,
Scalar primal_α_max, Scalar α_reduction_factor,
Scalar dual_α) {
if (iterations % 20 == 0) {
if (iterations == 0) {
slp::print("┏");
@@ -231,8 +244,9 @@ void print_iteration_diagnostics(int iterations, IterationType type,
// 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));
using std::log;
int backtracks =
static_cast<int>(log(primal_α / primal_α_max) / log(α_reduction_factor));
constexpr std::array ITERATION_TYPES = {"norm", "✓SOC", "XSOC"};
slp::println(
@@ -301,22 +315,22 @@ 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",
slp::println("┏{:━^23}┯{:━^18}┯{:━^10}┯{:━^9}┯{:━^4}┓", "", "", "", "", "");
slp::println("┃{:^23}│{:^18}│{:^10}│{:^9}│{:^4}┃", "solver trace", "percent",
"total (ms)", "each (ms)", "runs");
slp::println("┡{:━^25}┷{:━^18}┷{:━^10}┷{:━^9}┷{:━^4}┩", "", "", "", "", "");
slp::println("┡{:━^23}┷{:━^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}│",
slp::println("│{:<23} {:>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}┘", "");
slp::println("└{:─^68}┘", "");
}
#else
#define print_solver_diagnostics(...)
@@ -333,22 +347,22 @@ inline void print_autodiff_diagnostics(
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",
slp::println("┏{:━^23}┯{:━^18}┯{:━^10}┯{:━^9}┯{:━^4}┓", "", "", "", "", "");
slp::println("┃{:^23}│{:^18}│{:^10}│{:^9}│{:^4}┃", "autodiff trace",
"percent", "total (ms)", "each (ms)", "runs");
slp::println("┡{:━^25}┷{:━^18}┷{:━^10}┷{:━^9}┷{:━^4}┩", "", "", "", "", "");
slp::println("┡{:━^23}┷{:━^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}│",
slp::println("│{:<23} {:>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}┘", "");
slp::println("└{:─^68}┘", "");
}
#else
#define print_autodiff_diagnostics(...)

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

@@ -6,9 +6,20 @@
namespace slp {
/**
* scope_exit is a general-purpose scope guard intended to call its exit
* function when a scope is exited.
*
* @tparam F Function type.
*/
template <typename F>
class scope_exit {
public:
/**
* Constructs a scope_exit.
*
* @param f Function to call on scope exit.
*/
explicit scope_exit(F&& f) noexcept : m_f{std::forward<F>(f)} {}
~scope_exit() {
@@ -17,6 +28,11 @@ class scope_exit {
}
}
/**
* Move constructor.
*
* @param rhs scope_exit from which to move.
*/
scope_exit(scope_exit&& rhs) noexcept
: m_f{std::move(rhs.m_f)}, m_active{rhs.m_active} {
rhs.release();
@@ -25,6 +41,9 @@ class scope_exit {
scope_exit(const scope_exit&) = delete;
scope_exit& operator=(const scope_exit&) = delete;
/**
* Makes the scope_exit inactive.
*/
void release() noexcept { m_active = false; }
private:

4
wpimath/src/main/native/thirdparty/sleipnir/src/util/scoped_profiler.hpp → wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/scoped_profiler.hpp vendored
View File

@@ -5,8 +5,8 @@
#include <concepts>
#include <utility>
#include "util/setup_profiler.hpp"
#include "util/solve_profiler.hpp"
#include "sleipnir/util/setup_profiler.hpp"
#include "sleipnir/util/solve_profiler.hpp"
namespace slp {

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

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

17
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/spy.hpp vendored
View File

@@ -14,8 +14,6 @@
#include <Eigen/SparseCore>
#include <wpi/util/bit.hpp>
#include "sleipnir/util/symbol_exports.hpp"
namespace slp {
/**
@@ -45,10 +43,10 @@ namespace slp {
* zero)</li>
* </ol>
*
* @param[out] file A file stream.
* @param[in] mat The sparse matrix.
* @tparam Scalar Scalar type.
*/
class SLEIPNIR_DLLEXPORT Spy {
template <typename Scalar>
class Spy {
public:
/**
* Constructs a Spy instance.
@@ -86,18 +84,19 @@ class SLEIPNIR_DLLEXPORT Spy {
*
* @param mat The matrix.
*/
void add(const Eigen::SparseMatrix<double>& mat) {
void add(const Eigen::SparseMatrix<Scalar>& 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) {
for (typename Eigen::SparseMatrix<Scalar>::InnerIterator it{mat, k}; it;
++it) {
write32le(it.row());
write32le(it.col());
if (it.value() > 0.0) {
if (it.value() > Scalar(0)) {
m_file << '+';
} else if (it.value() < 0.0) {
} else if (it.value() < Scalar(0)) {
m_file << '-';
} else {
m_file << '0';

5
wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/gradient.cpp vendored Normal file
View File

@@ -0,0 +1,5 @@
// Copyright (c) Sleipnir contributors
#include "sleipnir/autodiff/gradient.hpp"
template class EXPORT_TEMPLATE_DEFINE(SLEIPNIR_DLLEXPORT) slp::Gradient<double>;

6
wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/hessian.cpp vendored Normal file
View File

@@ -0,0 +1,6 @@
// Copyright (c) Sleipnir contributors
#include "sleipnir/autodiff/hessian.hpp"
template class EXPORT_TEMPLATE_DEFINE(SLEIPNIR_DLLEXPORT)
slp::Hessian<double, Eigen::Lower | Eigen::Upper>;

5
wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/jacobian.cpp vendored Normal file
View File

@@ -0,0 +1,5 @@
// Copyright (c) Sleipnir contributors
#include "sleipnir/autodiff/jacobian.hpp"
template class EXPORT_TEMPLATE_DEFINE(SLEIPNIR_DLLEXPORT) slp::Jacobian<double>;

257
wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/variable_matrix.cpp vendored
View File

@@ -2,258 +2,5 @@
#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);
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;
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;
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{VariableMatrix::empty, 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
template slp::VariableMatrix<double> slp::solve(
const slp::VariableMatrix<double>& A, const slp::VariableMatrix<double>& B);

5
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/ocp.cpp vendored Normal file
View File

@@ -0,0 +1,5 @@
// Copyright (c) Sleipnir contributors
#include "sleipnir/optimization/ocp.hpp"
template class EXPORT_TEMPLATE_DEFINE(SLEIPNIR_DLLEXPORT) slp::OCP<double>;

392
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/problem.cpp vendored
View File

@@ -2,394 +2,4 @@
#include "sleipnir/optimization/problem.hpp"
#include <array>
#include <cmath>
#include <memory>
#include <optional>
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <fmt/chrono.h>
#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();
gch::small_vector<std::function<bool(const IterationInfo& info)>> callbacks;
for (const auto& callback : m_iteration_callbacks) {
callbacks.emplace_back(callback);
}
for (const auto& callback : m_persistent_iteration_callbacks) {
callbacks.emplace_back(callback);
}
// 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);
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();
}},
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);
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();
}},
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);
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();
}},
callbacks, options,
#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
bound_constraint_mask,
#endif
x);
}
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() ||
!m_persistent_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);
}
}
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
template class EXPORT_TEMPLATE_DEFINE(SLEIPNIR_DLLEXPORT) slp::Problem<double>;

658
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/interior_point.cpp vendored
View File

@@ -2,662 +2,12 @@
#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)>>
template SLEIPNIR_DLLEXPORT slp::ExitStatus slp::interior_point(
const InteriorPointMatrixCallbacks<double>& matrix_callbacks,
std::span<std::function<bool(const IterationInfo<double>& 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());
// 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
Eigen::Vector<double, Eigen::Dynamic>& x);

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

@@ -2,257 +2,8 @@
#include "sleipnir/optimization/solver/newton.hpp"
#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
template SLEIPNIR_DLLEXPORT slp::ExitStatus slp::newton(
const NewtonMatrixCallbacks<double>& matrix_callbacks,
std::span<std::function<bool(const IterationInfo<double>& info)>>
iteration_callbacks,
const Options& options, Eigen::Vector<double, Eigen::Dynamic>& x);

478
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/sqp.cpp vendored
View File

@@ -2,476 +2,8 @@
#include "sleipnir/optimization/solver/sqp.hpp"
#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());
// 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_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
template SLEIPNIR_DLLEXPORT slp::ExitStatus slp::sqp(
const SQPMatrixCallbacks<double>& matrix_callbacks,
std::span<std::function<bool(const IterationInfo<double>& info)>>
iteration_callbacks,
const slp::Options& options, Eigen::Vector<double, Eigen::Dynamic>& x);

6
wpimath/src/main/native/thirdparty/sleipnir/src/util/pool.cpp vendored
View File

@@ -2,11 +2,7 @@
#include "sleipnir/util/pool.hpp"
namespace slp {
PoolResource& global_pool_resource() {
slp::PoolResource& slp::global_pool_resource() {
thread_local PoolResource pool{16384};
return pool;
}
} // namespace slp

12
wpimath/src/test/native/cpp/optimization/NonlinearProblemTest.cpp
View File

@@ -19,7 +19,7 @@ auto Range(T start, T end, T step) {
TEST(ProblemTest, Quartic) {
testing::internal::CaptureStdout();
slp::Problem problem;
slp::Problem<double> problem;
auto x = problem.decision_variable();
x.set_value(20.0);
@@ -47,7 +47,7 @@ TEST(ProblemTest, RosenbrockWithCubicAndLineConstraint) {
// https://en.wikipedia.org/wiki/Test_functions_for_optimization#Test_functions_for_constrained_optimization
for (auto x0 : Range(-1.5, 1.5, 0.1)) {
for (auto y0 : Range(-0.5, 2.5, 0.1)) {
slp::Problem problem;
slp::Problem<double> problem;
auto x = problem.decision_variable();
x.set_value(x0);
@@ -88,7 +88,7 @@ TEST(ProblemTest, RosenbrockWithDiskConstraint) {
// https://en.wikipedia.org/wiki/Test_functions_for_optimization#Test_functions_for_constrained_optimization
for (auto x0 : Range(-1.5, 1.5, 0.1)) {
for (auto y0 : Range(-1.5, 1.5, 0.1)) {
slp::Problem problem;
slp::Problem<double> problem;
auto x = problem.decision_variable();
x.set_value(x0);
@@ -119,7 +119,7 @@ TEST(ProblemTest, RosenbrockWithDiskConstraint) {
TEST(ProblemTest, Minimum2DDistanceWithLinearConstraint) {
testing::internal::CaptureStdout();
slp::Problem problem;
slp::Problem<double> problem;
auto x = problem.decision_variable();
x.set_value(20.0);
@@ -155,7 +155,7 @@ TEST(ProblemTest, Minimum2DDistanceWithLinearConstraint) {
TEST(ProblemTest, ConflictingBounds) {
testing::internal::CaptureStdout();
slp::Problem problem;
slp::Problem<double> problem;
auto x = problem.decision_variable();
auto y = problem.decision_variable();
@@ -187,7 +187,7 @@ TEST(ProblemTest, WachterAndBieglerLineSearchFailure) {
// [1] Nocedal, J. and Wright, S. "Numerical Optimization", 2nd. ed., Ch. 19.
// Springer, 2006.
slp::Problem problem;
slp::Problem<double> problem;
auto x = problem.decision_variable();
auto s1 = problem.decision_variable();