diff --git a/upstream_utils/sleipnir.py b/upstream_utils/sleipnir.py index a445d0172e..ad8c66414a 100755 --- a/upstream_utils/sleipnir.py +++ b/upstream_utils/sleipnir.py @@ -48,7 +48,7 @@ using small_vector = wpi::util::SmallVector; 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() diff --git a/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch b/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch index 8873cebf15..9bda09e602 100644 --- a/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch +++ b/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch @@ -1,16 +1,56 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness 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 + +-#include + #include + ++#include ++ + namespace slp { + + /// Expression type. +@@ -29,12 +30,12 @@ enum class ExpressionType : uint8_t { + + /// Formatter for ExpressionType. + template <> +-struct std::formatter { ++struct fmt::formatter { + /// 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 { + } + + private: +- std::formatter m_underlying; ++ fmt::formatter 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 #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 + +-#include + #include + ++#include ++ + 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 { ++struct fmt::formatter { + /// 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 { + } + + private: +- std::formatter m_underlying; ++ fmt::formatter 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 #include #include +#include + - /** - * 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 -#include #include #include - -+#if __has_include() -+#include -+#else -+#include -+#endif + ++#include #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 -void print(std::format_string fmt, T&&... args) { +void print(fmt::format_string 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 -void print(std::FILE* f, std::format_string fmt, T&&... args) { +void print(std::FILE* f, fmt::format_string 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 -void println(std::format_string fmt, T&&... args) { +void println(fmt::format_string 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 -void println(std::FILE* f, std::format_string fmt, T&&... args) { +void println(std::FILE* f, fmt::format_string fmt, T&&... args) { diff --git a/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch b/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch index 8f317b0f00..4c6e3f00ab 100644 --- a/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch +++ b/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch @@ -1,7 +1,7 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness 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 constexpr void inc_ref_count(Expression* expr); template -constexpr void dec_ref_count(Expression* expr); +void dec_ref_count(Expression* expr); - /** - * Typedef for intrusive shared pointer to Expression. -@@ -801,7 +801,7 @@ constexpr void inc_ref_count(Expression* 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* expr) { + /// @tparam Scalar Scalar type. + /// @param expr The shared pointer's managed object. template -constexpr void dec_ref_count(Expression* expr) { +void dec_ref_count(Expression* 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&& expr) + explicit Variable(detail::ExpressionPtr&& 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. + /// diff --git a/upstream_utils/sleipnir_patches/0003-Use-wpi-byteswap.patch b/upstream_utils/sleipnir_patches/0003-Use-wpi-byteswap.patch index 7423819d9b..5805aa0b25 100644 --- a/upstream_utils/sleipnir_patches/0003-Use-wpi-byteswap.patch +++ b/upstream_utils/sleipnir_patches/0003-Use-wpi-byteswap.patch @@ -1,14 +1,14 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness 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); diff --git a/upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch b/upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch index 8dc94c1923..130d27ae08 100644 --- a/upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch +++ b/upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch @@ -1,7 +1,7 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness 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>& constraints) { std::array 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}│", diff --git a/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch b/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch index 2a20e218c6..fb38c351e7 100644 --- a/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch +++ b/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch @@ -1,18 +1,18 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness 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(constraint.type())]; } diff --git a/upstream_utils/sleipnir_patches/0006-Replace-std-unreachable.patch b/upstream_utils/sleipnir_patches/0006-Replace-std-unreachable.patch new file mode 100644 index 0000000000..0ac2097033 --- /dev/null +++ b/upstream_utils/sleipnir_patches/0006-Replace-std-unreachable.patch @@ -0,0 +1,86 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Tyler Veness +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 + +-#include +- + #include + ++#include "sleipnir/util/unreachable.hpp" ++ + namespace slp { + + /// Expression type. +@@ -61,7 +61,7 @@ struct fmt::formatter { + 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 + +-#include +- + #include + ++#include "sleipnir/util/unreachable.hpp" ++ + namespace slp { + + /// Solver exit status. Negative values indicate failure. +@@ -88,7 +88,7 @@ struct fmt::formatter { + 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 diff --git a/upstream_utils/sleipnir_patches/0006-Suppress-clang-tidy-false-positives.patch b/upstream_utils/sleipnir_patches/0007-Suppress-clang-tidy-false-positives.patch similarity index 69% rename from upstream_utils/sleipnir_patches/0006-Suppress-clang-tidy-false-positives.patch rename to upstream_utils/sleipnir_patches/0007-Suppress-clang-tidy-false-positives.patch index 8eab50ee95..7e4126c87f 100644 --- a/upstream_utils/sleipnir_patches/0006-Suppress-clang-tidy-false-positives.patch +++ b/upstream_utils/sleipnir_patches/0007-Suppress-clang-tidy-false-positives.patch @@ -1,20 +1,20 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness 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 inequality_constraints) { diff --git a/upstream_utils/sleipnir_patches/0007-Suppress-GCC-12-warning-false-positive.patch b/upstream_utils/sleipnir_patches/0008-Suppress-GCC-12-warning-false-positive.patch similarity index 76% rename from upstream_utils/sleipnir_patches/0007-Suppress-GCC-12-warning-false-positive.patch rename to upstream_utils/sleipnir_patches/0008-Suppress-GCC-12-warning-false-positive.patch index b4a01648a7..da79024ea8 100644 --- a/upstream_utils/sleipnir_patches/0007-Suppress-GCC-12-warning-false-positive.patch +++ b/upstream_utils/sleipnir_patches/0008-Suppress-GCC-12-warning-false-positive.patch @@ -1,17 +1,17 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness 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 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; } } diff --git a/upstream_utils/sleipnir_patches/0008-Use-operator-instead-of-multidimensional-array-subsc.patch b/upstream_utils/sleipnir_patches/0009-Use-operator-instead-of-multidimensional-array-subsc.patch similarity index 74% rename from upstream_utils/sleipnir_patches/0008-Use-operator-instead-of-multidimensional-array-subsc.patch rename to upstream_utils/sleipnir_patches/0009-Use-operator-instead-of-multidimensional-array-subsc.patch index c080886432..1513532d7e 100644 --- a/upstream_utils/sleipnir_patches/0008-Use-operator-instead-of-multidimensional-array-subsc.patch +++ b/upstream_utils/sleipnir_patches/0009-Use-operator-instead-of-multidimensional-array-subsc.patch @@ -1,25 +1,25 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness 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 auto value) : expr{value[0, 0].expr} { + Variable(SleipnirMatrixLike 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& 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& operator[](int row, int col) + Variable& operator()(int row, int col) requires(!std::is_const_v) @@ -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& operator[](int row, int col) const { + const Variable& 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) { 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& 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 operator[](Slice row_slice, Slice col_slice) { + VariableBlock 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 operator[](Slice row_slice, + const VariableBlock 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 operator[](Slice row_slice, int row_slice_length, + VariableBlock 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 operator[](Slice row_slice, + const VariableBlock 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& 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& 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() 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& operator[](int row, int col) { + Variable& operator()(int row, int col) { slp_assert(row >= 0 && row < rows()); slp_assert(col >= 0 && col < cols()); return m_storage[row * cols() + col]; -@@ -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& operator[](int row, int col) const { + const Variable& operator()(int row, int col) const { slp_assert(row >= 0 && row < rows()); slp_assert(col >= 0 && col < cols()); return m_storage[row * cols() + col]; -@@ -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 operator[](Slice row_slice, Slice col_slice) { + VariableBlock 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 operator[](Slice row_slice, + const VariableBlock 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 operator[](Slice row_slice, + VariableBlock 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 operator[]( + const VariableBlock 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() 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 cwise_reduce( +@@ -1253,7 +1253,7 @@ VariableMatrix 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 solve(const VariableMatrix& A, +@@ -1386,17 +1386,17 @@ VariableMatrix solve(const VariableMatrix& 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 solve(const VariableMatrix& A, +@@ -1413,15 +1413,15 @@ VariableMatrix solve(const VariableMatrix& 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 solve(const VariableMatrix& A, +@@ -1461,22 +1461,22 @@ VariableMatrix solve(const VariableMatrix& 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 solve(const VariableMatrix& A, +@@ -1607,14 +1607,14 @@ VariableMatrix solve(const VariableMatrix& 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 solve(const VariableMatrix& A, +@@ -1623,7 +1623,7 @@ VariableMatrix solve(const VariableMatrix& A, VariableMatrix 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 { +@@ -123,7 +123,7 @@ class OCP : public Problem { if (timestep_method == TimestepMethod::FIXED) { m_DT = VariableMatrix{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 { +@@ -132,12 +132,12 @@ class OCP : public Problem { // Set the member variable matrix to track the decision variable m_DT = VariableMatrix{1, m_num_steps + 1}; for (int i = 0; i < num_steps + 1; ++i) { @@ -913,7 +922,7 @@ index 88316894362ff3004627308c81c8f251291eae97..d62432a67af1c75b5cc0bbab54df1d78 } } -@@ -216,7 +216,7 @@ class OCP : public Problem { +@@ -206,7 +206,7 @@ class OCP : public Problem { 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 { +@@ -326,7 +326,7 @@ class OCP : public Problem { // 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 { +@@ -363,7 +363,7 @@ class OCP : public Problem { 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 { +@@ -386,7 +386,7 @@ class OCP : public Problem { 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, 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(); diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression.hpp index 46814576a3..0e6f31bf77 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression.hpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include @@ -35,21 +36,17 @@ constexpr void inc_ref_count(Expression* expr); template void dec_ref_count(Expression* expr); -/** - * Typedef for intrusive shared pointer to Expression. - * - * @tparam Scalar Scalar type. - */ +/// Typedef for intrusive shared pointer to Expression. +/// +/// @tparam Scalar Scalar type. template using ExpressionPtr = IntrusiveSharedPtr>; -/** - * Creates an intrusive shared pointer to an expression from the global pool - * allocator. - * - * @tparam T The derived expression type. - * @param args Constructor arguments for Expression. - */ +/// Creates an intrusive shared pointer to an expression from the global pool +/// allocator. +/// +/// @tparam T The derived expression type. +/// @param args Constructor arguments for Expression. template static ExpressionPtr make_expression_ptr(Args&&... args) { if constexpr (USE_POOL_ALLOCATOR) { @@ -67,7 +64,7 @@ template struct BinaryPlusExpression; template -struct ConstExpression; +struct ConstantExpression; template struct DivExpression; @@ -78,25 +75,19 @@ struct MultExpression; template struct UnaryMinusExpression; -/** - * Creates an intrusive shared pointer to a constant expression. - * - * @tparam Scalar Scalar type. - * @param value The expression value. - */ +/// Creates an intrusive shared pointer to a constant expression. +/// +/// @tparam Scalar Scalar type. +/// @param value The expression value. template ExpressionPtr constant_ptr(Scalar value); -/** - * An autodiff expression node. - * - * @tparam Scalar Scalar type. - */ +/// An autodiff expression node. +/// +/// @tparam Scalar Scalar type. template struct Expression { - /** - * Scalar type alias. - */ + /// Scalar type alias. using Scalar = Scalar_; /// The value of the expression node. @@ -121,54 +112,41 @@ struct Expression { /// Expression arguments. std::array, 2> args{nullptr, nullptr}; - /** - * Constructs a constant expression with a value of zero. - */ + /// Constructs a constant expression with a value of zero. constexpr Expression() = default; - /** - * Constructs a nullary expression (an operator with no arguments). - * - * @param value The expression value. - */ + /// Constructs a nullary expression (an operator with no arguments). + /// + /// @param value The expression value. explicit constexpr Expression(Scalar value) : val{value} {} - /** - * Constructs an unary expression (an operator with one argument). - * - * @param lhs Unary operator's operand. - */ + /// Constructs an unary expression (an operator with one argument). + /// + /// @param lhs Unary operator's operand. explicit constexpr Expression(ExpressionPtr lhs) : args{std::move(lhs), nullptr} {} - /** - * Constructs a binary expression (an operator with two arguments). - * - * @param lhs Binary operator's left operand. - * @param rhs Binary operator's right operand. - */ + /// Constructs a binary expression (an operator with two arguments). + /// + /// @param lhs Binary operator's left operand. + /// @param rhs Binary operator's right operand. constexpr Expression(ExpressionPtr lhs, ExpressionPtr rhs) : args{std::move(lhs), std::move(rhs)} {} virtual ~Expression() = default; - /** - * Returns true if the expression is the given constant. - * - * @param constant The constant. - * - * @return True if the expression is the given constant. - */ + /// Returns true if the expression is the given constant. + /// + /// @param constant The constant. + /// @return True if the expression is the given constant. constexpr bool is_constant(Scalar constant) const { return type() == ExpressionType::CONSTANT && val == constant; } - /** - * Expression-Expression multiplication operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ + /// Expression-Expression multiplication operator. + /// + /// @param lhs Operator left-hand side. + /// @param rhs Operator right-hand side. friend ExpressionPtr operator*(const ExpressionPtr& lhs, const ExpressionPtr& rhs) { using enum ExpressionType; @@ -215,12 +193,10 @@ struct Expression { } } - /** - * Expression-Expression division operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ + /// Expression-Expression division operator. + /// + /// @param lhs Operator left-hand side. + /// @param rhs Operator right-hand side. friend ExpressionPtr operator/(const ExpressionPtr& lhs, const ExpressionPtr& rhs) { using enum ExpressionType; @@ -252,12 +228,10 @@ struct Expression { } } - /** - * Expression-Expression addition operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ + /// Expression-Expression addition operator. + /// + /// @param lhs Operator left-hand side. + /// @param rhs Operator right-hand side. friend ExpressionPtr operator+(const ExpressionPtr& lhs, const ExpressionPtr& rhs) { using enum ExpressionType; @@ -287,23 +261,19 @@ struct Expression { } } - /** - * Expression-Expression compound addition operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ + /// Expression-Expression compound addition operator. + /// + /// @param lhs Operator left-hand side. + /// @param rhs Operator right-hand side. friend ExpressionPtr operator+=(ExpressionPtr& lhs, const ExpressionPtr& rhs) { return lhs = lhs + rhs; } - /** - * Expression-Expression subtraction operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ + /// Expression-Expression subtraction operator. + /// + /// @param lhs Operator left-hand side. + /// @param rhs Operator right-hand side. friend ExpressionPtr operator-(const ExpressionPtr& lhs, const ExpressionPtr& rhs) { using enum ExpressionType; @@ -338,11 +308,9 @@ struct Expression { } } - /** - * Unary minus operator. - * - * @param lhs Operand of unary minus. - */ + /// Unary minus operator. + /// + /// @param lhs Operand of unary minus. friend ExpressionPtr operator-(const ExpressionPtr& lhs) { using enum ExpressionType; @@ -366,71 +334,64 @@ struct Expression { } } - /** - * Unary plus operator. - * - * @param lhs Operand of unary plus. - */ + /// Unary plus operator. + /// + /// @param lhs Operand of unary plus. friend ExpressionPtr operator+(const ExpressionPtr& lhs) { return lhs; } - /** - * Either nullary operator with no arguments, unary operator with one - * argument, or binary operator with two arguments. This operator is used to - * update the node's value. - * - * @param lhs Left argument to binary operator. - * @param rhs Right argument to binary operator. - * @return The node's value. - */ + /// Either nullary operator with no arguments, unary operator with one + /// argument, or binary operator with two arguments. This operator is used to + /// update the node's value. + /// + /// @param lhs Left argument to binary operator. + /// @param rhs Right argument to binary operator. + /// @return The node's value. virtual Scalar value([[maybe_unused]] Scalar lhs, [[maybe_unused]] Scalar rhs) const = 0; - /** - * Returns the type of this expression (constant, linear, quadratic, or - * nonlinear). - * - * @return The type of this expression. - */ + /// Returns the type of this expression (constant, linear, quadratic, or + /// nonlinear). + /// + /// @return The type of this expression. virtual ExpressionType type() const = 0; - /** - * Returns ∂/∂l as a Scalar. - * - * @param lhs Left argument to binary operator. - * @param rhs Right argument to binary operator. - * @param parent_adjoint Adjoint of parent expression. - * @return ∂/∂l as a Scalar. - */ + /// Returns the name of this expression. + /// + /// @return The name of this expression. + virtual std::string_view name() const = 0; + + /// Returns ∂/∂l as a Scalar. + /// + /// @param lhs Left argument to binary operator. + /// @param rhs Right argument to binary operator. + /// @param parent_adjoint Adjoint of parent expression. + /// @return ∂/∂l as a Scalar. virtual Scalar grad_l([[maybe_unused]] Scalar lhs, [[maybe_unused]] Scalar rhs, [[maybe_unused]] Scalar parent_adjoint) const { return Scalar(0); } - /** - * Returns ∂/∂r as a Scalar. - * - * @param lhs Left argument to binary operator. - * @param rhs Right argument to binary operator. - * @param parent_adjoint Adjoint of parent expression. - * @return ∂/∂r as a Scalar. - */ + /// Returns ∂/∂r as a Scalar. + /// + /// @param lhs Left argument to binary operator. + /// @param rhs Right argument to binary operator. + /// @param parent_adjoint Adjoint of parent expression. + /// @return ∂/∂r as a Scalar. virtual Scalar grad_r([[maybe_unused]] Scalar lhs, [[maybe_unused]] Scalar rhs, [[maybe_unused]] Scalar parent_adjoint) const { return Scalar(0); } - /** - * Returns ∂/∂l as an Expression. - * - * @param lhs Left argument to binary operator. - * @param rhs Right argument to binary operator. - * @param parent_adjoint Adjoint of parent expression. - * @return ∂/∂l as an Expression. - */ + /// Returns ∂/∂l as an Expression. + /// + /// @param lhs Left argument to binary operator. + /// @param rhs Right argument to binary operator. + /// @param parent_adjoint Adjoint of parent expression. + /// @return ∂/∂l as an Expression. virtual ExpressionPtr grad_expr_l( [[maybe_unused]] const ExpressionPtr& lhs, [[maybe_unused]] const ExpressionPtr& rhs, @@ -438,14 +399,12 @@ struct Expression { return constant_ptr(Scalar(0)); } - /** - * Returns ∂/∂r as an Expression. - * - * @param lhs Left argument to binary operator. - * @param rhs Right argument to binary operator. - * @param parent_adjoint Adjoint of parent expression. - * @return ∂/∂r as an Expression. - */ + /// Returns ∂/∂r as an Expression. + /// + /// @param lhs Left argument to binary operator. + /// @param rhs Right argument to binary operator. + /// @param parent_adjoint Adjoint of parent expression. + /// @return ∂/∂r as an Expression. virtual ExpressionPtr grad_expr_r( [[maybe_unused]] const ExpressionPtr& lhs, [[maybe_unused]] const ExpressionPtr& rhs, @@ -456,7 +415,7 @@ struct Expression { template ExpressionPtr constant_ptr(Scalar value) { - return make_expression_ptr>(value); + return make_expression_ptr>(value); } template @@ -470,20 +429,16 @@ ExpressionPtr sinh(const ExpressionPtr& x); template ExpressionPtr sqrt(const ExpressionPtr& x); -/** - * Derived expression type for binary minus operator. - * - * @tparam Scalar Scalar type. - * @tparam T Expression type. - */ +/// Derived expression type for binary minus operator. +/// +/// @tparam Scalar Scalar type. +/// @tparam T Expression type. template struct BinaryMinusExpression final : Expression { - /** - * Constructs a binary expression (an operator with two arguments). - * - * @param lhs Binary operator's left operand. - * @param rhs Binary operator's right operand. - */ + /// Constructs a binary expression (an operator with two arguments). + /// + /// @param lhs Binary operator's left operand. + /// @param rhs Binary operator's right operand. constexpr BinaryMinusExpression(ExpressionPtr lhs, ExpressionPtr rhs) : Expression{std::move(lhs), std::move(rhs)} {} @@ -492,6 +447,8 @@ struct BinaryMinusExpression final : Expression { ExpressionType type() const override { return T; } + std::string_view name() const override { return "binary minus"; } + Scalar grad_l(Scalar, Scalar, Scalar parent_adjoint) const override { return parent_adjoint; } @@ -513,20 +470,16 @@ struct BinaryMinusExpression final : Expression { } }; -/** - * Derived expression type for binary plus operator. - * - * @tparam Scalar Scalar type. - * @tparam T Expression type. - */ +/// Derived expression type for binary plus operator. +/// +/// @tparam Scalar Scalar type. +/// @tparam T Expression type. template struct BinaryPlusExpression final : Expression { - /** - * Constructs a binary expression (an operator with two arguments). - * - * @param lhs Binary operator's left operand. - * @param rhs Binary operator's right operand. - */ + /// Constructs a binary expression (an operator with two arguments). + /// + /// @param lhs Binary operator's left operand. + /// @param rhs Binary operator's right operand. constexpr BinaryPlusExpression(ExpressionPtr lhs, ExpressionPtr rhs) : Expression{std::move(lhs), std::move(rhs)} {} @@ -535,6 +488,8 @@ struct BinaryPlusExpression final : Expression { ExpressionType type() const override { return T; } + std::string_view name() const override { return "binary plus"; } + Scalar grad_l(Scalar, Scalar, Scalar parent_adjoint) const override { return parent_adjoint; } @@ -556,18 +511,14 @@ struct BinaryPlusExpression final : Expression { } }; -/** - * Derived expression type for cbrt(). - * - * @tparam Scalar Scalar type. - */ +/// Derived expression type for cbrt(). +/// +/// @tparam Scalar Scalar type. template struct CbrtExpression final : Expression { - /** - * Constructs an unary expression (an operator with one argument). - * - * @param lhs Unary operator's operand. - */ + /// Constructs an unary expression (an operator with one argument). + /// + /// @param lhs Unary operator's operand. explicit constexpr CbrtExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {} @@ -578,6 +529,8 @@ struct CbrtExpression final : Expression { ExpressionType type() const override { return ExpressionType::NONLINEAR; } + std::string_view name() const override { return "cbrt"; } + Scalar grad_l(Scalar x, Scalar, Scalar parent_adjoint) const override { using std::cbrt; @@ -593,12 +546,10 @@ struct CbrtExpression final : Expression { } }; -/** - * cbrt() for Expressions. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// cbrt() for Expressions. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template ExpressionPtr cbrt(const ExpressionPtr& x) { using enum ExpressionType; @@ -619,65 +570,55 @@ ExpressionPtr cbrt(const ExpressionPtr& x) { return make_expression_ptr>(x); } -/** - * Derived expression type for constant. - * - * @tparam Scalar Scalar type. - */ +/// Derived expression type for constant. +/// +/// @tparam Scalar Scalar type. template -struct ConstExpression final : Expression { - /** - * Constructs a nullary expression (an operator with no arguments). - * - * @param value The expression value. - */ - explicit constexpr ConstExpression(Scalar value) +struct ConstantExpression final : Expression { + /// Constructs a nullary expression (an operator with no arguments). + /// + /// @param value The expression value. + explicit constexpr ConstantExpression(Scalar value) : Expression{value} {} Scalar value(Scalar, Scalar) const override { return this->val; } ExpressionType type() const override { return ExpressionType::CONSTANT; } + + std::string_view name() const override { return "constant"; } }; -/** - * Derived expression type for decision variable. - * - * @tparam Scalar Scalar type. - */ +/// Derived expression type for decision variable. +/// +/// @tparam Scalar Scalar type. template struct DecisionVariableExpression final : Expression { - /** - * Constructs a decision variable expression with a value of zero. - */ + /// Constructs a decision variable expression with a value of zero. constexpr DecisionVariableExpression() = default; - /** - * Constructs a nullary expression (an operator with no arguments). - * - * @param value The expression value. - */ + /// Constructs a nullary expression (an operator with no arguments). + /// + /// @param value The expression value. explicit constexpr DecisionVariableExpression(Scalar value) : Expression{value} {} Scalar value(Scalar, Scalar) const override { return this->val; } ExpressionType type() const override { return ExpressionType::LINEAR; } + + std::string_view name() const override { return "decision variable"; } }; -/** - * Derived expression type for binary division operator. - * - * @tparam Scalar Scalar type. - * @tparam T Expression type. - */ +/// Derived expression type for binary division operator. +/// +/// @tparam Scalar Scalar type. +/// @tparam T Expression type. template struct DivExpression final : Expression { - /** - * Constructs a binary expression (an operator with two arguments). - * - * @param lhs Binary operator's left operand. - * @param rhs Binary operator's right operand. - */ + /// Constructs a binary expression (an operator with two arguments). + /// + /// @param lhs Binary operator's left operand. + /// @param rhs Binary operator's right operand. constexpr DivExpression(ExpressionPtr lhs, ExpressionPtr rhs) : Expression{std::move(lhs), std::move(rhs)} {} @@ -685,6 +626,8 @@ struct DivExpression final : Expression { ExpressionType type() const override { return T; } + std::string_view name() const override { return "division"; } + Scalar grad_l(Scalar, Scalar rhs, Scalar parent_adjoint) const override { return parent_adjoint / rhs; }; @@ -706,20 +649,16 @@ struct DivExpression final : Expression { } }; -/** - * Derived expression type for binary multiplication operator. - * - * @tparam Scalar Scalar type. - * @tparam T Expression type. - */ +/// Derived expression type for binary multiplication operator. +/// +/// @tparam Scalar Scalar type. +/// @tparam T Expression type. template struct MultExpression final : Expression { - /** - * Constructs a binary expression (an operator with two arguments). - * - * @param lhs Binary operator's left operand. - * @param rhs Binary operator's right operand. - */ + /// Constructs a binary expression (an operator with two arguments). + /// + /// @param lhs Binary operator's left operand. + /// @param rhs Binary operator's right operand. constexpr MultExpression(ExpressionPtr lhs, ExpressionPtr rhs) : Expression{std::move(lhs), std::move(rhs)} {} @@ -727,6 +666,8 @@ struct MultExpression final : Expression { ExpressionType type() const override { return T; } + std::string_view name() const override { return "multiplication"; } + Scalar grad_l([[maybe_unused]] Scalar lhs, Scalar rhs, Scalar parent_adjoint) const override { return parent_adjoint * rhs; @@ -752,19 +693,15 @@ struct MultExpression final : Expression { } }; -/** - * Derived expression type for unary minus operator. - * - * @tparam Scalar Scalar type. - * @tparam T Expression type. - */ +/// Derived expression type for unary minus operator. +/// +/// @tparam Scalar Scalar type. +/// @tparam T Expression type. template struct UnaryMinusExpression final : Expression { - /** - * Constructs an unary expression (an operator with one argument). - * - * @param lhs Unary operator's operand. - */ + /// Constructs an unary expression (an operator with one argument). + /// + /// @param lhs Unary operator's operand. explicit constexpr UnaryMinusExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {} @@ -772,6 +709,8 @@ struct UnaryMinusExpression final : Expression { ExpressionType type() const override { return T; } + std::string_view name() const override { return "unary minus"; } + Scalar grad_l(Scalar, Scalar, Scalar parent_adjoint) const override { return -parent_adjoint; } @@ -783,23 +722,19 @@ struct UnaryMinusExpression final : Expression { } }; -/** - * Refcount increment for intrusive shared pointer. - * - * @tparam Scalar Scalar type. - * @param expr The shared pointer's managed object. - */ +/// Refcount increment for intrusive shared pointer. +/// +/// @tparam Scalar Scalar type. +/// @param expr The shared pointer's managed object. template constexpr void inc_ref_count(Expression* expr) { ++expr->ref_count; } -/** - * Refcount decrement for intrusive shared pointer. - * - * @tparam Scalar Scalar type. - * @param expr The shared pointer's managed object. - */ +/// Refcount decrement for intrusive shared pointer. +/// +/// @tparam Scalar Scalar type. +/// @param expr The shared pointer's managed object. template void dec_ref_count(Expression* expr) { // If a deeply nested tree is being deallocated all at once, calling the @@ -836,18 +771,14 @@ void dec_ref_count(Expression* expr) { } } -/** - * Derived expression type for abs(). - * - * @tparam Scalar Scalar type. - */ +/// Derived expression type for abs(). +/// +/// @tparam Scalar Scalar type. template struct AbsExpression final : Expression { - /** - * Constructs an unary expression (an operator with one argument). - * - * @param lhs Unary operator's operand. - */ + /// Constructs an unary expression (an operator with one argument). + /// + /// @param lhs Unary operator's operand. explicit constexpr AbsExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {} @@ -858,6 +789,8 @@ struct AbsExpression final : Expression { ExpressionType type() const override { return ExpressionType::NONLINEAR; } + std::string_view name() const override { return "abs"; } + Scalar grad_l(Scalar x, Scalar, Scalar parent_adjoint) const override { if (x < Scalar(0)) { return -parent_adjoint; @@ -881,12 +814,10 @@ struct AbsExpression final : Expression { } }; -/** - * abs() for Expressions. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// abs() for Expressions. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template ExpressionPtr abs(const ExpressionPtr& x) { using enum ExpressionType; @@ -906,18 +837,14 @@ ExpressionPtr abs(const ExpressionPtr& x) { return make_expression_ptr>(x); } -/** - * Derived expression type for acos(). - * - * @tparam Scalar Scalar type. - */ +/// Derived expression type for acos(). +/// +/// @tparam Scalar Scalar type. template struct AcosExpression final : Expression { - /** - * Constructs an unary expression (an operator with one argument). - * - * @param lhs Unary operator's operand. - */ + /// Constructs an unary expression (an operator with one argument). + /// + /// @param lhs Unary operator's operand. explicit constexpr AcosExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {} @@ -928,6 +855,8 @@ struct AcosExpression final : Expression { ExpressionType type() const override { return ExpressionType::NONLINEAR; } + std::string_view name() const override { return "acos"; } + Scalar grad_l(Scalar x, Scalar, Scalar parent_adjoint) const override { using std::sqrt; return -parent_adjoint / sqrt(Scalar(1) - x * x); @@ -940,12 +869,10 @@ struct AcosExpression final : Expression { } }; -/** - * acos() for Expressions. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// acos() for Expressions. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template ExpressionPtr acos(const ExpressionPtr& x) { using enum ExpressionType; @@ -964,18 +891,14 @@ ExpressionPtr acos(const ExpressionPtr& x) { return make_expression_ptr>(x); } -/** - * Derived expression type for asin(). - * - * @tparam Scalar Scalar type. - */ +/// Derived expression type for asin(). +/// +/// @tparam Scalar Scalar type. template struct AsinExpression final : Expression { - /** - * Constructs an unary expression (an operator with one argument). - * - * @param lhs Unary operator's operand. - */ + /// Constructs an unary expression (an operator with one argument). + /// + /// @param lhs Unary operator's operand. explicit constexpr AsinExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {} @@ -986,6 +909,8 @@ struct AsinExpression final : Expression { ExpressionType type() const override { return ExpressionType::NONLINEAR; } + std::string_view name() const override { return "asin"; } + Scalar grad_l(Scalar x, Scalar, Scalar parent_adjoint) const override { using std::sqrt; return parent_adjoint / sqrt(Scalar(1) - x * x); @@ -998,12 +923,10 @@ struct AsinExpression final : Expression { } }; -/** - * asin() for Expressions. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// asin() for Expressions. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template ExpressionPtr asin(const ExpressionPtr& x) { using enum ExpressionType; @@ -1023,18 +946,14 @@ ExpressionPtr asin(const ExpressionPtr& x) { return make_expression_ptr>(x); } -/** - * Derived expression type for atan(). - * - * @tparam Scalar Scalar type. - */ +/// Derived expression type for atan(). +/// +/// @tparam Scalar Scalar type. template struct AtanExpression final : Expression { - /** - * Constructs an unary expression (an operator with one argument). - * - * @param lhs Unary operator's operand. - */ + /// Constructs an unary expression (an operator with one argument). + /// + /// @param lhs Unary operator's operand. explicit constexpr AtanExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {} @@ -1045,6 +964,8 @@ struct AtanExpression final : Expression { ExpressionType type() const override { return ExpressionType::NONLINEAR; } + std::string_view name() const override { return "atan"; } + Scalar grad_l(Scalar x, Scalar, Scalar parent_adjoint) const override { return parent_adjoint / (Scalar(1) + x * x); } @@ -1056,12 +977,10 @@ struct AtanExpression final : Expression { } }; -/** - * atan() for Expressions. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// atan() for Expressions. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template ExpressionPtr atan(const ExpressionPtr& x) { using enum ExpressionType; @@ -1081,19 +1000,15 @@ ExpressionPtr atan(const ExpressionPtr& x) { return make_expression_ptr>(x); } -/** - * Derived expression type for atan2(). - * - * @tparam Scalar Scalar type. - */ +/// Derived expression type for atan2(). +/// +/// @tparam Scalar Scalar type. template struct Atan2Expression final : Expression { - /** - * Constructs a binary expression (an operator with two arguments). - * - * @param lhs Binary operator's left operand. - * @param rhs Binary operator's right operand. - */ + /// Constructs a binary expression (an operator with two arguments). + /// + /// @param lhs Binary operator's left operand. + /// @param rhs Binary operator's right operand. constexpr Atan2Expression(ExpressionPtr lhs, ExpressionPtr rhs) : Expression{std::move(lhs), std::move(rhs)} {} @@ -1105,6 +1020,8 @@ struct Atan2Expression final : Expression { ExpressionType type() const override { return ExpressionType::NONLINEAR; } + std::string_view name() const override { return "atan2"; } + Scalar grad_l(Scalar y, Scalar x, Scalar parent_adjoint) const override { return parent_adjoint * x / (y * y + x * x); } @@ -1126,13 +1043,11 @@ struct Atan2Expression final : Expression { } }; -/** - * atan2() for Expressions. - * - * @tparam Scalar Scalar type. - * @param y The y argument. - * @param x The x argument. - */ +/// atan2() for Expressions. +/// +/// @tparam Scalar Scalar type. +/// @param y The y argument. +/// @param x The x argument. template ExpressionPtr atan2(const ExpressionPtr& y, const ExpressionPtr& x) { @@ -1155,18 +1070,14 @@ ExpressionPtr atan2(const ExpressionPtr& y, return make_expression_ptr>(y, x); } -/** - * Derived expression type for cos(). - * - * @tparam Scalar Scalar type. - */ +/// Derived expression type for cos(). +/// +/// @tparam Scalar Scalar type. template struct CosExpression final : Expression { - /** - * Constructs an unary expression (an operator with one argument). - * - * @param lhs Unary operator's operand. - */ + /// Constructs an unary expression (an operator with one argument). + /// + /// @param lhs Unary operator's operand. explicit constexpr CosExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {} @@ -1177,6 +1088,8 @@ struct CosExpression final : Expression { ExpressionType type() const override { return ExpressionType::NONLINEAR; } + std::string_view name() const override { return "cos"; } + Scalar grad_l(Scalar x, Scalar, Scalar parent_adjoint) const override { using std::sin; return parent_adjoint * -sin(x); @@ -1189,12 +1102,10 @@ struct CosExpression final : Expression { } }; -/** - * cos() for Expressions. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// cos() for Expressions. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template ExpressionPtr cos(const ExpressionPtr& x) { using enum ExpressionType; @@ -1213,18 +1124,14 @@ ExpressionPtr cos(const ExpressionPtr& x) { return make_expression_ptr>(x); } -/** - * Derived expression type for cosh(). - * - * @tparam Scalar Scalar type. - */ +/// Derived expression type for cosh(). +/// +/// @tparam Scalar Scalar type. template struct CoshExpression final : Expression { - /** - * Constructs an unary expression (an operator with one argument). - * - * @param lhs Unary operator's operand. - */ + /// Constructs an unary expression (an operator with one argument). + /// + /// @param lhs Unary operator's operand. explicit constexpr CoshExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {} @@ -1235,6 +1142,8 @@ struct CoshExpression final : Expression { ExpressionType type() const override { return ExpressionType::NONLINEAR; } + std::string_view name() const override { return "cosh"; } + Scalar grad_l(Scalar x, Scalar, Scalar parent_adjoint) const override { using std::sinh; return parent_adjoint * sinh(x); @@ -1247,12 +1156,10 @@ struct CoshExpression final : Expression { } }; -/** - * cosh() for Expressions. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// cosh() for Expressions. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template ExpressionPtr cosh(const ExpressionPtr& x) { using enum ExpressionType; @@ -1271,18 +1178,14 @@ ExpressionPtr cosh(const ExpressionPtr& x) { return make_expression_ptr>(x); } -/** - * Derived expression type for erf(). - * - * @tparam Scalar Scalar type. - */ +/// Derived expression type for erf(). +/// +/// @tparam Scalar Scalar type. template struct ErfExpression final : Expression { - /** - * Constructs an unary expression (an operator with one argument). - * - * @param lhs Unary operator's operand. - */ + /// Constructs an unary expression (an operator with one argument). + /// + /// @param lhs Unary operator's operand. explicit constexpr ErfExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {} @@ -1293,6 +1196,8 @@ struct ErfExpression final : Expression { ExpressionType type() const override { return ExpressionType::NONLINEAR; } + std::string_view name() const override { return "erf"; } + Scalar grad_l(Scalar x, Scalar, Scalar parent_adjoint) const override { using std::exp; return parent_adjoint * Scalar(2.0 * std::numbers::inv_sqrtpi) * @@ -1307,12 +1212,10 @@ struct ErfExpression final : Expression { } }; -/** - * erf() for Expressions. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// erf() for Expressions. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template ExpressionPtr erf(const ExpressionPtr& x) { using enum ExpressionType; @@ -1332,18 +1235,14 @@ ExpressionPtr erf(const ExpressionPtr& x) { return make_expression_ptr>(x); } -/** - * Derived expression type for exp(). - * - * @tparam Scalar Scalar type. - */ +/// Derived expression type for exp(). +/// +/// @tparam Scalar Scalar type. template struct ExpExpression final : Expression { - /** - * Constructs an unary expression (an operator with one argument). - * - * @param lhs Unary operator's operand. - */ + /// Constructs an unary expression (an operator with one argument). + /// + /// @param lhs Unary operator's operand. explicit constexpr ExpExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {} @@ -1354,6 +1253,8 @@ struct ExpExpression final : Expression { ExpressionType type() const override { return ExpressionType::NONLINEAR; } + std::string_view name() const override { return "exp"; } + Scalar grad_l(Scalar x, Scalar, Scalar parent_adjoint) const override { using std::exp; return parent_adjoint * exp(x); @@ -1366,12 +1267,10 @@ struct ExpExpression final : Expression { } }; -/** - * exp() for Expressions. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// exp() for Expressions. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template ExpressionPtr exp(const ExpressionPtr& x) { using enum ExpressionType; @@ -1394,19 +1293,15 @@ template ExpressionPtr hypot(const ExpressionPtr& x, const ExpressionPtr& y); -/** - * Derived expression type for hypot(). - * - * @tparam Scalar Scalar type. - */ +/// Derived expression type for hypot(). +/// +/// @tparam Scalar Scalar type. template struct HypotExpression final : Expression { - /** - * Constructs a binary expression (an operator with two arguments). - * - * @param lhs Binary operator's left operand. - * @param rhs Binary operator's right operand. - */ + /// Constructs a binary expression (an operator with two arguments). + /// + /// @param lhs Binary operator's left operand. + /// @param rhs Binary operator's right operand. constexpr HypotExpression(ExpressionPtr lhs, ExpressionPtr rhs) : Expression{std::move(lhs), std::move(rhs)} {} @@ -1418,6 +1313,8 @@ struct HypotExpression final : Expression { ExpressionType type() const override { return ExpressionType::NONLINEAR; } + std::string_view name() const override { return "hypot"; } + Scalar grad_l(Scalar x, Scalar y, Scalar parent_adjoint) const override { using std::hypot; return parent_adjoint * x / hypot(x, y); @@ -1441,13 +1338,11 @@ struct HypotExpression final : Expression { } }; -/** - * hypot() for Expressions. - * - * @tparam Scalar Scalar type. - * @param x The x argument. - * @param y The y argument. - */ +/// hypot() for Expressions. +/// +/// @tparam Scalar Scalar type. +/// @param x The x argument. +/// @param y The y argument. template ExpressionPtr hypot(const ExpressionPtr& x, const ExpressionPtr& y) { @@ -1469,18 +1364,14 @@ ExpressionPtr hypot(const ExpressionPtr& x, return make_expression_ptr>(x, y); } -/** - * Derived expression type for log(). - * - * @tparam Scalar Scalar type. - */ +/// Derived expression type for log(). +/// +/// @tparam Scalar Scalar type. template struct LogExpression final : Expression { - /** - * Constructs an unary expression (an operator with one argument). - * - * @param lhs Unary operator's operand. - */ + /// Constructs an unary expression (an operator with one argument). + /// + /// @param lhs Unary operator's operand. explicit constexpr LogExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {} @@ -1491,6 +1382,8 @@ struct LogExpression final : Expression { ExpressionType type() const override { return ExpressionType::NONLINEAR; } + std::string_view name() const override { return "log"; } + Scalar grad_l(Scalar x, Scalar, Scalar parent_adjoint) const override { return parent_adjoint / x; } @@ -1502,12 +1395,10 @@ struct LogExpression final : Expression { } }; -/** - * log() for Expressions. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// log() for Expressions. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template ExpressionPtr log(const ExpressionPtr& x) { using enum ExpressionType; @@ -1527,18 +1418,14 @@ ExpressionPtr log(const ExpressionPtr& x) { return make_expression_ptr>(x); } -/** - * Derived expression type for log10(). - * - * @tparam Scalar Scalar type. - */ +/// Derived expression type for log10(). +/// +/// @tparam Scalar Scalar type. template struct Log10Expression final : Expression { - /** - * Constructs an unary expression (an operator with one argument). - * - * @param lhs Unary operator's operand. - */ + /// Constructs an unary expression (an operator with one argument). + /// + /// @param lhs Unary operator's operand. explicit constexpr Log10Expression(ExpressionPtr lhs) : Expression{std::move(lhs)} {} @@ -1549,6 +1436,8 @@ struct Log10Expression final : Expression { ExpressionType type() const override { return ExpressionType::NONLINEAR; } + std::string_view name() const override { return "log10"; } + Scalar grad_l(Scalar x, Scalar, Scalar parent_adjoint) const override { return parent_adjoint / (Scalar(std::numbers::ln10) * x); } @@ -1560,12 +1449,10 @@ struct Log10Expression final : Expression { } }; -/** - * log10() for Expressions. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// log10() for Expressions. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template ExpressionPtr log10(const ExpressionPtr& x) { using enum ExpressionType; @@ -1589,20 +1476,16 @@ template ExpressionPtr pow(const ExpressionPtr& base, const ExpressionPtr& power); -/** - * Derived expression type for pow(). - * - * @tparam Scalar Scalar type. - * @tparam T Expression type. - */ +/// Derived expression type for pow(). +/// +/// @tparam Scalar Scalar type. +/// @tparam T Expression type. template struct PowExpression final : Expression { - /** - * Constructs a binary expression (an operator with two arguments). - * - * @param lhs Binary operator's left operand. - * @param rhs Binary operator's right operand. - */ + /// Constructs a binary expression (an operator with two arguments). + /// + /// @param lhs Binary operator's left operand. + /// @param rhs Binary operator's right operand. constexpr PowExpression(ExpressionPtr lhs, ExpressionPtr rhs) : Expression{std::move(lhs), std::move(rhs)} {} @@ -1613,6 +1496,8 @@ struct PowExpression final : Expression { ExpressionType type() const override { return T; } + std::string_view name() const override { return "pow"; } + Scalar grad_l(Scalar base, Scalar power, Scalar parent_adjoint) const override { using std::pow; @@ -1651,13 +1536,11 @@ struct PowExpression final : Expression { } }; -/** - * pow() for Expressions. - * - * @tparam Scalar Scalar type. - * @param base The base. - * @param power The power. - */ +/// pow() for Expressions. +/// +/// @tparam Scalar Scalar type. +/// @param base The base. +/// @param power The power. template ExpressionPtr pow(const ExpressionPtr& base, const ExpressionPtr& power) { @@ -1694,18 +1577,14 @@ ExpressionPtr pow(const ExpressionPtr& base, return make_expression_ptr>(base, power); } -/** - * Derived expression type for sign(). - * - * @tparam Scalar Scalar type. - */ +/// Derived expression type for sign(). +/// +/// @tparam Scalar Scalar type. template struct SignExpression final : Expression { - /** - * Constructs an unary expression (an operator with one argument). - * - * @param lhs Unary operator's operand. - */ + /// Constructs an unary expression (an operator with one argument). + /// + /// @param lhs Unary operator's operand. explicit constexpr SignExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {} @@ -1720,14 +1599,14 @@ struct SignExpression final : Expression { } ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + std::string_view name() const override { return "sign"; } }; -/** - * sign() for Expressions. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// sign() for Expressions. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template ExpressionPtr sign(const ExpressionPtr& x) { using enum ExpressionType; @@ -1747,18 +1626,14 @@ ExpressionPtr sign(const ExpressionPtr& x) { return make_expression_ptr>(x); } -/** - * Derived expression type for sin(). - * - * @tparam Scalar Scalar type. - */ +/// Derived expression type for sin(). +/// +/// @tparam Scalar Scalar type. template struct SinExpression final : Expression { - /** - * Constructs an unary expression (an operator with one argument). - * - * @param lhs Unary operator's operand. - */ + /// Constructs an unary expression (an operator with one argument). + /// + /// @param lhs Unary operator's operand. explicit constexpr SinExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {} @@ -1769,6 +1644,8 @@ struct SinExpression final : Expression { ExpressionType type() const override { return ExpressionType::NONLINEAR; } + std::string_view name() const override { return "sin"; } + Scalar grad_l(Scalar x, Scalar, Scalar parent_adjoint) const override { using std::cos; return parent_adjoint * cos(x); @@ -1781,12 +1658,10 @@ struct SinExpression final : Expression { } }; -/** - * sin() for Expressions. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// sin() for Expressions. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template ExpressionPtr sin(const ExpressionPtr& x) { using enum ExpressionType; @@ -1806,18 +1681,14 @@ ExpressionPtr sin(const ExpressionPtr& x) { return make_expression_ptr>(x); } -/** - * Derived expression type for sinh(). - * - * @tparam Scalar Scalar type. - */ +/// Derived expression type for sinh(). +/// +/// @tparam Scalar Scalar type. template struct SinhExpression final : Expression { - /** - * Constructs an unary expression (an operator with one argument). - * - * @param lhs Unary operator's operand. - */ + /// Constructs an unary expression (an operator with one argument). + /// + /// @param lhs Unary operator's operand. explicit constexpr SinhExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {} @@ -1828,6 +1699,8 @@ struct SinhExpression final : Expression { ExpressionType type() const override { return ExpressionType::NONLINEAR; } + std::string_view name() const override { return "sinh"; } + Scalar grad_l(Scalar x, Scalar, Scalar parent_adjoint) const override { using std::cosh; return parent_adjoint * cosh(x); @@ -1840,12 +1713,10 @@ struct SinhExpression final : Expression { } }; -/** - * sinh() for Expressions. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// sinh() for Expressions. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template ExpressionPtr sinh(const ExpressionPtr& x) { using enum ExpressionType; @@ -1865,18 +1736,14 @@ ExpressionPtr sinh(const ExpressionPtr& x) { return make_expression_ptr>(x); } -/** - * Derived expression type for sqrt(). - * - * @tparam Scalar Scalar type. - */ +/// Derived expression type for sqrt(). +/// +/// @tparam Scalar Scalar type. template struct SqrtExpression final : Expression { - /** - * Constructs an unary expression (an operator with one argument). - * - * @param lhs Unary operator's operand. - */ + /// Constructs an unary expression (an operator with one argument). + /// + /// @param lhs Unary operator's operand. explicit constexpr SqrtExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {} @@ -1887,6 +1754,8 @@ struct SqrtExpression final : Expression { ExpressionType type() const override { return ExpressionType::NONLINEAR; } + std::string_view name() const override { return "sqrt"; } + Scalar grad_l(Scalar x, Scalar, Scalar parent_adjoint) const override { using std::sqrt; return parent_adjoint / (Scalar(2) * sqrt(x)); @@ -1899,12 +1768,10 @@ struct SqrtExpression final : Expression { } }; -/** - * sqrt() for Expressions. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// sqrt() for Expressions. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template ExpressionPtr sqrt(const ExpressionPtr& x) { using enum ExpressionType; @@ -1925,18 +1792,14 @@ ExpressionPtr sqrt(const ExpressionPtr& x) { return make_expression_ptr>(x); } -/** - * Derived expression type for tan(). - * - * @tparam Scalar Scalar type. - */ +/// Derived expression type for tan(). +/// +/// @tparam Scalar Scalar type. template struct TanExpression final : Expression { - /** - * Constructs an unary expression (an operator with one argument). - * - * @param lhs Unary operator's operand. - */ + /// Constructs an unary expression (an operator with one argument). + /// + /// @param lhs Unary operator's operand. explicit constexpr TanExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {} @@ -1947,6 +1810,8 @@ struct TanExpression final : Expression { ExpressionType type() const override { return ExpressionType::NONLINEAR; } + std::string_view name() const override { return "tan"; } + Scalar grad_l(Scalar x, Scalar, Scalar parent_adjoint) const override { using std::cos; @@ -1962,12 +1827,10 @@ struct TanExpression final : Expression { } }; -/** - * tan() for Expressions. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// tan() for Expressions. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template ExpressionPtr tan(const ExpressionPtr& x) { using enum ExpressionType; @@ -1987,18 +1850,14 @@ ExpressionPtr tan(const ExpressionPtr& x) { return make_expression_ptr>(x); } -/** - * Derived expression type for tanh(). - * - * @tparam Scalar Scalar type. - */ +/// Derived expression type for tanh(). +/// +/// @tparam Scalar Scalar type. template struct TanhExpression final : Expression { - /** - * Constructs an unary expression (an operator with one argument). - * - * @param lhs Unary operator's operand. - */ + /// Constructs an unary expression (an operator with one argument). + /// + /// @param lhs Unary operator's operand. explicit constexpr TanhExpression(ExpressionPtr lhs) : Expression{std::move(lhs)} {} @@ -2009,6 +1868,8 @@ struct TanhExpression final : Expression { ExpressionType type() const override { return ExpressionType::NONLINEAR; } + std::string_view name() const override { return "tanh"; } + Scalar grad_l(Scalar x, Scalar, Scalar parent_adjoint) const override { using std::cosh; @@ -2024,12 +1885,10 @@ struct TanhExpression final : Expression { } }; -/** - * tanh() for Expressions. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// tanh() for Expressions. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template ExpressionPtr tanh(const ExpressionPtr& x) { using enum ExpressionType; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_graph.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_graph.hpp index df3d93e9ad..bb542696fa 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_graph.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_graph.hpp @@ -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 gch::small_vector*> topological_sort( const ExpressionPtr& root) { @@ -70,13 +68,11 @@ gch::small_vector*> 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 void update_values(const gch::small_vector*>& list) { // Traverse graph from child to parent and update values diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_type.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_type.hpp index 9e323d87f4..8a2ae45780 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_type.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_type.hpp @@ -4,17 +4,15 @@ #include -#include +#include -#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 { + /// 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 + 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 m_underlying; +}; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient.hpp index 80720accf8..96b51f2229 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient.hpp @@ -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 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 variable, Variable 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 variable, SleipnirMatrixLike 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 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& value() { m_g = m_jacobian.value().transpose(); diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/adjoint_expression_graph.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient_expression_graph.hpp similarity index 76% rename from wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/adjoint_expression_graph.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient_expression_graph.hpp index a77323eee9..d21c1b5ad1 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/adjoint_expression_graph.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient_expression_graph.hpp @@ -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 -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& root) + /// Generates the gradient graph for the given expression. + /// + /// @param root The root node of the expression. + explicit GradientExpressionGraph(const Variable& root) : m_top_list{topological_sort(root.expr)} { for (const auto& node : m_top_list) { m_col_list.emplace_back(node->col); } } - /** - * Update the values of all nodes in this adjoint graph based on the values of - * their dependent nodes. - */ + /// 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 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 generate_tree( const VariableMatrix& 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>& triplets, int row, - const VariableMatrix& 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>& triplets, + int row, const VariableMatrix& wrt) const { slp_assert(wrt.cols() == 1); // Read docs/algorithms.md#Reverse_accumulation_automatic_differentiation diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/hessian.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/hessian.hpp index 10ee142ff8..b52371b182 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/hessian.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/hessian.hpp @@ -7,7 +7,7 @@ #include #include -#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 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 variable, Variable wrt) : Hessian{std::move(variable), VariableMatrix{std::move(wrt)}} {} - /** - * Constructs a Hessian object. - * - * @param variable Variable of which to compute the Hessian. - * @param wrt Vector of variables with respect to which to compute the - * Hessian. - */ + /// Constructs a Hessian object. + /// + /// @param variable Variable of which to compute the Hessian. + /// @param wrt Vector of variables with respect to which to compute the + /// Hessian. Hessian(Variable variable, SleipnirMatrixLike auto wrt) - : m_variables{detail::AdjointExpressionGraph{variable} - .generate_gradient_tree(wrt)}, + : m_variables{detail::GradientExpressionGraph{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 get() const { VariableMatrix 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& 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 m_variables; VariableMatrix m_wrt; - gch::small_vector> m_graphs; + gch::small_vector> m_graphs; Eigen::SparseMatrix m_H{m_variables.rows(), m_wrt.rows()}; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/jacobian.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/jacobian.hpp index c8e28a826f..082a6cf678 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/jacobian.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/jacobian.hpp @@ -7,7 +7,7 @@ #include #include -#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 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 variable, Variable wrt) : Jacobian{VariableMatrix{std::move(variable)}, VariableMatrix{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 variable, SleipnirMatrixLike auto wrt) : Jacobian{VariableMatrix{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 variables, SleipnirMatrixLike 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 get() const { VariableMatrix 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& 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 m_variables; VariableMatrix m_wrt; - gch::small_vector> m_graphs; + gch::small_vector> m_graphs; Eigen::SparseMatrix m_J{m_variables.rows(), m_wrt.rows()}; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/sleipnir_base.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/sleipnir_base.hpp index 874c0a4422..647f1b8b6a 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/sleipnir_base.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/sleipnir_base.hpp @@ -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 diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/slice.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/slice.hpp index 7df1ebc11d..be4f477d86 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/slice.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/slice.hpp @@ -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::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 requires(std::same_as || std::convertible_to) && @@ -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 requires(std::same_as || std::convertible_to) && @@ -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::max()); diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable.hpp index cb4c1a56ec..7616c02b45 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable.hpp @@ -29,7 +29,7 @@ namespace slp { namespace detail { template -class AdjointExpressionGraph; +class GradientExpressionGraph; } // namespace detail @@ -40,103 +40,80 @@ class Hessian; template class Jacobian; -/** - * An autodiff variable pointing to an expression node. - * - * @tparam Scalar_ Scalar type. - */ +/// An autodiff variable pointing to an expression node. +/// +/// @tparam Scalar_ Scalar type. template class Variable : public SleipnirBase { public: - /** - * Scalar type alias. - */ + /// Scalar type alias. using Scalar = Scalar_; - /** - * Constructs a linear Variable with a value of zero. - */ + /// Constructs a linear Variable with a value of zero. Variable() = default; - /** - * Constructs an empty Variable. - */ + /// Constructs an empty Variable. explicit Variable(std::nullptr_t) : expr{nullptr} {} - /** - * Constructs a Variable from a scalar type. - * - * @param value The value of the Variable. - */ + /// Constructs a Variable from a scalar type. + /// + /// @param value The value of the Variable. // NOLINTNEXTLINE (google-explicit-constructor) Variable(Scalar value) requires(!MatrixLike) - : expr{detail::make_expression_ptr>( + : expr{detail::make_expression_ptr>( value)} {} - /** - * Constructs a Variable from a scalar type. - * - * @param value The value of the Variable. - */ + /// Constructs a Variable from a scalar type. + /// + /// @param value The value of the Variable. // NOLINTNEXTLINE (google-explicit-constructor) Variable(SleipnirMatrixLike auto value) : expr{value(0, 0).expr} { slp_assert(value.rows() == 1 && value.cols() == 1); } - /** - * Constructs a Variable from a floating-point type. - * - * @param value The value of the Variable. - */ + /// Constructs a Variable from a floating-point type. + /// + /// @param value The value of the Variable. // NOLINTNEXTLINE (google-explicit-constructor) Variable(std::floating_point auto value) - : expr{detail::make_expression_ptr>( + : expr{detail::make_expression_ptr>( Scalar(value))} {} - /** - * Constructs a Variable from an integral type. - * - * @param value The value of the Variable. - */ + /// Constructs a Variable from an integral type. + /// + /// @param value The value of the Variable. // NOLINTNEXTLINE (google-explicit-constructor) Variable(std::integral auto value) - : expr{detail::make_expression_ptr>( + : expr{detail::make_expression_ptr>( Scalar(value))} {} - /** - * Constructs a Variable pointing to the specified expression. - * - * @param expr The autodiff variable. - */ + /// Constructs a Variable pointing to the specified expression. + /// + /// @param expr The autodiff variable. explicit Variable(const detail::ExpressionPtr& expr) : expr{expr} {} - /** - * Constructs a Variable pointing to the specified expression. - * - * @param expr The autodiff variable. - */ + /// Constructs a Variable pointing to the specified expression. + /// + /// @param expr The autodiff variable. explicit Variable(detail::ExpressionPtr&& expr) : expr{std::move(expr)} {} - /** - * Assignment operator for scalar. - * - * @param value The value of the Variable. - * @return This variable. - */ + /// Assignment operator for scalar. + /// + /// @param value The value of the Variable. + /// @return This variable. Variable& operator=(ScalarLike auto value) { - expr = detail::make_expression_ptr>(value); + expr = + detail::make_expression_ptr>(value); m_graph_initialized = false; return *this; } - /** - * Sets Variable's internal value. - * - * @param value The value of the Variable. - */ + /// Sets Variable's internal value. + /// + /// @param value The value of the Variable. void set_value(Scalar value) { #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS // We only need to check the first argument since unary and binary operators @@ -152,11 +129,9 @@ class Variable : public SleipnirBase { expr->val = Scalar(value); } - /** - * Returns the value of this variable. - * - * @return The value of this variable. - */ + /// Returns the value of this variable. + /// + /// @return The value of this variable. Scalar value() { if (!m_graph_initialized) { m_graph = detail::topological_sort(expr); @@ -167,144 +142,118 @@ class Variable : public SleipnirBase { return Scalar(expr->val); } - /** - * Returns the type of this expression (constant, linear, quadratic, or - * nonlinear). - * - * @return The type of this expression. - */ + /// Returns the type of this expression (constant, linear, quadratic, or + /// nonlinear). + /// + /// @return The type of this expression. ExpressionType type() const { return expr->type(); } - /** - * Variable-scalar multiplication operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - * @return Result of multiplication. - */ + /// Variable-scalar multiplication operator. + /// + /// @param lhs Operator left-hand side. + /// @param rhs Operator right-hand side. + /// @return Result of multiplication. template RHS> friend Variable operator*(const LHS& lhs, const RHS& rhs) { return Variable{Variable{lhs}.expr * rhs.expr}; } - /** - * Variable-scalar multiplication operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - * @return Result of multiplication. - */ + /// Variable-scalar multiplication operator. + /// + /// @param lhs Operator left-hand side. + /// @param rhs Operator right-hand side. + /// @return Result of multiplication. template LHS, ScalarLike RHS> friend Variable operator*(const LHS& lhs, const RHS& rhs) { return Variable{lhs.expr * Variable{rhs}.expr}; } - /** - * Variable-scalar multiplication operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - * @return Result of multiplication. - */ + /// Variable-scalar multiplication operator. + /// + /// @param lhs Operator left-hand side. + /// @param rhs Operator right-hand side. + /// @return Result of multiplication. friend Variable operator*(const Variable& lhs, const Variable& rhs) { return Variable{lhs.expr * rhs.expr}; } - /** - * Variable-Variable compound multiplication operator. - * - * @param rhs Operator right-hand side. - * @return Result of multiplication. - */ + /// Variable-Variable compound multiplication operator. + /// + /// @param rhs Operator right-hand side. + /// @return Result of multiplication. Variable& operator*=(const Variable& rhs) { *this = *this * rhs; return *this; } - /** - * Variable-Variable division operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - * @return Result of division. - */ + /// Variable-Variable division operator. + /// + /// @param lhs Operator left-hand side. + /// @param rhs Operator right-hand side. + /// @return Result of division. friend Variable operator/(const Variable& lhs, const Variable& rhs) { return Variable{lhs.expr / rhs.expr}; } - /** - * Variable-Variable compound division operator. - * - * @param rhs Operator right-hand side. - * @return Result of division. - */ + /// Variable-Variable compound division operator. + /// + /// @param rhs Operator right-hand side. + /// @return Result of division. Variable& operator/=(const Variable& rhs) { *this = *this / rhs; return *this; } - /** - * Variable-Variable addition operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - * @return Result of addition. - */ + /// Variable-Variable addition operator. + /// + /// @param lhs Operator left-hand side. + /// @param rhs Operator right-hand side. + /// @return Result of addition. friend Variable operator+(const Variable& lhs, const Variable& rhs) { return Variable{lhs.expr + rhs.expr}; } - /** - * Variable-Variable compound addition operator. - * - * @param rhs Operator right-hand side. - * @return Result of addition. - */ + /// Variable-Variable compound addition operator. + /// + /// @param rhs Operator right-hand side. + /// @return Result of addition. Variable& operator+=(const Variable& rhs) { *this = *this + rhs; return *this; } - /** - * Variable-Variable subtraction operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - * @return Result of subtraction. - */ + /// Variable-Variable subtraction operator. + /// + /// @param lhs Operator left-hand side. + /// @param rhs Operator right-hand side. + /// @return Result of subtraction. friend Variable operator-(const Variable& lhs, const Variable& rhs) { return Variable{lhs.expr - rhs.expr}; } - /** - * Variable-Variable compound subtraction operator. - * - * @param rhs Operator right-hand side. - * @return Result of subtraction. - */ + /// Variable-Variable compound subtraction operator. + /// + /// @param rhs Operator right-hand side. + /// @return Result of subtraction. Variable& operator-=(const Variable& rhs) { *this = *this - rhs; return *this; } - /** - * Unary minus operator. - * - * @param lhs Operand for unary minus. - */ + /// Unary minus operator. + /// + /// @param lhs Operand for unary minus. friend Variable operator-(const Variable& lhs) { return Variable{-lhs.expr}; } - /** - * Unary plus operator. - * - * @param lhs Operand for unary plus. - */ + /// Unary plus operator. + /// + /// @param lhs Operand for unary plus. friend Variable operator+(const Variable& lhs) { return Variable{+lhs.expr}; } @@ -387,7 +336,7 @@ class Variable : public SleipnirBase { const Variable& y, const Variable& z); - friend class detail::AdjointExpressionGraph; + friend class detail::GradientExpressionGraph; template requires(UpLo == Eigen::Lower) || (UpLo == (Eigen::Lower | Eigen::Upper)) friend class Hessian; @@ -405,312 +354,258 @@ Variable(T) -> Variable; template Variable(T) -> Variable; -/** - * abs() for Variables. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// abs() for Variables. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template Variable abs(const Variable& x) { return Variable{detail::abs(x.expr)}; } -/** - * acos() for Variables. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// acos() for Variables. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template Variable acos(const Variable& x) { return Variable{detail::acos(x.expr)}; } -/** - * asin() for Variables. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// asin() for Variables. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template Variable asin(const Variable& x) { return Variable{detail::asin(x.expr)}; } -/** - * atan() for Variables. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// atan() for Variables. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template Variable atan(const Variable& x) { return Variable{detail::atan(x.expr)}; } -/** - * atan2() for Variables. - * - * @tparam Scalar Scalar type. - * @param y The y argument. - * @param x The x argument. - */ +/// atan2() for Variables. +/// +/// @tparam Scalar Scalar type. +/// @param y The y argument. +/// @param x The x argument. template Variable atan2(const ScalarLike auto& y, const Variable& x) { return Variable{detail::atan2(Variable(y).expr, x.expr)}; } -/** - * std::atan2() for Variables. - * - * @tparam Scalar Scalar type. - * @param y The y argument. - * @param x The x argument. - */ +/// atan2() for Variables. +/// +/// @tparam Scalar Scalar type. +/// @param y The y argument. +/// @param x The x argument. template Variable atan2(const Variable& y, const ScalarLike auto& x) { return Variable{detail::atan2(y.expr, Variable(x).expr)}; } -/** - * std::atan2() for Variables. - * - * @tparam Scalar Scalar type. - * @param y The y argument. - * @param x The x argument. - */ +/// atan2() for Variables. +/// +/// @tparam Scalar Scalar type. +/// @param y The y argument. +/// @param x The x argument. template Variable atan2(const Variable& y, const Variable& x) { return Variable{detail::atan2(y.expr, x.expr)}; } -/** - * cbrt() for Variables. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// cbrt() for Variables. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template Variable cbrt(const Variable& x) { return Variable{detail::cbrt(x.expr)}; } -/** - * cos() for Variables. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// cos() for Variables. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template Variable cos(const Variable& x) { return Variable{detail::cos(x.expr)}; } -/** - * cosh() for Variables. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// cosh() for Variables. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template Variable cosh(const Variable& x) { return Variable{detail::cosh(x.expr)}; } -/** - * erf() for Variables. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// erf() for Variables. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template Variable erf(const Variable& x) { return Variable{detail::erf(x.expr)}; } -/** - * exp() for Variables. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// exp() for Variables. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template Variable exp(const Variable& x) { return Variable{detail::exp(x.expr)}; } -/** - * hypot() for Variables. - * - * @tparam Scalar Scalar type. - * @param x The x argument. - * @param y The y argument. - */ +/// hypot() for Variables. +/// +/// @tparam Scalar Scalar type. +/// @param x The x argument. +/// @param y The y argument. template Variable hypot(const ScalarLike auto& x, const Variable& y) { return Variable{detail::hypot(Variable(x).expr, y.expr)}; } -/** - * hypot() for Variables. - * - * @tparam Scalar Scalar type. - * @param x The x argument. - * @param y The y argument. - */ +/// hypot() for Variables. +/// +/// @tparam Scalar Scalar type. +/// @param x The x argument. +/// @param y The y argument. template Variable hypot(const Variable& x, const ScalarLike auto& y) { return Variable{detail::hypot(x.expr, Variable(y).expr)}; } -/** - * hypot() for Variables. - * - * @tparam Scalar Scalar type. - * @param x The x argument. - * @param y The y argument. - */ +/// hypot() for Variables. +/// +/// @tparam Scalar Scalar type. +/// @param x The x argument. +/// @param y The y argument. template Variable hypot(const Variable& x, const Variable& y) { return Variable{detail::hypot(x.expr, y.expr)}; } -/** - * log() for Variables. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// log() for Variables. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template Variable log(const Variable& x) { return Variable{detail::log(x.expr)}; } -/** - * log10() for Variables. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// log10() for Variables. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template Variable log10(const Variable& x) { return Variable{detail::log10(x.expr)}; } -/** - * pow() for Variables. - * - * @tparam Scalar Scalar type. - * @param base The base. - * @param power The power. - */ +/// pow() for Variables. +/// +/// @tparam Scalar Scalar type. +/// @param base The base. +/// @param power The power. template Variable pow(const ScalarLike auto& base, const Variable& power) { return Variable{detail::pow(Variable(base).expr, power.expr)}; } -/** - * std::pow() for Variables. - * - * @tparam Scalar Scalar type. - * @param base The base. - * @param power The power. - */ +/// pow() for Variables. +/// +/// @tparam Scalar Scalar type. +/// @param base The base. +/// @param power The power. template Variable pow(const Variable& base, const ScalarLike auto& power) { return Variable{detail::pow(base.expr, Variable(power).expr)}; } -/** - * std::pow() for Variables. - * - * @tparam Scalar Scalar type. - * @param base The base. - * @param power The power. - */ +/// pow() for Variables. +/// +/// @tparam Scalar Scalar type. +/// @param base The base. +/// @param power The power. template Variable pow(const Variable& base, const Variable& power) { return Variable{detail::pow(base.expr, power.expr)}; } -/** - * sign() for Variables. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// sign() for Variables. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template Variable sign(const Variable& x) { return Variable{detail::sign(x.expr)}; } -/** - * sin() for Variables. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// sin() for Variables. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template Variable sin(const Variable& x) { return Variable{detail::sin(x.expr)}; } -/** - * sinh() for Variables. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// sinh() for Variables. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template Variable sinh(const Variable& x) { return Variable{detail::sinh(x.expr)}; } -/** - * sqrt() for Variables. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// sqrt() for Variables. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template Variable sqrt(const Variable& x) { return Variable{detail::sqrt(x.expr)}; } -/** - * tan() for Variables. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// tan() for Variables. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template Variable tan(const Variable& x) { return Variable{detail::tan(x.expr)}; } -/** - * tanh() for Variables. - * - * @tparam Scalar Scalar type. - * @param x The argument. - */ +/// tanh() for Variables. +/// +/// @tparam Scalar Scalar type. +/// @param x The argument. template Variable tanh(const Variable& x) { return Variable{detail::tanh(x.expr)}; } -/** - * hypot() for Variables. - * - * @tparam Scalar Scalar type. - * @param x The x argument. - * @param y The y argument. - * @param z The z argument. - */ +/// hypot() for Variables. +/// +/// @tparam Scalar Scalar type. +/// @param x The x argument. +/// @param y The y argument. +/// @param z The z argument. template Variable hypot(const Variable& x, const Variable& y, const Variable& z) { @@ -781,21 +676,18 @@ auto make_constraints(LHS&& lhs, RHS&& rhs) { return constraints; } -/** - * A vector of equality constraints of the form cₑ(x) = 0. - * - * @tparam Scalar Scalar type. - */ +/// A vector of equality constraints of the form cₑ(x) = 0. +/// +/// @tparam Scalar Scalar type. template struct EqualityConstraints { /// A vector of scalar equality constraints. gch::small_vector> constraints; - /** - * Concatenates multiple equality constraints. - * - * @param equality_constraints The list of EqualityConstraints to concatenate. - */ + /// Concatenates multiple equality constraints. + /// + /// @param equality_constraints The list of EqualityConstraints to + /// concatenate. EqualityConstraints( std::initializer_list equality_constraints) { for (const auto& elem : equality_constraints) { @@ -804,13 +696,12 @@ struct EqualityConstraints { } } - /** - * Concatenates multiple equality constraints. - * - * This overload is for Python bindings only. - * - * @param equality_constraints The list of EqualityConstraints to concatenate. - */ + /// Concatenates multiple equality constraints. + /// + /// This overload is for Python bindings only. + /// + /// @param equality_constraints The list of EqualityConstraints to + /// concatenate. explicit EqualityConstraints( const std::vector& equality_constraints) { for (const auto& elem : equality_constraints) { @@ -819,15 +710,13 @@ struct EqualityConstraints { } } - /** - * Constructs an equality constraint from a left and right side. - * - * The standard form for equality constraints is c(x) = 0. This function takes - * a constraint of the form lhs = rhs and converts it to lhs - rhs = 0. - * - * @param lhs Left-hand side. - * @param rhs Right-hand side. - */ + /// Constructs an equality constraint from a left and right side. + /// + /// The standard form for equality constraints is c(x) = 0. This function + /// takes a constraint of the form lhs = rhs and converts it to lhs - rhs = 0. + /// + /// @param lhs Left-hand side. + /// @param rhs Right-hand side. template requires(ScalarLike || MatrixLike) && (ScalarLike || MatrixLike) && @@ -835,9 +724,7 @@ struct EqualityConstraints { EqualityConstraints(LHS&& lhs, RHS&& rhs) : constraints{make_constraints(lhs, rhs)} {} - /** - * Implicit conversion operator to bool. - */ + /// Implicit conversion operator to bool. // NOLINTNEXTLINE (google-explicit-constructor) operator bool() { return std::ranges::all_of(constraints, [](auto& constraint) { @@ -846,22 +733,18 @@ struct EqualityConstraints { } }; -/** - * A vector of inequality constraints of the form cᵢ(x) ≥ 0. - * - * @tparam Scalar Scalar type. - */ +/// A vector of inequality constraints of the form cᵢ(x) ≥ 0. +/// +/// @tparam Scalar Scalar type. template struct InequalityConstraints { /// A vector of scalar inequality constraints. gch::small_vector> constraints; - /** - * Concatenates multiple inequality constraints. - * - * @param inequality_constraints The list of InequalityConstraints to - * concatenate. - */ + /// Concatenates multiple inequality constraints. + /// + /// @param inequality_constraints The list of InequalityConstraints to + /// concatenate. InequalityConstraints( // NOLINT std::initializer_list inequality_constraints) { for (const auto& elem : inequality_constraints) { @@ -870,14 +753,12 @@ struct InequalityConstraints { } } - /** - * Concatenates multiple inequality constraints. - * - * This overload is for Python bindings only. - * - * @param inequality_constraints The list of InequalityConstraints to - * concatenate. - */ + /// Concatenates multiple inequality constraints. + /// + /// This overload is for Python bindings only. + /// + /// @param inequality_constraints The list of InequalityConstraints to + /// concatenate. explicit InequalityConstraints( const std::vector& inequality_constraints) { for (const auto& elem : inequality_constraints) { @@ -886,15 +767,14 @@ struct InequalityConstraints { } } - /** - * Constructs an inequality constraint from a left and right side. - * - * The standard form for inequality constraints is c(x) ≥ 0. This function - * takes a constraints of the form lhs ≥ rhs and converts it to lhs - rhs ≥ 0. - * - * @param lhs Left-hand side. - * @param rhs Right-hand side. - */ + /// Constructs an inequality constraint from a left and right side. + /// + /// The standard form for inequality constraints is c(x) ≥ 0. This function + /// takes a constraints of the form lhs ≥ rhs and converts it to lhs - rhs ≥ + /// 0. + /// + /// @param lhs Left-hand side. + /// @param rhs Right-hand side. template requires(ScalarLike || MatrixLike) && (ScalarLike || MatrixLike) && @@ -902,9 +782,7 @@ struct InequalityConstraints { InequalityConstraints(LHS&& lhs, RHS&& rhs) : constraints{make_constraints(lhs, rhs)} {} - /** - * Implicit conversion operator to bool. - */ + /// Implicit conversion operator to bool. // NOLINTNEXTLINE (google-explicit-constructor) operator bool() { return std::ranges::all_of(constraints, [](auto& constraint) { @@ -913,12 +791,10 @@ struct InequalityConstraints { } }; -/** - * Equality operator that returns an equality constraint for two Variables. - * - * @param lhs Left-hand side. - * @param rhs Left-hand side. - */ +/// Equality operator that returns an equality constraint for two Variables. +/// +/// @param lhs Left-hand side. +/// @param rhs Left-hand side. template requires(ScalarLike || MatrixLike) && SleipnirType && (ScalarLike || MatrixLike) && (!SleipnirType) @@ -926,12 +802,10 @@ auto operator==(LHS&& lhs, RHS&& rhs) { return EqualityConstraints::Scalar>{lhs, rhs}; } -/** - * Equality operator that returns an equality constraint for two Variables. - * - * @param lhs Left-hand side. - * @param rhs Left-hand side. - */ +/// Equality operator that returns an equality constraint for two Variables. +/// +/// @param lhs Left-hand side. +/// @param rhs Left-hand side. template requires(ScalarLike || MatrixLike) && (!SleipnirType) && (ScalarLike || MatrixLike) && SleipnirType @@ -939,12 +813,10 @@ auto operator==(LHS&& lhs, RHS&& rhs) { return EqualityConstraints::Scalar>{lhs, rhs}; } -/** - * Equality operator that returns an equality constraint for two Variables. - * - * @param lhs Left-hand side. - * @param rhs Left-hand side. - */ +/// Equality operator that returns an equality constraint for two Variables. +/// +/// @param lhs Left-hand side. +/// @param rhs Left-hand side. template requires(ScalarLike || MatrixLike) && SleipnirType && (ScalarLike || MatrixLike) && SleipnirType @@ -952,13 +824,11 @@ auto operator==(LHS&& lhs, RHS&& rhs) { return EqualityConstraints::Scalar>{lhs, rhs}; } -/** - * Less-than comparison operator that returns an inequality constraint for two - * Variables. - * - * @param lhs Left-hand side. - * @param rhs Left-hand side. - */ +/// Less-than comparison operator that returns an inequality constraint for two +/// Variables. +/// +/// @param lhs Left-hand side. +/// @param rhs Left-hand side. template requires(ScalarLike || MatrixLike) && (ScalarLike || MatrixLike) && @@ -967,13 +837,11 @@ auto operator<(LHS&& lhs, RHS&& rhs) { return rhs >= lhs; } -/** - * Less-than-or-equal-to comparison operator that returns an inequality - * constraint for two Variables. - * - * @param lhs Left-hand side. - * @param rhs Left-hand side. - */ +/// Less-than-or-equal-to comparison operator that returns an inequality +/// constraint for two Variables. +/// +/// @param lhs Left-hand side. +/// @param rhs Left-hand side. template requires(ScalarLike || MatrixLike) && (ScalarLike || MatrixLike) && @@ -982,13 +850,11 @@ auto operator<=(LHS&& lhs, RHS&& rhs) { return rhs >= lhs; } -/** - * Greater-than comparison operator that returns an inequality constraint for - * two Variables. - * - * @param lhs Left-hand side. - * @param rhs Left-hand side. - */ +/// Greater-than comparison operator that returns an inequality constraint for +/// two Variables. +/// +/// @param lhs Left-hand side. +/// @param rhs Left-hand side. template requires(ScalarLike || MatrixLike) && (ScalarLike || MatrixLike) && @@ -997,13 +863,11 @@ auto operator>(LHS&& lhs, RHS&& rhs) { return lhs >= rhs; } -/** - * Greater-than-or-equal-to comparison operator that returns an inequality - * constraint for two Variables. - * - * @param lhs Left-hand side. - * @param rhs Left-hand side. - */ +/// Greater-than-or-equal-to comparison operator that returns an inequality +/// constraint for two Variables. +/// +/// @param lhs Left-hand side. +/// @param rhs Left-hand side. template requires(ScalarLike || MatrixLike) && SleipnirType && (ScalarLike || MatrixLike) && (!SleipnirType) @@ -1011,13 +875,11 @@ auto operator>=(LHS&& lhs, RHS&& rhs) { return InequalityConstraints::Scalar>{lhs, rhs}; } -/** - * Greater-than-or-equal-to comparison operator that returns an inequality - * constraint for two Variables. - * - * @param lhs Left-hand side. - * @param rhs Left-hand side. - */ +/// Greater-than-or-equal-to comparison operator that returns an inequality +/// constraint for two Variables. +/// +/// @param lhs Left-hand side. +/// @param rhs Left-hand side. template requires(ScalarLike || MatrixLike) && (!SleipnirType) && (ScalarLike || MatrixLike) && SleipnirType @@ -1025,13 +887,11 @@ auto operator>=(LHS&& lhs, RHS&& rhs) { return InequalityConstraints::Scalar>{lhs, rhs}; } -/** - * Greater-than-or-equal-to comparison operator that returns an inequality - * constraint for two Variables. - * - * @param lhs Left-hand side. - * @param rhs Left-hand side. - */ +/// Greater-than-or-equal-to comparison operator that returns an inequality +/// constraint for two Variables. +/// +/// @param lhs Left-hand side. +/// @param rhs Left-hand side. template requires(ScalarLike || MatrixLike) && SleipnirType && (ScalarLike || MatrixLike) && SleipnirType @@ -1039,15 +899,26 @@ auto operator>=(LHS&& lhs, RHS&& rhs) { return InequalityConstraints::Scalar>{lhs, rhs}; } +/// Helper function for creating bound constraints. +/// +/// @param l Lower bound. +/// @param x Variable to bound. +/// @param u Upper bound. +template + requires(ScalarLike || MatrixLike) && SleipnirType && + (ScalarLike || MatrixLike) +auto bounds(L&& l, X&& x, U&& u) { + return InequalityConstraints{l <= x, x <= u}; +} + } // namespace slp namespace Eigen { -/** - * NumTraits specialization that allows instantiating Eigen types with Variable. - * - * @tparam Scalar Scalar type. - */ +/// NumTraits specialization that allows instantiating Eigen types with +/// Variable. +/// +/// @tparam Scalar Scalar type. template struct NumTraits> : NumTraits { /// Real type. diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_block.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_block.hpp index c5351fec9f..cf554832b8 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_block.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_block.hpp @@ -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 class VariableBlock : public SleipnirBase { public: - /** - * Scalar type alias. - */ + /// Scalar type alias. using Scalar = typename Mat::Scalar; - /** - * Copy constructor. - */ + /// Copy constructor. VariableBlock(const VariableBlock&) = 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& operator=(const VariableBlock& values) { if (this == &values) { return *this; @@ -65,17 +57,13 @@ class VariableBlock : public SleipnirBase { return *this; } - /** - * Move constructor. - */ + /// Move constructor. VariableBlock(VariableBlock&&) = 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& operator=(VariableBlock&& 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& 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 VariableBlock& operator=(const Eigen::MatrixBase& 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 requires std::same_as void set_value(const Eigen::MatrixBase& 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& 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& 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& operator()(int row, int col) requires(!std::is_const_v) { @@ -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& 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& operator[](int index) requires(!std::is_const_v) { @@ -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& 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 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 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 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 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 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 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 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 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 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 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 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 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& 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& 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& 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& 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& 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& 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& 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& 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() 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 T() const { std::remove_cv_t 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 value() { Eigen::Matrix 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 cwise_transform( function_ref(const Variable& 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: diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_matrix.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_matrix.hpp index 55788ce18f..d90645910d 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_matrix.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_matrix.hpp @@ -26,38 +26,28 @@ namespace slp { -/** - * A matrix of autodiff variables. - * - * @tparam Scalar_ Scalar type. - */ +/// A matrix of autodiff variables. +/// +/// @tparam Scalar_ Scalar type. template class VariableMatrix : public SleipnirBase { public: - /** - * Scalar type alias. - */ + /// Scalar type alias. using Scalar = Scalar_; - /** - * Constructs an empty VariableMatrix. - */ + /// Constructs an empty VariableMatrix. VariableMatrix() = default; - /** - * Constructs a zero-initialized VariableMatrix column vector with the given - * rows. - * - * @param rows The number of matrix rows. - */ + /// Constructs a zero-initialized VariableMatrix column vector with the given + /// rows. + /// + /// @param rows The number of matrix rows. explicit VariableMatrix(int rows) : VariableMatrix{rows, 1} {} - /** - * Constructs a zero-initialized VariableMatrix with the given dimensions. - * - * @param rows The number of matrix rows. - * @param cols The number of matrix columns. - */ + /// Constructs a zero-initialized VariableMatrix with the given dimensions. + /// + /// @param rows The number of matrix rows. + /// @param cols The number of matrix columns. VariableMatrix(int rows, int cols) : m_rows{rows}, m_cols{cols} { m_storage.reserve(rows * cols); for (int index = 0; index < rows * cols; ++index) { @@ -65,12 +55,10 @@ class VariableMatrix : public SleipnirBase { } } - /** - * Constructs an empty VariableMatrix with the given dimensions. - * - * @param rows The number of matrix rows. - * @param cols The number of matrix columns. - */ + /// Constructs an empty VariableMatrix with the given dimensions. + /// + /// @param rows The number of matrix rows. + /// @param cols The number of matrix columns. VariableMatrix(detail::empty_t, int rows, int cols) : m_rows{rows}, m_cols{cols} { m_storage.reserve(rows * cols); @@ -79,11 +67,9 @@ class VariableMatrix : public SleipnirBase { } } - /** - * Constructs a scalar VariableMatrix from a nested list of Variables. - * - * @param list The nested list of Variables. - */ + /// Constructs a scalar VariableMatrix from a nested list of Variables. + /// + /// @param list The nested list of Variables. VariableMatrix( std::initializer_list>> list) { // Get row and column counts for destination matrix @@ -105,13 +91,11 @@ class VariableMatrix : public SleipnirBase { } } - /** - * Constructs a scalar VariableMatrix from a nested list of scalars. - * - * This overload is for Python bindings only. - * - * @param list The nested list of Variables. - */ + /// Constructs a scalar VariableMatrix from a nested list of scalars. + /// + /// This overload is for Python bindings only. + /// + /// @param list The nested list of Variables. // NOLINTNEXTLINE (google-explicit-constructor) VariableMatrix(const std::vector>& list) { // Get row and column counts for destination matrix @@ -133,13 +117,11 @@ class VariableMatrix : public SleipnirBase { } } - /** - * Constructs a scalar VariableMatrix from a nested list of Variables. - * - * This overload is for Python bindings only. - * - * @param list The nested list of Variables. - */ + /// Constructs a scalar VariableMatrix from a nested list of Variables. + /// + /// This overload is for Python bindings only. + /// + /// @param list The nested list of Variables. // NOLINTNEXTLINE (google-explicit-constructor) VariableMatrix(const std::vector>>& list) { // Get row and column counts for destination matrix @@ -161,11 +143,9 @@ class VariableMatrix : public SleipnirBase { } } - /** - * Constructs a VariableMatrix from an Eigen matrix. - * - * @param values Eigen matrix of values. - */ + /// Constructs a VariableMatrix from an Eigen matrix. + /// + /// @param values Eigen matrix of values. template // NOLINTNEXTLINE (google-explicit-constructor) VariableMatrix(const Eigen::MatrixBase& values) @@ -179,11 +159,9 @@ class VariableMatrix : public SleipnirBase { } } - /** - * Constructs a VariableMatrix from an Eigen diagonal matrix. - * - * @param values Diagonal matrix of values. - */ + /// Constructs a VariableMatrix from an Eigen diagonal matrix. + /// + /// @param values Diagonal matrix of values. template // NOLINTNEXTLINE (google-explicit-constructor) VariableMatrix(const Eigen::DiagonalBase& values) @@ -201,31 +179,25 @@ class VariableMatrix : public SleipnirBase { } } - /** - * Constructs a scalar VariableMatrix from a Variable. - * - * @param variable Variable. - */ + /// Constructs a scalar VariableMatrix from a Variable. + /// + /// @param variable Variable. // NOLINTNEXTLINE (google-explicit-constructor) VariableMatrix(const Variable& variable) : m_rows{1}, m_cols{1} { m_storage.emplace_back(variable); } - /** - * Constructs a scalar VariableMatrix from a Variable. - * - * @param variable Variable. - */ + /// Constructs a scalar VariableMatrix from a Variable. + /// + /// @param variable Variable. // NOLINTNEXTLINE (google-explicit-constructor) VariableMatrix(Variable&& variable) : m_rows{1}, m_cols{1} { m_storage.emplace_back(std::move(variable)); } - /** - * Constructs a VariableMatrix from a VariableBlock. - * - * @param values VariableBlock of values. - */ + /// Constructs a VariableMatrix from a VariableBlock. + /// + /// @param values VariableBlock of values. // NOLINTNEXTLINE (google-explicit-constructor) VariableMatrix(const VariableBlock& values) : m_rows{values.rows()}, m_cols{values.cols()} { @@ -237,11 +209,9 @@ class VariableMatrix : public SleipnirBase { } } - /** - * Constructs a VariableMatrix from a VariableBlock. - * - * @param values VariableBlock of values. - */ + /// Constructs a VariableMatrix from a VariableBlock. + /// + /// @param values VariableBlock of values. // NOLINTNEXTLINE (google-explicit-constructor) VariableMatrix(const VariableBlock& values) : m_rows{values.rows()}, m_cols{values.cols()} { @@ -253,11 +223,9 @@ class VariableMatrix : public SleipnirBase { } } - /** - * Constructs a column vector wrapper around a Variable array. - * - * @param values Variable array to wrap. - */ + /// Constructs a column vector wrapper around a Variable array. + /// + /// @param values Variable array to wrap. explicit VariableMatrix(std::span> values) : m_rows{static_cast(values.size())}, m_cols{1} { m_storage.reserve(rows() * cols()); @@ -268,13 +236,11 @@ class VariableMatrix : public SleipnirBase { } } - /** - * Constructs a matrix wrapper around a Variable array. - * - * @param values Variable array to wrap. - * @param rows The number of matrix rows. - * @param cols The number of matrix columns. - */ + /// Constructs a matrix wrapper around a Variable array. + /// + /// @param values Variable array to wrap. + /// @param rows The number of matrix rows. + /// @param cols The number of matrix columns. VariableMatrix(std::span> values, int rows, int cols) : m_rows{rows}, m_cols{cols} { slp_assert(static_cast(values.size()) == rows * cols); @@ -286,12 +252,10 @@ class VariableMatrix : public SleipnirBase { } } - /** - * Assigns an Eigen matrix to a VariableMatrix. - * - * @param values Eigen matrix of values. - * @return This VariableMatrix. - */ + /// Assigns an Eigen matrix to a VariableMatrix. + /// + /// @param values Eigen matrix of values. + /// @return This VariableMatrix. template VariableMatrix& operator=(const Eigen::MatrixBase& values) { slp_assert(rows() == values.rows() && cols() == values.cols()); @@ -305,14 +269,12 @@ class VariableMatrix : public SleipnirBase { return *this; } - /** - * Assigns a scalar to the matrix. - * - * This only works for matrices with one row and one column. - * - * @param value Value to assign. - * @return This VariableMatrix. - */ + /// Assigns a scalar to the matrix. + /// + /// This only works for matrices with one row and one column. + /// + /// @param value Value to assign. + /// @return This VariableMatrix. VariableMatrix& operator=(ScalarLike auto value) { slp_assert(rows() == 1 && cols() == 1); @@ -321,11 +283,9 @@ class VariableMatrix : public SleipnirBase { return *this; } - /** - * Sets the VariableMatrix's internal values. - * - * @param values Eigen matrix of values. - */ + /// Sets the VariableMatrix's internal values. + /// + /// @param values Eigen matrix of values. template requires std::same_as void set_value(const Eigen::MatrixBase& values) { @@ -338,63 +298,53 @@ class VariableMatrix : public SleipnirBase { } } - /** - * Returns the element at the given row and column. - * - * @param row The row. - * @param col The column. - * @return The element at the given row and column. - */ + /// Returns the element at the given row and column. + /// + /// @param row The row. + /// @param col The column. + /// @return The element at the given row and column. Variable& operator()(int row, int col) { slp_assert(row >= 0 && row < rows()); slp_assert(col >= 0 && col < cols()); return m_storage[row * cols() + col]; } - /** - * Returns the element at the given row and column. - * - * @param row The row. - * @param col The column. - * @return The element at the given row and column. - */ + /// Returns the element at the given row and column. + /// + /// @param row The row. + /// @param col The column. + /// @return The element at the given row and column. const Variable& operator()(int row, int col) const { slp_assert(row >= 0 && row < rows()); slp_assert(col >= 0 && col < cols()); return m_storage[row * cols() + col]; } - /** - * Returns the element at the given index. - * - * @param index The index. - * @return The element at the given index. - */ + /// Returns the element at the given index. + /// + /// @param index The index. + /// @return The element at the given index. Variable& operator[](int index) { slp_assert(index >= 0 && index < rows() * cols()); return m_storage[index]; } - /** - * Returns the element at the given index. - * - * @param index The index. - * @return The element at the given index. - */ + /// Returns the element at the given index. + /// + /// @param index The index. + /// @return The element at the given index. const Variable& operator[](int index) const { slp_assert(index >= 0 && index < rows() * cols()); return m_storage[index]; } - /** - * 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 block(int row_offset, int col_offset, int block_rows, int block_cols) { slp_assert(row_offset >= 0 && row_offset <= rows()); @@ -404,15 +354,13 @@ class VariableMatrix : public SleipnirBase { return VariableBlock{*this, row_offset, col_offset, block_rows, block_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. const VariableBlock block(int row_offset, int col_offset, int block_rows, @@ -424,13 +372,11 @@ class VariableMatrix : public SleipnirBase { return VariableBlock{*this, row_offset, col_offset, block_rows, block_cols}; } - /** - * 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 operator()(Slice row_slice, Slice col_slice) { int row_slice_length = row_slice.adjust(rows()); int col_slice_length = col_slice.adjust(cols()); @@ -438,13 +384,11 @@ class VariableMatrix : public SleipnirBase { std::move(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 operator()(Slice row_slice, Slice col_slice) const { int row_slice_length = row_slice.adjust(rows()); @@ -453,19 +397,16 @@ class VariableMatrix : public SleipnirBase { std::move(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 operator()(Slice row_slice, int row_slice_length, Slice col_slice, @@ -474,18 +415,16 @@ class VariableMatrix : public SleipnirBase { std::move(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. const VariableBlock operator()( Slice row_slice, int row_slice_length, Slice col_slice, int col_slice_length) const { @@ -493,13 +432,11 @@ class VariableMatrix : public SleipnirBase { std::move(col_slice), 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 segment(int offset, int length) { slp_assert(cols() == 1); slp_assert(offset >= 0 && offset < rows()); @@ -507,13 +444,11 @@ class VariableMatrix : 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 segment(int offset, int length) const { slp_assert(cols() == 1); @@ -522,56 +457,46 @@ class VariableMatrix : 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 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. const VariableBlock 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 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. const VariableBlock col(int col) const { slp_assert(col >= 0 && col < cols()); return block(0, col, rows(), 1); } - /** - * Matrix multiplication operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ + /// Matrix multiplication operator. + /// + /// @param lhs Operator left-hand side. + /// @param rhs Operator right-hand side. template RHS> friend VariableMatrix operator*(const LHS& lhs, const RHS& rhs) { slp_assert(lhs.cols() == rhs.rows()); @@ -595,12 +520,10 @@ class VariableMatrix : public SleipnirBase { return result; } - /** - * Matrix multiplication operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ + /// Matrix multiplication operator. + /// + /// @param lhs Operator left-hand side. + /// @param rhs Operator right-hand side. template LHS, EigenMatrixLike RHS> friend VariableMatrix operator*(const LHS& lhs, const RHS& rhs) { slp_assert(lhs.cols() == rhs.rows()); @@ -620,12 +543,10 @@ class VariableMatrix : public SleipnirBase { return result; } - /** - * Matrix multiplication operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ + /// Matrix multiplication operator. + /// + /// @param lhs Operator left-hand side. + /// @param rhs Operator right-hand side. template LHS, SleipnirMatrixLike RHS> friend VariableMatrix operator*(const LHS& lhs, const RHS& rhs) { slp_assert(lhs.cols() == rhs.rows()); @@ -648,12 +569,10 @@ class VariableMatrix : public SleipnirBase { return result; } - /** - * Matrix-scalar multiplication operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ + /// Matrix-scalar multiplication operator. + /// + /// @param lhs Operator left-hand side. + /// @param rhs Operator right-hand side. template friend VariableMatrix operator*(const LHS& lhs, const Variable& rhs) { @@ -668,12 +587,10 @@ class VariableMatrix : public SleipnirBase { return result; } - /** - * Matrix-scalar multiplication operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ + /// Matrix-scalar multiplication operator. + /// + /// @param lhs Operator left-hand side. + /// @param rhs Operator right-hand side. template LHS, ScalarLike RHS> friend VariableMatrix operator*(const LHS& lhs, const RHS& rhs) { VariableMatrix result(detail::empty, lhs.rows(), lhs.cols()); @@ -687,12 +604,10 @@ class VariableMatrix : public SleipnirBase { return result; } - /** - * Scalar-matrix multiplication operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ + /// Scalar-matrix multiplication operator. + /// + /// @param lhs Operator left-hand side. + /// @param rhs Operator right-hand side. template friend VariableMatrix operator*(const Variable& lhs, const RHS& rhs) { @@ -707,12 +622,10 @@ class VariableMatrix : public SleipnirBase { return result; } - /** - * Scalar-matrix multiplication operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - */ + /// Scalar-matrix multiplication operator. + /// + /// @param lhs Operator left-hand side. + /// @param rhs Operator right-hand side. template RHS> friend VariableMatrix operator*(const LHS& lhs, const RHS& rhs) { VariableMatrix result(detail::empty, rhs.rows(), rhs.cols()); @@ -726,12 +639,10 @@ class VariableMatrix : public SleipnirBase { return result; } - /** - * 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. VariableMatrix& operator*=(const MatrixLike auto& rhs) { slp_assert(cols() == rhs.rows() && cols() == rhs.cols()); @@ -748,12 +659,10 @@ class VariableMatrix : public SleipnirBase { return *this; } - /** - * Compound matrix-scalar multiplication-assignment operator. - * - * @param rhs Variable to multiply. - * @return Result of multiplication. - */ + /// Compound matrix-scalar multiplication-assignment operator. + /// + /// @param rhs Variable to multiply. + /// @return Result of multiplication. VariableMatrix& operator*=(const ScalarLike auto& rhs) { for (int row = 0; row < rows(); ++row) { for (int col = 0; col < rhs.cols(); ++col) { @@ -764,13 +673,11 @@ class VariableMatrix : public SleipnirBase { return *this; } - /** - * Binary division operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - * @return Result of division. - */ + /// Binary division operator. + /// + /// @param lhs Operator left-hand side. + /// @param rhs Operator right-hand side. + /// @return Result of division. template friend VariableMatrix operator/(const LHS& lhs, const Variable& rhs) { @@ -785,13 +692,11 @@ class VariableMatrix : public SleipnirBase { return result; } - /** - * Binary division operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - * @return Result of division. - */ + /// Binary division operator. + /// + /// @param lhs Operator left-hand side. + /// @param rhs Operator right-hand side. + /// @return Result of division. template LHS, ScalarLike RHS> requires(!SleipnirScalarLike) friend VariableMatrix operator/(const LHS& lhs, const RHS& rhs) { @@ -806,13 +711,11 @@ class VariableMatrix : public SleipnirBase { return result; } - /** - * Binary division operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - * @return Result of division. - */ + /// Binary division operator. + /// + /// @param lhs Operator left-hand side. + /// @param rhs Operator right-hand side. + /// @return Result of division. template LHS> friend VariableMatrix operator/(const LHS& lhs, const Variable& rhs) { @@ -827,12 +730,10 @@ class VariableMatrix : public SleipnirBase { return result; } - /** - * 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. VariableMatrix& operator/=(const ScalarLike auto& rhs) { for (int row = 0; row < rows(); ++row) { for (int col = 0; col < cols(); ++col) { @@ -843,13 +744,11 @@ class VariableMatrix : public SleipnirBase { return *this; } - /** - * Binary addition operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - * @return Result of addition. - */ + /// Binary addition operator. + /// + /// @param lhs Operator left-hand side. + /// @param rhs Operator right-hand side. + /// @return Result of addition. template RHS> friend VariableMatrix operator+(const LHS& lhs, const RHS& rhs) { slp_assert(lhs.rows() == rhs.rows() && lhs.cols() == rhs.cols()); @@ -865,13 +764,11 @@ class VariableMatrix : public SleipnirBase { return result; } - /** - * Binary addition operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - * @return Result of addition. - */ + /// Binary addition operator. + /// + /// @param lhs Operator left-hand side. + /// @param rhs Operator right-hand side. + /// @return Result of addition. template LHS, EigenMatrixLike RHS> friend VariableMatrix operator+(const LHS& lhs, const RHS& rhs) { slp_assert(lhs.rows() == rhs.rows() && lhs.cols() == rhs.cols()); @@ -887,13 +784,11 @@ class VariableMatrix : public SleipnirBase { return result; } - /** - * Binary addition operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - * @return Result of addition. - */ + /// Binary addition operator. + /// + /// @param lhs Operator left-hand side. + /// @param rhs Operator right-hand side. + /// @return Result of addition. template LHS, SleipnirMatrixLike RHS> friend VariableMatrix operator+(const LHS& lhs, const RHS& rhs) { slp_assert(lhs.rows() == rhs.rows() && lhs.cols() == rhs.cols()); @@ -909,12 +804,10 @@ class VariableMatrix : public SleipnirBase { return result; } - /** - * 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. VariableMatrix& operator+=(const MatrixLike auto& rhs) { slp_assert(rows() == rhs.rows() && cols() == rhs.cols()); @@ -927,12 +820,10 @@ class VariableMatrix : 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. VariableMatrix& operator+=(const ScalarLike auto& rhs) { slp_assert(rows() == 1 && cols() == 1); @@ -945,13 +836,11 @@ class VariableMatrix : public SleipnirBase { return *this; } - /** - * Binary subtraction operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - * @return Result of subtraction. - */ + /// Binary subtraction operator. + /// + /// @param lhs Operator left-hand side. + /// @param rhs Operator right-hand side. + /// @return Result of subtraction. template RHS> friend VariableMatrix operator-(const LHS& lhs, const RHS& rhs) { slp_assert(lhs.rows() == rhs.rows() && lhs.cols() == rhs.cols()); @@ -967,13 +856,11 @@ class VariableMatrix : public SleipnirBase { return result; } - /** - * Binary subtraction operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - * @return Result of subtraction. - */ + /// Binary subtraction operator. + /// + /// @param lhs Operator left-hand side. + /// @param rhs Operator right-hand side. + /// @return Result of subtraction. template LHS, EigenMatrixLike RHS> friend VariableMatrix operator-(const LHS& lhs, const RHS& rhs) { slp_assert(lhs.rows() == rhs.rows() && lhs.cols() == rhs.cols()); @@ -989,13 +876,11 @@ class VariableMatrix : public SleipnirBase { return result; } - /** - * Binary subtraction operator. - * - * @param lhs Operator left-hand side. - * @param rhs Operator right-hand side. - * @return Result of subtraction. - */ + /// Binary subtraction operator. + /// + /// @param lhs Operator left-hand side. + /// @param rhs Operator right-hand side. + /// @return Result of subtraction. template LHS, SleipnirMatrixLike RHS> friend VariableMatrix operator-(const LHS& lhs, const RHS& rhs) { slp_assert(lhs.rows() == rhs.rows() && lhs.cols() == rhs.cols()); @@ -1011,12 +896,10 @@ class VariableMatrix : public SleipnirBase { return result; } - /** - * 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. VariableMatrix& operator-=(const MatrixLike auto& rhs) { slp_assert(rows() == rhs.rows() && cols() == rhs.cols()); @@ -1029,12 +912,10 @@ class VariableMatrix : 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. VariableMatrix& operator-=(const ScalarLike auto& rhs) { slp_assert(rows() == 1 && cols() == 1); @@ -1047,11 +928,9 @@ class VariableMatrix : public SleipnirBase { return *this; } - /** - * Unary minus operator. - * - * @param lhs Operand for unary minus. - */ + /// Unary minus operator. + /// + /// @param lhs Operand for unary minus. friend VariableMatrix operator-( const SleipnirMatrixLike auto& lhs) { VariableMatrix result{detail::empty, lhs.rows(), lhs.cols()}; @@ -1065,20 +944,16 @@ class VariableMatrix : public SleipnirBase { return result; } - /** - * Implicit conversion operator from 1x1 VariableMatrix to Variable. - */ + /// Implicit conversion operator from 1x1 VariableMatrix to Variable. // NOLINTNEXTLINE (google-explicit-constructor) operator Variable() 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. VariableMatrix T() const { VariableMatrix result{detail::empty, cols(), rows()}; @@ -1091,42 +966,32 @@ class VariableMatrix : 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_rows; } - /** - * 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_cols; } - /** - * 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 matrix. - * - * @param index The index of the element to return. - * @return An element of the variable matrix. - */ + /// Returns an element of the variable matrix. + /// + /// @param index The index of the element to return. + /// @return An element of the variable matrix. Scalar value(int index) { return (*this)[index].value(); } - /** - * 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 value() { Eigen::Matrix result{rows(), cols()}; @@ -1140,12 +1005,10 @@ class VariableMatrix : 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. VariableMatrix cwise_transform( function_ref(const Variable& x)> unary_op) const { @@ -1255,112 +1118,84 @@ class VariableMatrix : public SleipnirBase { #endif // DOXYGEN_SHOULD_SKIP_THIS - /** - * Returns begin iterator. - * - * @return Begin iterator. - */ + /// Returns begin iterator. + /// + /// @return Begin iterator. iterator begin() { return iterator{m_storage.begin()}; } - /** - * Returns end iterator. - * - * @return End iterator. - */ + /// Returns end iterator. + /// + /// @return End iterator. iterator end() { return iterator{m_storage.end()}; } - /** - * Returns const begin iterator. - * - * @return Const begin iterator. - */ + /// Returns const begin iterator. + /// + /// @return Const begin iterator. const_iterator begin() 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 end() const { return const_iterator{m_storage.end()}; } - /** - * Returns const begin iterator. - * - * @return Const begin iterator. - */ + /// Returns const begin iterator. + /// + /// @return Const begin iterator. 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.end()}; } - /** - * 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 m_storage.size(); } - /** - * Returns a variable matrix filled with zeroes. - * - * @param rows The number of matrix rows. - * @param cols The number of matrix columns. - * @return A variable matrix filled with zeroes. - */ + /// Returns a variable matrix filled with zeroes. + /// + /// @param rows The number of matrix rows. + /// @param cols The number of matrix columns. + /// @return A variable matrix filled with zeroes. static VariableMatrix zero(int rows, int cols) { VariableMatrix result{detail::empty, rows, cols}; @@ -1371,13 +1206,11 @@ class VariableMatrix : public SleipnirBase { return result; } - /** - * Returns a variable matrix filled with ones. - * - * @param rows The number of matrix rows. - * @param cols The number of matrix columns. - * @return A variable matrix filled with ones. - */ + /// Returns a variable matrix filled with ones. + /// + /// @param rows The number of matrix rows. + /// @param cols The number of matrix columns. + /// @return A variable matrix filled with ones. static VariableMatrix ones(int rows, int cols) { VariableMatrix result{detail::empty, rows, cols}; @@ -1402,14 +1235,12 @@ template VariableMatrix(const Eigen::DiagonalBase&) -> VariableMatrix; -/** - * Applies a coefficient-wise reduce operation to two matrices. - * - * @tparam Scalar Scalar type. - * @param lhs The left-hand side of the binary operator. - * @param rhs The right-hand side of the binary operator. - * @param binary_op The binary operator to use for the reduce operation. - */ +/// Applies a coefficient-wise reduce operation to two matrices. +/// +/// @tparam Scalar Scalar type. +/// @param lhs The left-hand side of the binary operator. +/// @param rhs The right-hand side of the binary operator. +/// @param binary_op The binary operator to use for the reduce operation. template VariableMatrix cwise_reduce( const VariableMatrix& lhs, const VariableMatrix& rhs, @@ -1429,17 +1260,15 @@ VariableMatrix cwise_reduce( return result; } -/** - * Assemble a VariableMatrix from a nested list of blocks. - * - * Each row's blocks must have the same height, and the assembled block rows - * must have the same width. For example, for the block matrix [[A, B], [C]] to - * be constructible, the number of rows in A and B must match, and the number of - * columns in [A, B] and [C] must match. - * - * @tparam Scalar Scalar type. - * @param list The nested list of blocks. - */ +/// Assemble a VariableMatrix from a nested list of blocks. +/// +/// Each row's blocks must have the same height, and the assembled block rows +/// must have the same width. For example, for the block matrix [[A, B], [C]] to +/// be constructible, the number of rows in A and B must match, and the number +/// of columns in [A, B] and [C] must match. +/// +/// @tparam Scalar Scalar type. +/// @param list The nested list of blocks. template VariableMatrix block( std::initializer_list>> list) { @@ -1486,19 +1315,17 @@ VariableMatrix block( return result; } -/** - * Assemble a VariableMatrix from a nested list of blocks. - * - * Each row's blocks must have the same height, and the assembled block rows - * must have the same width. For example, for the block matrix [[A, B], [C]] to - * be constructible, the number of rows in A and B must match, and the number of - * columns in [A, B] and [C] must match. - * - * This overload is for Python bindings only. - * - * @tparam Scalar Scalar type. - * @param list The nested list of blocks. - */ +/// Assemble a VariableMatrix from a nested list of blocks. +/// +/// Each row's blocks must have the same height, and the assembled block rows +/// must have the same width. For example, for the block matrix [[A, B], [C]] to +/// be constructible, the number of rows in A and B must match, and the number +/// of columns in [A, B] and [C] must match. +/// +/// This overload is for Python bindings only. +/// +/// @tparam Scalar Scalar type. +/// @param list The nested list of blocks. template VariableMatrix block( const std::vector>>& list) { @@ -1545,14 +1372,12 @@ VariableMatrix block( return result; } -/** - * Solves the VariableMatrix equation AX = B for X. - * - * @tparam Scalar Scalar type. - * @param A The left-hand side. - * @param B The right-hand side. - * @return The solution X. - */ +/// Solves the VariableMatrix equation AX = B for X. +/// +/// @tparam Scalar Scalar type. +/// @param A The left-hand side. +/// @param B The right-hand side. +/// @return The solution X. template VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) { diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/multistart.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/multistart.hpp index 305e70dc76..95279940f5 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/multistart.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/multistart.hpp @@ -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 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 -MultistartResult Multistart( +MultistartResult multistart( function_ref( const DecisionVariables& initial_guess)> solve, diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp.hpp index d62432a67a..e906c194e9 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp.hpp @@ -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 class OCP : public Problem { 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. + ///
    + ///
  • 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. OCP(int num_states, int num_inputs, std::chrono::duration dt, int num_steps, function_ref(const VariableMatrix& x, @@ -89,23 +87,23 @@ class OCP : public Problem { 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. + ///
    + ///
  • 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. OCP(int num_states, int num_inputs, std::chrono::duration dt, int num_steps, function_ref( @@ -158,36 +156,30 @@ class OCP : public Problem { } } - /** - * 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 requires ScalarLike || MatrixLike 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 requires ScalarLike || MatrixLike 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& x, const VariableMatrix& u)> callback) { @@ -198,14 +190,12 @@ class OCP : public Problem { } } - /** - * 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& t, const VariableMatrix& x, @@ -223,12 +213,10 @@ class OCP : public Problem { } } - /** - * 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 requires ScalarLike || MatrixLike void set_lower_input_bound(const T& lower_bound) { @@ -237,12 +225,10 @@ class OCP : public Problem { } } - /** - * 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 requires ScalarLike || MatrixLike void set_upper_input_bound(const T& upper_bound) { @@ -251,68 +237,54 @@ class OCP : public Problem { } } - /** - * 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 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 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& 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& 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& 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 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 final_state() { return m_X.col(m_num_steps); } private: @@ -328,15 +300,13 @@ class OCP : public Problem { VariableMatrix m_U; VariableMatrix 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 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 { 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 { } } - /** - * Apply direct transcription dynamics constraints. - */ + /// Apply direct transcription dynamics constraints. void constrain_direct_transcription() { Variable time{0}; @@ -412,9 +378,7 @@ class OCP : public Problem { } } - /** - * Apply single shooting dynamics constraints. - */ + /// Apply single shooting dynamics constraints. void constrain_single_shooting() { Variable time{0}; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/dynamics_type.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/dynamics_type.hpp index 835a12eef8..a812e30dbe 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/dynamics_type.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/dynamics_type.hpp @@ -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, diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/timestep_method.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/timestep_method.hpp index ef1cf88a08..564407de48 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/timestep_method.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/timestep_method.hpp @@ -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, diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/transcription_method.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/transcription_method.hpp index e0513eef89..5271d7605c 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/transcription_method.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/transcription_method.hpp @@ -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. diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp index a5db8e5902..4243d13c9d 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp @@ -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 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 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 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 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& 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&& 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& 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&& 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& 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&& 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& 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&& 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; + using SparseMatrix = Eigen::SparseMatrix; + using SparseVector = Eigen::SparseVector; + // Create the initial value column vector - Eigen::Vector 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( - NewtonMatrixCallbacks{ - [&](const Eigen::Vector& x) -> Scalar { - x_ad.set_value(x); - return f.value(); - }, - [&](const Eigen::Vector& x) - -> Eigen::SparseVector { - x_ad.set_value(x); - return g.value(); - }, - [&](const Eigen::Vector& x) - -> Eigen::SparseMatrix { - x_ad.set_value(x); - return H.value(); - }}, - callbacks, options, x); + status = newton(NewtonMatrixCallbacks{ + [&](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( SQPMatrixCallbacks{ - [&](const Eigen::Vector& x) -> Scalar { + [&](const DenseVector& x) -> Scalar { x_ad.set_value(x); return f.value(); }, - [&](const Eigen::Vector& x) - -> Eigen::SparseVector { + [&](const DenseVector& x) -> SparseVector { x_ad.set_value(x); return g.value(); }, - [&](const Eigen::Vector& x, - const Eigen::Vector& y) - -> Eigen::SparseMatrix { + [&](const DenseVector& x, const DenseVector& y) -> SparseMatrix { x_ad.set_value(x); y_ad.set_value(y); return H.value(); }, - [&](const Eigen::Vector& x) - -> Eigen::Vector { + [&](const DenseVector& x) -> DenseVector { x_ad.set_value(x); return c_e_ad.value(); }, - [&](const Eigen::Vector& x) - -> Eigen::SparseMatrix { + [&](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( InteriorPointMatrixCallbacks{ - [&](const Eigen::Vector& x) -> Scalar { + [&](const DenseVector& x) -> Scalar { x_ad.set_value(x); return f.value(); }, - [&](const Eigen::Vector& x) - -> Eigen::SparseVector { + [&](const DenseVector& x) -> SparseVector { x_ad.set_value(x); return g.value(); }, - [&](const Eigen::Vector& x, - const Eigen::Vector& y, - const Eigen::Vector& z) - -> Eigen::SparseMatrix { + [&](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& x) - -> Eigen::Vector { + [&](const DenseVector& x) -> DenseVector { x_ad.set_value(x); return c_e_ad.value(); }, - [&](const Eigen::Vector& x) - -> Eigen::SparseMatrix { + [&](const DenseVector& x) -> SparseMatrix { x_ad.set_value(x); return A_e.value(); }, - [&](const Eigen::Vector& x) - -> Eigen::Vector { + [&](const DenseVector& x) -> DenseVector { x_ad.set_value(x); return c_i_ad.value(); }, - [&](const Eigen::Vector& x) - -> Eigen::SparseMatrix { + [&](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 requires requires(F callback, const IterationInfo& info) { { callback(info) } -> std::same_as; @@ -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 requires requires(F callback, const IterationInfo& info) { { callback(info) } -> std::same_as; @@ -642,21 +595,17 @@ class Problem { m_iteration_callbacks.emplace_back(std::forward(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 requires requires(F callback, const IterationInfo& info) { { callback(info) } -> std::same_as; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/exit_status.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/exit_status.hpp index f3acdcaf2d..4d43cea244 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/exit_status.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/exit_status.hpp @@ -4,15 +4,13 @@ #include -#include +#include -#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 { + /// 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 + 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 m_underlying; +}; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point.hpp index 3d0e82e14a..57cc004129 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point.hpp @@ -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 ExitStatus interior_point( const InteriorPointMatrixCallbacks& matrix_callbacks, @@ -71,18 +69,20 @@ ExitStatus interior_point( const Eigen::ArrayX& bound_constraint_mask, #endif Eigen::Vector& x) { - /** - * Interior-point method step direction. - */ + using DenseVector = Eigen::Vector; + using SparseMatrix = Eigen::SparseMatrix; + using SparseVector = Eigen::SparseVector; + + /// Interior-point method step direction. struct Step { /// Primal step. - Eigen::Vector p_x; + DenseVector p_x; /// Equality constraint dual step. - Eigen::Vector p_y; + DenseVector p_y; /// Inequality constraint slack variable step. - Eigen::Vector p_s; + DenseVector p_s; /// Inequality constraint dual step. - Eigen::Vector p_z; + DenseVector p_z; }; using std::isfinite; @@ -132,39 +132,32 @@ ExitStatus interior_point( auto& A_i_prof = solve_profilers[17]; InteriorPointMatrixCallbacks matrices{ - [&](const Eigen::Vector& x) -> Scalar { + [&](const DenseVector& x) -> Scalar { ScopedProfiler prof{f_prof}; return matrix_callbacks.f(x); }, - [&](const Eigen::Vector& x) - -> Eigen::SparseVector { + [&](const DenseVector& x) -> SparseVector { ScopedProfiler prof{g_prof}; return matrix_callbacks.g(x); }, - [&](const Eigen::Vector& x, - const Eigen::Vector& y, - const Eigen::Vector& z) - -> Eigen::SparseMatrix { + [&](const DenseVector& x, const DenseVector& y, + const DenseVector& z) -> SparseMatrix { ScopedProfiler prof{H_prof}; return matrix_callbacks.H(x, y, z); }, - [&](const Eigen::Vector& x) - -> Eigen::Vector { + [&](const DenseVector& x) -> DenseVector { ScopedProfiler prof{c_e_prof}; return matrix_callbacks.c_e(x); }, - [&](const Eigen::Vector& x) - -> Eigen::SparseMatrix { + [&](const DenseVector& x) -> SparseMatrix { ScopedProfiler prof{A_e_prof}; return matrix_callbacks.A_e(x); }, - [&](const Eigen::Vector& x) - -> Eigen::Vector { + [&](const DenseVector& x) -> DenseVector { ScopedProfiler prof{c_i_prof}; return matrix_callbacks.c_i(x); }, - [&](const Eigen::Vector& x) - -> Eigen::SparseMatrix { + [&](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 c_e = matrices.c_e(x); - Eigen::Vector 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 g = matrices.g(x); - Eigen::SparseMatrix A_e = matrices.A_e(x); - Eigen::SparseMatrix 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 s = - Eigen::Vector::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 y = - Eigen::Vector::Zero(num_equality_constraints); - Eigen::Vector z = - Eigen::Vector::Ones(num_inequality_constraints); + DenseVector y = DenseVector::Zero(num_equality_constraints); + DenseVector z = DenseVector::Ones(num_inequality_constraints); - Eigen::SparseMatrix 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 Σ{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 top_left = + const SparseMatrix top_left = H + (A_i.transpose() * Σ * A_i).template triangularView(); 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::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::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 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 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 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 trial_x = x + α * step.p_x; - Eigen::Vector trial_y = y + α_z * step.p_y; - Eigen::Vector 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 trial_c_e = matrices.c_e(trial_x); - Eigen::Vector 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 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 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; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point_matrix_callbacks.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point_matrix_callbacks.hpp index 744e1cf21e..b0855cf622 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point_matrix_callbacks.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point_matrix_callbacks.hpp @@ -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 struct InteriorPointMatrixCallbacks { + /// Type alias for dense vector. + using DenseVector = Eigen::Vector; + /// Type alias for sparse matrix. + using SparseMatrix = Eigen::SparseMatrix; + /// Type alias for sparse vector. + using SparseVector = Eigen::SparseVector; + /// Cost function value f(x) getter. /// /// @@ -35,7 +40,7 @@ struct InteriorPointMatrixCallbacks { /// /// ///
1
- std::function& x)> f; + std::function f; /// Cost function gradient ∇f(x) getter. /// @@ -56,9 +61,7 @@ struct InteriorPointMatrixCallbacks { /// 1 /// /// - std::function( - const Eigen::Vector& x)> - g; + std::function g; /// Lagrangian Hessian ∇ₓₓ²L(x, y, z) getter. /// @@ -91,10 +94,8 @@ struct InteriorPointMatrixCallbacks { /// num_decision_variables /// /// - std::function( - const Eigen::Vector& x, - const Eigen::Vector& y, - const Eigen::Vector& z)> + std::function H; /// Equality constraint value cₑ(x) getter. @@ -116,18 +117,16 @@ struct InteriorPointMatrixCallbacks { /// 1 /// /// - std::function( - const Eigen::Vector& x)> - c_e; + std::function c_e; /// Equality constraint Jacobian ∂cₑ/∂x getter. /// - /// @verbatim + /// ``` /// [∇ᵀcₑ₁(xₖ)] /// Aₑ(x) = [∇ᵀcₑ₂(xₖ)] /// [ ⋮ ] /// [∇ᵀcₑₘ(xₖ)] - /// @endverbatim + /// ``` /// /// /// @@ -146,9 +145,7 @@ struct InteriorPointMatrixCallbacks { /// /// ///
num_decision_variables
- std::function( - const Eigen::Vector& x)> - A_e; + std::function A_e; /// Inequality constraint value cᵢ(x) getter. /// @@ -169,18 +166,16 @@ struct InteriorPointMatrixCallbacks { /// 1 /// /// - std::function( - const Eigen::Vector& x)> - c_i; + std::function c_i; /// Inequality constraint Jacobian ∂cᵢ/∂x getter. /// - /// @verbatim + /// ``` /// [∇ᵀcᵢ₁(xₖ)] /// Aᵢ(x) = [∇ᵀcᵢ₂(xₖ)] /// [ ⋮ ] /// [∇ᵀcᵢₘ(xₖ)] - /// @endverbatim + /// ``` /// /// /// @@ -199,9 +194,7 @@ struct InteriorPointMatrixCallbacks { /// /// ///
num_decision_variables
- std::function( - const Eigen::Vector& x)> - A_i; + std::function A_i; }; } // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/iteration_info.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/iteration_info.hpp index 234bf3c7db..60e9d65d79 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/iteration_info.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/iteration_info.hpp @@ -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 struct IterationInfo { /// The solver iteration. diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton.hpp index 20b3a0ecb1..4a1b4c4d08 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton.hpp @@ -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 ExitStatus newton( const NewtonMatrixCallbacks& matrix_callbacks, std::span& info)>> iteration_callbacks, const Options& options, Eigen::Vector& x) { + using DenseVector = Eigen::Vector; + using SparseMatrix = Eigen::SparseMatrix; + using SparseVector = Eigen::SparseVector; + 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 matrices{ - [&](const Eigen::Vector& x) -> Scalar { + [&](const DenseVector& x) -> Scalar { ScopedProfiler prof{f_prof}; return matrix_callbacks.f(x); }, - [&](const Eigen::Vector& x) - -> Eigen::SparseVector { + [&](const DenseVector& x) -> SparseVector { ScopedProfiler prof{g_prof}; return matrix_callbacks.g(x); }, - [&](const Eigen::Vector& x) - -> Eigen::SparseMatrix { + [&](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 g = matrices.g(x); - Eigen::SparseMatrix 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{}, - Eigen::SparseMatrix{}})) { + 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 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 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(g); - Eigen::Vector trial_x = x + α_max * p_x; + DenseVector trial_x = x + α_max * p_x; Scalar next_kkt_error = kkt_error(matrices.g(trial_x)); diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton_matrix_callbacks.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton_matrix_callbacks.hpp index d8ba9b1d34..85bc698a97 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton_matrix_callbacks.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton_matrix_callbacks.hpp @@ -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 struct NewtonMatrixCallbacks { + /// Type alias for dense vector. + using DenseVector = Eigen::Vector; + /// Type alias for sparse matrix. + using SparseMatrix = Eigen::SparseMatrix; + /// Type alias for sparse vector. + using SparseVector = Eigen::SparseVector; + /// Cost function value f(x) getter. /// /// @@ -35,7 +40,7 @@ struct NewtonMatrixCallbacks { /// /// ///
1
- std::function& x)> f; + std::function f; /// Cost function gradient ∇f(x) getter. /// @@ -56,9 +61,7 @@ struct NewtonMatrixCallbacks { /// 1 /// /// - std::function( - const Eigen::Vector& x)> - g; + std::function g; /// Lagrangian Hessian ∇ₓₓ²L(x) getter. /// @@ -81,9 +84,7 @@ struct NewtonMatrixCallbacks { /// num_decision_variables /// /// - std::function( - const Eigen::Vector& x)> - H; + std::function H; }; } // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/options.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/options.hpp index 8b9cb93400..5603569011 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/options.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/options.hpp @@ -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; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp.hpp index 6897d1f927..a4738668e6 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp.hpp @@ -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 ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, std::span& info)>> iteration_callbacks, const Options& options, Eigen::Vector& x) { - /** - * SQP step direction. - */ + using DenseVector = Eigen::Vector; + using SparseMatrix = Eigen::SparseMatrix; + using SparseVector = Eigen::SparseVector; + + /// SQP step direction. struct Step { /// Primal step. - Eigen::Vector p_x; + DenseVector p_x; /// Dual step. - Eigen::Vector p_y; + DenseVector p_y; }; using std::isfinite; @@ -113,28 +113,23 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, auto& A_e_prof = solve_profilers[15]; SQPMatrixCallbacks matrices{ - [&](const Eigen::Vector& x) -> Scalar { + [&](const DenseVector& x) -> Scalar { ScopedProfiler prof{f_prof}; return matrix_callbacks.f(x); }, - [&](const Eigen::Vector& x) - -> Eigen::SparseVector { + [&](const DenseVector& x) -> SparseVector { ScopedProfiler prof{g_prof}; return matrix_callbacks.g(x); }, - [&](const Eigen::Vector& x, - const Eigen::Vector& y) - -> Eigen::SparseMatrix { + [&](const DenseVector& x, const DenseVector& y) -> SparseMatrix { ScopedProfiler prof{H_prof}; return matrix_callbacks.H(x, y); }, - [&](const Eigen::Vector& x) - -> Eigen::Vector { + [&](const DenseVector& x) -> DenseVector { ScopedProfiler prof{c_e_prof}; return matrix_callbacks.c_e(x); }, - [&](const Eigen::Vector& x) - -> Eigen::SparseMatrix { + [&](const DenseVector& x) -> SparseMatrix { ScopedProfiler prof{A_e_prof}; return matrix_callbacks.A_e(x); }}; @@ -146,7 +141,7 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, setup_prof.start(); Scalar f = matrices.f(x); - Eigen::Vector 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& matrix_callbacks, return ExitStatus::TOO_FEW_DOFS; } - Eigen::SparseVector g = matrices.g(x); - Eigen::SparseMatrix A_e = matrices.A_e(x); + SparseVector g = matrices.g(x); + SparseMatrix A_e = matrices.A_e(x); - Eigen::Vector y = - Eigen::Vector::Zero(num_equality_constraints); + DenseVector y = DenseVector::Zero(num_equality_constraints); - Eigen::SparseMatrix 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& matrix_callbacks, // Call iteration callbacks for (const auto& callback : iteration_callbacks) { - if (callback({iterations, x, g, H, A_e, Eigen::SparseMatrix{}})) { + if (callback({iterations, x, g, H, A_e, SparseMatrix{}})) { return ExitStatus::CALLBACK_REQUESTED_STOP; } } @@ -251,24 +245,21 @@ ExitStatus sqp(const SQPMatrixCallbacks& 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::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::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 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 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& matrix_callbacks, auto compute_step = [&](Step& step) { // p = [ pˣ] // [−pʸ] - Eigen::Vector 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& matrix_callbacks, // Loop until a step is accepted while (1) { - Eigen::Vector trial_x = x + α * step.p_x; - Eigen::Vector 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 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& matrix_callbacks, auto soc_step = step; Scalar α_soc = α; - Eigen::Vector 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; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp_matrix_callbacks.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp_matrix_callbacks.hpp index 4975890db2..55aabcc6e8 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp_matrix_callbacks.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp_matrix_callbacks.hpp @@ -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 struct SQPMatrixCallbacks { + /// Type alias for dense vector. + using DenseVector = Eigen::Vector; + /// Type alias for sparse matrix. + using SparseMatrix = Eigen::SparseMatrix; + /// Type alias for sparse vector. + using SparseVector = Eigen::SparseVector; + /// Cost function value f(x) getter. /// /// @@ -35,7 +40,7 @@ struct SQPMatrixCallbacks { /// /// ///
1
- std::function& x)> f; + std::function f; /// Cost function gradient ∇f(x) getter. /// @@ -56,9 +61,7 @@ struct SQPMatrixCallbacks { /// 1 /// /// - std::function( - const Eigen::Vector& x)> - g; + std::function g; /// Lagrangian Hessian ∇ₓₓ²L(x, y) getter. /// @@ -86,10 +89,7 @@ struct SQPMatrixCallbacks { /// num_decision_variables /// /// - std::function( - const Eigen::Vector& x, - const Eigen::Vector& y)> - H; + std::function H; /// Equality constraint value cₑ(x) getter. /// @@ -110,18 +110,16 @@ struct SQPMatrixCallbacks { /// 1 /// /// - std::function( - const Eigen::Vector& x)> - c_e; + std::function c_e; /// Equality constraint Jacobian ∂cₑ/∂x getter. /// - /// @verbatim + /// ``` /// [∇ᵀcₑ₁(xₖ)] /// Aₑ(x) = [∇ᵀcₑ₂(xₖ)] /// [ ⋮ ] /// [∇ᵀcₑₘ(xₖ)] - /// @endverbatim + /// ``` /// /// /// @@ -140,9 +138,7 @@ struct SQPMatrixCallbacks { /// /// ///
num_decision_variables
- std::function( - const Eigen::Vector& x)> - A_e; + std::function A_e; }; } // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/bounds.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/bounds.hpp index 2d037e1c7a..85e46fbf46 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/bounds.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/bounds.hpp @@ -19,11 +19,9 @@ namespace slp { -/** - * Bound constraint metadata. - * - * @tparam Scalar Scalar type. - */ +/// Bound constraint metadata. +/// +/// @tparam Scalar Scalar type. template 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 Bounds get_bounds( std::span> decision_variables, @@ -182,20 +178,18 @@ Bounds 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 requires(static_cast(Eigen::DenseBase::IsVectorAtCompileTime)) void project_onto_bounds( diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/error_estimate.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/error_estimate.hpp index 7035503d83..d82d5bf85a 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/error_estimate.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/error_estimate.hpp @@ -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 Scalar error_estimate(const Eigen::Vector& g) { // Update the error estimate using the KKT conditions from equations (19.5a) @@ -27,17 +25,15 @@ Scalar error_estimate(const Eigen::Vector& g) { return g.template lpNorm(); } -/** - * 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 Scalar error_estimate(const Eigen::Vector& g, const Eigen::SparseMatrix& A_e, @@ -65,25 +61,23 @@ Scalar error_estimate(const Eigen::Vector& g, c_e.template lpNorm()}); } -/** - * 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 Scalar error_estimate(const Eigen::Vector& g, const Eigen::SparseMatrix& A_e, diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/filter.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/filter.hpp index 0c9795021f..c96ffb5b65 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/filter.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/filter.hpp @@ -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 struct FilterEntry { + /// Type alias for dense vector. + using DenseVector = Eigen::Vector; + /// 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& 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& s, - const Eigen::Vector& c_e, - const Eigen::Vector& 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 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::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& 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&& 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& 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&& 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& 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& back() const { return m_filter.back(); } private: diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/fraction_to_the_boundary_rule.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/fraction_to_the_boundary_rule.hpp index 52909a4c9e..f48b158870 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/fraction_to_the_boundary_rule.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/fraction_to_the_boundary_rule.hpp @@ -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 Scalar fraction_to_the_boundary_rule( - const Eigen::Ref>& x, - const Eigen::Ref>& p, - Scalar τ) { + const Eigen::Vector& x, + const Eigen::Vector& p, Scalar τ) { // α = max(α ∈ (0, 1] : x + αp ≥ (1 − τ)x) // // where x and τ are positive. diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/inertia.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/inertia.hpp index 3cfa6a4ed2..bd1a1a43d5 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/inertia.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/inertia.hpp @@ -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 explicit Inertia(const Eigen::Vector& 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 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; }; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/is_locally_infeasible.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/is_locally_infeasible.hpp index 9b5bee271b..c2a9748ad4 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/is_locally_infeasible.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/is_locally_infeasible.hpp @@ -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 bool is_equality_locally_infeasible( const Eigen::SparseMatrix& 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 bool is_inequality_locally_infeasible( const Eigen::SparseMatrix& A_i, diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/kkt_error.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/kkt_error.hpp index fa09eac183..7feea13236 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/kkt_error.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/kkt_error.hpp @@ -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 Scalar kkt_error(const Eigen::Vector& 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& 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 Scalar kkt_error(const Eigen::Vector& g, const Eigen::SparseMatrix& A_e, @@ -51,24 +47,22 @@ Scalar kkt_error(const Eigen::Vector& 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 Scalar kkt_error(const Eigen::Vector& g, const Eigen::SparseMatrix& A_e, diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/regularized_ldlt.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/regularized_ldlt.hpp index 0a6fc1233d..43a29285ee 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/regularized_ldlt.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/regularized_ldlt.hpp @@ -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 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; + /// Type alias for dense vector. + using DenseVector = Eigen::Vector; + /// Type alias for sparse matrix. + using SparseMatrix = Eigen::SparseMatrix; + + /// 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& 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 - Eigen::Vector solve( - const Eigen::MatrixBase& rhs) { + DenseVector solve(const Eigen::MatrixBase& 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 - Eigen::Vector solve( - const Eigen::SparseMatrixBase& rhs) { + DenseVector solve(const Eigen::SparseMatrixBase& 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>; - using DenseSolver = - Eigen::LDLT>; + using SparseSolver = Eigen::SimplicialLDLT; + using DenseSolver = Eigen::LDLT; 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& 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 regularization(Scalar δ, Scalar γ) { - Eigen::Vector 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{vec.asDiagonal()}; + return SparseMatrix{vec.asDiagonal()}; } }; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/assert.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/assert.hpp index 5d432608de..4f0203a4b8 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/assert.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/assert.hpp @@ -3,15 +3,12 @@ #pragma once #ifdef SLEIPNIR_PYTHON - #include #include #include -/** - * 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 -/** - * Abort in C++. - */ +/// Abort in C++. #define slp_assert(condition) assert(condition) - #endif diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/empty.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/empty.hpp index 4be351456d..82bf9eea86 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/empty.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/empty.hpp @@ -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 diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/function_ref.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/function_ref.hpp index c3f4212d72..f42866030f 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/function_ref.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/function_ref.hpp @@ -12,29 +12,23 @@ namespace slp { template 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 function_ref { 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& 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 requires(!std::is_same_v, function_ref> && std::is_invocable_r_v) @@ -49,21 +43,17 @@ class function_ref { }; } - /** - * 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& operator=( const function_ref& 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 requires std::is_invocable_r_v constexpr function_ref& operator=(F&& f) noexcept { @@ -77,22 +67,18 @@ class function_ref { 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& 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)...); } @@ -102,9 +88,7 @@ class function_ref { R (*callback_)(void*, Args...) = nullptr; }; -/** - * Swaps the referred callables of `lhs` and `rhs`. - */ +/// Swaps the referred callables of `lhs` and `rhs`. template constexpr void swap(function_ref& lhs, function_ref& rhs) noexcept { diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/intrusive_shared_ptr.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/intrusive_shared_ptr.hpp index 5e0229bcca..b4f0c32a4e 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/intrusive_shared_ptr.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/intrusive_shared_ptr.hpp @@ -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 class IntrusiveSharedPtr { public: template 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& 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 requires(!std::same_as && std::convertible_to) // 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& operator=( const IntrusiveSharedPtr& 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 requires(!std::same_as && std::convertible_to) // 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&& 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 requires(!std::same_as && std::convertible_to) // NOLINTNEXTLINE (google-explicit-constructor) constexpr IntrusiveSharedPtr(IntrusiveSharedPtr&& 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& operator=( IntrusiveSharedPtr&& 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 requires(!std::same_as && std::convertible_to) constexpr IntrusiveSharedPtr& 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& lhs, const IntrusiveSharedPtr& 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& lhs, const IntrusiveSharedPtr& 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& 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& 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& 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& 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 IntrusiveSharedPtr make_intrusive_shared(Args&&... args) { return IntrusiveSharedPtr{new T(std::forward(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 IntrusiveSharedPtr allocate_intrusive_shared(Alloc alloc, Args&&... args) { auto ptr = std::allocator_traits::allocate(alloc, sizeof(T)); diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/pool.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/pool.hpp index 1386056b40..32c449504e 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/pool.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/pool.hpp @@ -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 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 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&) = default; - /** - * Copy assignment operator. - * - * @return This pool allocator. - */ + /// Copy assignment operator. + /// + /// @return This pool allocator. constexpr PoolAllocator& operator=(const PoolAllocator&) = 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(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 PoolAllocator global_pool_allocator() { return PoolAllocator{&global_pool_resource()}; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/print.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/print.hpp index a89b7d4f98..b2920580ee 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/print.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/print.hpp @@ -7,21 +7,14 @@ #include #include -#if __has_include() #include -#else -#include -#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 void print(fmt::format_string fmt, T&&... args) { try { @@ -30,9 +23,7 @@ void print(fmt::format_string fmt, T&&... args) { } } -/** - * Wrapper around fmt::print() that squelches write failure exceptions. - */ +/// Wrapper around fmt::print() that squelches write failure exceptions. template void print(std::FILE* f, fmt::format_string fmt, T&&... args) { try { @@ -41,9 +32,7 @@ void print(std::FILE* f, fmt::format_string fmt, T&&... args) { } } -/** - * Wrapper around fmt::println() that squelches write failure exceptions. - */ +/// Wrapper around fmt::println() that squelches write failure exceptions. template void println(fmt::format_string fmt, T&&... args) { try { @@ -52,9 +41,7 @@ void println(fmt::format_string fmt, T&&... args) { } } -/** - * Wrapper around fmt::println() that squelches write failure exceptions. - */ +/// Wrapper around fmt::println() that squelches write failure exceptions. template void println(std::FILE* f, fmt::format_string fmt, T&&... args) { try { diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/print_diagnostics.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/print_diagnostics.hpp index 032be8fb7b..c50d7e171b 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/print_diagnostics.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/print_diagnostics.hpp @@ -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 > constexpr double to_ms(const std::chrono::duration& duration) { using std::chrono::duration_cast; @@ -44,12 +40,10 @@ constexpr double to_ms(const std::chrono::duration& duration) { return duration_cast(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 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 void print_too_few_dofs_error( const Eigen::Vector& 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 void print_c_e_local_infeasibility_error( const Eigen::Vector& 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 void print_c_i_local_infeasibility_error( const Eigen::Vector& 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 > void print_iteration_diagnostics(int iterations, IterationType type, const std::chrono::duration& 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 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& 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& setup_profilers) { auto setup_duration = to_ms(setup_profilers[0].duration()); diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/scope_exit.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/scope_exit.hpp index 9277f94c5c..289f6eb985 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/scope_exit.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/scope_exit.hpp @@ -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 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)} {} ~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: diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/scoped_profiler.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/scoped_profiler.hpp index 16f93fae6c..f84e0f38ef 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/scoped_profiler.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/scoped_profiler.hpp @@ -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 requires std::same_as || std::same_as 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& current_duration() const { return m_profiler->current_duration(); } diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/setup_profiler.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/setup_profiler.hpp index 031f99d2e5..f5a0e75259 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/setup_profiler.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/setup_profiler.hpp @@ -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& duration() const { return m_setup_duration; } diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/solve_profiler.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/solve_profiler.hpp index fb17f1dc39..d6f6beeb9a 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/solve_profiler.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/solve_profiler.hpp @@ -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& 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& 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& total_duration() const { return m_total_solve_duration; } diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/spy.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/spy.hpp index 4b810e54a8..49dc97ccea 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/spy.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/spy.hpp @@ -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. spy.py - * can display it as an animation. - * - * The file starts with the following header: - *
    - *
  1. Plot title (length as a little-endian int32, then characters)
    2. - *
  2. Row label (length as a little-endian int32, then characters)
    3. - *
  3. Column label (length as a little-endian int32, then characters)
    4. - *
