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.3 (#8463)

Browse Source
This commit is contained in:
Tyler Veness
2025-12-12 19:40:43 -08:00
committed by GitHub
parent d830c41063
commit cca035787c
58 changed files with 3141 additions and 3990 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
upstream_utils/sleipnir.py
View File

@@ -48,7 +48,7 @@ using small_vector = wpi::util::SmallVector<T>;
def main():
name = "sleipnir"
url = "https://github.com/SleipnirGroup/Sleipnir"
tag = "v0.3.2"
tag = "v0.3.3"
sleipnir = Lib(name, url, tag, copy_upstream_src)
sleipnir.main()

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

@@ -1,16 +1,56 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Wed, 29 May 2024 16:29:55 -0700
Subject: [PATCH 1/8] Use fmtlib
Subject: [PATCH 1/9] Use fmtlib
---
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(-)
include/sleipnir/autodiff/expression_type.hpp | 9 ++++---
include/sleipnir/optimization/problem.hpp | 1 +
.../optimization/solver/exit_status.hpp | 9 ++++---
include/sleipnir/util/assert.hpp | 5 ++--
include/sleipnir/util/print.hpp | 27 ++++++++++---------
5 files changed, 28 insertions(+), 23 deletions(-)
diff --git a/include/sleipnir/autodiff/expression_type.hpp b/include/sleipnir/autodiff/expression_type.hpp
index 200324a2c38c58dbe07e96c061658c9e65aacce6..2fde6bdf3cf72f46fb633b72e29e66209e9f6a83 100644
--- a/include/sleipnir/autodiff/expression_type.hpp
+++ b/include/sleipnir/autodiff/expression_type.hpp
@@ -4,9 +4,10 @@
#include <stdint.h>
-#include <format>
#include <utility>
+#include <fmt/base.h>
+
namespace slp {
/// Expression type.
@@ -29,12 +30,12 @@ enum class ExpressionType : uint8_t {
/// Formatter for ExpressionType.
template <>
-struct std::formatter<slp::ExpressionType> {
+struct fmt::formatter<slp::ExpressionType> {
/// Parse format string.
///
/// @param ctx Format parse context.
/// @return Format parse context iterator.
- constexpr auto parse(std::format_parse_context& ctx) {
+ constexpr auto parse(fmt::format_parse_context& ctx) {
return m_underlying.parse(ctx);
}
@@ -65,5 +66,5 @@ struct std::formatter<slp::ExpressionType> {
}
private:
- std::formatter<const char*> m_underlying;
+ fmt::formatter<const char*> m_underlying;
};
diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp
index 3185466605b6604068e2807e461d07d8c856c505..95a33952a5a368c7c81491dbe849a8096357dc38 100644
index 59acc732dbc5bfe520745089abd156da9a8a2e51..ba45ead1500b45616b74e1d108959127484cf4d6 100644
--- a/include/sleipnir/optimization/problem.hpp
+++ b/include/sleipnir/optimization/problem.hpp
@@ -15,6 +15,7 @@
@@ -21,24 +61,60 @@ index 3185466605b6604068e2807e461d07d8c856c505..95a33952a5a368c7c81491dbe849a809
#include <gch/small_vector.hpp>
#include "sleipnir/autodiff/expression_type.hpp"
diff --git a/include/sleipnir/optimization/solver/exit_status.hpp b/include/sleipnir/optimization/solver/exit_status.hpp
index e17f4490cc0f6704f0b5d29340ec6dfbbcbb8aee..c1e4d319c197bdad2ace19500fb3a0426060e87b 100644
--- a/include/sleipnir/optimization/solver/exit_status.hpp
+++ b/include/sleipnir/optimization/solver/exit_status.hpp
@@ -4,9 +4,10 @@
#include <stdint.h>
-#include <format>
#include <utility>
+#include <fmt/base.h>
+
namespace slp {
/// Solver exit status. Negative values indicate failure.
@@ -43,12 +44,12 @@ enum class ExitStatus : int8_t {
/// Formatter for ExitStatus.
template <>
-struct std::formatter<slp::ExitStatus> {
+struct fmt::formatter<slp::ExitStatus> {
/// Parse format string.
///
/// @param ctx Format parse context.
/// @return Format parse context iterator.
- constexpr auto parse(std::format_parse_context& ctx) {
+ constexpr auto parse(fmt::format_parse_context& ctx) {
return m_underlying.parse(ctx);
}
@@ -92,5 +93,5 @@ struct std::formatter<slp::ExitStatus> {
}
private:
- std::formatter<const char*> m_underlying;
+ fmt::formatter<const char*> m_underlying;
};
diff --git a/include/sleipnir/util/assert.hpp b/include/sleipnir/util/assert.hpp
index 0846928c3da7a6047a3c271dd2d377a3b755eeab..5d432608def05b6dee6b7cbdb9a0b91a6ab5e1c2 100644
index dd479f11080d67b03dbe72475be974908e96fc2f..4f0203a4b8df2f86e1e15cf1102a51c340665722 100644
--- a/include/sleipnir/util/assert.hpp
+++ b/include/sleipnir/util/assert.hpp
@@ -4,10 +4,11 @@
@@ -3,16 +3,17 @@
#pragma once
#ifdef SLEIPNIR_PYTHON
-#include <format>
#include <source_location>
#include <stdexcept>
+#include <fmt/format.h>
+
/**
* Throw an exception in Python.
*/
@@ -15,7 +16,7 @@
/// Throw an exception in Python.
#define slp_assert(condition) \
do { \
if (!(condition)) { \
auto location = std::source_location::current(); \
@@ -48,33 +124,26 @@ index 0846928c3da7a6047a3c271dd2d377a3b755eeab..5d432608def05b6dee6b7cbdb9a0b91a
location.line(), location.function_name(), #condition)); \
} \
diff --git a/include/sleipnir/util/print.hpp b/include/sleipnir/util/print.hpp
index 797df849f63d960cf10eaf847415595961868ab0..a89b7d4f9864965443405a8e79cddd8dbfc54ad3 100644
index d436bf297b85cf84fad642fa43aa97063e7d6ebb..b2920580eea0297fc387b5168df78a1515a5dd60 100644
--- a/include/sleipnir/util/print.hpp
+++ b/include/sleipnir/util/print.hpp
@@ -4,10 +4,15 @@
@@ -4,47 +4,48 @@
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
#include <cstdio>
-#include <print>
#include <system_error>
#include <utility>
+#if __has_include(<fmt/base.h>)
+#include <fmt/base.h>
+#else
+#include <fmt/core.h>
+#endif
+
+#include <fmt/base.h>
#endif
namespace slp {
@@ -15,45 +20,45 @@ namespace slp {
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
/**
- * Wrapper around std::print() that squelches write failure exceptions.
+ * Wrapper around fmt::print() that squelches write failure exceptions.
*/
-/// Wrapper around std::print() that squelches write failure exceptions.
+/// Wrapper around fmt::print() that squelches write failure exceptions.
template <typename... T>
-void print(std::format_string<T...> fmt, T&&... args) {
+void print(fmt::format_string<T...> fmt, T&&... args) {
@@ -85,10 +154,8 @@ index 797df849f63d960cf10eaf847415595961868ab0..a89b7d4f9864965443405a8e79cddd8d
}
}
/**
- * Wrapper around std::print() that squelches write failure exceptions.
+ * Wrapper around fmt::print() that squelches write failure exceptions.
*/
-/// Wrapper around std::print() that squelches write failure exceptions.
+/// Wrapper around fmt::print() that squelches write failure exceptions.
template <typename... T>
-void print(std::FILE* f, std::format_string<T...> fmt, T&&... args) {
+void print(std::FILE* f, fmt::format_string<T...> fmt, T&&... args) {
@@ -99,10 +166,8 @@ index 797df849f63d960cf10eaf847415595961868ab0..a89b7d4f9864965443405a8e79cddd8d
}
}
/**
- * Wrapper around std::println() that squelches write failure exceptions.
+ * Wrapper around fmt::println() that squelches write failure exceptions.
*/
-/// Wrapper around std::println() that squelches write failure exceptions.
+/// Wrapper around fmt::println() that squelches write failure exceptions.
template <typename... T>
-void println(std::format_string<T...> fmt, T&&... args) {
+void println(fmt::format_string<T...> fmt, T&&... args) {
@@ -113,10 +178,8 @@ index 797df849f63d960cf10eaf847415595961868ab0..a89b7d4f9864965443405a8e79cddd8d
}
}
/**
- * Wrapper around std::println() that squelches write failure exceptions.
+ * Wrapper around fmt::println() that squelches write failure exceptions.
*/
-/// Wrapper around std::println() that squelches write failure exceptions.
+/// Wrapper around fmt::println() that squelches write failure exceptions.
template <typename... T>
-void println(std::FILE* f, std::format_string<T...> fmt, T&&... args) {
+void println(std::FILE* f, fmt::format_string<T...> fmt, T&&... args) {

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

@@ -1,7 +1,7 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Sun, 16 Jun 2024 12:08:49 -0700
Subject: [PATCH 2/8] Use wpi::SmallVector
Subject: [PATCH 2/9] Use wpi::SmallVector
---
include/sleipnir/autodiff/expression.hpp | 4 ++--
@@ -10,21 +10,21 @@ Subject: [PATCH 2/8] Use wpi::SmallVector
3 files changed, 6 insertions(+), 6 deletions(-)
diff --git a/include/sleipnir/autodiff/expression.hpp b/include/sleipnir/autodiff/expression.hpp
index f5919de6c9c0be044335ce7764ded545215f0486..46814576a3db9f472329b880b94b1ab98d218867 100644
index 36d7e7248a77aaa843f02b262d3865711097a2cb..0e6f31bf77beb6759b8907da9a05e8405c5c5cca 100644
--- a/include/sleipnir/autodiff/expression.hpp
+++ b/include/sleipnir/autodiff/expression.hpp
@@ -33,7 +33,7 @@ struct Expression;
@@ -34,7 +34,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.
@@ -801,7 +801,7 @@ constexpr void inc_ref_count(Expression<Scalar>* expr) {
* @param expr The shared pointer's managed object.
*/
/// Typedef for intrusive shared pointer to Expression.
///
@@ -736,7 +736,7 @@ constexpr void inc_ref_count(Expression<Scalar>* expr) {
/// @tparam Scalar Scalar type.
/// @param expr The shared pointer's managed object.
template <typename Scalar>
-constexpr void dec_ref_count(Expression<Scalar>* expr) {
+void dec_ref_count(Expression<Scalar>* expr) {
@@ -32,45 +32,43 @@ index f5919de6c9c0be044335ce7764ded545215f0486..46814576a3db9f472329b880b94b1ab9
// 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 c78af7224b2ef93ad50b238117583e01940c53ce..0a55b906130d7506c80eb150644ac44c222d1368 100644
index cca246d6c33f4a7e3d0a89f5d9bd21a3f7916aeb..40d18f2a680d6537d698c1c774d6996c65a173d8 100644
--- a/include/sleipnir/autodiff/variable.hpp
+++ b/include/sleipnir/autodiff/variable.hpp
@@ -61,7 +61,7 @@ class Variable : public SleipnirBase {
/**
* Constructs an empty Variable.
*/
@@ -53,7 +53,7 @@ class Variable : public SleipnirBase {
Variable() = default;
/// Constructs an empty Variable.
- explicit constexpr Variable(std::nullptr_t) : expr{nullptr} {}
+ explicit Variable(std::nullptr_t) : expr{nullptr} {}
/**
* Constructs a Variable from a scalar type.
@@ -116,7 +116,7 @@ class Variable : public SleipnirBase {
*
* @param expr The autodiff variable.
*/
/// Constructs a Variable from a scalar type.
///
@@ -96,7 +96,7 @@ class Variable : public SleipnirBase {
/// Constructs a Variable pointing to the specified expression.
///
/// @param expr The autodiff variable.
- explicit constexpr Variable(detail::ExpressionPtr<Scalar>&& expr)
+ explicit Variable(detail::ExpressionPtr<Scalar>&& expr)
: expr{std::move(expr)} {}
/**
/// Assignment operator for scalar.
diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp
index bb66bebc01413a291242886366ce329bb5f4b70a..7ddf02c0e2f66aff8da422b874cbe9772f9fd00d 100644
index 4c5a98311b1ea542877f38db086b51fd0b7c00f1..88122498f384a68b2537b24e57c4a291a951e6dd 100644
--- a/include/sleipnir/autodiff/variable_matrix.hpp
+++ b/include/sleipnir/autodiff/variable_matrix.hpp
@@ -1281,14 +1281,14 @@ class VariableMatrix : public SleipnirBase {
*
* @return Const begin iterator.
*/
@@ -1134,12 +1134,12 @@ class VariableMatrix : public SleipnirBase {
/// Returns const 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 const end iterator.
*
* @return Const end iterator.
*/
/// Returns const 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 reverse begin iterator.
/// Returns reverse begin iterator.
///

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

@@ -1,14 +1,14 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Tue, 28 Jan 2025 22:19:14 -0800
Subject: [PATCH 3/8] Use wpi::byteswap()
Subject: [PATCH 3/9] Use wpi::byteswap()
---
include/sleipnir/util/spy.hpp | 3 ++-
1 file changed, 2 insertions(+), 1 deletion(-)
diff --git a/include/sleipnir/util/spy.hpp b/include/sleipnir/util/spy.hpp
index f9143f2b925064e9df5c763823dcf3d435e7aa28..4b810e54a8038162e03cf08fc8eab52b67b2cdd5 100644
index bf0925e282adffa933d2bc3be8b7441fc4d855d3..49dc97ccea736c51e08b752f863661df54171303 100644
--- a/include/sleipnir/util/spy.hpp
+++ b/include/sleipnir/util/spy.hpp
@@ -12,6 +12,7 @@
@@ -19,8 +19,8 @@ index f9143f2b925064e9df5c763823dcf3d435e7aa28..4b810e54a8038162e03cf08fc8eab52b
namespace slp {
@@ -114,7 +115,7 @@ class Spy {
*/
@@ -106,7 +107,7 @@ class Spy {
/// @param num A 32-bit signed integer.
void write32le(int32_t num) {
if constexpr (std::endian::native != std::endian::little) {
- num = std::byteswap(num);

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

@@ -1,7 +1,7 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Tue, 28 Jan 2025 22:19:31 -0800
Subject: [PATCH 4/8] Replace std::to_underlying()
Subject: [PATCH 4/9] Replace std::to_underlying()
---
include/sleipnir/optimization/problem.hpp | 8 ++++----
@@ -9,10 +9,10 @@ Subject: [PATCH 4/8] Replace std::to_underlying()
2 files changed, 7 insertions(+), 7 deletions(-)
diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp
index 95a33952a5a368c7c81491dbe849a8096357dc38..d20777a5b1912754dda5504313549197e867d34b 100644
index ba45ead1500b45616b74e1d108959127484cf4d6..22e7cdaa945648e2b9effcc30533d6602c839c22 100644
--- a/include/sleipnir/optimization/problem.hpp
+++ b/include/sleipnir/optimization/problem.hpp
@@ -708,11 +708,11 @@ class Problem {
@@ -657,11 +657,11 @@ class Problem {
// Print problem structure
slp::println("\nProblem structure:");
slp::println(" ↳ {} cost function",
@@ -27,7 +27,7 @@ index 95a33952a5a368c7c81491dbe849a8096357dc38..d20777a5b1912754dda5504313549197
if (m_decision_variables.size() == 1) {
slp::print("\n1 decision variable\n");
@@ -724,7 +724,7 @@ class Problem {
@@ -673,7 +673,7 @@ class Problem {
[](const gch::small_vector<Variable<Scalar>>& constraints) {
std::array<size_t, 5> counts{};
for (const auto& constraint : constraints) {
@@ -37,10 +37,10 @@ index 95a33952a5a368c7c81491dbe849a8096357dc38..d20777a5b1912754dda5504313549197
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
index 54277542c46e9490a9ef01c43dc8572e9367c97e..c50d7e171bc90ea47108de90471e7d778885f6af 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,
@@ -238,9 +238,9 @@ void print_iteration_diagnostics(int iterations, IterationType type,
slp::println(
"│{:4} {:4} {:9.3f} {:12e} {:13e} {:12e} {:12e} {:.2e} {:<5} {:.2e} "
"{:.2e} {:2d}│",

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

@@ -1,18 +1,18 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Sat, 8 Feb 2025 13:42:36 -0800
Subject: [PATCH 5/8] Replace std::views::zip()
Subject: [PATCH 5/9] Replace std::views::zip()
---
include/sleipnir/autodiff/adjoint_expression_graph.hpp | 5 ++++-
include/sleipnir/optimization/problem.hpp | 8 +++++---
include/sleipnir/autodiff/gradient_expression_graph.hpp | 5 ++++-
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 16bea7efeeca78d25b34b0b1242ca19cbd05a482..a77323eee9277fc3c77a11ab57ab5003d9ed4543 100644
--- a/include/sleipnir/autodiff/adjoint_expression_graph.hpp
+++ b/include/sleipnir/autodiff/adjoint_expression_graph.hpp
@@ -171,7 +171,10 @@ class AdjointExpressionGraph {
diff --git a/include/sleipnir/autodiff/gradient_expression_graph.hpp b/include/sleipnir/autodiff/gradient_expression_graph.hpp
index 414b98971580a6e3209eabfffd082de1dd5711c7..d21c1b5ad1b2f79ac4ec4e525e9b7fe7789a0461 100644
--- a/include/sleipnir/autodiff/gradient_expression_graph.hpp
+++ b/include/sleipnir/autodiff/gradient_expression_graph.hpp
@@ -161,7 +161,10 @@ class GradientExpressionGraph {
}
}
} else {
@@ -25,10 +25,10 @@ index 16bea7efeeca78d25b34b0b1242ca19cbd05a482..a77323eee9277fc3c77a11ab57ab5003
if (col != -1 && node->adjoint != Scalar(0)) {
triplets.emplace_back(row, col, node->adjoint);
diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp
index d20777a5b1912754dda5504313549197e867d34b..5256d08e5f9d8642049d8bb8323d76c7b3bbbef7 100644
index 22e7cdaa945648e2b9effcc30533d6602c839c22..70955fdc7148e5af737d3094a5602024df790b3d 100644
--- a/include/sleipnir/optimization/problem.hpp
+++ b/include/sleipnir/optimization/problem.hpp
@@ -726,9 +726,11 @@ class Problem {
@@ -675,9 +675,11 @@ class Problem {
for (const auto& constraint : constraints) {
++counts[static_cast<uint8_t>(constraint.type())];
}

86
upstream_utils/sleipnir_patches/0006-Replace-std-unreachable.patch Normal file
View File

@@ -0,0 +1,86 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Wed, 3 Dec 2025 23:38:53 -0800
Subject: [PATCH 6/9] Replace std::unreachable()
---
include/sleipnir/autodiff/expression_type.hpp | 6 +++---
.../sleipnir/optimization/solver/exit_status.hpp | 6 +++---
include/sleipnir/util/unreachable.hpp | 16 ++++++++++++++++
3 files changed, 22 insertions(+), 6 deletions(-)
create mode 100644 include/sleipnir/util/unreachable.hpp
diff --git a/include/sleipnir/autodiff/expression_type.hpp b/include/sleipnir/autodiff/expression_type.hpp
index 2fde6bdf3cf72f46fb633b72e29e66209e9f6a83..8a2ae457808b1ae505c19c8f65a5c68e6e11a082 100644
--- a/include/sleipnir/autodiff/expression_type.hpp
+++ b/include/sleipnir/autodiff/expression_type.hpp
@@ -4,10 +4,10 @@
#include <stdint.h>
-#include <utility>
-
#include <fmt/base.h>
+#include "sleipnir/util/unreachable.hpp"
+
namespace slp {
/// Expression type.
@@ -61,7 +61,7 @@ struct fmt::formatter<slp::ExpressionType> {
case NONLINEAR:
return m_underlying.format("nonlinear", ctx);
default:
- std::unreachable();
+ slp::unreachable();
}
}
diff --git a/include/sleipnir/optimization/solver/exit_status.hpp b/include/sleipnir/optimization/solver/exit_status.hpp
index c1e4d319c197bdad2ace19500fb3a0426060e87b..4d43cea244afd6c8b0235b84e9140359451fbf2f 100644
--- a/include/sleipnir/optimization/solver/exit_status.hpp
+++ b/include/sleipnir/optimization/solver/exit_status.hpp
@@ -4,10 +4,10 @@
#include <stdint.h>
-#include <utility>
-
#include <fmt/base.h>
+#include "sleipnir/util/unreachable.hpp"
+
namespace slp {
/// Solver exit status. Negative values indicate failure.
@@ -88,7 +88,7 @@ struct fmt::formatter<slp::ExitStatus> {
case TIMEOUT:
return m_underlying.format("timeout", ctx);
default:
- std::unreachable();
+ slp::unreachable();
}
}
diff --git a/include/sleipnir/util/unreachable.hpp b/include/sleipnir/util/unreachable.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..2e5ec3e716b36e892476fb3accafacbb1ba1fa7d
--- /dev/null
+++ b/include/sleipnir/util/unreachable.hpp
@@ -0,0 +1,16 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+namespace slp {
+
+[[noreturn]]
+inline void unreachable() {
+#if defined(_MSC_VER) && !defined(__clang__)
+ __assume(false);
+#else
+ __builtin_unreachable();
+#endif
+}
+
+} // namespace slp

12
upstream_utils/sleipnir_patches/0006-Suppress-clang-tidy-false-positives.patch → upstream_utils/sleipnir_patches/0007-Suppress-clang-tidy-false-positives.patch
View File

@@ -1,20 +1,20 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Mon, 10 Feb 2025 11:37:02 -0800
Subject: [PATCH 6/8] Suppress clang-tidy false positives
Subject: [PATCH 7/9] Suppress clang-tidy false positives
---
include/sleipnir/autodiff/variable.hpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp
index 0a55b906130d7506c80eb150644ac44c222d1368..30ec62161df75c6948bbf3d65432c852a0d926c2 100644
index 40d18f2a680d6537d698c1c774d6996c65a173d8..2091f339744f8f8980ccb1e44b0b54b06eb32418 100644
--- a/include/sleipnir/autodiff/variable.hpp
+++ b/include/sleipnir/autodiff/variable.hpp
@@ -862,7 +862,7 @@ struct InequalityConstraints {
* @param inequality_constraints The list of InequalityConstraints to
* concatenate.
*/
@@ -745,7 +745,7 @@ struct InequalityConstraints {
///
/// @param inequality_constraints The list of InequalityConstraints to
/// concatenate.
- InequalityConstraints(
+ InequalityConstraints( // NOLINT
std::initializer_list<InequalityConstraints> inequality_constraints) {

8
upstream_utils/sleipnir_patches/0007-Suppress-GCC-12-warning-false-positive.patch → upstream_utils/sleipnir_patches/0008-Suppress-GCC-12-warning-false-positive.patch
View File

@@ -1,17 +1,17 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Mon, 24 Feb 2025 15:12:03 -0800
Subject: [PATCH 7/8] Suppress GCC 12 warning false positive
Subject: [PATCH 8/9] Suppress GCC 12 warning false positive
---
include/sleipnir/autodiff/variable_matrix.hpp | 7 +++++++
1 file changed, 7 insertions(+)
diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp
index 7ddf02c0e2f66aff8da422b874cbe9772f9fd00d..351030b4041027ba63a2e6ec08f2077b3c35b5db 100644
index 88122498f384a68b2537b24e57c4a291a951e6dd..10b3a4cc5ab22e93fe4e0b44d4664adce9228867 100644
--- a/include/sleipnir/autodiff/variable_matrix.hpp
+++ b/include/sleipnir/autodiff/variable_matrix.hpp
@@ -578,6 +578,10 @@ class VariableMatrix : public SleipnirBase {
@@ -503,6 +503,10 @@ class VariableMatrix : public SleipnirBase {
VariableMatrix<Scalar> result(detail::empty, lhs.rows(), rhs.cols());
@@ -22,7 +22,7 @@ index 7ddf02c0e2f66aff8da422b874cbe9772f9fd00d..351030b4041027ba63a2e6ec08f2077b
for (int i = 0; i < lhs.rows(); ++i) {
for (int j = 0; j < rhs.cols(); ++j) {
Variable sum{Scalar(0)};
@@ -637,6 +641,9 @@ class VariableMatrix : public SleipnirBase {
@@ -558,6 +562,9 @@ class VariableMatrix : public SleipnirBase {
result[i, j] = sum;
}
}

331
upstream_utils/sleipnir_patches/0008-Use-operator-instead-of-multidimensional-array-subsc.patch → upstream_utils/sleipnir_patches/0009-Use-operator-instead-of-multidimensional-array-subsc.patch
View File

@@ -1,25 +1,25 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Sat, 12 Apr 2025 16:28:47 -0700
Subject: [PATCH 8/8] Use operator() instead of multidimensional array
Subject: [PATCH 9/9] Use operator() instead of multidimensional array
subscript operator
---
include/sleipnir/autodiff/hessian.hpp | 4 +-
include/sleipnir/autodiff/jacobian.hpp | 4 +-
include/sleipnir/autodiff/variable.hpp | 8 +-
include/sleipnir/autodiff/variable_block.hpp | 74 ++++----
include/sleipnir/autodiff/variable_block.hpp | 76 ++++-----
include/sleipnir/autodiff/variable_matrix.hpp | 158 +++++++++---------
include/sleipnir/optimization/ocp.hpp | 14 +-
include/sleipnir/optimization/problem.hpp | 6 +-
7 files changed, 134 insertions(+), 134 deletions(-)
7 files changed, 135 insertions(+), 135 deletions(-)
diff --git a/include/sleipnir/autodiff/hessian.hpp b/include/sleipnir/autodiff/hessian.hpp
index 629b6b88274f3d0e6126fd68ccbc219618386518..10ee142ff8f02a9b9f2dc73a6b9c9efad7341ad2 100644
index 5c8d18b41c4025f4492e7216f94299785a513552..b52371b182b755f87f4d038dbeb1795704ab730e 100644
--- a/include/sleipnir/autodiff/hessian.hpp
+++ b/include/sleipnir/autodiff/hessian.hpp
@@ -106,9 +106,9 @@ class Hessian {
auto grad = m_graphs[row].generate_gradient_tree(m_wrt);
@@ -98,9 +98,9 @@ class Hessian {
auto grad = m_graphs[row].generate_tree(m_wrt);
for (int col = 0; col < m_wrt.rows(); ++col) {
if (grad[col].expr != nullptr) {
- result[row, col] = std::move(grad[col]);
@@ -31,11 +31,11 @@ index 629b6b88274f3d0e6126fd68ccbc219618386518..10ee142ff8f02a9b9f2dc73a6b9c9efa
}
}
diff --git a/include/sleipnir/autodiff/jacobian.hpp b/include/sleipnir/autodiff/jacobian.hpp
index b7cedd63d554d6ccfa42c6d8deb62da27950cd53..c8e28a826f619bee201d3383a4dda23f148fa0b1 100644
index bb3a0319880bd240497e1e27ca26f0105b50b803..082a6cf6782f376cbb6ce05809e62e8021feb321 100644
--- a/include/sleipnir/autodiff/jacobian.hpp
+++ b/include/sleipnir/autodiff/jacobian.hpp
@@ -114,9 +114,9 @@ class Jacobian {
auto grad = m_graphs[row].generate_gradient_tree(m_wrt);
@@ -104,9 +104,9 @@ class Jacobian {
auto grad = m_graphs[row].generate_tree(m_wrt);
for (int col = 0; col < m_wrt.rows(); ++col) {
if (grad[col].expr != nullptr) {
- result[row, col] = std::move(grad[col]);
@@ -47,19 +47,19 @@ index b7cedd63d554d6ccfa42c6d8deb62da27950cd53..c8e28a826f619bee201d3383a4dda23f
}
}
diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp
index 30ec62161df75c6948bbf3d65432c852a0d926c2..cb4c1a56ecd16ee2cd27cdd3a866fea3226ce388 100644
index 2091f339744f8f8980ccb1e44b0b54b06eb32418..7616c02b456c755a56f68de86a755d29cd55d91d 100644
--- a/include/sleipnir/autodiff/variable.hpp
+++ b/include/sleipnir/autodiff/variable.hpp
@@ -80,7 +80,7 @@ class Variable : public SleipnirBase {
* @param value The value of the Variable.
*/
@@ -68,7 +68,7 @@ class Variable : public SleipnirBase {
///
/// @param value The value of the Variable.
// NOLINTNEXTLINE (google-explicit-constructor)
- Variable(SleipnirMatrixLike<Scalar> auto value) : expr{value[0, 0].expr} {
+ Variable(SleipnirMatrixLike<Scalar> auto value) : expr{value(0, 0).expr} {
slp_assert(value.rows() == 1 && value.cols() == 1);
}
@@ -740,7 +740,7 @@ auto make_constraints(LHS&& lhs, RHS&& rhs) {
@@ -635,7 +635,7 @@ auto make_constraints(LHS&& lhs, RHS&& rhs) {
for (int row = 0; row < rhs.rows(); ++row) {
for (int col = 0; col < rhs.cols(); ++col) {
// Make right-hand side zero
@@ -68,7 +68,7 @@ index 30ec62161df75c6948bbf3d65432c852a0d926c2..cb4c1a56ecd16ee2cd27cdd3a866fea3
}
}
@@ -756,7 +756,7 @@ auto make_constraints(LHS&& lhs, RHS&& rhs) {
@@ -651,7 +651,7 @@ auto make_constraints(LHS&& lhs, RHS&& rhs) {
for (int row = 0; row < lhs.rows(); ++row) {
for (int col = 0; col < lhs.cols(); ++col) {
// Make right-hand side zero
@@ -77,7 +77,7 @@ index 30ec62161df75c6948bbf3d65432c852a0d926c2..cb4c1a56ecd16ee2cd27cdd3a866fea3
}
}
@@ -774,7 +774,7 @@ auto make_constraints(LHS&& lhs, RHS&& rhs) {
@@ -669,7 +669,7 @@ auto make_constraints(LHS&& lhs, RHS&& rhs) {
for (int row = 0; row < lhs.rows(); ++row) {
for (int col = 0; col < lhs.cols(); ++col) {
// Make right-hand side zero
@@ -87,10 +87,10 @@ index 30ec62161df75c6948bbf3d65432c852a0d926c2..cb4c1a56ecd16ee2cd27cdd3a866fea3
}
diff --git a/include/sleipnir/autodiff/variable_block.hpp b/include/sleipnir/autodiff/variable_block.hpp
index d1b5ac928890dba3052918fc828371dedf26158d..c5351fec9f18f47e2fdfd724699036165c5b8506 100644
index 4018606df45941b578c861caf934495f8c9e368e..cf554832b82adb17b4b1d7b56842a77d7bf629dc 100644
--- a/include/sleipnir/autodiff/variable_block.hpp
+++ b/include/sleipnir/autodiff/variable_block.hpp
@@ -57,7 +57,7 @@ class VariableBlock : public SleipnirBase {
@@ -49,7 +49,7 @@ class VariableBlock : public SleipnirBase {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
@@ -99,7 +99,7 @@ index d1b5ac928890dba3052918fc828371dedf26158d..c5351fec9f18f47e2fdfd72469903616
}
}
}
@@ -92,7 +92,7 @@ class VariableBlock : public SleipnirBase {
@@ -80,7 +80,7 @@ class VariableBlock : public SleipnirBase {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
@@ -108,7 +108,7 @@ index d1b5ac928890dba3052918fc828371dedf26158d..c5351fec9f18f47e2fdfd72469903616
}
}
}
@@ -155,7 +155,7 @@ class VariableBlock : public SleipnirBase {
@@ -135,7 +135,7 @@ class VariableBlock : public SleipnirBase {
VariableBlock<Mat>& operator=(ScalarLike auto value) {
slp_assert(rows() == 1 && cols() == 1);
@@ -117,7 +117,7 @@ index d1b5ac928890dba3052918fc828371dedf26158d..c5351fec9f18f47e2fdfd72469903616
return *this;
}
@@ -170,7 +170,7 @@ class VariableBlock : public SleipnirBase {
@@ -148,7 +148,7 @@ class VariableBlock : public SleipnirBase {
void set_value(Scalar value) {
slp_assert(rows() == 1 && cols() == 1);
@@ -125,8 +125,8 @@ index d1b5ac928890dba3052918fc828371dedf26158d..c5351fec9f18f47e2fdfd72469903616
+ (*this)(0, 0).set_value(value);
}
/**
@@ -185,7 +185,7 @@ class VariableBlock : public SleipnirBase {
/// Assigns an Eigen matrix to the block.
@@ -161,7 +161,7 @@ class VariableBlock : public SleipnirBase {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
@@ -135,7 +135,7 @@ index d1b5ac928890dba3052918fc828371dedf26158d..c5351fec9f18f47e2fdfd72469903616
}
}
@@ -204,7 +204,7 @@ class VariableBlock : public SleipnirBase {
@@ -178,7 +178,7 @@ class VariableBlock : public SleipnirBase {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
@@ -144,7 +144,7 @@ index d1b5ac928890dba3052918fc828371dedf26158d..c5351fec9f18f47e2fdfd72469903616
}
}
}
@@ -220,7 +220,7 @@ class VariableBlock : public SleipnirBase {
@@ -192,7 +192,7 @@ class VariableBlock : public SleipnirBase {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
@@ -153,7 +153,7 @@ index d1b5ac928890dba3052918fc828371dedf26158d..c5351fec9f18f47e2fdfd72469903616
}
}
return *this;
@@ -237,7 +237,7 @@ class VariableBlock : public SleipnirBase {
@@ -207,7 +207,7 @@ class VariableBlock : public SleipnirBase {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
@@ -162,10 +162,10 @@ index d1b5ac928890dba3052918fc828371dedf26158d..c5351fec9f18f47e2fdfd72469903616
}
}
return *this;
@@ -250,13 +250,13 @@ class VariableBlock : public SleipnirBase {
* @param col The scalar subblock's column.
* @return A scalar subblock at the given row and column.
*/
@@ -218,13 +218,13 @@ class VariableBlock : public SleipnirBase {
/// @param row The scalar subblock's row.
/// @param col The scalar subblock's column.
/// @return A scalar subblock at the given row and column.
- Variable<Scalar>& operator[](int row, int col)
+ Variable<Scalar>& operator()(int row, int col)
requires(!std::is_const_v<Mat>)
@@ -178,11 +178,11 @@ index d1b5ac928890dba3052918fc828371dedf26158d..c5351fec9f18f47e2fdfd72469903616
+ m_col_slice.start + col * m_col_slice.step);
}
/**
@@ -266,11 +266,11 @@ class VariableBlock : public SleipnirBase {
* @param col The scalar subblock's column.
* @return A scalar subblock at the given row and column.
*/
/// Returns a scalar subblock at the given row and column.
@@ -232,11 +232,11 @@ class VariableBlock : public SleipnirBase {
/// @param row The scalar subblock's row.
/// @param col The scalar subblock's column.
/// @return A scalar subblock at the given row and column.
- const Variable<Scalar>& operator[](int row, int col) const {
+ const Variable<Scalar>& operator()(int row, int col) const {
slp_assert(row >= 0 && row < rows());
@@ -193,8 +193,8 @@ index d1b5ac928890dba3052918fc828371dedf26158d..c5351fec9f18f47e2fdfd72469903616
+ m_col_slice.start + col * m_col_slice.step);
}
/**
@@ -283,7 +283,7 @@ class VariableBlock : public SleipnirBase {
/// Returns a scalar subblock at the given index.
@@ -247,7 +247,7 @@ class VariableBlock : public SleipnirBase {
requires(!std::is_const_v<Mat>)
{
slp_assert(index >= 0 && index < rows() * cols());
@@ -202,17 +202,17 @@ index d1b5ac928890dba3052918fc828371dedf26158d..c5351fec9f18f47e2fdfd72469903616
+ return (*this)(index / cols(), index % cols());
}
/**
@@ -294,7 +294,7 @@ class VariableBlock : public SleipnirBase {
*/
/// Returns a scalar subblock at the given index.
@@ -256,7 +256,7 @@ class VariableBlock : public SleipnirBase {
/// @return A scalar subblock at the given index.
const Variable<Scalar>& operator[](int index) const {
slp_assert(index >= 0 && index < rows() * cols());
- return (*this)[index / cols(), index % cols()];
+ return (*this)(index / cols(), index % cols());
}
/**
@@ -312,8 +312,8 @@ class VariableBlock : public SleipnirBase {
/// Returns a block of the variable matrix.
@@ -272,8 +272,8 @@ class VariableBlock : public SleipnirBase {
slp_assert(col_offset >= 0 && col_offset <= cols());
slp_assert(block_rows >= 0 && block_rows <= rows() - row_offset);
slp_assert(block_cols >= 0 && block_cols <= cols() - col_offset);
@@ -222,8 +222,8 @@ index d1b5ac928890dba3052918fc828371dedf26158d..c5351fec9f18f47e2fdfd72469903616
+ {col_offset, col_offset + block_cols, 1});
}
/**
@@ -331,8 +331,8 @@ class VariableBlock : public SleipnirBase {
/// Returns a block slice of the variable matrix.
@@ -289,8 +289,8 @@ class VariableBlock : public SleipnirBase {
slp_assert(col_offset >= 0 && col_offset <= cols());
slp_assert(block_rows >= 0 && block_rows <= rows() - row_offset);
slp_assert(block_cols >= 0 && block_cols <= cols() - col_offset);
@@ -233,11 +233,11 @@ index d1b5ac928890dba3052918fc828371dedf26158d..c5351fec9f18f47e2fdfd72469903616
+ {col_offset, col_offset + block_cols, 1});
}
/**
@@ -342,10 +342,10 @@ class VariableBlock : public SleipnirBase {
* @param col_slice The column slice.
* @return A slice of the variable matrix.
*/
/// Returns a slice of the variable matrix.
@@ -298,10 +298,10 @@ class VariableBlock : public SleipnirBase {
/// @param row_slice The row slice.
/// @param col_slice The column slice.
/// @return A slice of the variable matrix.
- VariableBlock<Mat> operator[](Slice row_slice, Slice col_slice) {
+ VariableBlock<Mat> operator()(Slice row_slice, Slice col_slice) {
int row_slice_length = row_slice.adjust(m_row_slice_length);
@@ -246,11 +246,11 @@ index d1b5ac928890dba3052918fc828371dedf26158d..c5351fec9f18f47e2fdfd72469903616
+ return (*this)(row_slice, row_slice_length, col_slice, col_slice_length);
}
/**
@@ -355,11 +355,11 @@ class VariableBlock : public SleipnirBase {
* @param col_slice The column slice.
* @return A slice of the variable matrix.
*/
/// Returns a slice of the variable matrix.
@@ -309,11 +309,11 @@ class VariableBlock : public SleipnirBase {
/// @param row_slice The row slice.
/// @param col_slice The column slice.
/// @return A slice of the variable matrix.
- const VariableBlock<const Mat> operator[](Slice row_slice,
+ const VariableBlock<const Mat> operator()(Slice row_slice,
Slice col_slice) const {
@@ -260,26 +260,26 @@ index d1b5ac928890dba3052918fc828371dedf26158d..c5351fec9f18f47e2fdfd72469903616
+ return (*this)(row_slice, row_slice_length, col_slice, col_slice_length);
}
/**
@@ -374,7 +374,7 @@ class VariableBlock : public SleipnirBase {
* @param col_slice_length The column slice length.
* @return A slice of the variable matrix.
*/
/// Returns a slice of the variable matrix.
@@ -326,7 +326,7 @@ class VariableBlock : public SleipnirBase {
/// @param col_slice The column slice.
/// @param col_slice_length The column slice length.
/// @return A slice of the variable matrix.
- VariableBlock<Mat> operator[](Slice row_slice, int row_slice_length,
+ VariableBlock<Mat> operator()(Slice row_slice, int row_slice_length,
Slice col_slice, int col_slice_length) {
return VariableBlock{
*m_mat,
@@ -400,7 +400,7 @@ class VariableBlock : public SleipnirBase {
* @param col_slice_length The column slice length.
* @return A slice of the variable matrix.
*/
@@ -350,7 +350,7 @@ class VariableBlock : public SleipnirBase {
/// @param col_slice The column slice.
/// @param col_slice_length The column slice length.
/// @return A slice of the variable matrix.
- const VariableBlock<const Mat> operator[](Slice row_slice,
+ const VariableBlock<const Mat> operator()(Slice row_slice,
int row_slice_length,
Slice col_slice,
int col_slice_length) const {
@@ -519,7 +519,7 @@ class VariableBlock : public SleipnirBase {
@@ -453,7 +453,7 @@ class VariableBlock : public SleipnirBase {
VariableBlock<Mat>& operator*=(const ScalarLike auto& rhs) {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
@@ -288,7 +288,7 @@ index d1b5ac928890dba3052918fc828371dedf26158d..c5351fec9f18f47e2fdfd72469903616
}
}
@@ -537,7 +537,7 @@ class VariableBlock : public SleipnirBase {
@@ -469,7 +469,7 @@ class VariableBlock : public SleipnirBase {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
@@ -297,7 +297,7 @@ index d1b5ac928890dba3052918fc828371dedf26158d..c5351fec9f18f47e2fdfd72469903616
}
}
@@ -553,7 +553,7 @@ class VariableBlock : public SleipnirBase {
@@ -483,7 +483,7 @@ class VariableBlock : public SleipnirBase {
VariableBlock<Mat>& operator/=(const ScalarLike auto& rhs) {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
@@ -306,7 +306,7 @@ index d1b5ac928890dba3052918fc828371dedf26158d..c5351fec9f18f47e2fdfd72469903616
}
}
@@ -571,7 +571,7 @@ class VariableBlock : public SleipnirBase {
@@ -499,7 +499,7 @@ class VariableBlock : public SleipnirBase {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
@@ -315,7 +315,7 @@ index d1b5ac928890dba3052918fc828371dedf26158d..c5351fec9f18f47e2fdfd72469903616
}
}
@@ -589,7 +589,7 @@ class VariableBlock : public SleipnirBase {
@@ -515,7 +515,7 @@ class VariableBlock : public SleipnirBase {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
@@ -324,7 +324,7 @@ index d1b5ac928890dba3052918fc828371dedf26158d..c5351fec9f18f47e2fdfd72469903616
}
}
@@ -607,7 +607,7 @@ class VariableBlock : public SleipnirBase {
@@ -531,7 +531,7 @@ class VariableBlock : public SleipnirBase {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
@@ -333,7 +333,7 @@ index d1b5ac928890dba3052918fc828371dedf26158d..c5351fec9f18f47e2fdfd72469903616
}
}
@@ -625,7 +625,7 @@ class VariableBlock : public SleipnirBase {
@@ -547,7 +547,7 @@ class VariableBlock : public SleipnirBase {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
@@ -342,7 +342,16 @@ index d1b5ac928890dba3052918fc828371dedf26158d..c5351fec9f18f47e2fdfd72469903616
}
}
@@ -651,7 +651,7 @@ class VariableBlock : public SleipnirBase {
@@ -558,7 +558,7 @@ class VariableBlock : public SleipnirBase {
// NOLINTNEXTLINE (google-explicit-constructor)
operator Variable<Scalar>() const {
slp_assert(rows() == 1 && cols() == 1);
- return (*this)[0, 0];
+ return (*this)(0, 0);
}
/// Returns the transpose of the variable matrix.
@@ -569,7 +569,7 @@ class VariableBlock : public SleipnirBase {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
@@ -351,16 +360,16 @@ index d1b5ac928890dba3052918fc828371dedf26158d..c5351fec9f18f47e2fdfd72469903616
}
}
@@ -679,7 +679,7 @@ class VariableBlock : public SleipnirBase {
* @param col The column of the element to return.
* @return An element of the variable matrix.
*/
@@ -591,7 +591,7 @@ class VariableBlock : public SleipnirBase {
/// @param row The row of the element to return.
/// @param col The column of the element to return.
/// @return An element of the variable matrix.
- Scalar value(int row, int col) { return (*this)[row, col].value(); }
+ Scalar value(int row, int col) { return (*this)(row, col).value(); }
/**
* Returns an element of the variable block.
@@ -703,7 +703,7 @@ class VariableBlock : public SleipnirBase {
/// Returns an element of the variable block.
///
@@ -611,7 +611,7 @@ class VariableBlock : public SleipnirBase {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
@@ -369,7 +378,7 @@ index d1b5ac928890dba3052918fc828371dedf26158d..c5351fec9f18f47e2fdfd72469903616
}
}
@@ -723,7 +723,7 @@ class VariableBlock : public SleipnirBase {
@@ -629,7 +629,7 @@ class VariableBlock : public SleipnirBase {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
@@ -379,10 +388,10 @@ index d1b5ac928890dba3052918fc828371dedf26158d..c5351fec9f18f47e2fdfd72469903616
}
diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp
index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee867024ecddb2 100644
index 10b3a4cc5ab22e93fe4e0b44d4664adce9228867..d90645910d1db545fb34a726aa801e08739318cc 100644
--- a/include/sleipnir/autodiff/variable_matrix.hpp
+++ b/include/sleipnir/autodiff/variable_matrix.hpp
@@ -174,7 +174,7 @@ class VariableMatrix : public SleipnirBase {
@@ -154,7 +154,7 @@ class VariableMatrix : public SleipnirBase {
m_storage.reserve(values.rows() * values.cols());
for (int row = 0; row < values.rows(); ++row) {
for (int col = 0; col < values.cols(); ++col) {
@@ -391,7 +400,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
}
@@ -232,7 +232,7 @@ class VariableMatrix : public SleipnirBase {
@@ -204,7 +204,7 @@ class VariableMatrix : public SleipnirBase {
m_storage.reserve(rows() * cols());
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
@@ -400,7 +409,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
}
@@ -248,7 +248,7 @@ class VariableMatrix : public SleipnirBase {
@@ -218,7 +218,7 @@ class VariableMatrix : public SleipnirBase {
m_storage.reserve(rows() * cols());
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
@@ -409,7 +418,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
}
@@ -298,7 +298,7 @@ class VariableMatrix : public SleipnirBase {
@@ -262,7 +262,7 @@ class VariableMatrix : public SleipnirBase {
for (int row = 0; row < values.rows(); ++row) {
for (int col = 0; col < values.cols(); ++col) {
@@ -418,7 +427,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
@@ -316,7 +316,7 @@ class VariableMatrix : public SleipnirBase {
@@ -278,7 +278,7 @@ class VariableMatrix : public SleipnirBase {
VariableMatrix& operator=(ScalarLike auto value) {
slp_assert(rows() == 1 && cols() == 1);
@@ -427,7 +436,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
return *this;
}
@@ -333,7 +333,7 @@ class VariableMatrix : public SleipnirBase {
@@ -293,7 +293,7 @@ class VariableMatrix : public SleipnirBase {
for (int row = 0; row < values.rows(); ++row) {
for (int col = 0; col < values.cols(); ++col) {
@@ -436,61 +445,61 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
}
@@ -345,7 +345,7 @@ class VariableMatrix : public SleipnirBase {
* @param col The column.
* @return The element at the given row and column.
*/
@@ -303,7 +303,7 @@ class VariableMatrix : public SleipnirBase {
/// @param row The row.
/// @param col The column.
/// @return The element at the given row and column.
- Variable<Scalar>& operator[](int row, int col) {
+ Variable<Scalar>& operator()(int row, int col) {
slp_assert(row >= 0 && row < rows());
slp_assert(col >= 0 && col < cols());
return m_storage[row * cols() + col];
@@ -358,7 +358,7 @@ class VariableMatrix : public SleipnirBase {
* @param col The column.
* @return The element at the given row and column.
*/
@@ -314,7 +314,7 @@ class VariableMatrix : public SleipnirBase {
/// @param row The row.
/// @param col The column.
/// @return The element at the given row and column.
- const Variable<Scalar>& 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_storage[row * cols() + col];
@@ -431,7 +431,7 @@ class VariableMatrix : public SleipnirBase {
* @param col_slice The column slice.
* @return A slice of the variable matrix.
*/
@@ -377,7 +377,7 @@ class VariableMatrix : public SleipnirBase {
/// @param row_slice The row slice.
/// @param col_slice The column slice.
/// @return A slice of the variable matrix.
- VariableBlock<VariableMatrix> operator[](Slice row_slice, Slice col_slice) {
+ VariableBlock<VariableMatrix> operator()(Slice row_slice, Slice col_slice) {
int row_slice_length = row_slice.adjust(rows());
int col_slice_length = col_slice.adjust(cols());
return VariableBlock{*this, std::move(row_slice), row_slice_length,
@@ -445,7 +445,7 @@ class VariableMatrix : public SleipnirBase {
* @param col_slice The column slice.
* @return A slice of the variable matrix.
*/
@@ -389,7 +389,7 @@ class VariableMatrix : public SleipnirBase {
/// @param row_slice The row slice.
/// @param col_slice The column slice.
/// @return A slice of the variable matrix.
- const VariableBlock<const VariableMatrix> operator[](Slice row_slice,
+ const VariableBlock<const VariableMatrix> operator()(Slice row_slice,
Slice col_slice) const {
int row_slice_length = row_slice.adjust(rows());
int col_slice_length = col_slice.adjust(cols());
@@ -466,7 +466,7 @@ class VariableMatrix : public SleipnirBase {
* @return A slice of the variable matrix.
*
*/
@@ -407,7 +407,7 @@ class VariableMatrix : public SleipnirBase {
/// @param col_slice The column slice.
/// @param col_slice_length The column slice length.
/// @return A slice of the variable matrix.
- VariableBlock<VariableMatrix> operator[](Slice row_slice,
+ VariableBlock<VariableMatrix> operator()(Slice row_slice,
int row_slice_length,
Slice col_slice,
int col_slice_length) {
@@ -486,7 +486,7 @@ class VariableMatrix : public SleipnirBase {
* @param col_slice_length The column slice length.
* @return A slice of the variable matrix.
*/
@@ -425,7 +425,7 @@ class VariableMatrix : public SleipnirBase {
/// @param col_slice The column slice.
/// @param col_slice_length The column slice length.
/// @return A slice of the variable matrix.
- const VariableBlock<const VariableMatrix> operator[](
+ const VariableBlock<const VariableMatrix> operator()(
Slice row_slice, int row_slice_length, Slice col_slice,
int col_slice_length) const {
return VariableBlock{*this, std::move(row_slice), row_slice_length,
@@ -586,9 +586,9 @@ class VariableMatrix : public SleipnirBase {
@@ -511,9 +511,9 @@ class VariableMatrix : public SleipnirBase {
for (int j = 0; j < rhs.cols(); ++j) {
Variable sum{Scalar(0)};
for (int k = 0; k < lhs.cols(); ++k) {
@@ -502,7 +511,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
@@ -611,9 +611,9 @@ class VariableMatrix : public SleipnirBase {
@@ -534,9 +534,9 @@ class VariableMatrix : public SleipnirBase {
for (int j = 0; j < rhs.cols(); ++j) {
Variable sum{Scalar(0)};
for (int k = 0; k < lhs.cols(); ++k) {
@@ -514,7 +523,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
@@ -636,9 +636,9 @@ class VariableMatrix : public SleipnirBase {
@@ -557,9 +557,9 @@ class VariableMatrix : public SleipnirBase {
for (int j = 0; j < rhs.cols(); ++j) {
Variable sum{Scalar(0)};
for (int k = 0; k < lhs.cols(); ++k) {
@@ -526,7 +535,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
#if __GNUC__ >= 12
@@ -661,7 +661,7 @@ class VariableMatrix : public SleipnirBase {
@@ -580,7 +580,7 @@ class VariableMatrix : public SleipnirBase {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
@@ -535,7 +544,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
@@ -680,7 +680,7 @@ class VariableMatrix : public SleipnirBase {
@@ -597,7 +597,7 @@ class VariableMatrix : public SleipnirBase {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
@@ -544,7 +553,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
@@ -700,7 +700,7 @@ class VariableMatrix : public SleipnirBase {
@@ -615,7 +615,7 @@ class VariableMatrix : public SleipnirBase {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
@@ -553,7 +562,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
@@ -719,7 +719,7 @@ class VariableMatrix : public SleipnirBase {
@@ -632,7 +632,7 @@ class VariableMatrix : public SleipnirBase {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
@@ -562,7 +571,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
@@ -739,9 +739,9 @@ class VariableMatrix : public SleipnirBase {
@@ -650,9 +650,9 @@ class VariableMatrix : public SleipnirBase {
for (int j = 0; j < rhs.cols(); ++j) {
Variable sum{Scalar(0)};
for (int k = 0; k < cols(); ++k) {
@@ -574,7 +583,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
@@ -757,7 +757,7 @@ class VariableMatrix : public SleipnirBase {
@@ -666,7 +666,7 @@ class VariableMatrix : public SleipnirBase {
VariableMatrix& operator*=(const ScalarLike auto& rhs) {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < rhs.cols(); ++col) {
@@ -583,7 +592,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
@@ -778,7 +778,7 @@ class VariableMatrix : public SleipnirBase {
@@ -685,7 +685,7 @@ class VariableMatrix : public SleipnirBase {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
@@ -592,7 +601,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
@@ -799,7 +799,7 @@ class VariableMatrix : public SleipnirBase {
@@ -704,7 +704,7 @@ class VariableMatrix : public SleipnirBase {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
@@ -601,7 +610,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
@@ -820,7 +820,7 @@ class VariableMatrix : public SleipnirBase {
@@ -723,7 +723,7 @@ class VariableMatrix : public SleipnirBase {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
@@ -610,7 +619,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
@@ -836,7 +836,7 @@ class VariableMatrix : public SleipnirBase {
@@ -737,7 +737,7 @@ class VariableMatrix : public SleipnirBase {
VariableMatrix& operator/=(const ScalarLike auto& rhs) {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
@@ -619,7 +628,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
@@ -858,7 +858,7 @@ class VariableMatrix : public SleipnirBase {
@@ -757,7 +757,7 @@ class VariableMatrix : public SleipnirBase {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
@@ -628,7 +637,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
@@ -880,7 +880,7 @@ class VariableMatrix : public SleipnirBase {
@@ -777,7 +777,7 @@ class VariableMatrix : public SleipnirBase {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
@@ -637,7 +646,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
@@ -902,7 +902,7 @@ class VariableMatrix : public SleipnirBase {
@@ -797,7 +797,7 @@ class VariableMatrix : public SleipnirBase {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
@@ -646,7 +655,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
@@ -920,7 +920,7 @@ class VariableMatrix : public SleipnirBase {
@@ -813,7 +813,7 @@ class VariableMatrix : public SleipnirBase {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
@@ -655,7 +664,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
@@ -938,7 +938,7 @@ class VariableMatrix : public SleipnirBase {
@@ -829,7 +829,7 @@ class VariableMatrix : public SleipnirBase {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
@@ -664,7 +673,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
@@ -960,7 +960,7 @@ class VariableMatrix : public SleipnirBase {
@@ -849,7 +849,7 @@ class VariableMatrix : public SleipnirBase {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
@@ -673,7 +682,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
@@ -982,7 +982,7 @@ class VariableMatrix : public SleipnirBase {
@@ -869,7 +869,7 @@ class VariableMatrix : public SleipnirBase {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
@@ -682,7 +691,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
@@ -1004,7 +1004,7 @@ class VariableMatrix : public SleipnirBase {
@@ -889,7 +889,7 @@ class VariableMatrix : public SleipnirBase {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
@@ -691,7 +700,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
@@ -1022,7 +1022,7 @@ class VariableMatrix : public SleipnirBase {
@@ -905,7 +905,7 @@ class VariableMatrix : public SleipnirBase {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
@@ -700,7 +709,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
@@ -1040,7 +1040,7 @@ class VariableMatrix : public SleipnirBase {
@@ -921,7 +921,7 @@ class VariableMatrix : public SleipnirBase {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
@@ -709,7 +718,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
@@ -1058,7 +1058,7 @@ class VariableMatrix : public SleipnirBase {
@@ -937,7 +937,7 @@ class VariableMatrix : public SleipnirBase {
for (int row = 0; row < result.rows(); ++row) {
for (int col = 0; col < result.cols(); ++col) {
@@ -718,7 +727,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
@@ -1071,7 +1071,7 @@ class VariableMatrix : public SleipnirBase {
@@ -948,7 +948,7 @@ class VariableMatrix : public SleipnirBase {
// NOLINTNEXTLINE (google-explicit-constructor)
operator Variable<Scalar>() const {
slp_assert(rows() == 1 && cols() == 1);
@@ -726,8 +735,8 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
+ return (*this)(0, 0);
}
/**
@@ -1084,7 +1084,7 @@ class VariableMatrix : public SleipnirBase {
/// Returns the transpose of the variable matrix.
@@ -959,7 +959,7 @@ class VariableMatrix : public SleipnirBase {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
@@ -736,16 +745,16 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
@@ -1112,7 +1112,7 @@ class VariableMatrix : public SleipnirBase {
* @param col The column of the element to return.
* @return An element of the variable matrix.
*/
@@ -981,7 +981,7 @@ class VariableMatrix : public SleipnirBase {
/// @param row The row of the element to return.
/// @param col The column of the element to return.
/// @return An element of the variable matrix.
- Scalar value(int row, int col) { return (*this)[row, col].value(); }
+ Scalar value(int row, int col) { return (*this)(row, col).value(); }
/**
* Returns an element of the variable matrix.
@@ -1133,7 +1133,7 @@ class VariableMatrix : public SleipnirBase {
/// Returns an element of the variable matrix.
///
@@ -998,7 +998,7 @@ class VariableMatrix : public SleipnirBase {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
@@ -754,7 +763,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
@@ -1153,7 +1153,7 @@ class VariableMatrix : public SleipnirBase {
@@ -1016,7 +1016,7 @@ class VariableMatrix : public SleipnirBase {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
@@ -763,7 +772,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
@@ -1422,7 +1422,7 @@ VariableMatrix<Scalar> cwise_reduce(
@@ -1253,7 +1253,7 @@ VariableMatrix<Scalar> cwise_reduce(
for (int row = 0; row < lhs.rows(); ++row) {
for (int col = 0; col < lhs.cols(); ++col) {
@@ -772,7 +781,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
@@ -1561,17 +1561,17 @@ VariableMatrix<Scalar> solve(const VariableMatrix<Scalar>& A,
@@ -1386,17 +1386,17 @@ VariableMatrix<Scalar> solve(const VariableMatrix<Scalar>& A,
if (A.rows() == 1 && A.cols() == 1) {
// Compute optimal inverse instead of using Eigen's general solver
@@ -795,7 +804,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
VariableMatrix adj_A{{d, -b}, {-c, a}};
auto det_A = a * d - b * c;
@@ -1588,15 +1588,15 @@ VariableMatrix<Scalar> solve(const VariableMatrix<Scalar>& A,
@@ -1413,15 +1413,15 @@ VariableMatrix<Scalar> solve(const VariableMatrix<Scalar>& A,
//
// 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
@@ -820,7 +829,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
auto ae = a * e;
auto af = a * f;
@@ -1636,22 +1636,22 @@ VariableMatrix<Scalar> solve(const VariableMatrix<Scalar>& A,
@@ -1461,22 +1461,22 @@ VariableMatrix<Scalar> solve(const VariableMatrix<Scalar>& A,
//
// 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
@@ -859,7 +868,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
auto afk = a * f * k;
auto afl = a * f * l;
@@ -1782,14 +1782,14 @@ VariableMatrix<Scalar> solve(const VariableMatrix<Scalar>& A,
@@ -1607,14 +1607,14 @@ VariableMatrix<Scalar> solve(const VariableMatrix<Scalar>& A,
MatrixXv eigen_A{A.rows(), A.cols()};
for (int row = 0; row < A.rows(); ++row) {
for (int col = 0; col < A.cols(); ++col) {
@@ -876,7 +885,7 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
}
@@ -1798,7 +1798,7 @@ VariableMatrix<Scalar> solve(const VariableMatrix<Scalar>& A,
@@ -1623,7 +1623,7 @@ VariableMatrix<Scalar> solve(const VariableMatrix<Scalar>& A,
VariableMatrix<Scalar> X{detail::empty, A.cols(), B.cols()};
for (int row = 0; row < X.rows(); ++row) {
for (int col = 0; col < X.cols(); ++col) {
@@ -886,10 +895,10 @@ index 351030b4041027ba63a2e6ec08f2077b3c35b5db..55788ce18fcfaa8631ea46b021ee8670
}
diff --git a/include/sleipnir/optimization/ocp.hpp b/include/sleipnir/optimization/ocp.hpp
index 88316894362ff3004627308c81c8f251291eae97..d62432a67af1c75b5cc0bbab54df1d785aec2846 100644
index 091d985234ac8be5c177b9269c7608036f0daddb..e906c194e93a9e26b9cb5c20b720b87828463e80 100644
--- a/include/sleipnir/optimization/ocp.hpp
+++ b/include/sleipnir/optimization/ocp.hpp
@@ -125,7 +125,7 @@ class OCP : public Problem<Scalar> {
@@ -123,7 +123,7 @@ class OCP : public Problem<Scalar> {
if (timestep_method == TimestepMethod::FIXED) {
m_DT = VariableMatrix<Scalar>{1, m_num_steps + 1};
for (int i = 0; i < num_steps + 1; ++i) {
@@ -898,7 +907,7 @@ index 88316894362ff3004627308c81c8f251291eae97..d62432a67af1c75b5cc0bbab54df1d78
}
} else if (timestep_method == TimestepMethod::VARIABLE_SINGLE) {
Variable single_dt = this->decision_variable();
@@ -134,12 +134,12 @@ class OCP : public Problem<Scalar> {
@@ -132,12 +132,12 @@ class OCP : public Problem<Scalar> {
// Set the member variable matrix to track the decision variable
m_DT = VariableMatrix<Scalar>{1, m_num_steps + 1};
for (int i = 0; i < num_steps + 1; ++i) {
@@ -913,7 +922,7 @@ index 88316894362ff3004627308c81c8f251291eae97..d62432a67af1c75b5cc0bbab54df1d78
}
}
@@ -216,7 +216,7 @@ class OCP : public Problem<Scalar> {
@@ -206,7 +206,7 @@ class OCP : public Problem<Scalar> {
for (int i = 0; i < m_num_steps + 1; ++i) {
auto x = X().col(i);
auto u = U().col(i);
@@ -922,7 +931,7 @@ index 88316894362ff3004627308c81c8f251291eae97..d62432a67af1c75b5cc0bbab54df1d78
callback(time, x, u, dt);
time += dt;
@@ -358,7 +358,7 @@ class OCP : public Problem<Scalar> {
@@ -326,7 +326,7 @@ class OCP : public Problem<Scalar> {
// Derivation at https://mec560sbu.github.io/2016/09/30/direct_collocation/
for (int i = 0; i < m_num_steps; ++i) {
@@ -931,7 +940,7 @@ index 88316894362ff3004627308c81c8f251291eae97..d62432a67af1c75b5cc0bbab54df1d78
auto& f = m_dynamics;
@@ -397,7 +397,7 @@ class OCP : public Problem<Scalar> {
@@ -363,7 +363,7 @@ class OCP : public Problem<Scalar> {
auto x_begin = X().col(i);
auto x_end = X().col(i + 1);
auto u = U().col(i);
@@ -940,7 +949,7 @@ index 88316894362ff3004627308c81c8f251291eae97..d62432a67af1c75b5cc0bbab54df1d78
if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) {
this->subject_to(
@@ -422,7 +422,7 @@ class OCP : public Problem<Scalar> {
@@ -386,7 +386,7 @@ class OCP : public Problem<Scalar> {
auto x_begin = X().col(i);
auto x_end = X().col(i + 1);
auto u = U().col(i);
@@ -950,10 +959,10 @@ index 88316894362ff3004627308c81c8f251291eae97..d62432a67af1c75b5cc0bbab54df1d78
if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) {
x_end = rk4<const decltype(m_dynamics)&, VariableMatrix<Scalar>,
diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp
index 5256d08e5f9d8642049d8bb8323d76c7b3bbbef7..a5db8e5902e440afd9f9ee1cc44c60872db2e4c1 100644
index 70955fdc7148e5af737d3094a5602024df790b3d..4243d13c9d04eeee948f8e0e7a21d72b6ef0eaa6 100644
--- a/include/sleipnir/optimization/problem.hpp
+++ b/include/sleipnir/optimization/problem.hpp
@@ -98,7 +98,7 @@ class Problem {
@@ -92,7 +92,7 @@ class Problem {
for (int row = 0; row < rows; ++row) {
for (int col = 0; col < cols; ++col) {
m_decision_variables.emplace_back();
@@ -962,7 +971,7 @@ index 5256d08e5f9d8642049d8bb8323d76c7b3bbbef7..a5db8e5902e440afd9f9ee1cc44c6087
}
}
@@ -133,8 +133,8 @@ class Problem {
@@ -125,8 +125,8 @@ class Problem {
for (int row = 0; row < rows; ++row) {
for (int col = 0; col <= row; ++col) {
m_decision_variables.emplace_back();

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

File diff suppressed because it is too large Load Diff

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

@@ -10,14 +10,12 @@
namespace slp::detail {
/**
* Generate a topological sort of an expression graph from parent to child.
*
* https://en.wikipedia.org/wiki/Topological_sorting
*
* @tparam Scalar Scalar type.
* @param root The root node of the expression.
*/
/// Generate a topological sort of an expression graph from parent to child.
///
/// https://en.wikipedia.org/wiki/Topological_sorting
///
/// @tparam Scalar Scalar type.
/// @param root The root node of the expression.
template <typename Scalar>
gch::small_vector<Expression<Scalar>*> topological_sort(
const ExpressionPtr<Scalar>& root) {
@@ -70,13 +68,11 @@ gch::small_vector<Expression<Scalar>*> topological_sort(
return list;
}
/**
* 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.
*/
/// 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.
template <typename Scalar>
void update_values(const gch::small_vector<Expression<Scalar>*>& list) {
// Traverse graph from child to parent and update values

78
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_type.hpp vendored
View File

@@ -4,17 +4,15 @@
#include <stdint.h>
#include <string_view>
#include <fmt/base.h>
#include "sleipnir/util/symbol_exports.hpp"
#include "sleipnir/util/unreachable.hpp"
namespace slp {
/**
* Expression type.
*
* Used for autodiff caching.
*/
/// Expression type.
///
/// Used for autodiff caching.
enum class ExpressionType : uint8_t {
/// There is no expression.
NONE,
@@ -28,29 +26,45 @@ enum class ExpressionType : uint8_t {
NONLINEAR
};
/**
* Returns user-readable message corresponding to the expression type.
*
* @param type Expression type.
*/
SLEIPNIR_DLLEXPORT constexpr std::string_view to_message(
const ExpressionType& type) {
using enum ExpressionType;
switch (type) {
case NONE:
return "none";
case CONSTANT:
return "constant";
case LINEAR:
return "linear";
case QUADRATIC:
return "quadratic";
case NONLINEAR:
return "nonlinear";
default:
return "unknown";
}
}
} // namespace slp
/// Formatter for ExpressionType.
template <>
struct fmt::formatter<slp::ExpressionType> {
/// Parse format string.
///
/// @param ctx Format parse context.
/// @return Format parse context iterator.
constexpr auto parse(fmt::format_parse_context& ctx) {
return m_underlying.parse(ctx);
}
/// Format ExpressionType.
///
/// @tparam FmtContext Format context type.
/// @param type Expression type.
/// @param ctx Format context.
/// @return Format context iterator.
template <typename FmtContext>
auto format(const slp::ExpressionType& type, FmtContext& ctx) const {
using enum slp::ExpressionType;
switch (type) {
case NONE:
return m_underlying.format("none", ctx);
case CONSTANT:
return m_underlying.format("constant", ctx);
case LINEAR:
return m_underlying.format("linear", ctx);
case QUADRATIC:
return m_underlying.format("quadratic", ctx);
case NONLINEAR:
return m_underlying.format("nonlinear", ctx);
default:
slp::unreachable();
}
}
private:
fmt::formatter<const char*> m_underlying;
};

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

@@ -14,52 +14,42 @@
namespace slp {
/**
* This class calculates the gradient of a variable with respect to a vector of
* variables.
*
* The gradient is only recomputed if the variable expression is quadratic or
* higher order.
*
* @tparam Scalar Scalar type.
*/
/// This class calculates the gradient of a variable with respect to a vector of
/// variables.
///
/// The gradient is only recomputed if the variable expression is quadratic or
/// higher order.
///
/// @tparam Scalar Scalar type.
template <typename Scalar>
class Gradient {
public:
/**
* Constructs a Gradient object.
*
* @param variable Variable of which to compute the gradient.
* @param wrt Variable with respect to which to compute the gradient.
*/
/// Constructs a Gradient object.
///
/// @param variable Variable of which to compute the gradient.
/// @param wrt Variable with respect to which to compute the gradient.
Gradient(Variable<Scalar> variable, Variable<Scalar> wrt)
: m_jacobian{std::move(variable), std::move(wrt)} {}
/**
* Constructs a Gradient object.
*
* @param variable Variable of which to compute the gradient.
* @param wrt Vector of variables with respect to which to compute the
* gradient.
*/
/// Constructs a Gradient object.
///
/// @param variable Variable of which to compute the gradient.
/// @param wrt Vector of variables with respect to which to compute the
/// gradient.
Gradient(Variable<Scalar> variable, SleipnirMatrixLike<Scalar> auto wrt)
: m_jacobian{VariableMatrix{std::move(variable)}, std::move(wrt)} {}
/**
* Returns the gradient as a VariableMatrix.
*
* This is useful when constructing optimization problems with derivatives in
* them.
*
* @return The gradient as a VariableMatrix.
*/
/// Returns the gradient as a VariableMatrix.
///
/// This is useful when constructing optimization problems with derivatives in
/// them.
///
/// @return The gradient as a VariableMatrix.
VariableMatrix<Scalar> get() const { return m_jacobian.get().T(); }
/**
* Evaluates the gradient at wrt's value.
*
* @return The gradient at wrt's value.
*/
/// Evaluates the gradient at wrt's value.
///
/// @return The gradient at wrt's value.
const Eigen::SparseVector<Scalar>& value() {
m_g = m_jacobian.value().transpose();

72
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/adjoint_expression_graph.hpp → wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient_expression_graph.hpp vendored
View File

@@ -16,45 +16,38 @@
namespace slp::detail {
/**
* This class is an adaptor type that performs value updates of an expression's
* adjoint graph.
*
* @tparam Scalar Scalar type.
*/
/// This class is an adapter type that performs value updates of an expression
/// graph, generates a gradient tree, or appends gradient triplets for creating
/// a sparse matrix of gradients.
///
/// @tparam Scalar Scalar type.
template <typename Scalar>
class AdjointExpressionGraph {
class GradientExpressionGraph {
public:
/**
* Generates the adjoint graph for the given expression.
*
* @param root The root node of the expression.
*/
explicit AdjointExpressionGraph(const Variable<Scalar>& root)
/// Generates the gradient graph for the given expression.
///
/// @param root The root node of the expression.
explicit GradientExpressionGraph(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);
}
}
/**
* Update the values of all nodes in this adjoint graph based on the values of
* their dependent nodes.
*/
/// Update the values of all nodes in this graph based on the values of their
/// dependent nodes.
void update_values() { detail::update_values(m_top_list); }
/**
* Returns the variable's gradient tree.
*
* This function lazily allocates variables, so elements of the returned
* VariableMatrix will be empty if the corresponding element of wrt had no
* adjoint. Ensure Variable::expr isn't nullptr before calling member
* functions.
*
* @param wrt Variables with respect to which to compute the gradient.
* @return The variable's gradient tree.
*/
VariableMatrix<Scalar> generate_gradient_tree(
/// Returns the variable's gradient tree.
///
/// This function lazily allocates variables, so elements of the returned
/// VariableMatrix will be empty if the corresponding element of wrt had no
/// adjoint. Ensure Variable::expr isn't nullptr before calling member
/// functions.
///
/// @param wrt Variables with respect to which to compute the gradient.
/// @return The variable's gradient tree.
VariableMatrix<Scalar> generate_tree(
const VariableMatrix<Scalar>& wrt) const {
slp_assert(wrt.cols() == 1);
@@ -104,18 +97,15 @@ class AdjointExpressionGraph {
return grad;
}
/**
* Updates the adjoints in the expression graph (computes the gradient) then
* appends the adjoints of wrt to the sparse matrix triplets.
*
* @param triplets The sparse matrix triplets.
* @param row The row of wrt.
* @param wrt Vector of variables with respect to which to compute the
* Jacobian.
*/
void append_gradient_triplets(
gch::small_vector<Eigen::Triplet<Scalar>>& triplets, int row,
const VariableMatrix<Scalar>& wrt) const {
/// Updates the adjoints in the expression graph (computes the gradient) then
/// appends the adjoints of wrt to the sparse matrix triplets.
///
/// @param triplets The sparse matrix triplets.
/// @param row The row of wrt.
/// @param wrt Vector of variables with respect to which to compute the
/// Jacobian.
void append_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

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

@@ -7,7 +7,7 @@
#include <Eigen/SparseCore>
#include <gch/small_vector.hpp>
#include "sleipnir/autodiff/adjoint_expression_graph.hpp"
#include "sleipnir/autodiff/gradient_expression_graph.hpp"
#include "sleipnir/autodiff/variable.hpp"
#include "sleipnir/autodiff/variable_matrix.hpp"
#include "sleipnir/util/assert.hpp"
@@ -15,39 +15,33 @@
namespace slp {
/**
* This class calculates the Hessian of a variable with respect to a vector of
* variables.
*
* The gradient tree is cached so subsequent Hessian calculations are faster,
* and the Hessian is only recomputed if the variable expression is nonlinear.
*
* @tparam Scalar Scalar type.
* @tparam UpLo Which part of the Hessian to compute (Lower or Lower | Upper).
*/
/// This class calculates the Hessian of a variable with respect to a vector of
/// variables.
///
/// The gradient tree is cached so subsequent Hessian calculations are faster,
/// and the Hessian is only recomputed if the variable expression is nonlinear.
///
/// @tparam Scalar Scalar type.
/// @tparam UpLo Which part of the Hessian to compute (Lower or Lower | Upper).
template <typename Scalar, int UpLo>
requires(UpLo == Eigen::Lower) || (UpLo == (Eigen::Lower | Eigen::Upper))
class Hessian {
public:
/**
* Constructs a Hessian object.
*
* @param variable Variable of which to compute the Hessian.
* @param wrt Variable with respect to which to compute the Hessian.
*/
/// Constructs a Hessian object.
///
/// @param variable Variable of which to compute the Hessian.
/// @param wrt Variable with respect to which to compute the Hessian.
Hessian(Variable<Scalar> variable, Variable<Scalar> wrt)
: Hessian{std::move(variable), VariableMatrix<Scalar>{std::move(wrt)}} {}
/**
* Constructs a Hessian object.
*
* @param variable Variable of which to compute the Hessian.
* @param wrt Vector of variables with respect to which to compute the
* Hessian.
*/
/// Constructs a Hessian object.
///
/// @param variable Variable of which to compute the Hessian.
/// @param wrt Vector of variables with respect to which to compute the
/// Hessian.
Hessian(Variable<Scalar> variable, SleipnirMatrixLike<Scalar> auto wrt)
: m_variables{detail::AdjointExpressionGraph<Scalar>{variable}
.generate_gradient_tree(wrt)},
: m_variables{detail::GradientExpressionGraph<Scalar>{variable}
.generate_tree(wrt)},
m_wrt{wrt} {
slp_assert(m_wrt.cols() == 1);
@@ -74,7 +68,7 @@ class Hessian {
// If the row is linear, compute its gradient once here and cache its
// triplets. Constant rows are ignored because their gradients have no
// nonzero triplets.
m_graphs[row].append_gradient_triplets(m_cached_triplets, row, m_wrt);
m_graphs[row].append_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().
@@ -90,20 +84,18 @@ class Hessian {
}
}
/**
* Returns the Hessian as a VariableMatrix.
*
* This is useful when constructing optimization problems with derivatives in
* them.
*
* @return The Hessian as a VariableMatrix.
*/
/// Returns the Hessian as a VariableMatrix.
///
/// This is useful when constructing optimization problems with derivatives in
/// them.
///
/// @return The Hessian as a VariableMatrix.
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);
auto grad = m_graphs[row].generate_tree(m_wrt);
for (int col = 0; col < m_wrt.rows(); ++col) {
if (grad[col].expr != nullptr) {
result(row, col) = std::move(grad[col]);
@@ -116,11 +108,9 @@ class Hessian {
return result;
}
/**
* Evaluates the Hessian at wrt's value.
*
* @return The Hessian at wrt's value.
*/
/// Evaluates the Hessian at wrt's value.
///
/// @return The Hessian at wrt's value.
const Eigen::SparseMatrix<Scalar>& value() {
if (m_nonlinear_rows.empty()) {
return m_H;
@@ -136,7 +126,7 @@ class Hessian {
// Compute each nonlinear row of the Hessian
for (int row : m_nonlinear_rows) {
m_graphs[row].append_gradient_triplets(triplets, row, m_wrt);
m_graphs[row].append_triplets(triplets, row, m_wrt);
}
m_H.setFromTriplets(triplets.begin(), triplets.end());
@@ -151,7 +141,7 @@ class Hessian {
VariableMatrix<Scalar> m_variables;
VariableMatrix<Scalar> m_wrt;
gch::small_vector<detail::AdjointExpressionGraph<Scalar>> m_graphs;
gch::small_vector<detail::GradientExpressionGraph<Scalar>> m_graphs;
Eigen::SparseMatrix<Scalar> m_H{m_variables.rows(), m_wrt.rows()};

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

@@ -7,7 +7,7 @@
#include <Eigen/SparseCore>
#include <gch/small_vector.hpp>
#include "sleipnir/autodiff/adjoint_expression_graph.hpp"
#include "sleipnir/autodiff/gradient_expression_graph.hpp"
#include "sleipnir/autodiff/variable.hpp"
#include "sleipnir/autodiff/variable_matrix.hpp"
#include "sleipnir/util/assert.hpp"
@@ -17,45 +17,37 @@
namespace slp {
/**
* This class calculates the Jacobian of a vector of variables with respect to a
* vector of variables.
*
* The Jacobian is only recomputed if the variable expression is quadratic or
* higher order.
*
* @tparam Scalar Scalar type.
*/
/// This class calculates the Jacobian of a vector of variables with respect to
/// a vector of variables.
///
/// The Jacobian is only recomputed if the variable expression is quadratic or
/// higher order.
///
/// @tparam Scalar Scalar type.
template <typename Scalar>
class Jacobian {
public:
/**
* Constructs a Jacobian object.
*
* @param variable Variable of which to compute the Jacobian.
* @param wrt Variable with respect to which to compute the Jacobian.
*/
/// Constructs a Jacobian object.
///
/// @param variable Variable of which to compute the Jacobian.
/// @param wrt Variable with respect to which to compute the Jacobian.
Jacobian(Variable<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.
*/
/// 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.
*
* @param variables Vector of variables of which to compute the Jacobian.
* @param wrt Vector of variables with respect to which to compute the
* Jacobian.
*/
/// Constructs a Jacobian object.
///
/// @param variables Vector of variables of which to compute the Jacobian.
/// @param wrt Vector of variables with respect to which to compute the
/// Jacobian.
Jacobian(VariableMatrix<Scalar> variables,
SleipnirMatrixLike<Scalar> auto wrt)
: m_variables{std::move(variables)}, m_wrt{std::move(wrt)} {
@@ -85,7 +77,7 @@ class Jacobian {
// If the row is linear, compute its gradient once here and cache its
// triplets. Constant rows are ignored because their gradients have no
// nonzero triplets.
m_graphs[row].append_gradient_triplets(m_cached_triplets, row, m_wrt);
m_graphs[row].append_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().
@@ -98,20 +90,18 @@ class Jacobian {
}
}
/**
* Returns the Jacobian as a VariableMatrix.
*
* This is useful when constructing optimization problems with derivatives in
* them.
*
* @return The Jacobian as a VariableMatrix.
*/
/// Returns the Jacobian as a VariableMatrix.
///
/// This is useful when constructing optimization problems with derivatives in
/// them.
///
/// @return The Jacobian as a VariableMatrix.
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);
auto grad = m_graphs[row].generate_tree(m_wrt);
for (int col = 0; col < m_wrt.rows(); ++col) {
if (grad[col].expr != nullptr) {
result(row, col) = std::move(grad[col]);
@@ -124,11 +114,9 @@ class Jacobian {
return result;
}
/**
* Evaluates the Jacobian at wrt's value.
*
* @return The Jacobian at wrt's value.
*/
/// Evaluates the Jacobian at wrt's value.
///
/// @return The Jacobian at wrt's value.
const Eigen::SparseMatrix<Scalar>& value() {
if (m_nonlinear_rows.empty()) {
return m_J;
@@ -144,7 +132,7 @@ class Jacobian {
// Compute each nonlinear row of the Jacobian
for (int row : m_nonlinear_rows) {
m_graphs[row].append_gradient_triplets(triplets, row, m_wrt);
m_graphs[row].append_triplets(triplets, row, m_wrt);
}
m_J.setFromTriplets(triplets.begin(), triplets.end());
@@ -156,7 +144,7 @@ class Jacobian {
VariableMatrix<Scalar> m_variables;
VariableMatrix<Scalar> m_wrt;
gch::small_vector<detail::AdjointExpressionGraph<Scalar>> m_graphs;
gch::small_vector<detail::GradientExpressionGraph<Scalar>> m_graphs;
Eigen::SparseMatrix<Scalar> m_J{m_variables.rows(), m_wrt.rows()};

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

@@ -4,10 +4,8 @@
namespace slp {
/**
* Marker interface for concepts to determine whether a given scalar or matrix
* type belongs to Sleipnir.
*/
/// Marker interface for concepts to determine whether a given scalar or matrix
/// type belongs to Sleipnir.
class SleipnirBase {};
} // namespace slp

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

@@ -13,21 +13,15 @@ namespace slp {
namespace slicing {
/**
* Type tag used to designate an omitted argument of the slice.
*/
/// Type tag used to designate an omitted argument of the slice.
struct none_t {};
/**
* Designates an omitted argument of the slice.
*/
/// Designates an omitted argument of the slice.
static inline constexpr none_t _;
} // namespace slicing
/**
* Represents a sequence of elements in an iterable object.
*/
/// Represents a sequence of elements in an iterable object.
class SLEIPNIR_DLLEXPORT Slice {
public:
/// Start index (inclusive).
@@ -39,23 +33,17 @@ class SLEIPNIR_DLLEXPORT Slice {
/// Step.
int step = 1;
/**
* Constructs a Slice.
*/
/// Constructs a Slice.
constexpr Slice() = default;
/**
* Constructs a slice.
*/
/// Constructs a slice.
// NOLINTNEXTLINE (google-explicit-constructor)
constexpr Slice(slicing::none_t)
: Slice(0, std::numeric_limits<int>::max(), 1) {}
/**
* Constructs a slice.
*
* @param start Slice start index (inclusive).
*/
/// Constructs a slice.
///
/// @param start Slice start index (inclusive).
// NOLINTNEXTLINE (google-explicit-constructor)
constexpr Slice(int start) {
this->start = start;
@@ -63,12 +51,10 @@ class SLEIPNIR_DLLEXPORT Slice {
this->step = 1;
}
/**
* Constructs a slice.
*
* @param start Slice start index (inclusive).
* @param stop Slice stop index (exclusive).
*/
/// Constructs a slice.
///
/// @param start Slice start index (inclusive).
/// @param stop Slice stop index (exclusive).
template <typename Start, typename Stop>
requires(std::same_as<Start, slicing::none_t> ||
std::convertible_to<Start, int>) &&
@@ -77,13 +63,11 @@ class SLEIPNIR_DLLEXPORT Slice {
constexpr Slice(Start start, Stop stop)
: Slice(std::move(start), std::move(stop), 1) {}
/**
* Constructs a slice.
*
* @param start Slice start index (inclusive).
* @param stop Slice stop index (exclusive).
* @param step Slice step.
*/
/// Constructs a slice.
///
/// @param start Slice start index (inclusive).
/// @param stop Slice stop index (exclusive).
/// @param step Slice step.
template <typename Start, typename Stop, typename Step>
requires(std::same_as<Start, slicing::none_t> ||
std::convertible_to<Start, int>) &&
@@ -126,13 +110,11 @@ class SLEIPNIR_DLLEXPORT Slice {
}
}
/**
* Adjusts start and end slice indices assuming a sequence of the specified
* length.
*
* @param length The sequence length.
* @return The slice length.
*/
/// Adjusts start and end slice indices assuming a sequence of the specified
/// length.
///
/// @param length The sequence length.
/// @return The slice length.
constexpr int adjust(int length) {
slp_assert(step != 0);
slp_assert(step >= -std::numeric_limits<int>::max());

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

File diff suppressed because it is too large Load Diff

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

@@ -17,30 +17,22 @@
namespace slp {
/**
* A submatrix of autodiff variables with reference semantics.
*
* @tparam Mat The type of the matrix whose storage this class points to.
*/
/// A submatrix of autodiff variables with reference semantics.
///
/// @tparam Mat The type of the matrix whose storage this class points to.
template <typename Mat>
class VariableBlock : public SleipnirBase {
public:
/**
* Scalar type alias.
*/
/// Scalar type alias.
using Scalar = typename Mat::Scalar;
/**
* Copy constructor.
*/
/// Copy constructor.
VariableBlock(const VariableBlock<Mat>&) = default;
/**
* Assigns a VariableBlock to the block.
*
* @param values VariableBlock of values.
* @return This VariableBlock.
*/
/// Assigns a VariableBlock to the block.
///
/// @param values VariableBlock of values.
/// @return This VariableBlock.
VariableBlock<Mat>& operator=(const VariableBlock<Mat>& values) {
if (this == &values) {
return *this;
@@ -65,17 +57,13 @@ class VariableBlock : public SleipnirBase {
return *this;
}
/**
* Move constructor.
*/
/// Move constructor.
VariableBlock(VariableBlock<Mat>&&) = default;
/**
* Assigns a VariableBlock to the block.
*
* @param values VariableBlock of values.
* @return This VariableBlock.
*/
/// Assigns a VariableBlock to the block.
///
/// @param values VariableBlock of values.
/// @return This VariableBlock.
VariableBlock<Mat>& operator=(VariableBlock<Mat>&& values) {
if (this == &values) {
return *this;
@@ -100,23 +88,19 @@ class VariableBlock : public SleipnirBase {
return *this;
}
/**
* Constructs a Variable block pointing to all of the given matrix.
*
* @param mat The matrix to which to point.
*/
/// Constructs a Variable block pointing to all of the given matrix.
///
/// @param mat The matrix to which to point.
// 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.
*
* @param mat The matrix to which to point.
* @param row_offset The block's row offset.
* @param col_offset The block's column offset.
* @param block_rows The number of rows in the block.
* @param block_cols The number of columns in the block.
*/
/// Constructs a Variable block pointing to a subset of the given matrix.
///
/// @param mat The matrix to which to point.
/// @param row_offset The block's row offset.
/// @param col_offset The block's column offset.
/// @param block_rows The number of rows in the block.
/// @param block_cols The number of columns in the block.
VariableBlock(Mat& mat, int row_offset, int col_offset, int block_rows,
int block_cols)
: m_mat{&mat},
@@ -125,17 +109,15 @@ class VariableBlock : public SleipnirBase {
m_col_slice{col_offset, col_offset + block_cols, 1},
m_col_slice_length{m_col_slice.adjust(mat.cols())} {}
/**
* Constructs a Variable block pointing to a subset of the given matrix.
*
* Note that the slices are taken as is rather than adjusted.
*
* @param mat The matrix to which to point.
* @param row_slice The block's row slice.
* @param row_slice_length The block's row length.
* @param col_slice The block's column slice.
* @param col_slice_length The block's column length.
*/
/// Constructs a Variable block pointing to a subset of the given matrix.
///
/// Note that the slices are taken as is rather than adjusted.
///
/// @param mat The matrix to which to point.
/// @param row_slice The block's row slice.
/// @param row_slice_length The block's row length.
/// @param col_slice The block's column slice.
/// @param col_slice_length The block's column length.
VariableBlock(Mat& mat, Slice row_slice, int row_slice_length,
Slice col_slice, int col_slice_length)
: m_mat{&mat},
@@ -144,14 +126,12 @@ class VariableBlock : public SleipnirBase {
m_col_slice{std::move(col_slice)},
m_col_slice_length{col_slice_length} {}
/**
* Assigns a scalar to the block.
*
* This only works for blocks with one row and one column.
*
* @param value Value to assign.
* @return This VariableBlock.
*/
/// Assigns a scalar to the block.
///
/// This only works for blocks with one row and one column.
///
/// @param value Value to assign.
/// @return This VariableBlock.
VariableBlock<Mat>& operator=(ScalarLike auto value) {
slp_assert(rows() == 1 && cols() == 1);
@@ -160,25 +140,21 @@ class VariableBlock : public SleipnirBase {
return *this;
}
/**
* Assigns a scalar to the block.
*
* This only works for blocks with one row and one column.
*
* @param value Value to assign.
*/
/// 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(Scalar value) {
slp_assert(rows() == 1 && cols() == 1);
(*this)(0, 0).set_value(value);
}
/**
* Assigns an Eigen matrix to the block.
*
* @param values Eigen matrix of values to assign.
* @return This VariableBlock.
*/
/// Assigns an Eigen matrix to the block.
///
/// @param values Eigen matrix of values to assign.
/// @return This VariableBlock.
template <typename Derived>
VariableBlock<Mat>& operator=(const Eigen::MatrixBase<Derived>& values) {
slp_assert(rows() == values.rows() && cols() == values.cols());
@@ -192,11 +168,9 @@ class VariableBlock : public SleipnirBase {
return *this;
}
/**
* Sets block's internal values.
*
* @param values Eigen matrix of values.
*/
/// Sets block's internal values.
///
/// @param values Eigen matrix of values.
template <typename Derived>
requires std::same_as<typename Derived::Scalar, Scalar>
void set_value(const Eigen::MatrixBase<Derived>& values) {
@@ -209,12 +183,10 @@ class VariableBlock : public SleipnirBase {
}
}
/**
* Assigns a VariableMatrix to the block.
*
* @param values VariableMatrix of values.
* @return This VariableBlock.
*/
/// Assigns a VariableMatrix to the block.
///
/// @param values VariableMatrix of values.
/// @return This VariableBlock.
VariableBlock<Mat>& operator=(const Mat& values) {
slp_assert(rows() == values.rows() && cols() == values.cols());
@@ -226,12 +198,10 @@ class VariableBlock : public SleipnirBase {
return *this;
}
/**
* Assigns a VariableMatrix to the block.
*
* @param values VariableMatrix of values.
* @return This VariableBlock.
*/
/// Assigns a VariableMatrix to the block.
///
/// @param values VariableMatrix of values.
/// @return This VariableBlock.
VariableBlock<Mat>& operator=(Mat&& values) {
slp_assert(rows() == values.rows() && cols() == values.cols());
@@ -243,13 +213,11 @@ class VariableBlock : public SleipnirBase {
return *this;
}
/**
* Returns a scalar subblock at the given row and column.
*
* @param row The scalar subblock's row.
* @param col The scalar subblock's column.
* @return A scalar subblock at the given row and column.
*/
/// Returns a scalar subblock at the given row and column.
///
/// @param row The scalar subblock's row.
/// @param col The scalar subblock's column.
/// @return A scalar subblock at the given row and column.
Variable<Scalar>& operator()(int row, int col)
requires(!std::is_const_v<Mat>)
{
@@ -259,13 +227,11 @@ class VariableBlock : public SleipnirBase {
m_col_slice.start + col * m_col_slice.step);
}
/**
* Returns a scalar subblock at the given row and column.
*
* @param row The scalar subblock's row.
* @param col The scalar subblock's column.
* @return A scalar subblock at the given row and column.
*/
/// Returns a scalar subblock at the given row and column.
///
/// @param row The scalar subblock's row.
/// @param col The scalar subblock's column.
/// @return A scalar subblock at the given row and column.
const Variable<Scalar>& operator()(int row, int col) const {
slp_assert(row >= 0 && row < rows());
slp_assert(col >= 0 && col < cols());
@@ -273,12 +239,10 @@ class VariableBlock : public SleipnirBase {
m_col_slice.start + col * m_col_slice.step);
}
/**
* Returns a scalar subblock at the given index.
*
* @param index The scalar subblock's index.
* @return A scalar subblock at the given index.
*/
/// Returns a scalar subblock at the given index.
///
/// @param index The scalar subblock's index.
/// @return A scalar subblock at the given index.
Variable<Scalar>& operator[](int index)
requires(!std::is_const_v<Mat>)
{
@@ -286,26 +250,22 @@ class VariableBlock : public SleipnirBase {
return (*this)(index / cols(), index % cols());
}
/**
* Returns a scalar subblock at the given index.
*
* @param index The scalar subblock's index.
* @return A scalar subblock at the given index.
*/
/// Returns a scalar subblock at the given index.
///
/// @param index The scalar subblock's index.
/// @return A scalar subblock at the given index.
const Variable<Scalar>& operator[](int index) const {
slp_assert(index >= 0 && index < rows() * cols());
return (*this)(index / cols(), index % cols());
}
/**
* Returns a block of the variable matrix.
*
* @param row_offset The row offset of the block selection.
* @param col_offset The column offset of the block selection.
* @param block_rows The number of rows in the block selection.
* @param block_cols The number of columns in the block selection.
* @return A block of the variable matrix.
*/
/// Returns a block of the variable matrix.
///
/// @param row_offset The row offset of the block selection.
/// @param col_offset The column offset of the block selection.
/// @param block_rows The number of rows in the block selection.
/// @param block_cols The number of columns in the block selection.
/// @return A block of the variable matrix.
VariableBlock<Mat> block(int row_offset, int col_offset, int block_rows,
int block_cols) {
slp_assert(row_offset >= 0 && row_offset <= rows());
@@ -316,15 +276,13 @@ class VariableBlock : public SleipnirBase {
{col_offset, col_offset + block_cols, 1});
}
/**
* Returns a block slice of the variable matrix.
*
* @param row_offset The row offset of the block selection.
* @param col_offset The column offset of the block selection.
* @param block_rows The number of rows in the block selection.
* @param block_cols The number of columns in the block selection.
* @return A block slice of the variable matrix.
*/
/// Returns a block slice of the variable matrix.
///
/// @param row_offset The row offset of the block selection.
/// @param col_offset The column offset of the block selection.
/// @param block_rows The number of rows in the block selection.
/// @param block_cols The number of columns in the block selection.
/// @return A block slice of the variable matrix.
const VariableBlock<const Mat> block(int row_offset, int col_offset,
int block_rows, int block_cols) const {
slp_assert(row_offset >= 0 && row_offset <= rows());
@@ -335,26 +293,22 @@ class VariableBlock : public SleipnirBase {
{col_offset, col_offset + block_cols, 1});
}
/**
* Returns a slice of the variable matrix.
*
* @param row_slice The row slice.
* @param col_slice The column slice.
* @return A slice of the variable matrix.
*/
/// Returns a slice of the variable matrix.
///
/// @param row_slice The row slice.
/// @param col_slice The column slice.
/// @return A slice of the variable matrix.
VariableBlock<Mat> operator()(Slice row_slice, Slice col_slice) {
int row_slice_length = row_slice.adjust(m_row_slice_length);
int col_slice_length = col_slice.adjust(m_col_slice_length);
return (*this)(row_slice, row_slice_length, col_slice, col_slice_length);
}
/**
* Returns a slice of the variable matrix.
*
* @param row_slice The row slice.
* @param col_slice The column slice.
* @return A slice of the variable matrix.
*/
/// Returns a slice of the variable matrix.
///
/// @param row_slice The row slice.
/// @param col_slice The column slice.
/// @return A slice of the variable matrix.
const VariableBlock<const Mat> operator()(Slice row_slice,
Slice col_slice) const {
int row_slice_length = row_slice.adjust(m_row_slice_length);
@@ -362,18 +316,16 @@ class VariableBlock : public SleipnirBase {
return (*this)(row_slice, row_slice_length, col_slice, col_slice_length);
}
/**
* Returns a slice of the variable matrix.
*
* The given slices aren't adjusted. This overload is for Python bindings
* only.
*
* @param row_slice The row slice.
* @param row_slice_length The row slice length.
* @param col_slice The column slice.
* @param col_slice_length The column slice length.
* @return A slice of the variable matrix.
*/
/// Returns a slice of the variable matrix.
///
/// The given slices aren't adjusted. This overload is for Python bindings
/// only.
///
/// @param row_slice The row slice.
/// @param row_slice_length The row slice length.
/// @param col_slice The column slice.
/// @param col_slice_length The column slice length.
/// @return A slice of the variable matrix.
VariableBlock<Mat> operator()(Slice row_slice, int row_slice_length,
Slice col_slice, int col_slice_length) {
return VariableBlock{
@@ -388,18 +340,16 @@ class VariableBlock : public SleipnirBase {
col_slice_length};
}
/**
* Returns a slice of the variable matrix.
*
* The given slices aren't adjusted. This overload is for Python bindings
* only.
*
* @param row_slice The row slice.
* @param row_slice_length The row slice length.
* @param col_slice The column slice.
* @param col_slice_length The column slice length.
* @return A slice of the variable matrix.
*/
/// Returns a slice of the variable matrix.
///
/// The given slices aren't adjusted. This overload is for Python bindings
/// only.
///
/// @param row_slice The row slice.
/// @param row_slice_length The row slice length.
/// @param col_slice The column slice.
/// @param col_slice_length The column slice length.
/// @return A slice of the variable matrix.
const VariableBlock<const Mat> operator()(Slice row_slice,
int row_slice_length,
Slice col_slice,
@@ -416,13 +366,11 @@ class VariableBlock : public SleipnirBase {
col_slice_length};
}
/**
* Returns a segment of the variable vector.
*
* @param offset The offset of the segment.
* @param length The length of the segment.
* @return A segment of the variable vector.
*/
/// Returns a segment of the variable vector.
///
/// @param offset The offset of the segment.
/// @param length The length of the segment.
/// @return A segment of the variable vector.
VariableBlock<Mat> segment(int offset, int length) {
slp_assert(cols() == 1);
slp_assert(offset >= 0 && offset < rows());
@@ -430,13 +378,11 @@ class VariableBlock : public SleipnirBase {
return block(offset, 0, length, 1);
}
/**
* Returns a segment of the variable vector.
*
* @param offset The offset of the segment.
* @param length The length of the segment.
* @return A segment of the variable vector.
*/
/// Returns a segment of the variable vector.
///
/// @param offset The offset of the segment.
/// @param length The length of the segment.
/// @return A segment of the variable vector.
const VariableBlock<Mat> segment(int offset, int length) const {
slp_assert(cols() == 1);
slp_assert(offset >= 0 && offset < rows());
@@ -444,56 +390,46 @@ class VariableBlock : public SleipnirBase {
return block(offset, 0, length, 1);
}
/**
* Returns a row slice of the variable matrix.
*
* @param row The row to slice.
* @return A row slice of the variable matrix.
*/
/// Returns a row slice of the variable matrix.
///
/// @param row The row to slice.
/// @return A row slice of the variable matrix.
VariableBlock<Mat> row(int row) {
slp_assert(row >= 0 && row < rows());
return block(row, 0, 1, cols());
}
/**
* Returns a row slice of the variable matrix.
*
* @param row The row to slice.
* @return A row slice of the variable matrix.
*/
/// Returns a row slice of the variable matrix.
///
/// @param row The row to slice.
/// @return A row slice of the variable matrix.
VariableBlock<const Mat> row(int row) const {
slp_assert(row >= 0 && row < rows());
return block(row, 0, 1, cols());
}
/**
* Returns a column slice of the variable matrix.
*
* @param col The column to slice.
* @return A column slice of the variable matrix.
*/
/// Returns a column slice of the variable matrix.
///
/// @param col The column to slice.
/// @return A column slice of the variable matrix.
VariableBlock<Mat> col(int col) {
slp_assert(col >= 0 && col < cols());
return block(0, col, rows(), 1);
}
/**
* Returns a column slice of the variable matrix.
*
* @param col The column to slice.
* @return A column slice of the variable matrix.
*/
/// Returns a column slice of the variable matrix.
///
/// @param col The column to slice.
/// @return A column slice of the variable matrix.
VariableBlock<const Mat> col(int col) const {
slp_assert(col >= 0 && col < cols());
return block(0, col, rows(), 1);
}
/**
* Compound matrix multiplication-assignment operator.
*
* @param rhs Variable to multiply.
* @return Result of multiplication.
*/
/// Compound matrix multiplication-assignment operator.
///
/// @param rhs Variable to multiply.
/// @return Result of multiplication.
VariableBlock<Mat>& operator*=(const MatrixLike auto& rhs) {
slp_assert(cols() == rhs.rows() && cols() == rhs.cols());
@@ -510,12 +446,10 @@ class VariableBlock : public SleipnirBase {
return *this;
}
/**
* Compound matrix multiplication-assignment operator.
*
* @param rhs Variable to multiply.
* @return Result of multiplication.
*/
/// Compound matrix multiplication-assignment operator.
///
/// @param rhs Variable to multiply.
/// @return Result of multiplication.
VariableBlock<Mat>& operator*=(const ScalarLike auto& rhs) {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
@@ -526,12 +460,10 @@ class VariableBlock : public SleipnirBase {
return *this;
}
/**
* Compound matrix division-assignment operator.
*
* @param rhs Variable to divide.
* @return Result of division.
*/
/// Compound matrix division-assignment operator.
///
/// @param rhs Variable to divide.
/// @return Result of division.
VariableBlock<Mat>& operator/=(const MatrixLike auto& rhs) {
slp_assert(rhs.rows() == 1 && rhs.cols() == 1);
@@ -544,12 +476,10 @@ class VariableBlock : public SleipnirBase {
return *this;
}
/**
* Compound matrix division-assignment operator.
*
* @param rhs Variable to divide.
* @return Result of division.
*/
/// Compound matrix division-assignment operator.
///
/// @param rhs Variable to divide.
/// @return Result of division.
VariableBlock<Mat>& operator/=(const ScalarLike auto& rhs) {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
@@ -560,12 +490,10 @@ class VariableBlock : public SleipnirBase {
return *this;
}
/**
* Compound addition-assignment operator.
*
* @param rhs Variable to add.
* @return Result of addition.
*/
/// Compound addition-assignment operator.
///
/// @param rhs Variable to add.
/// @return Result of addition.
VariableBlock<Mat>& operator+=(const MatrixLike auto& rhs) {
slp_assert(rows() == rhs.rows() && cols() == rhs.cols());
@@ -578,12 +506,10 @@ class VariableBlock : public SleipnirBase {
return *this;
}
/**
* Compound addition-assignment operator.
*
* @param rhs Variable to add.
* @return Result of addition.
*/
/// Compound addition-assignment operator.
///
/// @param rhs Variable to add.
/// @return Result of addition.
VariableBlock<Mat>& operator+=(const ScalarLike auto& rhs) {
slp_assert(rows() == 1 && cols() == 1);
@@ -596,12 +522,10 @@ class VariableBlock : public SleipnirBase {
return *this;
}
/**
* Compound subtraction-assignment operator.
*
* @param rhs Variable to subtract.
* @return Result of subtraction.
*/
/// Compound subtraction-assignment operator.
///
/// @param rhs Variable to subtract.
/// @return Result of subtraction.
VariableBlock<Mat>& operator-=(const MatrixLike auto& rhs) {
slp_assert(rows() == rhs.rows() && cols() == rhs.cols());
@@ -614,12 +538,10 @@ class VariableBlock : public SleipnirBase {
return *this;
}
/**
* Compound subtraction-assignment operator.
*
* @param rhs Variable to subtract.
* @return Result of subtraction.
*/
/// Compound subtraction-assignment operator.
///
/// @param rhs Variable to subtract.
/// @return Result of subtraction.
VariableBlock<Mat>& operator-=(const ScalarLike auto& rhs) {
slp_assert(rows() == 1 && cols() == 1);
@@ -632,20 +554,16 @@ class VariableBlock : public SleipnirBase {
return *this;
}
/**
* Implicit conversion operator from 1x1 VariableBlock to Variable.
*/
/// Implicit conversion operator from 1x1 VariableBlock to Variable.
// NOLINTNEXTLINE (google-explicit-constructor)
operator Variable<Scalar>() const {
slp_assert(rows() == 1 && cols() == 1);
return (*this)(0, 0);
}
/**
* Returns the transpose of the variable matrix.
*
* @return The transpose of the variable matrix.
*/
/// Returns the transpose of the variable matrix.
///
/// @return The transpose of the variable matrix.
std::remove_cv_t<Mat> T() const {
std::remove_cv_t<Mat> result{detail::empty, cols(), rows()};
@@ -658,45 +576,35 @@ class VariableBlock : public SleipnirBase {
return result;
}
/**
* Returns the number of rows in the matrix.
*
* @return The number of rows in the matrix.
*/
/// Returns the number of rows in the matrix.
///
/// @return The number of rows in the matrix.
int rows() const { return m_row_slice_length; }
/**
* Returns the number of columns in the matrix.
*
* @return The number of columns in the matrix.
*/
/// Returns the number of columns in the matrix.
///
/// @return The number of columns in the matrix.
int cols() const { return m_col_slice_length; }
/**
* Returns an element of the variable matrix.
*
* @param row The row of the element to return.
* @param col The column of the element to return.
* @return An element of the variable matrix.
*/
/// Returns an element of the variable matrix.
///
/// @param row The row of the element to return.
/// @param col The column of the element to return.
/// @return An element of the variable matrix.
Scalar value(int row, int col) { return (*this)(row, col).value(); }
/**
* Returns an element of the variable block.
*
* @param index The index of the element to return.
* @return An element of the variable block.
*/
/// Returns an element of the variable block.
///
/// @param index The index of the element to return.
/// @return An element of the variable block.
Scalar value(int index) {
slp_assert(index >= 0 && index < rows() * cols());
return value(index / cols(), index % cols());
}
/**
* Returns the contents of the variable matrix.
*
* @return The contents of the variable matrix.
*/
/// Returns the contents of the variable matrix.
///
/// @return The contents of the variable matrix.
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> value() {
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> result{rows(),
cols()};
@@ -710,12 +618,10 @@ class VariableBlock : public SleipnirBase {
return result;
}
/**
* Transforms the matrix coefficient-wise with an unary operator.
*
* @param unary_op The unary operator to use for the transform operation.
* @return Result of the unary operator.
*/
/// Transforms the matrix coefficient-wise with an unary operator.
///
/// @param unary_op The unary operator to use for the transform operation.
/// @return Result of the unary operator.
std::remove_cv_t<Mat> cwise_transform(
function_ref<Variable<Scalar>(const Variable<Scalar>& x)> unary_op)
const {
@@ -827,103 +733,77 @@ class VariableBlock : public SleipnirBase {
#endif // DOXYGEN_SHOULD_SKIP_THIS
/**
* Returns begin iterator.
*
* @return Begin iterator.
*/
/// Returns begin iterator.
///
/// @return Begin iterator.
iterator begin() { return iterator(this, 0); }
/**
* Returns end iterator.
*
* @return End iterator.
*/
/// Returns end iterator.
///
/// @return End iterator.
iterator end() { return iterator(this, rows() * cols()); }
/**
* Returns begin iterator.
*
* @return Begin iterator.
*/
/// Returns begin iterator.
///
/// @return Begin iterator.
const_iterator begin() const { return const_iterator(this, 0); }
/**
* Returns end iterator.
*
* @return End iterator.
*/
/// Returns end iterator.
///
/// @return End iterator.
const_iterator end() const { return const_iterator(this, rows() * cols()); }
/**
* Returns const begin iterator.
*
* @return Const begin iterator.
*/
/// Returns const begin iterator.
///
/// @return Const begin iterator.
const_iterator cbegin() const { return const_iterator(this, 0); }
/**
* Returns const end iterator.
*
* @return Const end iterator.
*/
/// Returns const end iterator.
///
/// @return Const end iterator.
const_iterator cend() const { return const_iterator(this, rows() * cols()); }
/**
* Returns reverse begin iterator.
*
* @return Reverse begin iterator.
*/
/// Returns reverse begin iterator.
///
/// @return Reverse begin iterator.
reverse_iterator rbegin() { return reverse_iterator{end()}; }
/**
* Returns reverse end iterator.
*
* @return Reverse end iterator.
*/
/// 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.
*/
/// 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.
*/
/// 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.
*/
/// 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.
*/
/// 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.
*
* @return Number of elements in matrix.
*/
/// Returns number of elements in matrix.
///
/// @return Number of elements in matrix.
size_t size() const { return rows() * cols(); }
private:

911
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

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

@@ -13,13 +13,11 @@
namespace slp {
/**
* The result of a multistart solve.
*
* @tparam Scalar Scalar type.
* @tparam DecisionVariables The type containing the decision variable initial
* guess.
*/
/// The result of a multistart solve.
///
/// @tparam Scalar Scalar type.
/// @tparam DecisionVariables The type containing the decision variable initial
/// guess.
template <typename Scalar, typename DecisionVariables>
struct MultistartResult {
/// The solver exit status.
@@ -30,23 +28,21 @@ struct MultistartResult {
DecisionVariables variables;
};
/**
* Solves an optimization problem from different starting points in parallel,
* then returns the solution with the lowest cost.
*
* Each solve is performed on a separate thread. Solutions from successful
* 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.
*/
/// Solves an optimization problem from different starting points in parallel,
/// then returns the solution with the lowest cost.
///
/// Each solve is performed on a separate thread. Solutions from successful
/// 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 Scalar, typename DecisionVariables>
MultistartResult<Scalar, DecisionVariables> Multistart(
MultistartResult<Scalar, DecisionVariables> multistart(
function_ref<MultistartResult<Scalar, DecisionVariables>(
const DecisionVariables& initial_guess)>
solve,

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

@@ -19,54 +19,52 @@
namespace slp {
/**
* This class allows the user to pose and solve a constrained optimal control
* problem (OCP) in a variety of ways.
*
* The system is transcripted by one of three methods (direct transcription,
* direct collocation, or single-shooting) and additional constraints can be
* added.
*
* In direct transcription, each state is a decision variable constrained to the
* integrated dynamics of the previous state. In direct collocation, the
* trajectory is modeled as a series of cubic polynomials where the centerpoint
* slope is constrained. In single-shooting, states depend explicitly as a
* function of all previous states and all previous inputs.
*
* Explicit ODEs are integrated using RK4.
*
* For explicit ODEs, the function must be in the form dx/dt = f(t, x, u).
* For discrete state transition functions, the function must be in the form
* xₖ₊₁ = f(t, xₖ, uₖ).
*
* Direct collocation requires an explicit ODE. Direct transcription and
* single-shooting can use either an ODE or state transition function.
*
* https://underactuated.mit.edu/trajopt.html goes into more detail on each
* transcription method.
*
* @tparam Scalar Scalar type.
*/
/// This class allows the user to pose and solve a constrained optimal control
/// problem (OCP) in a variety of ways.
///
/// The system is transcripted by one of three methods (direct transcription,
/// direct collocation, or single-shooting) and additional constraints can be
/// added.
///
/// In direct transcription, each state is a decision variable constrained to
/// the integrated dynamics of the previous state. In direct collocation, the
/// trajectory is modeled as a series of cubic polynomials where the centerpoint
/// slope is constrained. In single-shooting, states depend explicitly as a
/// function of all previous states and all previous inputs.
///
/// Explicit ODEs are integrated using RK4.
///
/// For explicit ODEs, the function must be in the form dx/dt = f(t, x, u).
/// For discrete state transition functions, the function must be in the form
/// xₖ₊₁ = f(t, xₖ, uₖ).
///
/// Direct collocation requires an explicit ODE. Direct transcription and
/// single-shooting can use either an ODE or state transition function.
///
/// https://underactuated.mit.edu/trajopt.html goes into more detail on each
/// transcription method.
///
/// @tparam Scalar Scalar type.
template <typename Scalar>
class OCP : public Problem<Scalar> {
public:
/**
* Build an optimization problem using a system evolution function (explicit
* ODE or discrete state transition function).
*
* @param num_states The number of system states.
* @param num_inputs The number of system inputs.
* @param dt The timestep for fixed-step integration.
* @param num_steps The number of control points.
* @param dynamics Function representing an explicit or implicit ODE, or a
* discrete state transition function.
* - Explicit: dx/dt = f(x, u, *)
* - Implicit: f([x dx/dt]', u, *) = 0
* - State transition: xₖ₊₁ = f(xₖ, uₖ)
* @param dynamics_type The type of system evolution function.
* @param timestep_method The timestep method.
* @param transcription_method The transcription method.
*/
/// Build an optimization problem using a system evolution function (explicit
/// ODE or discrete state transition function).
///
/// @param num_states The number of system states.
/// @param num_inputs The number of system inputs.
/// @param dt The timestep for fixed-step integration.
/// @param num_steps The number of control points.
/// @param dynamics Function representing an explicit or implicit ODE, or a
/// discrete state transition function.
/// <ul>
/// <li>Explicit: dx/dt = f(x, u, *)</li>
/// <li>Implicit: f([x dx/dt]', u, *) = 0</li>
/// <li>State transition: xₖ₊₁ = f(xₖ, uₖ)</li>
/// </ul>
/// @param dynamics_type The type of system evolution function.
/// @param timestep_method The timestep method.
/// @param transcription_method The transcription method.
OCP(int num_states, int num_inputs, std::chrono::duration<Scalar> dt,
int num_steps,
function_ref<VariableMatrix<Scalar>(const VariableMatrix<Scalar>& x,
@@ -89,23 +87,23 @@ class OCP : public Problem<Scalar> {
timestep_method,
transcription_method} {}
/**
* Build an optimization problem using a system evolution function (explicit
* ODE or discrete state transition function).
*
* @param num_states The number of system states.
* @param num_inputs The number of system inputs.
* @param dt The timestep for fixed-step integration.
* @param num_steps The number of control points.
* @param dynamics Function representing an explicit or implicit ODE, or a
* discrete state transition function.
* - Explicit: dx/dt = f(t, x, u, *)
* - Implicit: f(t, [x dx/dt]', u, *) = 0
* - State transition: xₖ₊₁ = f(t, xₖ, uₖ, dt)
* @param dynamics_type The type of system evolution function.
* @param timestep_method The timestep method.
* @param transcription_method The transcription method.
*/
/// Build an optimization problem using a system evolution function (explicit
/// ODE or discrete state transition function).
///
/// @param num_states The number of system states.
/// @param num_inputs The number of system inputs.
/// @param dt The timestep for fixed-step integration.
/// @param num_steps The number of control points.
/// @param dynamics Function representing an explicit or implicit ODE, or a
/// discrete state transition function.
/// <ul>
/// <li>Explicit: dx/dt = f(t, x, u, *)</li>
/// <li>Implicit: f(t, [x dx/dt]', u, *) = 0</li>
/// <li>State transition: xₖ₊₁ = f(t, xₖ, uₖ, dt)</li>
/// </ul>
/// @param dynamics_type The type of system evolution function.
/// @param timestep_method The timestep method.
/// @param transcription_method The transcription method.
OCP(int num_states, int num_inputs, std::chrono::duration<Scalar> dt,
int num_steps,
function_ref<VariableMatrix<Scalar>(
@@ -158,36 +156,30 @@ class OCP : public Problem<Scalar> {
}
}
/**
* Utility function to constrain the initial state.
*
* @param initial_state the initial state to constrain to.
*/
/// Utility function to constrain the initial state.
///
/// @param initial_state the initial state to constrain to.
template <typename T>
requires ScalarLike<T> || MatrixLike<T>
void constrain_initial_state(const T& initial_state) {
this->subject_to(this->initial_state() == initial_state);
}
/**
* Utility function to constrain the final state.
*
* @param final_state the final state to constrain to.
*/
/// Utility function to constrain the final state.
///
/// @param final_state the final state to constrain to.
template <typename T>
requires ScalarLike<T> || MatrixLike<T>
void constrain_final_state(const T& final_state) {
this->subject_to(this->final_state() == final_state);
}
/**
* Set the constraint evaluation function. This function is called
* `num_steps+1` times, with the corresponding state and input
* VariableMatrices.
*
* @param callback The callback f(x, u) where x is the state and u is the
* input vector.
*/
/// Set the constraint evaluation function. This function is called
/// `num_steps+1` times, with the corresponding state and input
/// VariableMatrices.
///
/// @param callback The callback f(x, u) where x is the state and u is the
/// input vector.
void for_each_step(const function_ref<void(const VariableMatrix<Scalar>& x,
const VariableMatrix<Scalar>& u)>
callback) {
@@ -198,14 +190,12 @@ class OCP : public Problem<Scalar> {
}
}
/**
* Set the constraint evaluation function. This function is called
* `num_steps+1` times, with the corresponding state and input
* VariableMatrices.
*
* @param callback The callback f(t, x, u, dt) where t is time, x is the state
* vector, u is the input vector, and dt is the timestep duration.
*/
/// Set the constraint evaluation function. This function is called
/// `num_steps+1` times, with the corresponding state and input
/// VariableMatrices.
///
/// @param callback The callback f(t, x, u, dt) where t is time, x is the
/// state vector, u is the input vector, and dt is the timestep duration.
void for_each_step(
const function_ref<
void(const Variable<Scalar>& t, const VariableMatrix<Scalar>& x,
@@ -223,12 +213,10 @@ class OCP : public Problem<Scalar> {
}
}
/**
* Convenience function to set a lower bound on the input.
*
* @param lower_bound The lower bound that inputs must always be above. Must
* be shaped (num_inputs)x1.
*/
/// Convenience function to set a lower bound on the input.
///
/// @param lower_bound The lower bound that inputs must always be above. Must
/// be shaped (num_inputs)x1.
template <typename T>
requires ScalarLike<T> || MatrixLike<T>
void set_lower_input_bound(const T& lower_bound) {
@@ -237,12 +225,10 @@ class OCP : public Problem<Scalar> {
}
}
/**
* Convenience function to set an upper bound on the input.
*
* @param upper_bound The upper bound that inputs must always be below. Must
* be shaped (num_inputs)x1.
*/
/// Convenience function to set an upper bound on the input.
///
/// @param upper_bound The upper bound that inputs must always be below. Must
/// be shaped (num_inputs)x1.
template <typename T>
requires ScalarLike<T> || MatrixLike<T>
void set_upper_input_bound(const T& upper_bound) {
@@ -251,68 +237,54 @@ class OCP : public Problem<Scalar> {
}
}
/**
* Convenience function to set a lower bound on the timestep.
*
* @param min_timestep The minimum timestep.
*/
/// Convenience function to set a lower bound on the timestep.
///
/// @param min_timestep The minimum timestep.
void set_min_timestep(std::chrono::duration<Scalar> min_timestep) {
this->subject_to(dt() >= min_timestep.count());
}
/**
* Convenience function to set an upper bound on the timestep.
*
* @param max_timestep The maximum timestep.
*/
/// Convenience function to set an upper bound on the timestep.
///
/// @param max_timestep The maximum timestep.
void set_max_timestep(std::chrono::duration<Scalar> max_timestep) {
this->subject_to(dt() <= max_timestep.count());
}
/**
* Get the state variables. After the problem is solved, this will contain the
* optimized trajectory.
*
* Shaped (num_states)x(num_steps+1).
*
* @return The state variable matrix.
*/
/// Get the state variables. After the problem is solved, this will contain
/// the optimized trajectory.
///
/// Shaped (num_states)x(num_steps+1).
///
/// @return The state variable matrix.
VariableMatrix<Scalar>& X() { return m_X; }
/**
* Get the input variables. After the problem is solved, this will contain the
* inputs corresponding to the optimized trajectory.
*
* Shaped (num_inputs)x(num_steps+1), although the last input step is unused
* in the trajectory.
*
* @return The input variable matrix.
*/
/// Get the input variables. After the problem is solved, this will contain
/// the inputs corresponding to the optimized trajectory.
///
/// Shaped (num_inputs)x(num_steps+1), although the last input step is unused
/// in the trajectory.
///
/// @return The input variable matrix.
VariableMatrix<Scalar>& U() { return m_U; }
/**
* Get the timestep variables. After the problem is solved, this will contain
* the timesteps corresponding to the optimized trajectory.
*
* Shaped 1x(num_steps+1), although the last timestep is unused in
* the trajectory.
*
* @return The timestep variable matrix.
*/
/// Get the timestep variables. After the problem is solved, this will contain
/// the timesteps corresponding to the optimized trajectory.
///
/// Shaped 1x(num_steps+1), although the last timestep is unused in the
/// trajectory.
///
/// @return The timestep variable matrix.
VariableMatrix<Scalar>& dt() { return m_DT; }
/**
* Convenience function to get the initial state in the trajectory.
*
* @return The initial state of the trajectory.
*/
/// Convenience function to get the initial state in the trajectory.
///
/// @return The initial state of the trajectory.
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.
*/
/// Convenience function to get the final state in the trajectory.
///
/// @return The final state of the trajectory.
VariableMatrix<Scalar> final_state() { return m_X.col(m_num_steps); }
private:
@@ -328,15 +300,13 @@ class OCP : public Problem<Scalar> {
VariableMatrix<Scalar> m_U;
VariableMatrix<Scalar> m_DT;
/**
* Performs 4th order Runge-Kutta integration of dx/dt = f(t, x, u) for dt.
*
* @param f The function to integrate. It must take two arguments x and u.
* @param x The initial value of x.
* @param u The value u held constant over the integration period.
* @param t0 The initial time.
* @param dt The time over which to integrate.
*/
/// Performs 4th order Runge-Kutta integration of dx/dt = f(t, x, u) for dt.
///
/// @param f The function to integrate. It must take two arguments x and u.
/// @param x The initial value of x.
/// @param u The value u held constant over the integration period.
/// @param t0 The initial time.
/// @param dt The time over which to integrate.
template <typename F, typename State, typename Input, typename Time>
State rk4(F&& f, State x, Input u, Time t0, Time dt) {
auto halfdt = dt * Scalar(0.5);
@@ -348,9 +318,7 @@ class OCP : public Problem<Scalar> {
return x + (k1 + k2 * Scalar(2) + k3 * Scalar(2) + k4) * (dt / Scalar(6));
}
/**
* Apply direct collocation dynamics constraints.
*/
/// Apply direct collocation dynamics constraints.
void constrain_direct_collocation() {
slp_assert(m_dynamics_type == DynamicsType::EXPLICIT_ODE);
@@ -387,9 +355,7 @@ class OCP : public Problem<Scalar> {
}
}
/**
* Apply direct transcription dynamics constraints.
*/
/// Apply direct transcription dynamics constraints.
void constrain_direct_transcription() {
Variable<Scalar> time{0};
@@ -412,9 +378,7 @@ class OCP : public Problem<Scalar> {
}
}
/**
* Apply single shooting dynamics constraints.
*/
/// Apply single shooting dynamics constraints.
void constrain_single_shooting() {
Variable<Scalar> time{0};

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

@@ -6,9 +6,7 @@
namespace slp {
/**
* Enum describing a type of system dynamics constraints.
*/
/// Enum describing a type of system dynamics constraints.
enum class DynamicsType : uint8_t {
/// The dynamics are a function in the form dx/dt = f(t, x, u).
EXPLICIT_ODE,

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

@@ -6,9 +6,7 @@
namespace slp {
/**
* Enum describing the type of system timestep.
*/
/// Enum describing the type of system timestep.
enum class TimestepMethod : uint8_t {
/// The timestep is a fixed constant.
FIXED,

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

@@ -6,9 +6,7 @@
namespace slp {
/**
* Enum describing an OCP transcription method.
*/
/// Enum describing an OCP transcription method.
enum class TranscriptionMethod : uint8_t {
/// Each state is a decision variable constrained to the integrated dynamics
/// of the previous state.

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

@@ -40,55 +40,49 @@
namespace slp {
/**
* This class allows the user to pose a constrained nonlinear optimization
* problem in natural mathematical notation and solve it.
*
* This class supports problems of the form:
@verbatim
minₓ f(x)
subject to cₑ(x) = 0
cᵢ(x) ≥ 0
@endverbatim
*
* where f(x) is the scalar cost function, x is the vector of decision variables
* (variables the solver can tweak to minimize the cost function), cᵢ(x) are the
* inequality constraints, and cₑ(x) are the equality constraints. Constraints
* are equations or inequalities of the decision variables that constrain what
* values the solver is allowed to use when searching for an optimal solution.
*
* The nice thing about this class is users don't have to put their system in
* the form shown above manually; they can write it in natural mathematical form
* and it'll be converted for them.
*
* @tparam Scalar Scalar type.
*/
/// This class allows the user to pose a constrained nonlinear optimization
/// problem in natural mathematical notation and solve it.
///
/// This class supports problems of the form:
///
/// ```
/// minₓ f(x)
/// subject to cₑ(x) = 0
/// cᵢ(x) ≥ 0
/// ```
///
/// where f(x) is the scalar cost function, x is the vector of decision
/// variables (variables the solver can tweak to minimize the cost function),
/// cᵢ(x) are the inequality constraints, and cₑ(x) are the equality
/// constraints. Constraints are equations or inequalities of the decision
/// variables that constrain what values the solver is allowed to use when
/// searching for an optimal solution.
///
/// The nice thing about this class is users don't have to put their system in
/// the form shown above manually; they can write it in natural mathematical
/// form and it'll be converted for them.
///
/// @tparam Scalar Scalar type.
template <typename Scalar>
class Problem {
public:
/**
* Construct the optimization problem.
*/
/// Construct the optimization problem.
Problem() noexcept = default;
/**
* Create a decision variable in the optimization problem.
*
* @return A decision variable in the optimization problem.
*/
/// Create a decision variable in the optimization problem.
///
/// @return A decision variable in the optimization problem.
[[nodiscard]]
Variable<Scalar> decision_variable() {
m_decision_variables.emplace_back();
return m_decision_variables.back();
}
/**
* Create a matrix of decision variables in the optimization problem.
*
* @param rows Number of matrix rows.
* @param cols Number of matrix columns.
* @return A matrix of decision variables in the optimization problem.
*/
/// Create a matrix of decision variables in the optimization problem.
///
/// @param rows Number of matrix rows.
/// @param cols Number of matrix columns.
/// @return A matrix of decision variables in the optimization problem.
[[nodiscard]]
VariableMatrix<Scalar> decision_variable(int rows, int cols = 1) {
m_decision_variables.reserve(m_decision_variables.size() + rows * cols);
@@ -105,17 +99,15 @@ class Problem {
return vars;
}
/**
* Create a symmetric matrix of decision variables in the optimization
* problem.
*
* Variable instances are reused across the diagonal, which helps reduce
* problem dimensionality.
*
* @param rows Number of matrix rows.
* @return A symmetric matrix of decision varaibles in the optimization
* problem.
*/
/// Create a symmetric matrix of decision variables in the optimization
/// problem.
///
/// Variable instances are reused across the diagonal, which helps reduce
/// problem dimensionality.
///
/// @param rows Number of matrix rows.
/// @return A symmetric matrix of decision varaibles in the optimization
/// problem.
[[nodiscard]]
VariableMatrix<Scalar> symmetric_decision_variable(int rows) {
// We only need to store the lower triangle of an n x n symmetric matrix;
@@ -141,62 +133,52 @@ class Problem {
return vars;
}
/**
* Tells the solver to minimize the output of the given cost function.
*
* Note that this is optional. If only constraints are specified, the solver
* will find the closest solution to the initial conditions that's in the
* feasible set.
*
* @param cost The cost function to minimize.
*/
/// Tells the solver to minimize the output of the given cost function.
///
/// Note that this is optional. If only constraints are specified, the solver
/// will find the closest solution to the initial conditions that's in the
/// feasible set.
///
/// @param cost The cost function to minimize.
void minimize(const Variable<Scalar>& cost) { m_f = cost; }
/**
* Tells the solver to minimize the output of the given cost function.
*
* Note that this is optional. If only constraints are specified, the solver
* will find the closest solution to the initial conditions that's in the
* feasible set.
*
* @param cost The cost function to minimize.
*/
/// Tells the solver to minimize the output of the given cost function.
///
/// Note that this is optional. If only constraints are specified, the solver
/// will find the closest solution to the initial conditions that's in the
/// feasible set.
///
/// @param cost The cost function to minimize.
void minimize(Variable<Scalar>&& cost) { m_f = std::move(cost); }
/**
* Tells the solver to maximize the output of the given objective function.
*
* Note that this is optional. If only constraints are specified, the solver
* will find the closest solution to the initial conditions that's in the
* feasible set.
*
* @param objective The objective function to maximize.
*/
/// Tells the solver to maximize the output of the given objective function.
///
/// Note that this is optional. If only constraints are specified, the solver
/// will find the closest solution to the initial conditions that's in the
/// feasible set.
///
/// @param objective The objective function to maximize.
void maximize(const Variable<Scalar>& objective) {
// Maximizing a cost function is the same as minimizing its negative
m_f = -objective;
}
/**
* Tells the solver to maximize the output of the given objective function.
*
* Note that this is optional. If only constraints are specified, the solver
* will find the closest solution to the initial conditions that's in the
* feasible set.
*
* @param objective The objective function to maximize.
*/
/// Tells the solver to maximize the output of the given objective function.
///
/// Note that this is optional. If only constraints are specified, the solver
/// will find the closest solution to the initial conditions that's in the
/// feasible set.
///
/// @param objective The objective function to maximize.
void maximize(Variable<Scalar>&& objective) {
// Maximizing a cost function is the same as minimizing its negative
m_f = -std::move(objective);
}
/**
* Tells the solver to solve the problem while satisfying the given equality
* constraint.
*
* @param constraint The constraint to satisfy.
*/
/// Tells the solver to solve the problem while satisfying the given equality
/// constraint.
///
/// @param constraint The constraint to satisfy.
void subject_to(const EqualityConstraints<Scalar>& constraint) {
m_equality_constraints.reserve(m_equality_constraints.size() +
constraint.constraints.size());
@@ -204,12 +186,10 @@ class Problem {
std::back_inserter(m_equality_constraints));
}
/**
* Tells the solver to solve the problem while satisfying the given equality
* constraint.
*
* @param constraint The constraint to satisfy.
*/
/// Tells the solver to solve the problem while satisfying the given equality
/// constraint.
///
/// @param constraint The constraint to satisfy.
void subject_to(EqualityConstraints<Scalar>&& constraint) {
m_equality_constraints.reserve(m_equality_constraints.size() +
constraint.constraints.size());
@@ -217,12 +197,10 @@ class Problem {
std::back_inserter(m_equality_constraints));
}
/**
* Tells the solver to solve the problem while satisfying the given inequality
* constraint.
*
* @param constraint The constraint to satisfy.
*/
/// Tells the solver to solve the problem while satisfying the given
/// inequality constraint.
///
/// @param constraint The constraint to satisfy.
void subject_to(const InequalityConstraints<Scalar>& constraint) {
m_inequality_constraints.reserve(m_inequality_constraints.size() +
constraint.constraints.size());
@@ -230,12 +208,10 @@ class Problem {
std::back_inserter(m_inequality_constraints));
}
/**
* Tells the solver to solve the problem while satisfying the given inequality
* constraint.
*
* @param constraint The constraint to satisfy.
*/
/// Tells the solver to solve the problem while satisfying the given
/// inequality constraint.
///
/// @param constraint The constraint to satisfy.
void subject_to(InequalityConstraints<Scalar>&& constraint) {
m_inequality_constraints.reserve(m_inequality_constraints.size() +
constraint.constraints.size());
@@ -243,11 +219,9 @@ class Problem {
std::back_inserter(m_inequality_constraints));
}
/**
* Returns the cost function's type.
*
* @return The cost function's type.
*/
/// Returns the cost function's type.
///
/// @return The cost function's type.
ExpressionType cost_function_type() const {
if (m_f) {
return m_f.value().type();
@@ -256,11 +230,9 @@ class Problem {
}
}
/**
* Returns the type of the highest order equality constraint.
*
* @return The type of the highest order equality constraint.
*/
/// Returns the type of the highest order equality constraint.
///
/// @return The type of the highest order equality constraint.
ExpressionType equality_constraint_type() const {
if (!m_equality_constraints.empty()) {
return std::ranges::max(m_equality_constraints, {},
@@ -271,11 +243,9 @@ class Problem {
}
}
/**
* Returns the type of the highest order inequality constraint.
*
* @return The type of the highest order inequality constraint.
*/
/// Returns the type of the highest order inequality constraint.
///
/// @return The type of the highest order inequality constraint.
ExpressionType inequality_constraint_type() const {
if (!m_inequality_constraints.empty()) {
return std::ranges::max(m_inequality_constraints, {},
@@ -286,20 +256,22 @@ class Problem {
}
}
/**
* Solve the optimization problem. The solution will be stored in the original
* variables used to construct the problem.
*
* @param options Solver options.
* @param spy Enables writing sparsity patterns of H, Aₑ, and Aᵢ to files
* named H.spy, A_e.spy, and A_i.spy respectively during solve. Use
* tools/spy.py to plot them.
* @return The solver status.
*/
/// Solve the optimization problem. The solution will be stored in the
/// original variables used to construct the problem.
///
/// @param options Solver options.
/// @param spy Enables writing sparsity patterns of H, Aₑ, and Aᵢ to files
/// named H.spy, A_e.spy, and A_i.spy respectively during solve. Use
/// tools/spy.py to plot them.
/// @return The solver status.
ExitStatus solve(const Options& options = Options{},
[[maybe_unused]] bool spy = false) {
using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
using SparseMatrix = Eigen::SparseMatrix<Scalar>;
using SparseVector = Eigen::SparseVector<Scalar>;
// Create the initial value column vector
Eigen::Vector<Scalar, Eigen::Dynamic> x{m_decision_variables.size()};
DenseVector x{m_decision_variables.size()};
for (size_t i = 0; i < m_decision_variables.size(); ++i) {
x[i] = m_decision_variables[i].value();
}
@@ -385,23 +357,20 @@ class Problem {
#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);
status = newton<Scalar>(NewtonMatrixCallbacks<Scalar>{
[&](const DenseVector& x) -> Scalar {
x_ad.set_value(x);
return f.value();
},
[&](const DenseVector& x) -> SparseVector {
x_ad.set_value(x);
return g.value();
},
[&](const DenseVector& x) -> SparseMatrix {
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");
@@ -449,29 +418,24 @@ class Problem {
// Invoke SQP solver
status = sqp<Scalar>(
SQPMatrixCallbacks<Scalar>{
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x) -> Scalar {
[&](const DenseVector& x) -> Scalar {
x_ad.set_value(x);
return f.value();
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseVector<Scalar> {
[&](const DenseVector& x) -> SparseVector {
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> {
[&](const DenseVector& x, const DenseVector& y) -> SparseMatrix {
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> {
[&](const DenseVector& x) -> DenseVector {
x_ad.set_value(x);
return c_e_ad.value();
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseMatrix<Scalar> {
[&](const DenseVector& x) -> SparseMatrix {
x_ad.set_value(x);
return A_e.value();
}},
@@ -550,41 +514,34 @@ class Problem {
// Invoke interior-point method solver
status = interior_point<Scalar>(
InteriorPointMatrixCallbacks<Scalar>{
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x) -> Scalar {
[&](const DenseVector& x) -> Scalar {
x_ad.set_value(x);
return f.value();
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseVector<Scalar> {
[&](const DenseVector& x) -> SparseVector {
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> {
[&](const DenseVector& x, const DenseVector& y,
const DenseVector& z) -> SparseMatrix {
x_ad.set_value(x);
y_ad.set_value(y);
z_ad.set_value(z);
return H.value();
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::Vector<Scalar, Eigen::Dynamic> {
[&](const DenseVector& x) -> DenseVector {
x_ad.set_value(x);
return c_e_ad.value();
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseMatrix<Scalar> {
[&](const DenseVector& x) -> SparseMatrix {
x_ad.set_value(x);
return A_e.value();
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::Vector<Scalar, Eigen::Dynamic> {
[&](const DenseVector& x) -> DenseVector {
x_ad.set_value(x);
return c_i_ad.value();
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseMatrix<Scalar> {
[&](const DenseVector& x) -> SparseMatrix {
x_ad.set_value(x);
return A_i.value();
}},
@@ -597,7 +554,7 @@ class Problem {
if (options.diagnostics) {
print_autodiff_diagnostics(ad_setup_profilers);
slp::println("\nExit: {}", to_message(status));
slp::println("\nExit: {}", status);
}
// Assign the solution to the original Variable instances
@@ -606,13 +563,11 @@ class Problem {
return status;
}
/**
* Adds a callback to be called at the beginning of each solver iteration.
*
* The callback for this overload should return void.
*
* @param callback The callback.
*/
/// Adds a callback to be called at the beginning of each solver iteration.
///
/// The callback for this overload should return void.
///
/// @param callback The callback.
template <typename F>
requires requires(F callback, const IterationInfo<Scalar>& info) {
{ callback(info) } -> std::same_as<void>;
@@ -626,14 +581,12 @@ class Problem {
});
}
/**
* Adds a callback to be called at the beginning of each solver iteration.
*
* The callback for this overload should return bool.
*
* @param callback The callback. Returning true from the callback causes the
* solver to exit early with the solution it has so far.
*/
/// Adds a callback to be called at the beginning of each solver iteration.
///
/// The callback for this overload should return bool.
///
/// @param callback The callback. Returning true from the callback causes the
/// solver to exit early with the solution it has so far.
template <typename F>
requires requires(F callback, const IterationInfo<Scalar>& info) {
{ callback(info) } -> std::same_as<bool>;
@@ -642,21 +595,17 @@ class Problem {
m_iteration_callbacks.emplace_back(std::forward<F>(callback));
}
/**
* Clears the registered callbacks.
*/
/// Clears the registered callbacks.
void clear_callbacks() { m_iteration_callbacks.clear(); }
/**
* Adds a callback to be called at the beginning of each solver iteration.
*
* Language bindings should call this in the Problem constructor to register
* callbacks that shouldn't be removed by clear_callbacks(). Persistent
* callbacks run after non-persistent callbacks.
*
* @param callback The callback. Returning true from the callback causes the
* solver to exit early with the solution it has so far.
*/
/// Adds a callback to be called at the beginning of each solver iteration.
///
/// Language bindings should call this in the Problem constructor to register
/// callbacks that shouldn't be removed by clear_callbacks(). Persistent
/// callbacks run after non-persistent callbacks.
///
/// @param callback The callback. Returning true from the callback causes the
/// solver to exit early with the solution it has so far.
template <typename F>
requires requires(F callback, const IterationInfo<Scalar>& info) {
{ callback(info) } -> std::same_as<bool>;

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

@@ -4,15 +4,13 @@
#include <stdint.h>
#include <string_view>
#include <fmt/base.h>
#include "sleipnir/util/symbol_exports.hpp"
#include "sleipnir/util/unreachable.hpp"
namespace slp {
/**
* Solver exit status. Negative values indicate failure.
*/
/// Solver exit status. Negative values indicate failure.
enum class ExitStatus : int8_t {
/// Solved the problem to the desired tolerance.
SUCCESS = 0,
@@ -42,41 +40,58 @@ enum class ExitStatus : int8_t {
TIMEOUT = -9,
};
/**
* Returns user-readable message corresponding to the solver exit status.
*
* @param exit_status Solver exit status.
*/
SLEIPNIR_DLLEXPORT constexpr std::string_view to_message(
const ExitStatus& exit_status) {
using enum ExitStatus;
switch (exit_status) {
case SUCCESS:
return "success";
case CALLBACK_REQUESTED_STOP:
return "callback requested stop";
case TOO_FEW_DOFS:
return "too few degrees of freedom";
case LOCALLY_INFEASIBLE:
return "locally infeasible";
case GLOBALLY_INFEASIBLE:
return "globally infeasible";
case FACTORIZATION_FAILED:
return "factorization failed";
case LINE_SEARCH_FAILED:
return "line search failed";
case NONFINITE_INITIAL_COST_OR_CONSTRAINTS:
return "nonfinite initial cost or constraints";
case DIVERGING_ITERATES:
return "diverging iterates";
case MAX_ITERATIONS_EXCEEDED:
return "max iterations exceeded";
case TIMEOUT:
return "timeout";
default:
return "unknown";
}
}
} // namespace slp
/// Formatter for ExitStatus.
template <>
struct fmt::formatter<slp::ExitStatus> {
/// Parse format string.
///
/// @param ctx Format parse context.
/// @return Format parse context iterator.
constexpr auto parse(fmt::format_parse_context& ctx) {
return m_underlying.parse(ctx);
}
/// Format ExitStatus.
///
/// @tparam FmtContext Format context type.
/// @param exit_status Exit status.
/// @param ctx Format context.
/// @return Format context iterator.
template <typename FmtContext>
auto format(const slp::ExitStatus& exit_status, FmtContext& ctx) const {
using enum slp::ExitStatus;
switch (exit_status) {
case SUCCESS:
return m_underlying.format("success", ctx);
case CALLBACK_REQUESTED_STOP:
return m_underlying.format("callback requested stop", ctx);
case TOO_FEW_DOFS:
return m_underlying.format("too few degrees of freedom", ctx);
case LOCALLY_INFEASIBLE:
return m_underlying.format("locally infeasible", ctx);
case GLOBALLY_INFEASIBLE:
return m_underlying.format("globally infeasible", ctx);
case FACTORIZATION_FAILED:
return m_underlying.format("factorization failed", ctx);
case LINE_SEARCH_FAILED:
return m_underlying.format("line search failed", ctx);
case NONFINITE_INITIAL_COST_OR_CONSTRAINTS:
return m_underlying.format("nonfinite initial cost or constraints",
ctx);
case DIVERGING_ITERATES:
return m_underlying.format("diverging iterates", ctx);
case MAX_ITERATIONS_EXCEEDED:
return m_underlying.format("max iterations exceeded", ctx);
case TIMEOUT:
return m_underlying.format("timeout", ctx);
default:
slp::unreachable();
}
}
private:
fmt::formatter<const char*> m_underlying;
};

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

@@ -37,30 +37,28 @@
namespace slp {
/**
Finds the optimal solution to a nonlinear program using the interior-point
method.
A nonlinear program has the form:
@verbatim
min_x f(x)
subject to cₑ(x) = 0
cᵢ(x) ≥ 0
@endverbatim
where f(x) is the cost function, cₑ(x) are the equality constraints, and cᵢ(x)
are the inequality constraints.
@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.
@param[in] options Solver options.
@param[in,out] x The initial guess and output location for the decision
variables.
@return The exit status.
*/
/// Finds the optimal solution to a nonlinear program using the interior-point
/// method.
///
/// A nonlinear program has the form:
///
/// ```
/// min_x f(x)
/// subject to cₑ(x) = 0
/// 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.
/// @param[in] options Solver options.
/// @param[in,out] x The initial guess and output location for the decision
/// variables.
/// @return The exit status.
template <typename Scalar>
ExitStatus interior_point(
const InteriorPointMatrixCallbacks<Scalar>& matrix_callbacks,
@@ -71,18 +69,20 @@ ExitStatus interior_point(
const Eigen::ArrayX<bool>& bound_constraint_mask,
#endif
Eigen::Vector<Scalar, Eigen::Dynamic>& x) {
/**
* Interior-point method step direction.
*/
using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
using SparseMatrix = Eigen::SparseMatrix<Scalar>;
using SparseVector = Eigen::SparseVector<Scalar>;
/// Interior-point method step direction.
struct Step {
/// Primal step.
Eigen::Vector<Scalar, Eigen::Dynamic> p_x;
DenseVector p_x;
/// Equality constraint dual step.
Eigen::Vector<Scalar, Eigen::Dynamic> p_y;
DenseVector p_y;
/// Inequality constraint slack variable step.
Eigen::Vector<Scalar, Eigen::Dynamic> p_s;
DenseVector p_s;
/// Inequality constraint dual step.
Eigen::Vector<Scalar, Eigen::Dynamic> p_z;
DenseVector p_z;
};
using std::isfinite;
@@ -132,39 +132,32 @@ ExitStatus interior_point(
auto& A_i_prof = solve_profilers[17];
InteriorPointMatrixCallbacks<Scalar> matrices{
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x) -> Scalar {
[&](const DenseVector& x) -> Scalar {
ScopedProfiler prof{f_prof};
return matrix_callbacks.f(x);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseVector<Scalar> {
[&](const DenseVector& x) -> SparseVector {
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> {
[&](const DenseVector& x, const DenseVector& y,
const DenseVector& z) -> SparseMatrix {
ScopedProfiler prof{H_prof};
return matrix_callbacks.H(x, y, z);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::Vector<Scalar, Eigen::Dynamic> {
[&](const DenseVector& x) -> DenseVector {
ScopedProfiler prof{c_e_prof};
return matrix_callbacks.c_e(x);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseMatrix<Scalar> {
[&](const DenseVector& x) -> SparseMatrix {
ScopedProfiler prof{A_e_prof};
return matrix_callbacks.A_e(x);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::Vector<Scalar, Eigen::Dynamic> {
[&](const DenseVector& x) -> DenseVector {
ScopedProfiler prof{c_i_prof};
return matrix_callbacks.c_i(x);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseMatrix<Scalar> {
[&](const DenseVector& x) -> SparseMatrix {
ScopedProfiler prof{A_i_prof};
return matrix_callbacks.A_i(x);
}};
@@ -176,8 +169,8 @@ ExitStatus interior_point(
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);
DenseVector c_e = matrices.c_e(x);
DenseVector c_i = matrices.c_i(x);
int num_decision_variables = x.rows();
int num_equality_constraints = c_e.rows();
@@ -192,22 +185,19 @@ ExitStatus interior_point(
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);
SparseVector g = matrices.g(x);
SparseMatrix A_e = matrices.A_e(x);
SparseMatrix A_i = matrices.A_i(x);
Eigen::Vector<Scalar, Eigen::Dynamic> s =
Eigen::Vector<Scalar, Eigen::Dynamic>::Ones(num_inequality_constraints);
DenseVector s = DenseVector::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);
DenseVector y = DenseVector::Zero(num_equality_constraints);
DenseVector z = DenseVector::Ones(num_inequality_constraints);
Eigen::SparseMatrix<Scalar> H = matrices.H(x, y, z);
SparseMatrix H = matrices.H(x, y, z);
// Ensure matrix callback dimensions are consistent
slp_assert(g.rows() == num_decision_variables);
@@ -340,38 +330,33 @@ ExitStatus interior_point(
// S = diag(s)
// Z = diag(z)
// Σ = S⁻¹Z
const Eigen::SparseMatrix<Scalar> Σ{s.cwiseInverse().asDiagonal() *
z.asDiagonal()};
const SparseMatrix Σ{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 =
const SparseMatrix 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) {
for (typename SparseMatrix::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) {
for (typename SparseMatrix::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);
SparseMatrix 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()};
DenseVector rhs{x.rows() + y.rows()};
rhs.segment(0, x.rows()) =
-g + A_e.transpose() * y +
A_i.transpose() * (-Σ * c_i + μ * s.cwiseInverse() + z);
@@ -399,7 +384,7 @@ ExitStatus interior_point(
auto compute_step = [&](Step& step) {
// p = [ pˣ]
// [−pʸ]
Eigen::Vector<Scalar, Eigen::Dynamic> p = solver.solve(rhs);
DenseVector p = solver.solve(rhs);
step.p_x = p.segment(0, x.rows());
step.p_y = -p.segment(x.rows(), y.rows());
@@ -427,13 +412,13 @@ ExitStatus interior_point(
// 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;
DenseVector trial_x = x + α * step.p_x;
DenseVector trial_y = y + α_z * step.p_y;
DenseVector 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);
DenseVector trial_c_e = matrices.c_e(trial_x);
DenseVector 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
@@ -448,7 +433,7 @@ ExitStatus interior_point(
continue;
}
Eigen::Vector<Scalar, Eigen::Dynamic> trial_s;
DenseVector trial_s;
if (options.feasible_ipm && c_i.cwiseGreater(Scalar(0)).all()) {
// If the inequality constraints are all feasible, prevent them from
// becoming infeasible again.
@@ -483,7 +468,7 @@ ExitStatus interior_point(
Scalar α_soc = α;
Scalar α_z_soc = α_z;
Eigen::Vector<Scalar, Eigen::Dynamic> c_e_soc = c_e;
DenseVector c_e_soc = c_e;
bool step_acceptable = false;
for (int soc_iteration = 0; soc_iteration < 5 && !step_acceptable;

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

@@ -9,13 +9,18 @@
namespace slp {
/**
* Matrix callbacks for the interior-point method solver.
*
* @tparam Scalar Scalar type.
*/
/// Matrix callbacks for the interior-point method solver.
///
/// @tparam Scalar Scalar type.
template <typename Scalar>
struct InteriorPointMatrixCallbacks {
/// Type alias for dense vector.
using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
/// Type alias for sparse matrix.
using SparseMatrix = Eigen::SparseMatrix<Scalar>;
/// Type alias for sparse vector.
using SparseVector = Eigen::SparseVector<Scalar>;
/// Cost function value f(x) getter.
///
/// <table>
@@ -35,7 +40,7 @@ struct InteriorPointMatrixCallbacks {
/// <td>1</td>
/// </tr>
/// </table>
std::function<Scalar(const Eigen::Vector<Scalar, Eigen::Dynamic>& x)> f;
std::function<Scalar(const DenseVector& x)> f;
/// Cost function gradient ∇f(x) getter.
///
@@ -56,9 +61,7 @@ struct InteriorPointMatrixCallbacks {
/// <td>1</td>
/// </tr>
/// </table>
std::function<Eigen::SparseVector<Scalar>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& x)>
g;
std::function<SparseVector(const DenseVector& x)> g;
/// Lagrangian Hessian ∇ₓₓ²L(x, y, z) getter.
///
@@ -91,10 +94,8 @@ struct InteriorPointMatrixCallbacks {
/// <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)>
std::function<SparseMatrix(const DenseVector& x, const DenseVector& y,
const DenseVector& z)>
H;
/// Equality constraint value cₑ(x) getter.
@@ -116,18 +117,16 @@ struct InteriorPointMatrixCallbacks {
/// <td>1</td>
/// </tr>
/// </table>
std::function<Eigen::Vector<Scalar, Eigen::Dynamic>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& x)>
c_e;
std::function<DenseVector(const DenseVector& x)> c_e;
/// Equality constraint Jacobian ∂cₑ/∂x getter.
///
/// @verbatim
/// ```
/// [∇ᵀcₑ₁(xₖ)]
/// Aₑ(x) = [∇ᵀcₑ₂(xₖ)]
/// [ ⋮ ]
/// [∇ᵀcₑₘ(xₖ)]
/// @endverbatim
/// ```
///
/// <table>
/// <tr>
@@ -146,9 +145,7 @@ struct InteriorPointMatrixCallbacks {
/// <td>num_decision_variables</td>
/// </tr>
/// </table>
std::function<Eigen::SparseMatrix<Scalar>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& x)>
A_e;
std::function<SparseMatrix(const DenseVector& x)> A_e;
/// Inequality constraint value cᵢ(x) getter.
///
@@ -169,18 +166,16 @@ struct InteriorPointMatrixCallbacks {
/// <td>1</td>
/// </tr>
/// </table>
std::function<Eigen::Vector<Scalar, Eigen::Dynamic>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& x)>
c_i;
std::function<DenseVector(const DenseVector& x)> c_i;
/// Inequality constraint Jacobian ∂cᵢ/∂x getter.
///
/// @verbatim
/// ```
/// [∇ᵀcᵢ₁(xₖ)]
/// Aᵢ(x) = [∇ᵀcᵢ₂(xₖ)]
/// [ ⋮ ]
/// [∇ᵀcᵢₘ(xₖ)]
/// @endverbatim
/// ```
///
/// <table>
/// <tr>
@@ -199,9 +194,7 @@ struct InteriorPointMatrixCallbacks {
/// <td>num_decision_variables</td>
/// </tr>
/// </table>
std::function<Eigen::SparseMatrix<Scalar>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& x)>
A_i;
std::function<SparseMatrix(const DenseVector& x)> A_i;
};
} // namespace slp

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

@@ -7,11 +7,9 @@
namespace slp {
/**
* Solver iteration information exposed to an iteration callback.
*
* @tparam Scalar Scalar type.
*/
/// Solver iteration information exposed to an iteration callback.
///
/// @tparam Scalar Scalar type.
template <typename Scalar>
struct IterationInfo {
/// The solver iteration.

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

@@ -31,32 +31,34 @@
namespace slp {
/**
Finds the optimal solution to a nonlinear program using Newton's method.
A nonlinear program has the form:
@verbatim
min_x f(x)
@endverbatim
where f(x) is the cost function.
@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.
@param[in] options Solver options.
@param[in,out] x The initial guess and output location for the decision
variables.
@return The exit status.
*/
/// Finds the optimal solution to a nonlinear program using Newton's method.
///
/// A nonlinear program has the form:
///
/// ```
/// min_x f(x)
/// ```
///
/// 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.
/// @param[in] options Solver options.
/// @param[in,out] x The initial guess and output location for the decision
/// variables.
/// @return The exit status.
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 DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
using SparseMatrix = Eigen::SparseMatrix<Scalar>;
using SparseVector = Eigen::SparseVector<Scalar>;
using std::isfinite;
const auto solve_start_time = std::chrono::steady_clock::now();
@@ -92,17 +94,15 @@ ExitStatus newton(
auto& H_prof = solve_profilers[11];
NewtonMatrixCallbacks<Scalar> matrices{
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x) -> Scalar {
[&](const DenseVector& x) -> Scalar {
ScopedProfiler prof{f_prof};
return matrix_callbacks.f(x);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseVector<Scalar> {
[&](const DenseVector& x) -> SparseVector {
ScopedProfiler prof{g_prof};
return matrix_callbacks.g(x);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseMatrix<Scalar> {
[&](const DenseVector& x) -> SparseMatrix {
ScopedProfiler prof{H_prof};
return matrix_callbacks.H(x);
}};
@@ -117,8 +117,8 @@ ExitStatus newton(
int num_decision_variables = x.rows();
Eigen::SparseVector<Scalar> g = matrices.g(x);
Eigen::SparseMatrix<Scalar> H = matrices.H(x);
SparseVector g = matrices.g(x);
SparseMatrix H = matrices.H(x);
// Ensure matrix callback dimensions are consistent
slp_assert(g.rows() == num_decision_variables);
@@ -170,8 +170,7 @@ ExitStatus newton(
// Call iteration callbacks
for (const auto& callback : iteration_callbacks) {
if (callback({iterations, x, g, H, Eigen::SparseMatrix<Scalar>{},
Eigen::SparseMatrix<Scalar>{}})) {
if (callback({iterations, x, g, H, SparseMatrix{}, SparseMatrix{}})) {
return ExitStatus::CALLBACK_REQUESTED_STOP;
}
}
@@ -187,7 +186,7 @@ ExitStatus newton(
kkt_matrix_decomp_profiler.stop();
ScopedProfiler kkt_system_solve_profiler{kkt_system_solve_prof};
Eigen::Vector<Scalar, Eigen::Dynamic> p_x = solver.solve(-g);
DenseVector p_x = solver.solve(-g);
kkt_system_solve_profiler.stop();
ScopedProfiler line_search_profiler{line_search_prof};
@@ -198,7 +197,7 @@ ExitStatus newton(
// 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;
DenseVector trial_x = x + α * p_x;
Scalar trial_f = matrices.f(trial_x);
@@ -227,7 +226,7 @@ ExitStatus newton(
if (α < α_min) {
Scalar current_kkt_error = kkt_error<Scalar>(g);
Eigen::Vector<Scalar, Eigen::Dynamic> trial_x = x + α_max * p_x;
DenseVector trial_x = x + α_max * p_x;
Scalar next_kkt_error = kkt_error<Scalar>(matrices.g(trial_x));

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

@@ -9,13 +9,18 @@
namespace slp {
/**
* Matrix callbacks for the Newton's method solver.
*
* @tparam Scalar Scalar type.
*/
/// Matrix callbacks for the Newton's method solver.
///
/// @tparam Scalar Scalar type.
template <typename Scalar>
struct NewtonMatrixCallbacks {
/// Type alias for dense vector.
using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
/// Type alias for sparse matrix.
using SparseMatrix = Eigen::SparseMatrix<Scalar>;
/// Type alias for sparse vector.
using SparseVector = Eigen::SparseVector<Scalar>;
/// Cost function value f(x) getter.
///
/// <table>
@@ -35,7 +40,7 @@ struct NewtonMatrixCallbacks {
/// <td>1</td>
/// </tr>
/// </table>
std::function<Scalar(const Eigen::Vector<Scalar, Eigen::Dynamic>& x)> f;
std::function<Scalar(const DenseVector& x)> f;
/// Cost function gradient ∇f(x) getter.
///
@@ -56,9 +61,7 @@ struct NewtonMatrixCallbacks {
/// <td>1</td>
/// </tr>
/// </table>
std::function<Eigen::SparseVector<Scalar>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& x)>
g;
std::function<SparseVector(const DenseVector& x)> g;
/// Lagrangian Hessian ∇ₓₓ²L(x) getter.
///
@@ -81,9 +84,7 @@ struct NewtonMatrixCallbacks {
/// <td>num_decision_variables</td>
/// </tr>
/// </table>
std::function<Eigen::SparseMatrix<Scalar>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& x)>
H;
std::function<SparseMatrix(const DenseVector& x)> H;
};
} // namespace slp

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

@@ -9,9 +9,7 @@
namespace slp {
/**
* Solver options.
*/
/// Solver options.
struct SLEIPNIR_DLLEXPORT Options {
/// The solver will stop once the error is below this tolerance.
double tolerance = 1e-8;

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

@@ -32,42 +32,42 @@
namespace slp {
/**
Finds the optimal solution to a nonlinear program using Sequential Quadratic
Programming (SQP).
A nonlinear program has the form:
@verbatim
min_x f(x)
subject to cₑ(x) = 0
@endverbatim
where f(x) is the cost function and cₑ(x) are the equality constraints.
@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.
@param[in] options Solver options.
@param[in,out] x The initial guess and output location for the decision
variables.
@return The exit status.
*/
/// Finds the optimal solution to a nonlinear program using Sequential Quadratic
/// Programming (SQP).
///
/// A nonlinear program has the form:
///
/// ```
/// min_x f(x)
/// 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.
/// @param[in] options Solver options.
/// @param[in,out] x The initial guess and output location for the decision
/// variables.
/// @return The exit status.
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.
*/
using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
using SparseMatrix = Eigen::SparseMatrix<Scalar>;
using SparseVector = Eigen::SparseVector<Scalar>;
/// SQP step direction.
struct Step {
/// Primal step.
Eigen::Vector<Scalar, Eigen::Dynamic> p_x;
DenseVector p_x;
/// Dual step.
Eigen::Vector<Scalar, Eigen::Dynamic> p_y;
DenseVector p_y;
};
using std::isfinite;
@@ -113,28 +113,23 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
auto& A_e_prof = solve_profilers[15];
SQPMatrixCallbacks<Scalar> matrices{
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x) -> Scalar {
[&](const DenseVector& x) -> Scalar {
ScopedProfiler prof{f_prof};
return matrix_callbacks.f(x);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseVector<Scalar> {
[&](const DenseVector& x) -> SparseVector {
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> {
[&](const DenseVector& x, const DenseVector& y) -> SparseMatrix {
ScopedProfiler prof{H_prof};
return matrix_callbacks.H(x, y);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::Vector<Scalar, Eigen::Dynamic> {
[&](const DenseVector& x) -> DenseVector {
ScopedProfiler prof{c_e_prof};
return matrix_callbacks.c_e(x);
},
[&](const Eigen::Vector<Scalar, Eigen::Dynamic>& x)
-> Eigen::SparseMatrix<Scalar> {
[&](const DenseVector& x) -> SparseMatrix {
ScopedProfiler prof{A_e_prof};
return matrix_callbacks.A_e(x);
}};
@@ -146,7 +141,7 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
setup_prof.start();
Scalar f = matrices.f(x);
Eigen::Vector<Scalar, Eigen::Dynamic> c_e = matrices.c_e(x);
DenseVector c_e = matrices.c_e(x);
int num_decision_variables = x.rows();
int num_equality_constraints = c_e.rows();
@@ -160,13 +155,12 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
return ExitStatus::TOO_FEW_DOFS;
}
Eigen::SparseVector<Scalar> g = matrices.g(x);
Eigen::SparseMatrix<Scalar> A_e = matrices.A_e(x);
SparseVector g = matrices.g(x);
SparseMatrix A_e = matrices.A_e(x);
Eigen::Vector<Scalar, Eigen::Dynamic> y =
Eigen::Vector<Scalar, Eigen::Dynamic>::Zero(num_equality_constraints);
DenseVector y = DenseVector::Zero(num_equality_constraints);
Eigen::SparseMatrix<Scalar> H = matrices.H(x, y);
SparseMatrix H = matrices.H(x, y);
// Ensure matrix callback dimensions are consistent
slp_assert(g.rows() == num_decision_variables);
@@ -235,7 +229,7 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
// Call iteration callbacks
for (const auto& callback : iteration_callbacks) {
if (callback({iterations, x, g, H, A_e, Eigen::SparseMatrix<Scalar>{}})) {
if (callback({iterations, x, g, H, A_e, SparseMatrix{}})) {
return ExitStatus::CALLBACK_REQUESTED_STOP;
}
}
@@ -251,24 +245,21 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
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) {
for (typename SparseMatrix::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) {
for (typename SparseMatrix::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);
SparseMatrix 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()};
DenseVector rhs{x.rows() + y.rows()};
rhs.segment(0, x.rows()) = -g + A_e.transpose() * y;
rhs.segment(x.rows(), y.rows()) = -c_e;
@@ -293,7 +284,7 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
auto compute_step = [&](Step& step) {
// p = [ pˣ]
// [−pʸ]
Eigen::Vector<Scalar, Eigen::Dynamic> p = solver.solve(rhs);
DenseVector p = solver.solve(rhs);
step.p_x = p.segment(0, x.rows());
step.p_y = -p.segment(x.rows(), y.rows());
};
@@ -306,11 +297,11 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
// 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;
DenseVector trial_x = x + α * step.p_x;
DenseVector 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);
DenseVector trial_c_e = matrices.c_e(trial_x);
// If f(xₖ + αpₖˣ) or cₑ(xₖ + αpₖˣ) aren't finite, reduce step size
// immediately
@@ -343,7 +334,7 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
auto soc_step = step;
Scalar α_soc = α;
Eigen::Vector<Scalar, Eigen::Dynamic> c_e_soc = c_e;
DenseVector c_e_soc = c_e;
bool step_acceptable = false;
for (int soc_iteration = 0; soc_iteration < 5 && !step_acceptable;

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

@@ -9,13 +9,18 @@
namespace slp {
/**
* Matrix callbacks for the Sequential Quadratic Programming (SQP) solver.
*
* @tparam Scalar Scalar type.
*/
/// Matrix callbacks for the Sequential Quadratic Programming (SQP) solver.
///
/// @tparam Scalar Scalar type.
template <typename Scalar>
struct SQPMatrixCallbacks {
/// Type alias for dense vector.
using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
/// Type alias for sparse matrix.
using SparseMatrix = Eigen::SparseMatrix<Scalar>;
/// Type alias for sparse vector.
using SparseVector = Eigen::SparseVector<Scalar>;
/// Cost function value f(x) getter.
///
/// <table>
@@ -35,7 +40,7 @@ struct SQPMatrixCallbacks {
/// <td>1</td>
/// </tr>
/// </table>
std::function<Scalar(const Eigen::Vector<Scalar, Eigen::Dynamic>& x)> f;
std::function<Scalar(const DenseVector& x)> f;
/// Cost function gradient ∇f(x) getter.
///
@@ -56,9 +61,7 @@ struct SQPMatrixCallbacks {
/// <td>1</td>
/// </tr>
/// </table>
std::function<Eigen::SparseVector<Scalar>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& x)>
g;
std::function<SparseVector(const DenseVector& x)> g;
/// Lagrangian Hessian ∇ₓₓ²L(x, y) getter.
///
@@ -86,10 +89,7 @@ struct SQPMatrixCallbacks {
/// <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;
std::function<SparseMatrix(const DenseVector& x, const DenseVector& y)> H;
/// Equality constraint value cₑ(x) getter.
///
@@ -110,18 +110,16 @@ struct SQPMatrixCallbacks {
/// <td>1</td>
/// </tr>
/// </table>
std::function<Eigen::Vector<Scalar, Eigen::Dynamic>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& x)>
c_e;
std::function<DenseVector(const DenseVector& x)> c_e;
/// Equality constraint Jacobian ∂cₑ/∂x getter.
///
/// @verbatim
/// ```
/// [∇ᵀcₑ₁(xₖ)]
/// Aₑ(x) = [∇ᵀcₑ₂(xₖ)]
/// [ ⋮ ]
/// [∇ᵀcₑₘ(xₖ)]
/// @endverbatim
/// ```
///
/// <table>
/// <tr>
@@ -140,9 +138,7 @@ struct SQPMatrixCallbacks {
/// <td>num_decision_variables</td>
/// </tr>
/// </table>
std::function<Eigen::SparseMatrix<Scalar>(
const Eigen::Vector<Scalar, Eigen::Dynamic>& x)>
A_e;
std::function<SparseMatrix(const DenseVector& x)> A_e;
};
} // namespace slp

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

@@ -19,11 +19,9 @@
namespace slp {
/**
* Bound constraint metadata.
*
* @tparam Scalar Scalar type.
*/
/// Bound constraint metadata.
///
/// @tparam Scalar Scalar type.
template <typename Scalar>
struct Bounds {
/// Which constraints, if any, are bound constraints.
@@ -38,23 +36,21 @@ struct Bounds {
conflicting_bound_indices;
};
/**
* A "bound constraint" is any linear constraint in one scalar variable.
*
* Computes which constraints, if any, are bound constraints, the tightest
* bounds on each decision variable, and whether or not they're feasible (given
* previously encountered bounds),
*
* @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
* cᵢ(decision_variables) ≥ 0.
* @param A_i The Jacobian of inequality_constraints wrt decision_variables,
* evaluated anywhere, in *row-major* storage; in practice, since we typically
* store Jacobians column-major, the user of this function must perform a
* transpose.
*/
/// A "bound constraint" is any linear constraint in one scalar variable.
///
/// Computes which constraints, if any, are bound constraints, the tightest
/// bounds on each decision variable, and whether or not they're feasible (given
/// previously encountered bounds),
///
/// @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
/// cᵢ(decision_variables) ≥ 0.
/// @param A_i The Jacobian of inequality_constraints wrt decision_variables,
/// evaluated anywhere, in *row-major* storage; in practice, since we
/// typically store Jacobians column-major, the user of this function must
/// perform a transpose.
template <typename Scalar>
Bounds<Scalar> get_bounds(
std::span<Variable<Scalar>> decision_variables,
@@ -182,20 +178,18 @@ Bounds<Scalar> get_bounds(
conflicting_bound_indices};
}
/**
* Projects the decision variables onto the given bounds, while ensuring some
* configurable distance from the boundary if possible. This is designed to
* match the algorithm given in section 3.6 of [2].
*
* @param x A vector of decision variables.
* @param decision_variable_indices_to_bounds An array of bounds (stored [lower,
* upper]) for each decision variable in x.
* @param κ_1 A constant controlling distance from the lower or upper bound when
* the difference between the upper and lower bound is small.
* @param κ_2 A constant controlling distance from the lower or upper bound when
* the difference between the upper and lower bound is large (including when
* one of the bounds is ±∞).
*/
/// Projects the decision variables onto the given bounds, while ensuring some
/// configurable distance from the boundary if possible. This is designed to
/// match the algorithm given in section 3.6 of [2].
///
/// @param x A vector of decision variables.
/// @param decision_variable_indices_to_bounds An array of bounds (stored
/// [lower, upper]) for each decision variable in x.
/// @param κ_1 A constant controlling distance from the lower or upper bound
/// when the difference between the upper and lower bound is small.
/// @param κ_2 A constant controlling distance from the lower or upper bound
/// when the difference between the upper and lower bound is large
/// (including when one of the bounds is ±∞).
template <typename Derived>
requires(static_cast<bool>(Eigen::DenseBase<Derived>::IsVectorAtCompileTime))
void project_onto_bounds(

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

@@ -11,12 +11,10 @@
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.
*/
/// Returns the error estimate using the KKT conditions for Newton's method.
///
/// @tparam Scalar Scalar type.
/// @param g Gradient of the cost function ∇f.
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)
@@ -27,17 +25,15 @@ Scalar error_estimate(const Eigen::Vector<Scalar, Eigen::Dynamic>& g) {
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.
* @param c_e The problem's equality constraints cₑ(x) evaluated at the current
* iterate.
* @param y Equality constraint dual variables.
*/
/// 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.
/// @param c_e The problem's equality constraints cₑ(x) evaluated at the current
/// iterate.
/// @param y Equality constraint dual variables.
template <typename Scalar>
Scalar error_estimate(const Eigen::Vector<Scalar, Eigen::Dynamic>& g,
const Eigen::SparseMatrix<Scalar>& A_e,
@@ -65,25 +61,23 @@ Scalar error_estimate(const Eigen::Vector<Scalar, Eigen::Dynamic>& g,
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.
* @param c_e The problem's equality constraints cₑ(x) evaluated at the current
* iterate.
* @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at
* the current iterate.
* @param c_i The problem's inequality constraints cᵢ(x) evaluated at the
* current iterate.
* @param s Inequality constraint slack variables.
* @param y Equality constraint dual variables.
* @param z Inequality constraint dual variables.
* @param μ Barrier parameter.
*/
/// 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.
/// @param c_e The problem's equality constraints cₑ(x) evaluated at the current
/// iterate.
/// @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at
/// the current iterate.
/// @param c_i The problem's inequality constraints cᵢ(x) evaluated at the
/// current iterate.
/// @param s Inequality constraint slack variables.
/// @param y Equality constraint dual variables.
/// @param z Inequality constraint dual variables.
/// @param μ Barrier parameter.
template <typename Scalar>
Scalar error_estimate(const Eigen::Vector<Scalar, Eigen::Dynamic>& g,
const Eigen::SparseMatrix<Scalar>& A_e,

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

@@ -14,13 +14,14 @@
namespace slp {
/**
* Filter entry consisting of cost and constraint violation.
*
* @tparam Scalar Scalar type.
*/
/// Filter entry consisting of cost and constraint violation.
///
/// @tparam Scalar Scalar type.
template <typename Scalar>
struct FilterEntry {
/// Type alias for dense vector.
using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
/// The cost function's value
Scalar cost{0};
@@ -29,73 +30,58 @@ struct FilterEntry {
constexpr FilterEntry() = default;
/**
* Constructs a FilterEntry.
*
* @param cost The cost function's value.
* @param constraint_violation The constraint violation.
*/
/// Constructs a FilterEntry.
///
/// @param cost The cost function's value.
/// @param constraint_violation The constraint violation.
constexpr FilterEntry(Scalar cost, Scalar constraint_violation)
: cost{cost}, constraint_violation{constraint_violation} {}
/**
* Constructs a Newton's method filter entry.
*
* @param f The cost function value.
*/
/// Constructs a Newton's method filter entry.
///
/// @param f The cost function value.
explicit FilterEntry(Scalar f) : FilterEntry{f, Scalar(0)} {}
/**
* Constructs a Sequential Quadratic Programming filter entry.
*
* @param f The cost function value.
* @param c_e The equality constraint values (nonzero means violation).
*/
FilterEntry(Scalar f, const Eigen::Vector<Scalar, Eigen::Dynamic>& c_e)
/// Constructs a Sequential Quadratic Programming filter entry.
///
/// @param f The cost function value.
/// @param c_e The equality constraint values (nonzero means violation).
FilterEntry(Scalar f, const DenseVector& c_e)
: FilterEntry{f, c_e.template lpNorm<1>()} {}
/**
* Constructs an interior-point method filter entry.
*
* @param f The cost function value.
* @param s The inequality constraint slack variables.
* @param c_e The equality constraint values (nonzero means violation).
* @param c_i The inequality constraint values (negative means violation).
* @param μ The barrier parameter.
*/
FilterEntry(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 μ)
/// Constructs an interior-point method filter entry.
///
/// @param f The cost function value.
/// @param s The inequality constraint slack variables.
/// @param c_e The equality constraint values (nonzero means violation).
/// @param c_i The inequality constraint values (negative means violation).
/// @param μ The barrier parameter.
FilterEntry(Scalar f, DenseVector& s, const DenseVector& c_e,
const DenseVector& c_i, Scalar μ)
: FilterEntry{f - μ * s.array().log().sum(),
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.
*/
/// Step filter.
///
/// See the section on filters in chapter 15 of [1].
///
/// @tparam Scalar Scalar type.
template <typename Scalar>
class Filter {
public:
/// The maximum constraint violation
Scalar max_constraint_violation{1e4};
/**
* Constructs an empty filter.
*/
/// Constructs an empty filter.
Filter() {
// Initial filter entry rejects constraint violations above max
m_filter.emplace_back(std::numeric_limits<Scalar>::infinity(),
max_constraint_violation);
}
/**
* Resets the filter.
*/
/// Resets the filter.
void reset() {
m_filter.clear();
@@ -104,11 +90,9 @@ class Filter {
max_constraint_violation);
}
/**
* Adds a new entry to the filter.
*
* @param entry The entry to add to the filter.
*/
/// Adds a new entry to the filter.
///
/// @param entry The entry to add to the filter.
void add(const FilterEntry<Scalar>& entry) {
// Remove dominated entries
erase_if(m_filter, [&](const auto& elem) {
@@ -119,11 +103,9 @@ class Filter {
m_filter.push_back(entry);
}
/**
* Adds a new entry to the filter.
*
* @param entry The entry to add to the filter.
*/
/// Adds a new entry to the filter.
///
/// @param entry The entry to add to the filter.
void add(FilterEntry<Scalar>&& entry) {
// Remove dominated entries
erase_if(m_filter, [&](const auto& elem) {
@@ -134,13 +116,11 @@ class Filter {
m_filter.push_back(entry);
}
/**
* Returns true if the given iterate is accepted by the filter.
*
* @param entry The entry to attempt adding to the filter.
* @param α The step size (0, 1].
* @return True if the given iterate is accepted by the filter.
*/
/// Returns true if the given iterate is accepted by the filter.
///
/// @param entry The entry to attempt adding to the filter.
/// @param α The step size (0, 1].
/// @return True if the given iterate is accepted by the filter.
bool try_add(const FilterEntry<Scalar>& entry, Scalar α) {
if (is_acceptable(entry, α)) {
add(entry);
@@ -150,13 +130,11 @@ class Filter {
}
}
/**
* Returns true if the given iterate is accepted by the filter.
*
* @param entry The entry to attempt adding to the filter.
* @param α The step size (0, 1].
* @return True if the given iterate is accepted by the filter.
*/
/// Returns true if the given iterate is accepted by the filter.
///
/// @param entry The entry to attempt adding to the filter.
/// @param α The step size (0, 1].
/// @return True if the given iterate is accepted by the filter.
bool try_add(FilterEntry<Scalar>&& entry, Scalar α) {
if (is_acceptable(entry, α)) {
add(std::move(entry));
@@ -166,13 +144,11 @@ class Filter {
}
}
/**
* Returns true if the given entry is acceptable to the filter.
*
* @param entry The entry to check.
* @param α The step size (0, 1].
* @return True if the given entry is acceptable to the filter.
*/
/// Returns true if the given entry is acceptable to the 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<Scalar>& entry, Scalar α) {
using std::isfinite;
using std::pow;
@@ -194,11 +170,9 @@ class Filter {
});
}
/**
* Returns the most recently added filter entry.
*
* @return The most recently added filter entry.
*/
/// Returns the most recently added filter entry.
///
/// @return The most recently added filter entry.
const FilterEntry<Scalar>& back() const { return m_filter.back(); }
private:

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

@@ -8,21 +8,18 @@
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].
*/
/// 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].
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 τ) {
const Eigen::Vector<Scalar, Eigen::Dynamic>& x,
const Eigen::Vector<Scalar, Eigen::Dynamic>& p, Scalar τ) {
// α = max(α ∈ (0, 1] : x + αp ≥ (1 − τ)x)
//
// where x and τ are positive.

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

@@ -6,10 +6,8 @@
namespace slp {
/**
* Represents the inertia of a matrix (the number of positive, negative, and
* zero eigenvalues).
*/
/// Represents the inertia of a matrix (the number of positive, negative, and
/// zero eigenvalues).
class Inertia {
public:
/// The number of positive eigenvalues.
@@ -21,24 +19,20 @@ class Inertia {
constexpr Inertia() = default;
/**
* Constructs Inertia with the given number of positive, negative, and zero
* eigenvalues.
*
* @param positive The number of positive eigenvalues.
* @param negative The number of negative eigenvalues.
* @param zero The number of zero eigenvalues.
*/
/// Constructs Inertia with the given number of positive, negative, and zero
/// eigenvalues.
///
/// @param positive The number of positive eigenvalues.
/// @param negative The number of negative eigenvalues.
/// @param zero The number of zero eigenvalues.
constexpr Inertia(int positive, int negative, int zero)
: positive{positive}, negative{negative}, zero{zero} {}
/**
* Constructs Inertia from the D matrix of an LDLT decomposition
* (see https://en.wikipedia.org/wiki/Sylvester's_law_of_inertia).
*
* @tparam Scalar Scalar type.
* @param D The D matrix of an LDLT decomposition in vector form.
*/
/// 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::Vector<Scalar, Eigen::Dynamic>& D) {
for (const auto& elem : D) {
@@ -52,13 +46,11 @@ 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.
*/
/// 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<
@@ -74,11 +66,9 @@ class Inertia {
}
}
/**
* Inertia equality operator.
*
* @return True if Inertia is equal.
*/
/// Inertia equality operator.
///
/// @return True if Inertia is equal.
bool operator==(const Inertia&) const = default;
};

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

@@ -9,15 +9,13 @@
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.
*/
/// 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.
template <typename Scalar>
bool is_equality_locally_infeasible(
const Eigen::SparseMatrix<Scalar>& A_e,
@@ -32,15 +30,13 @@ bool is_equality_locally_infeasible(
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.
*/
/// 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.
template <typename Scalar>
bool is_inequality_locally_infeasible(
const Eigen::SparseMatrix<Scalar>& A_i,

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

@@ -9,12 +9,10 @@
namespace slp {
/**
* Returns the KKT error for Newton's method.
*
* @tparam Scalar Scalar type.
* @param g Gradient of the cost function ∇f.
*/
/// Returns the KKT error for Newton's method.
///
/// @tparam Scalar Scalar type.
/// @param g Gradient of the cost function ∇f.
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
@@ -25,17 +23,15 @@ Scalar kkt_error(const Eigen::Vector<Scalar, Eigen::Dynamic>& g) {
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.
* @param c_e The problem's equality constraints cₑ(x) evaluated at the current
* iterate.
* @param y Equality constraint dual variables.
*/
/// 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.
/// @param c_e The problem's equality constraints cₑ(x) evaluated at the current
/// iterate.
/// @param y Equality constraint dual variables.
template <typename Scalar>
Scalar kkt_error(const Eigen::Vector<Scalar, Eigen::Dynamic>& g,
const Eigen::SparseMatrix<Scalar>& A_e,
@@ -51,24 +47,22 @@ Scalar kkt_error(const Eigen::Vector<Scalar, Eigen::Dynamic>& g,
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.
* @param c_e The problem's equality constraints cₑ(x) evaluated at the current
* iterate.
* @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at
* the current iterate.
* @param c_i The problem's inequality constraints cᵢ(x) evaluated at the
* current iterate.
* @param s Inequality constraint slack variables.
* @param y Equality constraint dual variables.
* @param z Inequality constraint dual variables.
* @param μ Barrier parameter.
*/
/// 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.
/// @param c_e The problem's equality constraints cₑ(x) evaluated at the current
/// iterate.
/// @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at
/// the current iterate.
/// @param c_i The problem's inequality constraints cᵢ(x) evaluated at the
/// current iterate.
/// @param s Inequality constraint slack variables.
/// @param y Equality constraint dual variables.
/// @param z Inequality constraint dual variables.
/// @param μ Barrier parameter.
template <typename Scalar>
Scalar kkt_error(const Eigen::Vector<Scalar, Eigen::Dynamic>& g,
const Eigen::SparseMatrix<Scalar>& A_e,

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

@@ -13,40 +13,39 @@
namespace slp {
/**
* Solves systems of linear equations using a regularized LDLT factorization.
*
* @tparam Scalar Scalar type.
*/
/// Solves systems of linear equations using a regularized LDLT factorization.
///
/// @tparam Scalar Scalar type.
template <typename Scalar>
class RegularizedLDLT {
public:
/**
* Constructs a RegularizedLDLT instance.
*
* @param num_decision_variables The number of decision variables in the
* system.
* @param num_equality_constraints The number of equality constraints in the
* system.
*/
/// Type alias for dense matrix.
using DenseMatrix = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
/// Type alias for dense vector.
using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
/// Type alias for sparse matrix.
using SparseMatrix = Eigen::SparseMatrix<Scalar>;
/// Constructs a RegularizedLDLT instance.
///
/// @param num_decision_variables The number of decision variables in the
/// system.
/// @param num_equality_constraints The number of equality constraints in the
/// system.
RegularizedLDLT(int num_decision_variables, int num_equality_constraints)
: m_num_decision_variables{num_decision_variables},
m_num_equality_constraints{num_equality_constraints} {}
/**
* Reports whether previous computation was successful.
*
* @return Whether previous computation was successful.
*/
/// Reports whether previous computation was successful.
///
/// @return Whether previous computation was successful.
Eigen::ComputationInfo info() const { return m_info; }
/**
* Computes the regularized LDLT factorization of a matrix.
*
* @param lhs Left-hand side of the system.
* @return The factorization.
*/
RegularizedLDLT& compute(const Eigen::SparseMatrix<Scalar>& lhs) {
/// Computes the regularized LDLT factorization of a matrix.
///
/// @param lhs Left-hand side of the system.
/// @return The factorization.
RegularizedLDLT& compute(const SparseMatrix& lhs) {
// The regularization procedure is based on algorithm B.1 of [1]
// Max density is 50% due to the caller only providing the lower triangle.
@@ -128,15 +127,12 @@ class RegularizedLDLT {
}
}
/**
* Solves the system of equations using a regularized LDLT factorization.
*
* @param rhs Right-hand side of the system.
* @return The solution.
*/
/// Solves the system of equations using a regularized LDLT factorization.
///
/// @param rhs Right-hand side of the system.
/// @return The solution.
template <typename Rhs>
Eigen::Vector<Scalar, Eigen::Dynamic> solve(
const Eigen::MatrixBase<Rhs>& rhs) {
DenseVector solve(const Eigen::MatrixBase<Rhs>& rhs) {
if (m_is_sparse) {
return m_sparse_solver.solve(rhs);
} else {
@@ -144,15 +140,12 @@ class RegularizedLDLT {
}
}
/**
* Solves the system of equations using a regularized LDLT factorization.
*
* @param rhs Right-hand side of the system.
* @return The solution.
*/
/// Solves the system of equations using a regularized LDLT factorization.
///
/// @param rhs Right-hand side of the system.
/// @return The solution.
template <typename Rhs>
Eigen::Vector<Scalar, Eigen::Dynamic> solve(
const Eigen::SparseMatrixBase<Rhs>& rhs) {
DenseVector solve(const Eigen::SparseMatrixBase<Rhs>& rhs) {
if (m_is_sparse) {
return m_sparse_solver.solve(rhs);
} else {
@@ -160,17 +153,14 @@ class RegularizedLDLT {
}
}
/**
* Returns the Hessian regularization factor.
*
* @return Hessian regularization factor.
*/
/// Returns the Hessian regularization factor.
///
/// @return Hessian regularization factor.
Scalar hessian_regularization() const { return m_prev_δ; }
private:
using SparseSolver = Eigen::SimplicialLDLT<Eigen::SparseMatrix<Scalar>>;
using DenseSolver =
Eigen::LDLT<Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>>;
using SparseSolver = Eigen::SimplicialLDLT<SparseMatrix>;
using DenseSolver = Eigen::LDLT<DenseMatrix>;
SparseSolver m_sparse_solver;
DenseSolver m_dense_solver;
@@ -194,13 +184,11 @@ class RegularizedLDLT {
// Number of non-zeros in LHS.
int m_non_zeros = -1;
/**
* Computes factorization of a sparse matrix.
*
* @param lhs Matrix to factorize.
* @return The factorization.
*/
SparseSolver& compute_sparse(const Eigen::SparseMatrix<Scalar>& lhs) {
/// Computes factorization of a sparse matrix.
///
/// @param lhs Matrix to factorize.
/// @return The factorization.
SparseSolver& compute_sparse(const SparseMatrix& lhs) {
// Reanalize lhs's sparsity pattern if it changed
int non_zeros = lhs.nonZeros();
if (m_non_zeros != non_zeros) {
@@ -213,21 +201,18 @@ class RegularizedLDLT {
return m_sparse_solver;
}
/**
* Returns regularization matrix.
*
* @param δ The Hessian regularization factor.
* @param γ The equality constraint Jacobian regularization factor.
* @return Regularization matrix.
*/
Eigen::SparseMatrix<Scalar> regularization(Scalar δ, Scalar γ) {
Eigen::Vector<Scalar, Eigen::Dynamic> vec{m_num_decision_variables +
m_num_equality_constraints};
/// Returns regularization matrix.
///
/// @param δ The Hessian regularization factor.
/// @param γ The equality constraint Jacobian regularization factor.
/// @return Regularization matrix.
SparseMatrix regularization(Scalar δ, Scalar γ) {
DenseVector vec{m_num_decision_variables + m_num_equality_constraints};
vec.segment(0, m_num_decision_variables).setConstant(δ);
vec.segment(m_num_decision_variables, m_num_equality_constraints)
.setConstant(-γ);
return Eigen::SparseMatrix<Scalar>{vec.asDiagonal()};
return SparseMatrix{vec.asDiagonal()};
}
};

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

@@ -3,15 +3,12 @@
#pragma once
#ifdef SLEIPNIR_PYTHON
#include <source_location>
#include <stdexcept>
#include <fmt/format.h>
/**
* Throw an exception in Python.
*/
/// Throw an exception in Python.
#define slp_assert(condition) \
do { \
if (!(condition)) { \
@@ -21,14 +18,9 @@
location.line(), location.function_name(), #condition)); \
} \
} while (0)
#else
#include <cassert>
/**
* Abort in C++.
*/
/// Abort in C++.
#define slp_assert(condition) assert(condition)
#endif

8
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/empty.hpp vendored
View File

@@ -4,14 +4,10 @@
namespace slp::detail {
/**
* Type tag used to designate an uninitialized VariableMatrix.
*/
/// Type tag used to designate an uninitialized VariableMatrix.
struct empty_t {};
/**
* Designates an uninitialized VariableMatrix.
*/
/// Designates an uninitialized VariableMatrix.
static constexpr empty_t empty{};
} // namespace slp::detail

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

@@ -12,29 +12,23 @@ namespace slp {
template <class F>
class function_ref;
/**
* An implementation of std::function_ref, a lightweight non-owning reference to
* a callable.
*/
/// An implementation of std::function_ref, a lightweight non-owning reference
/// to a callable.
template <class R, class... Args>
class function_ref<R(Args...)> {
public:
constexpr function_ref() noexcept = delete;
/**
* Creates a `function_ref` which refers to the same callable as `rhs`.
*
* @param rhs Other `function_ref`.
*/
/// Creates a `function_ref` which refers to the same callable as `rhs`.
///
/// @param rhs Other `function_ref`.
constexpr function_ref(const function_ref<R(Args...)>& rhs) noexcept =
default;
/**
* Constructs a `function_ref` referring to `f`.
*
* @tparam F Callable type.
* @param f Callable to which to refer.
*/
/// Constructs a `function_ref` referring to `f`.
///
/// @tparam F Callable type.
/// @param f Callable to which to refer.
template <typename F>
requires(!std::is_same_v<std::decay_t<F>, function_ref> &&
std::is_invocable_r_v<R, F &&, Args...>)
@@ -49,21 +43,17 @@ class function_ref<R(Args...)> {
};
}
/**
* Makes `*this` refer to the same callable as `rhs`.
*
* @param rhs Other `function_ref`.
* @return `*this`
*/
/// Makes `*this` refer to the same callable as `rhs`.
///
/// @param rhs Other `function_ref`.
/// @return `*this`
constexpr function_ref<R(Args...)>& operator=(
const function_ref<R(Args...)>& rhs) noexcept = default;
/**
* Makes `*this` refer to `f`.
*
* @param f Callable to which to refer.
* @return `*this`
*/
/// Makes `*this` refer to `f`.
///
/// @param f Callable to which to refer.
/// @return `*this`
template <typename F>
requires std::is_invocable_r_v<R, F&&, Args...>
constexpr function_ref<R(Args...)>& operator=(F&& f) noexcept {
@@ -77,22 +67,18 @@ class function_ref<R(Args...)> {
return *this;
}
/**
* Swaps the referred callables of `*this` and `rhs`.
*
* @param rhs Other `function_ref`.
*/
/// Swaps the referred callables of `*this` and `rhs`.
///
/// @param rhs Other `function_ref`.
constexpr void swap(function_ref<R(Args...)>& rhs) noexcept {
std::swap(obj_, rhs.obj_);
std::swap(callback_, rhs.callback_);
}
/**
* Call the stored callable with the given arguments.
*
* @param args The arguments.
* @return The return value of the callable.
*/
/// Call the stored callable with the given arguments.
///
/// @param args The arguments.
/// @return The return value of the callable.
R operator()(Args... args) const {
return callback_(obj_, std::forward<Args>(args)...);
}
@@ -102,9 +88,7 @@ class function_ref<R(Args...)> {
R (*callback_)(void*, Args...) = nullptr;
};
/**
* Swaps the referred callables of `lhs` and `rhs`.
*/
/// Swaps the referred callables of `lhs` and `rhs`.
template <typename R, typename... Args>
constexpr void swap(function_ref<R(Args...)>& lhs,
function_ref<R(Args...)>& rhs) noexcept {

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

@@ -9,45 +9,37 @@
namespace slp {
/**
* A custom intrusive shared pointer implementation without thread
* synchronization overhead.
*
* Types used with this class should have three things:
*
* 1. A zero-initialized public counter variable that serves as the shared
* pointer's reference count.
* 2. A free function `void inc_ref_count(T*)` that increments the reference
* count.
* 3. A free function `void dec_ref_count(T*)` that decrements the reference
* count and deallocates the pointed to object if the reference count reaches
* zero.
*
* @tparam T The type of the object to be reference counted.
*/
/// A custom intrusive shared pointer implementation without thread
/// synchronization overhead.
///
/// Types used with this class should have three things:
///
/// 1. A zero-initialized public counter variable that serves as the shared
/// pointer's reference count.
/// 2. A free function `void inc_ref_count(T*)` that increments the reference
/// count.
/// 3. A free function `void dec_ref_count(T*)` that decrements the reference
/// count and deallocates the pointed to object if the reference count
/// reaches zero.
///
/// @tparam T The type of the object to be reference counted.
template <typename T>
class IntrusiveSharedPtr {
public:
template <typename>
friend class IntrusiveSharedPtr;
/**
* Constructs an empty intrusive shared pointer.
*/
/// Constructs an empty intrusive shared pointer.
constexpr IntrusiveSharedPtr() noexcept = default;
/**
* Constructs an empty intrusive shared pointer.
*/
/// Constructs an empty intrusive shared pointer.
// NOLINTNEXTLINE (google-explicit-constructor)
constexpr IntrusiveSharedPtr(std::nullptr_t) noexcept {}
/**
* Constructs an intrusive shared pointer from the given pointer and takes
* ownership.
*
* @param ptr The pointer of which to take ownership.
*/
/// Constructs an intrusive shared pointer from the given pointer and takes
/// ownership.
///
/// @param ptr The pointer of which to take ownership.
explicit constexpr IntrusiveSharedPtr(T* ptr) noexcept : m_ptr{ptr} {
if (m_ptr != nullptr) {
inc_ref_count(m_ptr);
@@ -60,11 +52,9 @@ class IntrusiveSharedPtr {
}
}
/**
* Copy constructs from the given intrusive shared pointer.
*
* @param rhs The other intrusive shared pointer.
*/
/// Copy constructs from the given intrusive shared pointer.
///
/// @param rhs The other intrusive shared pointer.
constexpr IntrusiveSharedPtr(const IntrusiveSharedPtr<T>& rhs) noexcept
: m_ptr{rhs.m_ptr} {
if (m_ptr != nullptr) {
@@ -72,11 +62,9 @@ class IntrusiveSharedPtr {
}
}
/**
* Copy constructs from the given intrusive shared pointer.
*
* @param rhs The other intrusive shared pointer.
*/
/// Copy constructs from the given intrusive shared pointer.
///
/// @param rhs The other intrusive shared pointer.
template <typename U>
requires(!std::same_as<T, U> && std::convertible_to<U*, T*>)
// NOLINTNEXTLINE (google-explicit-constructor)
@@ -87,12 +75,10 @@ class IntrusiveSharedPtr {
}
}
/**
* Makes a copy of the given intrusive shared pointer.
*
* @param rhs The other intrusive shared pointer.
* @return This intrusive shared pointer.
*/
/// Makes a copy of the given intrusive shared pointer.
///
/// @param rhs The other intrusive shared pointer.
/// @return This intrusive shared pointer.
// NOLINTNEXTLINE (google-explicit-constructor)
constexpr IntrusiveSharedPtr<T>& operator=(
const IntrusiveSharedPtr<T>& rhs) noexcept {
@@ -113,12 +99,10 @@ class IntrusiveSharedPtr {
return *this;
}
/**
* Makes a copy of the given intrusive shared pointer.
*
* @param rhs The other intrusive shared pointer.
* @return This intrusive shared pointer.
*/
/// Makes a copy of the given intrusive shared pointer.
///
/// @param rhs The other intrusive shared pointer.
/// @return This intrusive shared pointer.
template <typename U>
requires(!std::same_as<T, U> && std::convertible_to<U*, T*>)
// NOLINTNEXTLINE (google-explicit-constructor)
@@ -141,31 +125,25 @@ class IntrusiveSharedPtr {
return *this;
}
/**
* Move constructs from the given intrusive shared pointer.
*
* @param rhs The other intrusive shared pointer.
*/
/// Move constructs from the given intrusive shared pointer.
///
/// @param rhs The other intrusive shared pointer.
constexpr IntrusiveSharedPtr(IntrusiveSharedPtr<T>&& rhs) noexcept
: m_ptr{std::exchange(rhs.m_ptr, nullptr)} {}
/**
* Move constructs from the given intrusive shared pointer.
*
* @param rhs The other intrusive shared pointer.
*/
/// Move constructs from the given intrusive shared pointer.
///
/// @param rhs The other intrusive shared pointer.
template <typename U>
requires(!std::same_as<T, U> && std::convertible_to<U*, T*>)
// NOLINTNEXTLINE (google-explicit-constructor)
constexpr IntrusiveSharedPtr(IntrusiveSharedPtr<U>&& rhs) noexcept
: m_ptr{std::exchange(rhs.m_ptr, nullptr)} {}
/**
* Move assigns from the given intrusive shared pointer.
*
* @param rhs The other intrusive shared pointer.
* @return This intrusive shared pointer.
*/
/// Move assigns from the given intrusive shared pointer.
///
/// @param rhs The other intrusive shared pointer.
/// @return This intrusive shared pointer.
constexpr IntrusiveSharedPtr<T>& operator=(
IntrusiveSharedPtr<T>&& rhs) noexcept {
if (m_ptr == rhs.m_ptr) {
@@ -177,12 +155,10 @@ class IntrusiveSharedPtr {
return *this;
}
/**
* Move assigns from the given intrusive shared pointer.
*
* @param rhs The other intrusive shared pointer.
* @return This intrusive shared pointer.
*/
/// Move assigns from the given intrusive shared pointer.
///
/// @param rhs The other intrusive shared pointer.
/// @return This intrusive shared pointer.
template <typename U>
requires(!std::same_as<T, U> && std::convertible_to<U*, T*>)
constexpr IntrusiveSharedPtr<T>& operator=(
@@ -196,95 +172,75 @@ class IntrusiveSharedPtr {
return *this;
}
/**
* Returns the internal pointer.
*
* @return The internal pointer.
*/
/// Returns the internal pointer.
///
/// @return The internal pointer.
constexpr T* get() const noexcept { return m_ptr; }
/**
* Returns the object pointed to by the internal pointer.
*
* @return The object pointed to by the internal pointer.
*/
/// Returns the object pointed to by the internal pointer.
///
/// @return The object pointed to by the internal pointer.
constexpr T& operator*() const noexcept { return *m_ptr; }
/**
* Returns the internal pointer.
*
* @return The internal pointer.
*/
/// Returns the internal pointer.
///
/// @return The internal pointer.
constexpr T* operator->() const noexcept { return m_ptr; }
/**
* Returns true if the internal pointer isn't nullptr.
*
* @return True if the internal pointer isn't nullptr.
*/
/// Returns true if the internal pointer isn't nullptr.
///
/// @return True if the internal pointer isn't nullptr.
explicit constexpr operator bool() const noexcept { return m_ptr != nullptr; }
/**
* Returns true if the given intrusive shared pointers point to the same
* object.
*
* @param lhs The left-hand side.
* @param rhs The right-hand side.
*/
/// Returns true if the given intrusive shared pointers point to the same
/// object.
///
/// @param lhs The left-hand side.
/// @param rhs The right-hand side.
friend constexpr bool operator==(const IntrusiveSharedPtr<T>& lhs,
const IntrusiveSharedPtr<T>& rhs) noexcept {
return lhs.m_ptr == rhs.m_ptr;
}
/**
* Returns true if the given intrusive shared pointers point to different
* objects.
*
* @param lhs The left-hand side.
* @param rhs The right-hand side.
*/
/// Returns true if the given intrusive shared pointers point to different
/// objects.
///
/// @param lhs The left-hand side.
/// @param rhs The right-hand side.
friend constexpr bool operator!=(const IntrusiveSharedPtr<T>& lhs,
const IntrusiveSharedPtr<T>& rhs) noexcept {
return lhs.m_ptr != rhs.m_ptr;
}
/**
* Returns true if the left-hand intrusive shared pointer points to nullptr.
*
* @param lhs The left-hand side.
*/
/// Returns true if the left-hand intrusive shared pointer points to nullptr.
///
/// @param lhs The left-hand side.
friend constexpr bool operator==(const IntrusiveSharedPtr<T>& lhs,
std::nullptr_t) noexcept {
return lhs.m_ptr == nullptr;
}
/**
* Returns true if the right-hand intrusive shared pointer points to nullptr.
*
* @param rhs The right-hand side.
*/
/// Returns true if the right-hand intrusive shared pointer points to nullptr.
///
/// @param rhs The right-hand side.
friend constexpr bool operator==(std::nullptr_t,
const IntrusiveSharedPtr<T>& rhs) noexcept {
return nullptr == rhs.m_ptr;
}
/**
* Returns true if the left-hand intrusive shared pointer doesn't point to
* nullptr.
*
* @param lhs The left-hand side.
*/
/// Returns true if the left-hand intrusive shared pointer doesn't point to
/// nullptr.
///
/// @param lhs The left-hand side.
friend constexpr bool operator!=(const IntrusiveSharedPtr<T>& lhs,
std::nullptr_t) noexcept {
return lhs.m_ptr != nullptr;
}
/**
* Returns true if the right-hand intrusive shared pointer doesn't point to
* nullptr.
*
* @param rhs The right-hand side.
*/
/// Returns true if the right-hand intrusive shared pointer doesn't point to
/// nullptr.
///
/// @param rhs The right-hand side.
friend constexpr bool operator!=(std::nullptr_t,
const IntrusiveSharedPtr<T>& rhs) noexcept {
return nullptr != rhs.m_ptr;
@@ -294,30 +250,26 @@ class IntrusiveSharedPtr {
T* m_ptr = nullptr;
};
/**
* Constructs an object of type T and wraps it in an intrusive shared pointer
* using args as the parameter list for the constructor of T.
*
* @tparam T Type of object for intrusive shared pointer.
* @tparam Args Types of constructor arguments.
* @param args Constructor arguments for T.
*/
/// Constructs an object of type T and wraps it in an intrusive shared pointer
/// using args as the parameter list for the constructor of T.
///
/// @tparam T Type of object for intrusive shared pointer.
/// @tparam Args Types of constructor arguments.
/// @param args Constructor arguments for T.
template <typename T, typename... Args>
IntrusiveSharedPtr<T> make_intrusive_shared(Args&&... args) {
return IntrusiveSharedPtr<T>{new T(std::forward<Args>(args)...)};
}
/**
* Constructs an object of type T and wraps it in an intrusive shared pointer
* using alloc as the storage allocator of T and args as the parameter list for
* the constructor of T.
*
* @tparam T Type of object for intrusive shared pointer.
* @tparam Alloc Type of allocator for T.
* @tparam Args Types of constructor arguments.
* @param alloc The allocator for T.
* @param args Constructor arguments for T.
*/
/// Constructs an object of type T and wraps it in an intrusive shared pointer
/// using alloc as the storage allocator of T and args as the parameter list for
/// the constructor of T.
///
/// @tparam T Type of object for intrusive shared pointer.
/// @tparam Alloc Type of allocator for T.
/// @tparam Args Types of constructor arguments.
/// @param alloc The allocator for T.
/// @param args Constructor arguments for T.
template <typename T, typename Alloc, typename... Args>
IntrusiveSharedPtr<T> allocate_intrusive_shared(Alloc alloc, Args&&... args) {
auto ptr = std::allocator_traits<Alloc>::allocate(alloc, sizeof(T));

162
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/pool.hpp vendored
View File

@@ -11,54 +11,40 @@
namespace slp {
/**
* This class implements a pool memory resource.
*
* The pool allocates chunks of memory and splits them into blocks managed by a
* free list. Allocations return pointers from the free list, and deallocations
* return pointers to the free list.
*/
/// This class implements a pool memory resource.
///
/// The pool allocates chunks of memory and splits them into blocks managed by a
/// free list. Allocations return pointers from the free list, and deallocations
/// return pointers to the free list.
class SLEIPNIR_DLLEXPORT PoolResource {
public:
/**
* Constructs a default PoolResource.
*
* @param blocks_per_chunk Number of blocks per chunk of memory.
*/
/// Constructs a default PoolResource.
///
/// @param blocks_per_chunk Number of blocks per chunk of memory.
explicit PoolResource(size_t blocks_per_chunk)
: blocks_per_chunk{blocks_per_chunk} {}
/**
* Copy constructor.
*/
/// Copy constructor.
PoolResource(const PoolResource&) = delete;
/**
* Copy assignment operator.
*
* @return This pool resource.
*/
/// Copy assignment operator.
///
/// @return This pool resource.
PoolResource& operator=(const PoolResource&) = delete;
/**
* Move constructor.
*/
/// Move constructor.
PoolResource(PoolResource&&) = default;
/**
* Move assignment operator.
*
* @return This pool resource.
*/
/// Move assignment operator.
///
/// @return This pool resource.
PoolResource& operator=(PoolResource&&) = default;
/**
* Returns a block of memory from the pool.
*
* @param bytes Number of bytes in the block.
* @param alignment Alignment of the block (unused).
* @return A block of memory from the pool.
*/
/// Returns a block of memory from the pool.
///
/// @param bytes Number of bytes in the block.
/// @param alignment Alignment of the block (unused).
/// @return A block of memory from the pool.
[[nodiscard]]
void* allocate(size_t bytes, [[maybe_unused]] size_t alignment =
alignof(std::max_align_t)) {
@@ -71,34 +57,30 @@ class SLEIPNIR_DLLEXPORT PoolResource {
return ptr;
}
/**
* Gives a block of memory back to the pool.
*
* @param p A pointer to the block of memory.
* @param bytes Number of bytes in the block (unused).
* @param alignment Alignment of the block (unused).
*/
/// Gives a block of memory back to the pool.
///
/// @param p A pointer to the block of memory.
/// @param bytes Number of bytes in the block (unused).
/// @param alignment Alignment of the block (unused).
void deallocate(
void* p, [[maybe_unused]] size_t bytes,
[[maybe_unused]] size_t alignment = alignof(std::max_align_t)) {
m_free_list.emplace_back(p);
}
/**
* Returns true if this pool resource has the same backing storage as another.
*
* @param other The other pool resource.
* @return True if this pool resource has the same backing storage as another.
*/
/// Returns true if this pool resource has the same backing storage as
/// another.
///
/// @param other The other pool resource.
/// @return True if this pool resource has the same backing storage as
/// another.
bool is_equal(const PoolResource& other) const noexcept {
return this == &other;
}
/**
* Returns the number of blocks from this pool resource that are in use.
*
* @return The number of blocks from this pool resource that are in use.
*/
/// Returns the number of blocks from this pool resource that are in use.
///
/// @return The number of blocks from this pool resource that are in use.
size_t blocks_in_use() const noexcept {
return m_buffer.size() * blocks_per_chunk - m_free_list.size();
}
@@ -108,12 +90,10 @@ class SLEIPNIR_DLLEXPORT PoolResource {
gch::small_vector<void*> m_free_list;
size_t blocks_per_chunk;
/**
* Adds a memory chunk to the pool, partitions it into blocks with the given
* number of bytes, and appends pointers to them to the free list.
*
* @param bytes_per_block Number of bytes in the block.
*/
/// Adds a memory chunk to the pool, partitions it into blocks with the given
/// number of bytes, and appends pointers to them to the free list.
///
/// @param bytes_per_block Number of bytes in the block.
void add_chunk(size_t bytes_per_block) {
m_buffer.emplace_back(new std::byte[bytes_per_block * blocks_per_chunk]);
for (int i = blocks_per_chunk - 1; i >= 0; --i) {
@@ -122,55 +102,41 @@ class SLEIPNIR_DLLEXPORT PoolResource {
}
};
/**
* This class is an allocator for the pool resource.
*
* @tparam T The type of object in the pool.
*/
/// This class is an allocator for the pool resource.
///
/// @tparam T The type of object in the pool.
template <typename T>
class PoolAllocator {
public:
/**
* The type of object in the pool.
*/
/// The type of object in the pool.
using value_type = T;
/**
* Constructs a pool allocator with the given pool memory resource.
*
* @param r The pool resource.
*/
/// Constructs a pool allocator with the given pool memory resource.
///
/// @param r The pool resource.
explicit constexpr PoolAllocator(PoolResource* r) : m_memory_resource{r} {}
/**
* Copy constructor.
*/
/// Copy constructor.
constexpr PoolAllocator(const PoolAllocator<T>&) = default;
/**
* Copy assignment operator.
*
* @return This pool allocator.
*/
/// Copy assignment operator.
///
/// @return This pool allocator.
constexpr PoolAllocator<T>& operator=(const PoolAllocator<T>&) = default;
/**
* Returns a block of memory from the pool.
*
* @param n Number of bytes in the block.
* @return A block of memory from the pool.
*/
/// Returns a block of memory from the pool.
///
/// @param n Number of bytes in the block.
/// @return A block of memory from the pool.
[[nodiscard]]
constexpr T* allocate(size_t n) {
return static_cast<T*>(m_memory_resource->allocate(n));
}
/**
* Gives a block of memory back to the pool.
*
* @param p A pointer to the block of memory.
* @param n Number of bytes in the block.
*/
/// Gives a block of memory back to the pool.
///
/// @param p A pointer to the block of memory.
/// @param n Number of bytes in the block.
constexpr void deallocate(T* p, size_t n) {
m_memory_resource->deallocate(p, n);
}
@@ -179,16 +145,12 @@ class PoolAllocator {
PoolResource* m_memory_resource;
};
/**
* Returns a global pool memory resource.
*/
/// Returns a global pool memory resource.
SLEIPNIR_DLLEXPORT PoolResource& global_pool_resource();
/**
* Returns an allocator for a global pool memory resource.
*
* @tparam T The type of object in the pool.
*/
/// Returns an allocator for a global pool memory resource.
///
/// @tparam T The type of object in the pool.
template <typename T>
PoolAllocator<T> global_pool_allocator() {
return PoolAllocator<T>{&global_pool_resource()};

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

@@ -7,21 +7,14 @@
#include <system_error>
#include <utility>
#if __has_include(<fmt/base.h>)
#include <fmt/base.h>
#else
#include <fmt/core.h>
#endif
#endif
namespace slp {
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
/**
* Wrapper around fmt::print() that squelches write failure exceptions.
*/
/// Wrapper around fmt::print() that squelches write failure exceptions.
template <typename... T>
void print(fmt::format_string<T...> fmt, T&&... args) {
try {
@@ -30,9 +23,7 @@ void print(fmt::format_string<T...> fmt, T&&... args) {
}
}
/**
* Wrapper around fmt::print() that squelches write failure exceptions.
*/
/// Wrapper around fmt::print() that squelches write failure exceptions.
template <typename... T>
void print(std::FILE* f, fmt::format_string<T...> fmt, T&&... args) {
try {
@@ -41,9 +32,7 @@ void print(std::FILE* f, fmt::format_string<T...> fmt, T&&... args) {
}
}
/**
* Wrapper around fmt::println() that squelches write failure exceptions.
*/
/// Wrapper around fmt::println() that squelches write failure exceptions.
template <typename... T>
void println(fmt::format_string<T...> fmt, T&&... args) {
try {
@@ -52,9 +41,7 @@ void println(fmt::format_string<T...> fmt, T&&... args) {
}
}
/**
* Wrapper around fmt::println() that squelches write failure exceptions.
*/
/// Wrapper around fmt::println() that squelches write failure exceptions.
template <typename... T>
void println(std::FILE* f, fmt::format_string<T...> fmt, T&&... args) {
try {

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

@@ -21,9 +21,7 @@
namespace slp {
/**
* Iteration type.
*/
/// Iteration type.
enum class IterationType : uint8_t {
/// Normal iteration.
NORMAL,
@@ -33,10 +31,8 @@ enum class IterationType : uint8_t {
REJECTED_SOC
};
/**
* Converts std::chrono::duration to a number of milliseconds rounded to three
* decimals.
*/
/// Converts std::chrono::duration to a number of milliseconds rounded to three
/// decimals.
template <typename Rep, typename Period = std::ratio<1>>
constexpr double to_ms(const std::chrono::duration<Rep, Period>& duration) {
using std::chrono::duration_cast;
@@ -44,12 +40,10 @@ constexpr double to_ms(const std::chrono::duration<Rep, Period>& duration) {
return duration_cast<microseconds>(duration).count() / 1e3;
}
/**
* Renders value as power of 10.
*
* @tparam Scalar Scalar type.
* @param value Value.
*/
/// Renders value as power of 10.
///
/// @tparam Scalar Scalar type.
/// @param value Value.
template <typename Scalar>
std::string power_of_10(Scalar value) {
if (value == Scalar(0)) {
@@ -89,13 +83,11 @@ std::string power_of_10(Scalar value) {
}
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
/**
* 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.
*/
/// 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.
template <typename Scalar>
void print_too_few_dofs_error(
const Eigen::Vector<Scalar, Eigen::Dynamic>& c_e) {
@@ -112,13 +104,11 @@ void print_too_few_dofs_error(
#endif
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
/**
* 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.
*/
/// 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.
template <typename Scalar>
void print_c_e_local_infeasibility_error(
const Eigen::Vector<Scalar, Eigen::Dynamic>& c_e) {
@@ -137,13 +127,11 @@ void print_c_e_local_infeasibility_error(
#endif
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
/**
* 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.
*/
/// 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.
template <typename Scalar>
void print_c_i_local_infeasibility_error(
const Eigen::Vector<Scalar, Eigen::Dynamic>& c_i) {
@@ -181,25 +169,23 @@ inline void print_bound_constraint_global_infeasibility_error(
#endif
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
/**
* 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.
* @param error The error.
* @param cost The cost.
* @param infeasibility The infeasibility.
* @param complementarity The complementarity.
* @param μ The barrier parameter.
* @param δ The Hessian regularization factor.
* @param primal_α The primal step size.
* @param primal_α_max The max primal step size.
* @param α_reduction_factor Factor by which primal_α is reduced during
* backtracking.
* @param dual_α The dual step size.
*/
/// 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.
/// @param error The error.
/// @param cost The cost.
/// @param infeasibility The infeasibility.
/// @param complementarity The complementarity.
/// @param μ The barrier parameter.
/// @param δ The Hessian regularization factor.
/// @param primal_α The primal step size.
/// @param primal_α_max The max primal step size.
/// @param α_reduction_factor Factor by which primal_α is reduced during
/// backtracking.
/// @param dual_α The dual step size.
template <typename Scalar, typename Rep, typename Period = std::ratio<1>>
void print_iteration_diagnostics(int iterations, IterationType type,
const std::chrono::duration<Rep, Period>& time,
@@ -261,9 +247,7 @@ void print_iteration_diagnostics(int iterations, IterationType type,
#endif
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
/**
* Prints bottom of iteration diagnostics table.
*/
/// Prints bottom of iteration diagnostics table.
inline void print_bottom_iteration_diagnostics() {
slp::println("└{:─^108}┘", "");
}
@@ -271,12 +255,10 @@ inline void print_bottom_iteration_diagnostics() {
#define print_bottom_iteration_diagnostics(...)
#endif
/**
* Renders histogram of the given normalized value.
*
* @tparam Width Width of the histogram in characters.
* @param value Normalized value from 0 to 1.
*/
/// Renders histogram of the given normalized value.
///
/// @tparam Width Width of the histogram in characters.
/// @param value Normalized value from 0 to 1.
template <int Width>
requires(Width > 0)
std::string histogram(double value) {
@@ -306,11 +288,9 @@ std::string histogram(double value) {
}
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
/**
* Prints solver diagnostics.
*
* @param solve_profilers Solve profilers.
*/
/// Prints solver diagnostics.
///
/// @param solve_profilers Solve profilers.
inline void print_solver_diagnostics(
const gch::small_vector<SolveProfiler>& solve_profilers) {
auto solve_duration = to_ms(solve_profilers[0].total_duration());
@@ -337,11 +317,9 @@ inline void print_solver_diagnostics(
#endif
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
/**
* Prints autodiff diagnostics.
*
* @param setup_profilers Autodiff setup profilers.
*/
/// Prints autodiff diagnostics.
///
/// @param setup_profilers Autodiff setup profilers.
inline void print_autodiff_diagnostics(
const gch::small_vector<SetupProfiler>& setup_profilers) {
auto setup_duration = to_ms(setup_profilers[0].duration());

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

@@ -6,20 +6,16 @@
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.
*/
/// 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.
*/
/// 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() {
@@ -28,11 +24,9 @@ class scope_exit {
}
}
/**
* Move constructor.
*
* @param rhs scope_exit from which to move.
*/
/// 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();
@@ -41,9 +35,7 @@ class scope_exit {
scope_exit(const scope_exit&) = delete;
scope_exit& operator=(const scope_exit&) = delete;
/**
* Makes the scope_exit inactive.
*/
/// Makes the scope_exit inactive.
void release() noexcept { m_active = false; }
private:

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

@@ -10,37 +10,29 @@
namespace slp {
/**
* Starts a profiler in the constructor and stops it in the destructor.
*/
/// Starts a profiler in the constructor and stops it in the destructor.
template <typename Profiler>
requires std::same_as<Profiler, SetupProfiler> ||
std::same_as<Profiler, SolveProfiler>
class ScopedProfiler {
public:
/**
* Starts a profiler.
*
* @param profiler The profiler.
*/
/// Starts a profiler.
///
/// @param profiler The profiler.
explicit ScopedProfiler(Profiler& profiler) noexcept : m_profiler{&profiler} {
m_profiler->start();
}
/**
* Stops a profiler.
*/
/// Stops a profiler.
~ScopedProfiler() {
if (m_active) {
m_profiler->stop();
}
}
/**
* Move constructor.
*
* @param rhs The other ScopedProfiler.
*/
/// Move constructor.
///
/// @param rhs The other ScopedProfiler.
ScopedProfiler(ScopedProfiler&& rhs) noexcept
: m_profiler{std::move(rhs.m_profiler)}, m_active{rhs.m_active} {
rhs.m_active = false;
@@ -49,11 +41,9 @@ class ScopedProfiler {
ScopedProfiler(const ScopedProfiler&) = delete;
ScopedProfiler& operator=(const ScopedProfiler&) = delete;
/**
* Stops the profiler.
*
* If this is called, the destructor is a no-op.
*/
/// Stops the profiler.
///
/// If this is called, the destructor is a no-op.
void stop() {
if (m_active) {
m_profiler->stop();
@@ -61,11 +51,9 @@ class ScopedProfiler {
}
}
/**
* Returns the most recent solve duration in milliseconds as a double.
*
* @return The most recent solve duration in milliseconds as a double.
*/
/// Returns the most recent solve duration in milliseconds as a double.
///
/// @return The most recent solve duration in milliseconds as a double.
const std::chrono::duration<double>& current_duration() const {
return m_profiler->current_duration();
}

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

@@ -8,31 +8,23 @@
namespace slp {
/**
* Records the number of profiler measurements (start/stop pairs) and the
* average duration between each start and stop call.
*/
/// Records the number of profiler measurements (start/stop pairs) and the
/// average duration between each start and stop call.
class SetupProfiler {
public:
/**
* Constructs a SetupProfiler.
*
* @param name Name of measurement to show in diagnostics.
*/
/// Constructs a SetupProfiler.
///
/// @param name Name of measurement to show in diagnostics.
explicit SetupProfiler(std::string_view name) : m_name{name} {}
/**
* Tell the profiler to start measuring setup time.
*/
/// Tell the profiler to start measuring setup time.
void start() {
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
m_setup_start_time = std::chrono::steady_clock::now();
#endif
}
/**
* Tell the profiler to stop measuring setup time.
*/
/// Tell the profiler to stop measuring setup time.
void stop() {
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
m_setup_stop_time = std::chrono::steady_clock::now();
@@ -40,18 +32,14 @@ class SetupProfiler {
#endif
}
/**
* Returns name of measurement to show in diagnostics.
*
* @return Name of measurement to show in diagnostics.
*/
/// Returns name of measurement to show in diagnostics.
///
/// @return Name of measurement to show in diagnostics.
std::string_view name() const { return m_name; }
/**
* Returns the setup duration in milliseconds as a double.
*
* @return The setup duration in milliseconds as a double.
*/
/// Returns the setup duration in milliseconds as a double.
///
/// @return The setup duration in milliseconds as a double.
const std::chrono::duration<double>& duration() const {
return m_setup_duration;
}

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

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

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

@@ -16,48 +16,44 @@
namespace slp {
/**
* Writes the sparsity pattern of a sparse matrix to a file.
*
* Each file represents the sparsity pattern of one matrix over time. <a
* href="https://github.com/SleipnirGroup/Sleipnir/blob/main/tools/spy.py">spy.py</a>
* can display it as an animation.
*
* The file starts with the following header:
* <ol>
* <li>Plot title (length as a little-endian int32, then characters)</li>
* <li>Row label (length as a little-endian int32, then characters)</li>
* <li>Column label (length as a little-endian int32, then characters)</li>
* </ol>
*
* Then, each sparsity pattern starts with:
* <ol>
* <li>Number of coordinates as a little-endian int32</li>
* </ol>
*
* followed by that many coordinates in the following format:
* <ol>
* <li>Row index as a little-endian int32</li>
* <li>Column index as a little-endian int32</li>
* <li>Sign as a character ('+' for positive, '-' for negative, or '0' for
* zero)</li>
* </ol>
*
* @tparam Scalar Scalar type.
*/
/// Writes the sparsity pattern of a sparse matrix to a file.
///
/// Each file represents the sparsity pattern of one matrix over time. <a
/// href="https://github.com/SleipnirGroup/Sleipnir/blob/main/tools/spy.py">spy.py</a>
/// can display it as an animation.
///
/// The file starts with the following header:
/// <ol>
/// <li>Plot title (length as a little-endian int32, then characters)</li>
/// <li>Row label (length as a little-endian int32, then characters)</li>
/// <li>Column label (length as a little-endian int32, then characters)</li>
/// </ol>
///
/// Then, each sparsity pattern starts with:
/// <ol>
/// <li>Number of coordinates as a little-endian int32</li>
/// </ol>
///
/// followed by that many coordinates in the following format:
/// <ol>
/// <li>Row index as a little-endian int32</li>
/// <li>Column index as a little-endian int32</li>
/// <li>Sign as a character ('+' for positive, '-' for negative, or '0' for
/// zero)</li>
/// </ol>
///
/// @tparam Scalar Scalar type.
template <typename Scalar>
class Spy {
public:
/**
* Constructs a Spy instance.
*
* @param filename The filename.
* @param title Plot title.
* @param row_label Row label.
* @param col_label Column label.
* @param rows The sparse matrix's number of rows.
* @param cols The sparse matrix's number of columns.
*/
/// Constructs a Spy instance.
///
/// @param filename The filename.
/// @param title Plot title.
/// @param row_label Row label.
/// @param col_label Column label.
/// @param rows The sparse matrix's number of rows.
/// @param cols The sparse matrix's number of columns.
Spy(std::string_view filename, std::string_view title,
std::string_view row_label, std::string_view col_label, int rows,
int cols)
@@ -79,11 +75,9 @@ class Spy {
write32le(cols);
}
/**
* Adds a matrix to the file.
*
* @param mat The matrix.
*/
/// Adds a matrix to the file.
///
/// @param mat The matrix.
void add(const Eigen::SparseMatrix<Scalar>& mat) {
// Write number of coordinates
write32le(mat.nonZeros());
@@ -108,11 +102,9 @@ class Spy {
private:
std::ofstream m_file;
/**
* Writes a 32-bit signed integer to the file as little-endian.
*
* @param num A 32-bit signed integer.
*/
/// Writes a 32-bit signed integer to the file as little-endian.
///
/// @param num A 32-bit signed integer.
void write32le(int32_t num) {
if constexpr (std::endian::native != std::endian::little) {
num = wpi::util::byteswap(num);

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

@@ -0,0 +1,16 @@
// Copyright (c) Sleipnir contributors
#pragma once
namespace slp {
[[noreturn]]
inline void unreachable() {
#if defined(_MSC_VER) && !defined(__clang__)
__assume(false);
#else
__builtin_unreachable();
#endif
}
} // namespace slp