- * - * Then, each sparsity pattern starts with: - *
    - *
  1. Number of coordinates as a little-endian int32
    2. - *
- * - * followed by that many coordinates in the following format: - *
    - *
  1. Row index as a little-endian int32
    2. - *
  2. Column index as a little-endian int32
    3. - *
  3. Sign as a character ('+' for positive, '-' for negative, or '0' for - * zero)
    4. - *
- * - * @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. spy.py +/// can display it as an animation. +/// +/// The file starts with the following header: +///
    +///
  1. Plot title (length as a little-endian int32, then characters)
    2. +///
  2. Row label (length as a little-endian int32, then characters)
    3. +///
  3. Column label (length as a little-endian int32, then characters)
    4. +///
+/// +/// Then, each sparsity pattern starts with: +///
    +///
  1. Number of coordinates as a little-endian int32
    2. +///
+/// +/// followed by that many coordinates in the following format: +///
    +///
  1. Row index as a little-endian int32
    2. +///
  2. Column index as a little-endian int32
    3. +///
  3. Sign as a character ('+' for positive, '-' for negative, or '0' for +/// zero)
    4. +///
+/// +/// @tparam Scalar Scalar type. template 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& 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); diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/unreachable.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/unreachable.hpp new file mode 100644 index 0000000000..2e5ec3e716 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/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