From 32bf24aa19c260a650c53c1e5b8299f8c74db4ee Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 27 Jun 2026 12:10:39 -0700 Subject: [PATCH] [upstream_utils] Upgrade to Sleipnir 0.6.3 (#9030) Also drop patches since we're now on C++23. --- .../src/main/native/cpp/CartPoleBenchmark.hpp | 4 +- docs/build.gradle | 1 + upstream_utils/sleipnir.py | 2 +- .../sleipnir_patches/0001-Use-fmtlib.patch | 286 ----- ...r.patch => 0001-Use-wpi-SmallVector.patch} | 2 +- .../0002-Downgrade-to-Eigen-5.0.1.patch | 307 ++++++ .../0003-Use-wpi-byteswap.patch | 30 - .../0004-Replace-std-to_underlying.patch | 89 -- .../0005-Replace-std-views-zip.patch | 28 - .../0006-Replace-std-unreachable.patch | 86 -- ...-Suppress-clang-tidy-false-positives.patch | 22 - ...ppress-GCC-12-warning-false-positive.patch | 34 - ...ress-Doxygen-warning-false-positives.patch | 31 - ...tead-of-multidimensional-array-subsc.patch | 996 ------------------ .../sleipnir/autodiff/expression_type.hpp | 20 +- .../include/sleipnir/autodiff/hessian.hpp | 4 +- .../include/sleipnir/autodiff/jacobian.hpp | 4 +- .../include/sleipnir/autodiff/variable.hpp | 24 +- .../sleipnir/autodiff/variable_block.hpp | 94 +- .../sleipnir/autodiff/variable_matrix.hpp | 173 +-- .../include/sleipnir/optimization/ocp.hpp | 14 +- .../include/sleipnir/optimization/problem.hpp | 24 +- .../optimization/solver/exit_status.hpp | 20 +- .../solver/util/feasibility_restoration.hpp | 4 - .../optimization/solver/util/inertia.hpp | 22 +- .../sleipnir/include/sleipnir/util/assert.hpp | 5 +- .../sleipnir/include/sleipnir/util/print.hpp | 27 +- .../sleipnir/util/print_diagnostics.hpp | 3 +- .../sleipnir/include/sleipnir/util/spy.hpp | 3 +- .../include/sleipnir/util/to_underlying.hpp | 14 - .../include/sleipnir/util/unreachable.hpp | 16 - 31 files changed, 536 insertions(+), 1853 deletions(-) delete mode 100644 upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch rename upstream_utils/sleipnir_patches/{0002-Use-wpi-SmallVector.patch => 0001-Use-wpi-SmallVector.patch} (98%) create mode 100644 upstream_utils/sleipnir_patches/0002-Downgrade-to-Eigen-5.0.1.patch delete mode 100644 upstream_utils/sleipnir_patches/0003-Use-wpi-byteswap.patch delete mode 100644 upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch delete mode 100644 upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch delete mode 100644 upstream_utils/sleipnir_patches/0006-Replace-std-unreachable.patch delete mode 100644 upstream_utils/sleipnir_patches/0007-Suppress-clang-tidy-false-positives.patch delete mode 100644 upstream_utils/sleipnir_patches/0008-Suppress-GCC-12-warning-false-positive.patch delete mode 100644 upstream_utils/sleipnir_patches/0009-Suppress-Doxygen-warning-false-positives.patch delete mode 100644 upstream_utils/sleipnir_patches/0010-Use-operator-instead-of-multidimensional-array-subsc.patch delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/to_underlying.hpp delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/unreachable.hpp diff --git a/benchmark/src/main/native/cpp/CartPoleBenchmark.hpp b/benchmark/src/main/native/cpp/CartPoleBenchmark.hpp index 413d300891..c6bd844191 100644 --- a/benchmark/src/main/native/cpp/CartPoleBenchmark.hpp +++ b/benchmark/src/main/native/cpp/CartPoleBenchmark.hpp @@ -72,9 +72,9 @@ inline void BM_CartPole(benchmark::State& state) { // Initial guess for (int k = 0; k < N + 1; ++k) { - X(0, k).set_value( + X[0, k].set_value( std::lerp(x_initial[0], x_final[0], static_cast(k) / N)); - X(1, k).set_value( + X[1, k].set_value( std::lerp(x_initial[1], x_final[1], static_cast(k) / N)); } diff --git a/docs/build.gradle b/docs/build.gradle index cd0469f943..23cd3aa90d 100644 --- a/docs/build.gradle +++ b/docs/build.gradle @@ -103,6 +103,7 @@ doxygen.sourceSets.main { exclude 'sleipnir/optimization/solver/interior_point.hpp' exclude 'sleipnir/optimization/solver/newton.hpp' exclude 'sleipnir/optimization/solver/sqp.hpp' + exclude 'sleipnir/optimization/solver/util/feasibility_restoration.hpp' // apriltag exclude 'apriltag_pose.h' diff --git a/upstream_utils/sleipnir.py b/upstream_utils/sleipnir.py index 40df4321f7..684ecbf73f 100755 --- a/upstream_utils/sleipnir.py +++ b/upstream_utils/sleipnir.py @@ -46,7 +46,7 @@ using small_vector = wpi::util::SmallVector; def main(): name = "sleipnir" url = "https://github.com/SleipnirGroup/Sleipnir" - tag = "v0.6.2" + tag = "v0.6.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 deleted file mode 100644 index edd3969f37..0000000000 --- a/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch +++ /dev/null @@ -1,286 +0,0 @@ -From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 -From: Tyler Veness -Date: Wed, 29 May 2024 16:29:55 -0700 -Subject: [PATCH 01/10] Use fmtlib - ---- - include/sleipnir/autodiff/expression_type.hpp | 16 +++++++---- - include/sleipnir/optimization/problem.hpp | 1 + - .../optimization/solver/exit_status.hpp | 16 +++++++---- - .../optimization/solver/util/inertia.hpp | 22 ++++++++------- - include/sleipnir/util/assert.hpp | 5 ++-- - include/sleipnir/util/print.hpp | 27 ++++++++++--------- - 6 files changed, 53 insertions(+), 34 deletions(-) - -diff --git a/include/sleipnir/autodiff/expression_type.hpp b/include/sleipnir/autodiff/expression_type.hpp -index 12d0568f628d5b97c0c2f5291851d4b20921f9d3..d06d32dac6c7b6faeedefeaa107cedac8446a3ab 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. -@@ -27,14 +28,16 @@ enum class ExpressionType : uint8_t { - - } // namespace slp - -+// @cond Suppress Doxygen -+ - /// 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); - } - -@@ -45,7 +48,8 @@ struct std::formatter { - /// @param ctx Format context. - /// @return Format context iterator. - template -- auto format(const slp::ExpressionType& type, FmtContext& ctx) const { -+ constexpr auto format(const slp::ExpressionType& type, -+ FmtContext& ctx) const { - using enum slp::ExpressionType; - - switch (type) { -@@ -65,5 +69,7 @@ struct std::formatter { - } - - private: -- std::formatter m_underlying; -+ fmt::formatter m_underlying; - }; -+ -+// @endcond -diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp -index ccff8b9425b45fd1c790bc4da87b81f501a70d9c..41b44578b1e2783dbceb300b7c72ee0c8d1202df 100644 ---- a/include/sleipnir/optimization/problem.hpp -+++ b/include/sleipnir/optimization/problem.hpp -@@ -15,6 +15,7 @@ - - #include - #include -+#include - #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 0a48df7423b5a3dccd8e611e91befd32487fafdc..8786d6d64ac44ac88133df65a79636ec133b1b64 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. -@@ -45,14 +46,16 @@ enum class ExitStatus : int8_t { - - } // namespace slp - -+// @cond Suppress Doxygen -+ - /// Formatter for ExitStatus. - template <> --struct std::formatter { -+struct fmt::formatter { - /// Parses 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); - } - -@@ -63,7 +66,8 @@ struct std::formatter { - /// @param ctx Format context. - /// @return Format context iterator. - template -- auto format(const slp::ExitStatus& exit_status, FmtContext& ctx) const { -+ constexpr auto format(const slp::ExitStatus& exit_status, -+ FmtContext& ctx) const { - using enum slp::ExitStatus; - - switch (exit_status) { -@@ -97,5 +101,7 @@ struct std::formatter { - } - - private: -- std::formatter m_underlying; -+ fmt::formatter m_underlying; - }; -+ -+// @endcond -diff --git a/include/sleipnir/optimization/solver/util/inertia.hpp b/include/sleipnir/optimization/solver/util/inertia.hpp -index 49ee606f7e3f02f2f5c24079eb27a412dad3055f..2d2ce114be9c40e645d685b63465b977e0b70439 100644 ---- a/include/sleipnir/optimization/solver/util/inertia.hpp -+++ b/include/sleipnir/optimization/solver/util/inertia.hpp -@@ -2,10 +2,10 @@ - - #pragma once - --#include - #include - - #include -+#include - - namespace slp { - -@@ -77,14 +77,16 @@ class Inertia { - - } // namespace slp - -+// @cond Suppress Doxygen -+ - /// Formatter for Inertia. - template <> --struct std::formatter { -+struct fmt::formatter { - /// Parses 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); - } - -@@ -95,18 +97,20 @@ struct std::formatter { - /// @param ctx Format context. - /// @return Format context iterator. - template -- auto format(const slp::Inertia& inertia, FmtContext& ctx) const { -+ constexpr auto format(const slp::Inertia& inertia, FmtContext& ctx) const { - auto out = ctx.out(); - -- out = std::format_to(out, "("); -+ out = fmt::format_to(out, "("); - out = m_underlying.format(inertia.positive, ctx); -- out = std::format_to(out, ", "); -+ out = fmt::format_to(out, ", "); - out = m_underlying.format(inertia.negative, ctx); -- out = std::format_to(out, ", "); -+ out = fmt::format_to(out, ", "); - out = m_underlying.format(inertia.zero, ctx); -- return std::format_to(out, ")"); -+ return fmt::format_to(out, ")"); - } - - private: -- std::formatter m_underlying; -+ fmt::formatter m_underlying; - }; -+ -+// @endcond -diff --git a/include/sleipnir/util/assert.hpp b/include/sleipnir/util/assert.hpp -index 29dfe5416530a476417de3a4b6434d3b28d73120..783a0e71a1cb54bd1d6d17fa9d2bedd2038a8ef5 100644 ---- a/include/sleipnir/util/assert.hpp -+++ b/include/sleipnir/util/assert.hpp -@@ -3,16 +3,17 @@ - #pragma once - - #ifdef SLEIPNIR_PYTHON --#include - #include - #include - -+#include -+ - /// Throws an exception in Python. - #define slp_assert(condition) \ - do { \ - if (!(condition)) { \ - auto location = std::source_location::current(); \ -- throw std::invalid_argument(std::format( \ -+ throw std::invalid_argument(fmt::format( \ - "{}:{}: {}: Assertion `{}' failed.", location.file_name(), \ - location.line(), location.function_name(), #condition)); \ - } \ -diff --git a/include/sleipnir/util/print.hpp b/include/sleipnir/util/print.hpp -index d436bf297b85cf84fad642fa43aa97063e7d6ebb..b2920580eea0297fc387b5168df78a1515a5dd60 100644 ---- a/include/sleipnir/util/print.hpp -+++ b/include/sleipnir/util/print.hpp -@@ -4,47 +4,48 @@ - - #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS - #include --#include - #include - #include -+ -+#include - #endif - - namespace slp { - - #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS - --/// 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) { - try { -- std::print(fmt, std::forward(args)...); -+ fmt::print(fmt, std::forward(args)...); - } catch (const std::system_error&) { - } - } - --/// 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) { - try { -- std::print(f, fmt, std::forward(args)...); -+ fmt::print(f, fmt, std::forward(args)...); - } catch (const std::system_error&) { - } - } - --/// 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) { - try { -- std::println(fmt, std::forward(args)...); -+ fmt::println(fmt, std::forward(args)...); - } catch (const std::system_error&) { - } - } - --/// 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) { - try { -- std::println(f, fmt, std::forward(args)...); -+ fmt::println(f, fmt, std::forward(args)...); - } catch (const std::system_error&) { - } - } diff --git a/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch b/upstream_utils/sleipnir_patches/0001-Use-wpi-SmallVector.patch similarity index 98% rename from upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch rename to upstream_utils/sleipnir_patches/0001-Use-wpi-SmallVector.patch index 3a8bf7ffa3..53ad72d6b0 100644 --- a/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch +++ b/upstream_utils/sleipnir_patches/0001-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 02/10] Use wpi::SmallVector +Subject: [PATCH 1/2] Use wpi::SmallVector --- include/sleipnir/autodiff/expression.hpp | 4 ++-- diff --git a/upstream_utils/sleipnir_patches/0002-Downgrade-to-Eigen-5.0.1.patch b/upstream_utils/sleipnir_patches/0002-Downgrade-to-Eigen-5.0.1.patch new file mode 100644 index 0000000000..f7e0e7f993 --- /dev/null +++ b/upstream_utils/sleipnir_patches/0002-Downgrade-to-Eigen-5.0.1.patch @@ -0,0 +1,307 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Tyler Veness +Date: Sun, 26 Apr 2026 16:56:01 -0700 +Subject: [PATCH 2/2] Downgrade to Eigen 5.0.1 + +--- + include/sleipnir/autodiff/variable.hpp | 20 ++++++-- + include/sleipnir/autodiff/variable_block.hpp | 30 +++++++++--- + include/sleipnir/autodiff/variable_matrix.hpp | 46 ++++++++++++------- + 3 files changed, 69 insertions(+), 27 deletions(-) + +diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp +index ad8baa3b58047f5257c210b88446324cca121d4a..6c692cbe113a3aaf7f02727260ed0b6d1f4a3c23 100644 +--- a/include/sleipnir/autodiff/variable.hpp ++++ b/include/sleipnir/autodiff/variable.hpp +@@ -733,7 +733,11 @@ 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 +- constraints.emplace_back(lhs - rhs[row, col]); ++ if constexpr (EigenMatrixLike) { ++ constraints.emplace_back(lhs - rhs(row, col)); ++ } else { ++ constraints.emplace_back(lhs - rhs[row, col]); ++ } + } + } + +@@ -749,7 +753,11 @@ 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 +- constraints.emplace_back(lhs[row, col] - rhs); ++ if constexpr (EigenMatrixLike) { ++ constraints.emplace_back(lhs(row, col) - rhs); ++ } else { ++ constraints.emplace_back(lhs[row, col] - rhs); ++ } + } + } + +@@ -767,7 +775,13 @@ 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 +- constraints.emplace_back(lhs[row, col] - rhs[row, col]); ++ if constexpr (!EigenMatrixLike && !EigenMatrixLike) { ++ constraints.emplace_back(lhs[row, col] - rhs[row, col]); ++ } else if constexpr (!EigenMatrixLike && EigenMatrixLike) { ++ constraints.emplace_back(lhs[row, col] - rhs(row, col)); ++ } else if constexpr (EigenMatrixLike && !EigenMatrixLike) { ++ constraints.emplace_back(lhs(row, col) - rhs[row, col]); ++ } + } + } + +diff --git a/include/sleipnir/autodiff/variable_block.hpp b/include/sleipnir/autodiff/variable_block.hpp +index 55cafb5d48afff2dbdff07516d85759180ef74c7..cf1b2a37614f748db679071df1780a569527983f 100644 +--- a/include/sleipnir/autodiff/variable_block.hpp ++++ b/include/sleipnir/autodiff/variable_block.hpp +@@ -161,7 +161,7 @@ class VariableBlock : public SleipnirBase { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] = values[row, col]; ++ (*this)[row, col] = values(row, col); + } + } + +@@ -178,7 +178,7 @@ class VariableBlock : public SleipnirBase { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col].set_value(values[row, col]); ++ (*this)[row, col].set_value(values(row, col)); + } + } + } +@@ -438,7 +438,11 @@ class VariableBlock : public SleipnirBase { + for (int j = 0; j < rhs.cols(); ++j) { + Variable sum{Scalar(0)}; + for (int k = 0; k < cols(); ++k) { +- sum += lhs_old_row[k] * rhs[k, j]; ++ if constexpr (EigenMatrixLike) { ++ sum += lhs_old_row[k] * rhs(k, j); ++ } else { ++ sum += lhs_old_row[k] * rhs[k, j]; ++ } + } + (*this)[i, j] = sum; + } +@@ -470,7 +474,11 @@ class VariableBlock : public SleipnirBase { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] /= rhs[0, 0]; ++ if constexpr (EigenMatrixLike) { ++ (*this)[row, col] /= rhs(0, 0); ++ } else { ++ (*this)[row, col] /= rhs[0, 0]; ++ } + } + } + +@@ -500,7 +508,11 @@ class VariableBlock : public SleipnirBase { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] += rhs[row, col]; ++ if constexpr (EigenMatrixLike) { ++ (*this)[row, col] += rhs(row, col); ++ } else { ++ (*this)[row, col] += rhs[row, col]; ++ } + } + } + +@@ -532,7 +544,11 @@ class VariableBlock : public SleipnirBase { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] -= rhs[row, col]; ++ if constexpr (EigenMatrixLike) { ++ (*this)[row, col] -= rhs(row, col); ++ } else { ++ (*this)[row, col] -= rhs[row, col]; ++ } + } + } + +@@ -612,7 +628,7 @@ class VariableBlock : public SleipnirBase { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- result[row, col] = value(row, col); ++ result(row, col) = value(row, col); + } + } + +diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp +index 72559959cfc4cb38e099a2268c66088ea19551f5..59393e2eabdf9107de8ff6b554a48d83f8b9ef1e 100644 +--- a/include/sleipnir/autodiff/variable_matrix.hpp ++++ b/include/sleipnir/autodiff/variable_matrix.hpp +@@ -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) { +- m_storage.emplace_back(values[row, col]); ++ m_storage.emplace_back(values(row, col)); + } + } + } +@@ -262,7 +262,7 @@ class VariableMatrix : public SleipnirBase { + + for (int row = 0; row < values.rows(); ++row) { + for (int col = 0; col < values.cols(); ++col) { +- (*this)[row, col] = values[row, col]; ++ (*this)[row, col] = values(row, col); + } + } + +@@ -293,7 +293,7 @@ class VariableMatrix : public SleipnirBase { + + for (int row = 0; row < values.rows(); ++row) { + for (int col = 0; col < values.cols(); ++col) { +- (*this)[row, col].set_value(values[row, col]); ++ (*this)[row, col].set_value(values(row, col)); + } + } + } +@@ -573,7 +573,7 @@ class VariableMatrix : public SleipnirBase { + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { +- result[row, col] = lhs[row, col] * rhs; ++ result[row, col] = lhs(row, col) * rhs; + } + } + +@@ -608,7 +608,7 @@ class VariableMatrix : public SleipnirBase { + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { +- result[row, col] = rhs[row, col] * lhs; ++ result[row, col] = rhs(row, col) * lhs; + } + } + +@@ -644,7 +644,11 @@ class VariableMatrix : public SleipnirBase { + for (int j = 0; j < rhs.cols(); ++j) { + Variable sum{Scalar(0)}; + for (int k = 0; k < cols(); ++k) { +- sum += lhs_old_row[k] * rhs[k, j]; ++ if constexpr (EigenMatrixLike) { ++ sum += lhs_old_row[k] * rhs(k, j); ++ } else { ++ sum += lhs_old_row[k] * rhs[k, j]; ++ } + } + (*this)[i, j] = sum; + } +@@ -679,7 +683,7 @@ class VariableMatrix : public SleipnirBase { + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { +- result[row, col] = lhs[row, col] / rhs; ++ result[row, col] = lhs(row, col) / rhs; + } + } + +@@ -751,7 +755,7 @@ class VariableMatrix : public SleipnirBase { + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { +- result[row, col] = lhs[row, col] + rhs[row, col]; ++ result[row, col] = lhs(row, col) + rhs[row, col]; + } + } + +@@ -771,7 +775,7 @@ class VariableMatrix : public SleipnirBase { + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { +- result[row, col] = lhs[row, col] + rhs[row, col]; ++ result[row, col] = lhs[row, col] + rhs(row, col); + } + } + +@@ -807,7 +811,11 @@ class VariableMatrix : public SleipnirBase { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] += rhs[row, col]; ++ if constexpr (EigenMatrixLike) { ++ (*this)[row, col] += rhs(row, col); ++ } else { ++ (*this)[row, col] += rhs[row, col]; ++ } + } + } + +@@ -843,7 +851,7 @@ class VariableMatrix : public SleipnirBase { + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { +- result[row, col] = lhs[row, col] - rhs[row, col]; ++ result[row, col] = lhs(row, col) - rhs[row, col]; + } + } + +@@ -863,7 +871,7 @@ class VariableMatrix : public SleipnirBase { + + for (int row = 0; row < result.rows(); ++row) { + for (int col = 0; col < result.cols(); ++col) { +- result[row, col] = lhs[row, col] - rhs[row, col]; ++ result[row, col] = lhs[row, col] - rhs(row, col); + } + } + +@@ -899,7 +907,11 @@ class VariableMatrix : public SleipnirBase { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- (*this)[row, col] -= rhs[row, col]; ++ if constexpr (EigenMatrixLike) { ++ (*this)[row, col] -= rhs(row, col); ++ } else { ++ (*this)[row, col] -= rhs[row, col]; ++ } + } + } + +@@ -992,7 +1004,7 @@ class VariableMatrix : public SleipnirBase { + + for (int row = 0; row < rows(); ++row) { + for (int col = 0; col < cols(); ++col) { +- result[row, col] = value(row, col); ++ result(row, col) = value(row, col); + } + } + +@@ -1617,14 +1629,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) { +- eigen_A[row, col] = A[row, col]; ++ eigen_A(row, col) = A[row, col]; + } + } + + MatrixXv eigen_B{B.rows(), B.cols()}; + for (int row = 0; row < B.rows(); ++row) { + for (int col = 0; col < B.cols(); ++col) { +- eigen_B[row, col] = B[row, col]; ++ eigen_B(row, col) = B[row, col]; + } + } + +@@ -1633,7 +1645,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) { +- X[row, col] = eigen_X[row, col]; ++ X[row, col] = eigen_X(row, col); + } + } + diff --git a/upstream_utils/sleipnir_patches/0003-Use-wpi-byteswap.patch b/upstream_utils/sleipnir_patches/0003-Use-wpi-byteswap.patch deleted file mode 100644 index 15eacc8b43..0000000000 --- a/upstream_utils/sleipnir_patches/0003-Use-wpi-byteswap.patch +++ /dev/null @@ -1,30 +0,0 @@ -From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 -From: Tyler Veness -Date: Tue, 28 Jan 2025 22:19:14 -0800 -Subject: [PATCH 03/10] 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 bf0925e282adffa933d2bc3be8b7441fc4d855d3..49dc97ccea736c51e08b752f863661df54171303 100644 ---- a/include/sleipnir/util/spy.hpp -+++ b/include/sleipnir/util/spy.hpp -@@ -12,6 +12,7 @@ - #include - - #include -+#include - - namespace slp { - -@@ -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); -+ num = wpi::util::byteswap(num); - } - m_file.write(reinterpret_cast(&num), sizeof(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 deleted file mode 100644 index 0eb62d9e04..0000000000 --- a/upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch +++ /dev/null @@ -1,89 +0,0 @@ -From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 -From: Tyler Veness -Date: Tue, 28 Jan 2025 22:19:31 -0800 -Subject: [PATCH 04/10] Replace std::to_underlying() - ---- - include/sleipnir/optimization/problem.hpp | 9 +++++---- - include/sleipnir/util/print_diagnostics.hpp | 3 ++- - include/sleipnir/util/to_underlying.hpp | 14 ++++++++++++++ - 3 files changed, 21 insertions(+), 5 deletions(-) - create mode 100644 include/sleipnir/util/to_underlying.hpp - -diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp -index 41b44578b1e2783dbceb300b7c72ee0c8d1202df..49921b98de3452b6ca9f2e33a86daa254c0d3a01 100644 ---- a/include/sleipnir/optimization/problem.hpp -+++ b/include/sleipnir/optimization/problem.hpp -@@ -38,6 +38,7 @@ - #include "sleipnir/util/profiler.hpp" - #include "sleipnir/util/spy.hpp" - #include "sleipnir/util/symbol_exports.hpp" -+#include "sleipnir/util/to_underlying.hpp" - - namespace slp { - -@@ -773,11 +774,11 @@ class Problem { - // Print problem structure - slp::println("\nProblem structure:"); - slp::println(" ↳ {} cost function", -- types[std::to_underlying(cost_function_type())]); -+ types[slp::to_underlying(cost_function_type())]); - slp::println(" ↳ {} equality constraints", -- types[std::to_underlying(equality_constraint_type())]); -+ types[slp::to_underlying(equality_constraint_type())]); - slp::println(" ↳ {} inequality constraints", -- types[std::to_underlying(inequality_constraint_type())]); -+ types[slp::to_underlying(inequality_constraint_type())]); - - if (m_decision_variables.size() == 1) { - slp::print("\n1 decision variable\n"); -@@ -789,7 +790,7 @@ class Problem { - [](const gch::small_vector>& constraints) { - std::array counts{}; - for (const auto& constraint : constraints) { -- ++counts[std::to_underlying(constraint.type())]; -+ ++counts[slp::to_underlying(constraint.type())]; - } - for (const auto& [count, name] : - std::views::zip(counts, std::array{"empty", "constant", "linear", -diff --git a/include/sleipnir/util/print_diagnostics.hpp b/include/sleipnir/util/print_diagnostics.hpp -index 87ccad9ef4b05ee5aeaaf0e5ff9cfbc7e995d5ed..831ae3e5d1002005a8aa00fc80accd705d5cca7d 100644 ---- a/include/sleipnir/util/print_diagnostics.hpp -+++ b/include/sleipnir/util/print_diagnostics.hpp -@@ -17,6 +17,7 @@ - - #include "sleipnir/util/print.hpp" - #include "sleipnir/util/profiler.hpp" -+#include "sleipnir/util/to_underlying.hpp" - - namespace slp { - -@@ -230,7 +231,7 @@ void print_iteration_diagnostics(int iterations, IterationType type, - slp::println( - "│{:4} {:1} {:9.3f} {:10.4e} {:11.4e} {:10.4e} {:8.2e} {:8.2e} {:<5} " - "{:<5} {:8.2e} {:8.2e} {:8.2e} {:8.2e} {:2d}│", -- iterations, ITERATION_TYPES[std::to_underlying(type)], to_ms(time), error, -+ iterations, ITERATION_TYPES[slp::to_underlying(type)], to_ms(time), error, - cost, infeasibility, complementarity, μ, power_of_10(δ), power_of_10(γ), - full_primal_step_inf_norm, full_dual_step_inf_norm, primal_α, dual_α, - backtracks); -diff --git a/include/sleipnir/util/to_underlying.hpp b/include/sleipnir/util/to_underlying.hpp -new file mode 100644 -index 0000000000000000000000000000000000000000..3f9b4835b912c5a0d998c43828f255c61d0f573e ---- /dev/null -+++ b/include/sleipnir/util/to_underlying.hpp -@@ -0,0 +1,14 @@ -+// Copyright (c) Sleipnir contributors -+ -+#pragma once -+ -+#include -+ -+namespace slp { -+ -+template -+constexpr std::underlying_type_t to_underlying(Enum e) noexcept { -+ return static_cast>(e); -+} -+ -+} // namespace slp diff --git a/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch b/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch deleted file mode 100644 index e4201f6483..0000000000 --- a/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch +++ /dev/null @@ -1,28 +0,0 @@ -From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 -From: Tyler Veness -Date: Sat, 8 Feb 2025 13:42:36 -0800 -Subject: [PATCH 05/10] Replace std::views::zip() - ---- - include/sleipnir/optimization/problem.hpp | 8 +++++--- - 1 file changed, 5 insertions(+), 3 deletions(-) - -diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp -index 49921b98de3452b6ca9f2e33a86daa254c0d3a01..d3feadd577bd53147a8b07da76910e046ccbf95d 100644 ---- a/include/sleipnir/optimization/problem.hpp -+++ b/include/sleipnir/optimization/problem.hpp -@@ -792,9 +792,11 @@ class Problem { - for (const auto& constraint : constraints) { - ++counts[slp::to_underlying(constraint.type())]; - } -- for (const auto& [count, name] : -- std::views::zip(counts, std::array{"empty", "constant", "linear", -- "quadratic", "nonlinear"})) { -+ for (size_t i = 0; i < counts.size(); ++i) { -+ constexpr std::array names{"empty", "constant", "linear", -+ "quadratic", "nonlinear"}; -+ const auto& count = counts[i]; -+ const auto& name = names[i]; - if (count > 0) { - slp::println(" ↳ {} {}", count, name); - } diff --git a/upstream_utils/sleipnir_patches/0006-Replace-std-unreachable.patch b/upstream_utils/sleipnir_patches/0006-Replace-std-unreachable.patch deleted file mode 100644 index 6172fb5d6a..0000000000 --- a/upstream_utils/sleipnir_patches/0006-Replace-std-unreachable.patch +++ /dev/null @@ -1,86 +0,0 @@ -From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 -From: Tyler Veness -Date: Wed, 3 Dec 2025 23:38:53 -0800 -Subject: [PATCH 06/10] 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 d06d32dac6c7b6faeedefeaa107cedac8446a3ab..1957fc0339b5538fbdc56a8ea2d8503c89ed4b59 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. -@@ -64,7 +64,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 8786d6d64ac44ac88133df65a79636ec133b1b64..d1bf7e62aa844003cfcd7f3df6fd9c9a65563586 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. -@@ -96,7 +96,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/0007-Suppress-clang-tidy-false-positives.patch b/upstream_utils/sleipnir_patches/0007-Suppress-clang-tidy-false-positives.patch deleted file mode 100644 index 0042d2f1e8..0000000000 --- a/upstream_utils/sleipnir_patches/0007-Suppress-clang-tidy-false-positives.patch +++ /dev/null @@ -1,22 +0,0 @@ -From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 -From: Tyler Veness -Date: Mon, 10 Feb 2025 11:37:02 -0800 -Subject: [PATCH 07/10] 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 ad8baa3b58047f5257c210b88446324cca121d4a..5f32d7ea16864ad9a4fdcc4b067ef7dff2e9027d 100644 ---- a/include/sleipnir/autodiff/variable.hpp -+++ b/include/sleipnir/autodiff/variable.hpp -@@ -843,7 +843,7 @@ struct InequalityConstraints { - /// - /// @param inequality_constraints The list of InequalityConstraints to - /// concatenate. -- InequalityConstraints( -+ InequalityConstraints( // NOLINT - std::initializer_list inequality_constraints) { - for (const auto& elem : inequality_constraints) { - constraints.insert(constraints.end(), elem.constraints.begin(), diff --git a/upstream_utils/sleipnir_patches/0008-Suppress-GCC-12-warning-false-positive.patch b/upstream_utils/sleipnir_patches/0008-Suppress-GCC-12-warning-false-positive.patch deleted file mode 100644 index a21f791c5b..0000000000 --- a/upstream_utils/sleipnir_patches/0008-Suppress-GCC-12-warning-false-positive.patch +++ /dev/null @@ -1,34 +0,0 @@ -From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 -From: Tyler Veness -Date: Mon, 24 Feb 2025 15:12:03 -0800 -Subject: [PATCH 08/10] 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 72559959cfc4cb38e099a2268c66088ea19551f5..284cfd3aafdee76e8623df402e9c1bdd90dab116 100644 ---- a/include/sleipnir/autodiff/variable_matrix.hpp -+++ b/include/sleipnir/autodiff/variable_matrix.hpp -@@ -503,6 +503,10 @@ class VariableMatrix : public SleipnirBase { - - VariableMatrix result(detail::empty, lhs.rows(), rhs.cols()); - -+#if __GNUC__ >= 12 -+#pragma GCC diagnostic push -+#pragma GCC diagnostic ignored "-Wmaybe-uninitialized" -+#endif - for (int i = 0; i < lhs.rows(); ++i) { - for (int j = 0; j < rhs.cols(); ++j) { - Variable sum{Scalar(0)}; -@@ -558,6 +562,9 @@ class VariableMatrix : public SleipnirBase { - result[i, j] = sum; - } - } -+#if __GNUC__ >= 12 -+#pragma GCC diagnostic pop -+#endif - - return result; - } diff --git a/upstream_utils/sleipnir_patches/0009-Suppress-Doxygen-warning-false-positives.patch b/upstream_utils/sleipnir_patches/0009-Suppress-Doxygen-warning-false-positives.patch deleted file mode 100644 index 4fac23d741..0000000000 --- a/upstream_utils/sleipnir_patches/0009-Suppress-Doxygen-warning-false-positives.patch +++ /dev/null @@ -1,31 +0,0 @@ -From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 -From: Tyler Veness -Date: Sun, 29 Mar 2026 16:09:30 -0700 -Subject: [PATCH 09/10] Suppress Doxygen warning false positives - ---- - .../optimization/solver/util/feasibility_restoration.hpp | 4 ++++ - 1 file changed, 4 insertions(+) - -diff --git a/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp b/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp -index 6324ddcf2d24a0cd5367420af4330e1c59b58004..ed128e5a583d51bc963f03f6b88c0a15e6440545 100644 ---- a/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp -+++ b/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp -@@ -96,6 +96,8 @@ compute_p_n(const Eigen::Vector& c, Scalar ρ, - return {std::move(p), std::move(n)}; - } - -+// @cond Suppress Doxygen -+ - /// Finds the iterate that minimizes the constraint violation while not - /// deviating too far from the starting point. This is a fallback procedure when - /// the normal Sequential Quadratic Programming method fails to converge to a -@@ -624,6 +626,8 @@ ExitStatus feasibility_restoration( - } - } - -+// @endcond -+ - } // namespace slp - - #include "sleipnir/optimization/solver/interior_point.hpp" diff --git a/upstream_utils/sleipnir_patches/0010-Use-operator-instead-of-multidimensional-array-subsc.patch b/upstream_utils/sleipnir_patches/0010-Use-operator-instead-of-multidimensional-array-subsc.patch deleted file mode 100644 index 9315b59ce4..0000000000 --- a/upstream_utils/sleipnir_patches/0010-Use-operator-instead-of-multidimensional-array-subsc.patch +++ /dev/null @@ -1,996 +0,0 @@ -From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 -From: Tyler Veness -Date: Sat, 12 Apr 2025 16:28:47 -0700 -Subject: [PATCH 10/10] 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 | 80 ++++----- - include/sleipnir/autodiff/variable_matrix.hpp | 158 +++++++++--------- - include/sleipnir/optimization/ocp.hpp | 14 +- - include/sleipnir/optimization/problem.hpp | 6 +- - 7 files changed, 137 insertions(+), 137 deletions(-) - -diff --git a/include/sleipnir/autodiff/hessian.hpp b/include/sleipnir/autodiff/hessian.hpp -index 56100d882f9eafe2cdd4773e99a9166c21221ce8..ee42cd3fc2cdc9545f4dbfb1949a644620f30215 100644 ---- a/include/sleipnir/autodiff/hessian.hpp -+++ b/include/sleipnir/autodiff/hessian.hpp -@@ -111,9 +111,9 @@ class Hessian { - auto grad = detail::gradient_tree(m_top_lists[row], m_wrt); - for (int col = 0; col < m_wrt.rows(); ++col) { - if (grad[col].expr != nullptr) { -- result[row, col] = std::move(grad[col]); -+ result(row, col) = std::move(grad[col]); - } else { -- result[row, col] = Variable{Scalar(0)}; -+ result(row, col) = Variable{Scalar(0)}; - } - } - } -diff --git a/include/sleipnir/autodiff/jacobian.hpp b/include/sleipnir/autodiff/jacobian.hpp -index f97b5737c8fc007299bb4a1dbe5b057c1e3645de..b202f776d00feb5eaa0703796d226ab52f89aad5 100644 ---- a/include/sleipnir/autodiff/jacobian.hpp -+++ b/include/sleipnir/autodiff/jacobian.hpp -@@ -114,9 +114,9 @@ class Jacobian { - auto grad = detail::gradient_tree(m_top_lists[row], m_wrt); - for (int col = 0; col < m_wrt.rows(); ++col) { - if (grad[col].expr != nullptr) { -- result[row, col] = std::move(grad[col]); -+ result(row, col) = std::move(grad[col]); - } else { -- result[row, col] = Variable{Scalar(0)}; -+ result(row, col) = Variable{Scalar(0)}; - } - } - } -diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp -index 5f32d7ea16864ad9a4fdcc4b067ef7dff2e9027d..5d7b56de884cdb73fc9f08fcfa1982b252bdd88e 100644 ---- a/include/sleipnir/autodiff/variable.hpp -+++ b/include/sleipnir/autodiff/variable.hpp -@@ -73,7 +73,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); - } - -@@ -733,7 +733,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 -- constraints.emplace_back(lhs - rhs[row, col]); -+ constraints.emplace_back(lhs - rhs(row, col)); - } - } - -@@ -749,7 +749,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 -- constraints.emplace_back(lhs[row, col] - rhs); -+ constraints.emplace_back(lhs(row, col) - rhs); - } - } - -@@ -767,7 +767,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 -- constraints.emplace_back(lhs[row, col] - rhs[row, col]); -+ constraints.emplace_back(lhs(row, col) - rhs(row, col)); - } - } - -diff --git a/include/sleipnir/autodiff/variable_block.hpp b/include/sleipnir/autodiff/variable_block.hpp -index 55cafb5d48afff2dbdff07516d85759180ef74c7..9c9e9017487fb0fd19c584c58e5ba3c6c6cd3e7d 100644 ---- a/include/sleipnir/autodiff/variable_block.hpp -+++ b/include/sleipnir/autodiff/variable_block.hpp -@@ -49,7 +49,7 @@ class VariableBlock : public SleipnirBase { - - for (int row = 0; row < rows(); ++row) { - for (int col = 0; col < cols(); ++col) { -- (*this)[row, col] = values[row, col]; -+ (*this)(row, col) = values(row, col); - } - } - } -@@ -80,7 +80,7 @@ class VariableBlock : public SleipnirBase { - - for (int row = 0; row < rows(); ++row) { - for (int col = 0; col < cols(); ++col) { -- (*this)[row, col] = values[row, col]; -+ (*this)(row, col) = values(row, col); - } - } - } -@@ -135,7 +135,7 @@ class VariableBlock : public SleipnirBase { - VariableBlock& operator=(ScalarLike auto value) { - slp_assert(rows() == 1 && cols() == 1); - -- (*this)[0, 0] = value; -+ (*this)(0, 0) = value; - - return *this; - } -@@ -148,7 +148,7 @@ class VariableBlock : public SleipnirBase { - void set_value(Scalar value) { - slp_assert(rows() == 1 && cols() == 1); - -- (*this)[0, 0].set_value(value); -+ (*this)(0, 0).set_value(value); - } - - /// 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) { -- (*this)[row, col] = values[row, col]; -+ (*this)(row, col) = values(row, col); - } - } - -@@ -178,7 +178,7 @@ class VariableBlock : public SleipnirBase { - - for (int row = 0; row < rows(); ++row) { - for (int col = 0; col < cols(); ++col) { -- (*this)[row, col].set_value(values[row, col]); -+ (*this)(row, col).set_value(values(row, col)); - } - } - } -@@ -192,7 +192,7 @@ class VariableBlock : public SleipnirBase { - - for (int row = 0; row < rows(); ++row) { - for (int col = 0; col < cols(); ++col) { -- (*this)[row, col] = values[row, col]; -+ (*this)(row, col) = values(row, col); - } - } - return *this; -@@ -207,7 +207,7 @@ class VariableBlock : public SleipnirBase { - - for (int row = 0; row < rows(); ++row) { - for (int col = 0; col < cols(); ++col) { -- (*this)[row, col] = std::move(values[row, col]); -+ (*this)(row, col) = std::move(values(row, col)); - } - } - return *this; -@@ -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) - { - slp_assert(row >= 0 && row < rows()); - slp_assert(col >= 0 && col < cols()); -- return (*m_mat)[m_row_slice.start + row * m_row_slice.step, -- m_col_slice.start + col * m_col_slice.step]; -+ return (*m_mat)(m_row_slice.start + row * m_row_slice.step, -+ m_col_slice.start + col * m_col_slice.step); - } - - /// 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()); - slp_assert(col >= 0 && col < cols()); -- return (*m_mat)[m_row_slice.start + row * m_row_slice.step, -- m_col_slice.start + col * m_col_slice.step]; -+ return (*m_mat)(m_row_slice.start + row * m_row_slice.step, -+ m_col_slice.start + col * m_col_slice.step); - } - - /// 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()); -- return (*this)[index / cols(), index % cols()]; -+ return (*this)(index / cols(), index % cols()); - } - - /// 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()); - } - - /// 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); -- return (*this)[Slice{row_offset, row_offset + block_rows, 1}, -- Slice{col_offset, col_offset + block_cols, 1}]; -+ return (*this)({row_offset, row_offset + block_rows, 1}, -+ {col_offset, col_offset + block_cols, 1}); - } - - /// 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); -- return (*this)[Slice{row_offset, row_offset + block_rows, 1}, -- Slice{col_offset, col_offset + block_cols, 1}]; -+ return (*this)({row_offset, row_offset + block_rows, 1}, -+ {col_offset, col_offset + block_cols, 1}); - } - - /// 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); - int col_slice_length = col_slice.adjust(m_col_slice_length); -- return (*this)[row_slice, row_slice_length, col_slice, col_slice_length]; -+ return (*this)(row_slice, row_slice_length, col_slice, col_slice_length); - } - - /// 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 { - 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]; -+ return (*this)(row_slice, row_slice_length, col_slice, col_slice_length); - } - - /// 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, -@@ -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 { -@@ -438,9 +438,9 @@ class VariableBlock : public SleipnirBase { - for (int j = 0; j < rhs.cols(); ++j) { - Variable sum{Scalar(0)}; - for (int k = 0; k < cols(); ++k) { -- sum += lhs_old_row[k] * rhs[k, j]; -+ sum += lhs_old_row[k] * rhs(k, j); - } -- (*this)[i, j] = sum; -+ (*this)(i, j) = sum; - } - } - -@@ -454,7 +454,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) { -- (*this)[row, col] *= rhs; -+ (*this)(row, col) *= rhs; - } - } - -@@ -470,7 +470,7 @@ class VariableBlock : public SleipnirBase { - - for (int row = 0; row < rows(); ++row) { - for (int col = 0; col < cols(); ++col) { -- (*this)[row, col] /= rhs[0, 0]; -+ (*this)(row, col) /= rhs(0, 0); - } - } - -@@ -484,7 +484,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) { -- (*this)[row, col] /= rhs; -+ (*this)(row, col) /= rhs; - } - } - -@@ -500,7 +500,7 @@ class VariableBlock : public SleipnirBase { - - for (int row = 0; row < rows(); ++row) { - for (int col = 0; col < cols(); ++col) { -- (*this)[row, col] += rhs[row, col]; -+ (*this)(row, col) += rhs(row, col); - } - } - -@@ -516,7 +516,7 @@ class VariableBlock : public SleipnirBase { - - for (int row = 0; row < rows(); ++row) { - for (int col = 0; col < cols(); ++col) { -- (*this)[row, col] += rhs; -+ (*this)(row, col) += rhs; - } - } - -@@ -532,7 +532,7 @@ class VariableBlock : public SleipnirBase { - - for (int row = 0; row < rows(); ++row) { - for (int col = 0; col < cols(); ++col) { -- (*this)[row, col] -= rhs[row, col]; -+ (*this)(row, col) -= rhs(row, col); - } - } - -@@ -548,7 +548,7 @@ class VariableBlock : public SleipnirBase { - - for (int row = 0; row < rows(); ++row) { - for (int col = 0; col < cols(); ++col) { -- (*this)[row, col] -= rhs; -+ (*this)(row, col) -= rhs; - } - } - -@@ -559,7 +559,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. -@@ -570,7 +570,7 @@ class VariableBlock : public SleipnirBase { - - for (int row = 0; row < rows(); ++row) { - for (int col = 0; col < cols(); ++col) { -- result[col, row] = (*this)[row, col]; -+ result(col, row) = (*this)(row, col); - } - } - -@@ -592,7 +592,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. - /// -@@ -612,7 +612,7 @@ class VariableBlock : public SleipnirBase { - - for (int row = 0; row < rows(); ++row) { - for (int col = 0; col < cols(); ++col) { -- result[row, col] = value(row, col); -+ result(row, col) = value(row, col); - } - } - -@@ -630,7 +630,7 @@ class VariableBlock : public SleipnirBase { - - for (int row = 0; row < rows(); ++row) { - for (int col = 0; col < cols(); ++col) { -- result[row, col] = unary_op((*this)[row, col]); -+ result(row, col) = unary_op((*this)(row, col)); - } - } - -diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp -index 284cfd3aafdee76e8623df402e9c1bdd90dab116..2f91d1714ccee8271c0186c877dd5f84720da692 100644 ---- a/include/sleipnir/autodiff/variable_matrix.hpp -+++ b/include/sleipnir/autodiff/variable_matrix.hpp -@@ -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) { -- m_storage.emplace_back(values[row, col]); -+ m_storage.emplace_back(values(row, col)); - } - } - } -@@ -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) { -- m_storage.emplace_back(values[row, col]); -+ m_storage.emplace_back(values(row, col)); - } - } - } -@@ -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) { -- m_storage.emplace_back(values[row, col]); -+ m_storage.emplace_back(values(row, col)); - } - } - } -@@ -262,7 +262,7 @@ class VariableMatrix : public SleipnirBase { - - for (int row = 0; row < values.rows(); ++row) { - for (int col = 0; col < values.cols(); ++col) { -- (*this)[row, col] = values[row, col]; -+ (*this)(row, col) = values(row, col); - } - } - -@@ -278,7 +278,7 @@ class VariableMatrix : public SleipnirBase { - VariableMatrix& operator=(ScalarLike auto value) { - slp_assert(rows() == 1 && cols() == 1); - -- (*this)[0, 0] = value; -+ (*this)(0, 0) = value; - - return *this; - } -@@ -293,7 +293,7 @@ class VariableMatrix : public SleipnirBase { - - for (int row = 0; row < values.rows(); ++row) { - for (int col = 0; col < values.cols(); ++col) { -- (*this)[row, col].set_value(values[row, col]); -+ (*this)(row, col).set_value(values(row, col)); - } - } - } -@@ -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]; -@@ -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]; -@@ -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, -@@ -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()); -@@ -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) { -@@ -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, -@@ -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) { -- sum += lhs(i, k) * rhs[k, j]; -+ sum += lhs(i, k) * rhs(k, j); - } -- result[i, j] = sum; -+ result(i, j) = sum; - } - } - -@@ -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) { -- sum += lhs[i, k] * rhs(k, j); -+ sum += lhs(i, k) * rhs(k, j); - } -- result[i, j] = sum; -+ result(i, j) = sum; - } - } - -@@ -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) { -- sum += lhs[i, k] * rhs[k, j]; -+ sum += lhs(i, k) * rhs(k, j); - } -- result[i, j] = sum; -+ result(i, j) = sum; - } - } - #if __GNUC__ >= 12 -@@ -580,7 +580,7 @@ class VariableMatrix : public SleipnirBase { - - for (int row = 0; row < result.rows(); ++row) { - for (int col = 0; col < result.cols(); ++col) { -- result[row, col] = lhs[row, col] * rhs; -+ result(row, col) = lhs(row, col) * rhs; - } - } - -@@ -597,7 +597,7 @@ class VariableMatrix : public SleipnirBase { - - for (int row = 0; row < result.rows(); ++row) { - for (int col = 0; col < result.cols(); ++col) { -- result[row, col] = lhs[row, col] * rhs; -+ result(row, col) = lhs(row, col) * rhs; - } - } - -@@ -615,7 +615,7 @@ class VariableMatrix : public SleipnirBase { - - for (int row = 0; row < result.rows(); ++row) { - for (int col = 0; col < result.cols(); ++col) { -- result[row, col] = rhs[row, col] * lhs; -+ result(row, col) = rhs(row, col) * lhs; - } - } - -@@ -632,7 +632,7 @@ class VariableMatrix : public SleipnirBase { - - for (int row = 0; row < result.rows(); ++row) { - for (int col = 0; col < result.cols(); ++col) { -- result[row, col] = rhs[row, col] * lhs; -+ result(row, col) = rhs(row, col) * lhs; - } - } - -@@ -651,9 +651,9 @@ class VariableMatrix : public SleipnirBase { - for (int j = 0; j < rhs.cols(); ++j) { - Variable sum{Scalar(0)}; - for (int k = 0; k < cols(); ++k) { -- sum += lhs_old_row[k] * rhs[k, j]; -+ sum += lhs_old_row[k] * rhs(k, j); - } -- (*this)[i, j] = sum; -+ (*this)(i, j) = sum; - } - } - -@@ -667,7 +667,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) { -- (*this)[row, col] *= rhs; -+ (*this)(row, col) *= rhs; - } - } - -@@ -686,7 +686,7 @@ class VariableMatrix : public SleipnirBase { - - for (int row = 0; row < result.rows(); ++row) { - for (int col = 0; col < result.cols(); ++col) { -- result[row, col] = lhs[row, col] / rhs; -+ result(row, col) = lhs(row, col) / rhs; - } - } - -@@ -705,7 +705,7 @@ class VariableMatrix : public SleipnirBase { - - for (int row = 0; row < result.rows(); ++row) { - for (int col = 0; col < result.cols(); ++col) { -- result[row, col] = lhs[row, col] / rhs; -+ result(row, col) = lhs(row, col) / rhs; - } - } - -@@ -724,7 +724,7 @@ class VariableMatrix : public SleipnirBase { - - for (int row = 0; row < result.rows(); ++row) { - for (int col = 0; col < result.cols(); ++col) { -- result[row, col] = lhs[row, col] / rhs; -+ result(row, col) = lhs(row, col) / rhs; - } - } - -@@ -738,7 +738,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) { -- (*this)[row, col] /= rhs; -+ (*this)(row, col) /= rhs; - } - } - -@@ -758,7 +758,7 @@ class VariableMatrix : public SleipnirBase { - - for (int row = 0; row < result.rows(); ++row) { - for (int col = 0; col < result.cols(); ++col) { -- result[row, col] = lhs[row, col] + rhs[row, col]; -+ result(row, col) = lhs(row, col) + rhs(row, col); - } - } - -@@ -778,7 +778,7 @@ class VariableMatrix : public SleipnirBase { - - for (int row = 0; row < result.rows(); ++row) { - for (int col = 0; col < result.cols(); ++col) { -- result[row, col] = lhs[row, col] + rhs[row, col]; -+ result(row, col) = lhs(row, col) + rhs(row, col); - } - } - -@@ -798,7 +798,7 @@ class VariableMatrix : public SleipnirBase { - - for (int row = 0; row < result.rows(); ++row) { - for (int col = 0; col < result.cols(); ++col) { -- result[row, col] = lhs[row, col] + rhs[row, col]; -+ result(row, col) = lhs(row, col) + rhs(row, col); - } - } - -@@ -814,7 +814,7 @@ class VariableMatrix : public SleipnirBase { - - for (int row = 0; row < rows(); ++row) { - for (int col = 0; col < cols(); ++col) { -- (*this)[row, col] += rhs[row, col]; -+ (*this)(row, col) += rhs(row, col); - } - } - -@@ -830,7 +830,7 @@ class VariableMatrix : public SleipnirBase { - - for (int row = 0; row < rows(); ++row) { - for (int col = 0; col < cols(); ++col) { -- (*this)[row, col] += rhs; -+ (*this)(row, col) += rhs; - } - } - -@@ -850,7 +850,7 @@ class VariableMatrix : public SleipnirBase { - - for (int row = 0; row < result.rows(); ++row) { - for (int col = 0; col < result.cols(); ++col) { -- result[row, col] = lhs[row, col] - rhs[row, col]; -+ result(row, col) = lhs(row, col) - rhs(row, col); - } - } - -@@ -870,7 +870,7 @@ class VariableMatrix : public SleipnirBase { - - for (int row = 0; row < result.rows(); ++row) { - for (int col = 0; col < result.cols(); ++col) { -- result[row, col] = lhs[row, col] - rhs[row, col]; -+ result(row, col) = lhs(row, col) - rhs(row, col); - } - } - -@@ -890,7 +890,7 @@ class VariableMatrix : public SleipnirBase { - - for (int row = 0; row < result.rows(); ++row) { - for (int col = 0; col < result.cols(); ++col) { -- result[row, col] = lhs[row, col] - rhs[row, col]; -+ result(row, col) = lhs(row, col) - rhs(row, col); - } - } - -@@ -906,7 +906,7 @@ class VariableMatrix : public SleipnirBase { - - for (int row = 0; row < rows(); ++row) { - for (int col = 0; col < cols(); ++col) { -- (*this)[row, col] -= rhs[row, col]; -+ (*this)(row, col) -= rhs(row, col); - } - } - -@@ -922,7 +922,7 @@ class VariableMatrix : public SleipnirBase { - - for (int row = 0; row < rows(); ++row) { - for (int col = 0; col < cols(); ++col) { -- (*this)[row, col] -= rhs; -+ (*this)(row, col) -= rhs; - } - } - -@@ -938,7 +938,7 @@ class VariableMatrix : public SleipnirBase { - - for (int row = 0; row < result.rows(); ++row) { - for (int col = 0; col < result.cols(); ++col) { -- result[row, col] = -lhs[row, col]; -+ result(row, col) = -lhs(row, col); - } - } - -@@ -949,7 +949,7 @@ class VariableMatrix : 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. -@@ -960,7 +960,7 @@ class VariableMatrix : public SleipnirBase { - - for (int row = 0; row < rows(); ++row) { - for (int col = 0; col < cols(); ++col) { -- result[col, row] = (*this)[row, col]; -+ result(col, row) = (*this)(row, col); - } - } - -@@ -982,7 +982,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. - /// -@@ -999,7 +999,7 @@ class VariableMatrix : public SleipnirBase { - - for (int row = 0; row < rows(); ++row) { - for (int col = 0; col < cols(); ++col) { -- result[row, col] = value(row, col); -+ result(row, col) = value(row, col); - } - } - -@@ -1017,7 +1017,7 @@ class VariableMatrix : public SleipnirBase { - - for (int row = 0; row < rows(); ++row) { - for (int col = 0; col < cols(); ++col) { -- result[row, col] = unary_op((*this)[row, col]); -+ result(row, col) = unary_op((*this)(row, col)); - } - } - -@@ -1270,7 +1270,7 @@ VariableMatrix cwise_reduce( - - for (int row = 0; row < lhs.rows(); ++row) { - for (int col = 0; col < lhs.cols(); ++col) { -- result[row, col] = binary_op(lhs[row, col], rhs[row, col]); -+ result(row, col) = binary_op(lhs(row, col), rhs(row, col)); - } - } - -@@ -1403,17 +1403,17 @@ VariableMatrix solve(const VariableMatrix& A, - - if (A.rows() == 1 && A.cols() == 1) { - // Compute optimal inverse instead of using Eigen's general solver -- return B[0, 0] / A[0, 0]; -+ return B(0, 0) / A(0, 0); - } else if (A.rows() == 2 && A.cols() == 2) { - // Compute optimal inverse instead of using Eigen's general solver - // - // [a b]⁻¹ ___1___ [ d −b] - // [c d] = ad − bc [−c a] - -- const auto& a = A[0, 0]; -- const auto& b = A[0, 1]; -- const auto& c = A[1, 0]; -- const auto& d = A[1, 1]; -+ const auto& a = A(0, 0); -+ const auto& b = A(0, 1); -+ const auto& c = A(1, 0); -+ const auto& d = A(1, 1); - - VariableMatrix adj_A{{d, -b}, {-c, a}}; - auto det_A = a * d - b * c; -@@ -1430,15 +1430,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 - -- const auto& a = A[0, 0]; -- const auto& b = A[0, 1]; -- const auto& c = A[0, 2]; -- const auto& d = A[1, 0]; -- const auto& e = A[1, 1]; -- const auto& f = A[1, 2]; -- const auto& g = A[2, 0]; -- const auto& h = A[2, 1]; -- const auto& i = A[2, 2]; -+ const auto& a = A(0, 0); -+ const auto& b = A(0, 1); -+ const auto& c = A(0, 2); -+ const auto& d = A(1, 0); -+ const auto& e = A(1, 1); -+ const auto& f = A(1, 2); -+ const auto& g = A(2, 0); -+ const auto& h = A(2, 1); -+ const auto& i = A(2, 2); - - auto ae = a * e; - auto af = a * f; -@@ -1478,22 +1478,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 - -- const auto& a = A[0, 0]; -- const auto& b = A[0, 1]; -- const auto& c = A[0, 2]; -- const auto& d = A[0, 3]; -- const auto& e = A[1, 0]; -- const auto& f = A[1, 1]; -- const auto& g = A[1, 2]; -- const auto& h = A[1, 3]; -- const auto& i = A[2, 0]; -- const auto& j = A[2, 1]; -- const auto& k = A[2, 2]; -- const auto& l = A[2, 3]; -- const auto& m = A[3, 0]; -- const auto& n = A[3, 1]; -- const auto& o = A[3, 2]; -- const auto& p = A[3, 3]; -+ const auto& a = A(0, 0); -+ const auto& b = A(0, 1); -+ const auto& c = A(0, 2); -+ const auto& d = A(0, 3); -+ const auto& e = A(1, 0); -+ const auto& f = A(1, 1); -+ const auto& g = A(1, 2); -+ const auto& h = A(1, 3); -+ const auto& i = A(2, 0); -+ const auto& j = A(2, 1); -+ const auto& k = A(2, 2); -+ const auto& l = A(2, 3); -+ const auto& m = A(3, 0); -+ const auto& n = A(3, 1); -+ const auto& o = A(3, 2); -+ const auto& p = A(3, 3); - - auto afk = a * f * k; - auto afl = a * f * l; -@@ -1624,14 +1624,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) { -- eigen_A[row, col] = A[row, col]; -+ eigen_A(row, col) = A(row, col); - } - } - - MatrixXv eigen_B{B.rows(), B.cols()}; - for (int row = 0; row < B.rows(); ++row) { - for (int col = 0; col < B.cols(); ++col) { -- eigen_B[row, col] = B[row, col]; -+ eigen_B(row, col) = B(row, col); - } - } - -@@ -1640,7 +1640,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) { -- X[row, col] = eigen_X[row, col]; -+ X(row, col) = eigen_X(row, col); - } - } - -diff --git a/include/sleipnir/optimization/ocp.hpp b/include/sleipnir/optimization/ocp.hpp -index 5e20b9b8debc13611d5d719b589d8fd896c35a90..e5163f9c2dbbfa0a580d029e61c741e6c9d00fd9 100644 ---- a/include/sleipnir/optimization/ocp.hpp -+++ b/include/sleipnir/optimization/ocp.hpp -@@ -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) { -- m_DT[0, i] = dt.count(); -+ m_DT(0, i) = dt.count(); - } - } else if (timestep_method == TimestepMethod::VARIABLE_SINGLE) { - Variable single_dt = this->decision_variable(); -@@ -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) { -- m_DT[0, i] = single_dt; -+ m_DT(0, i) = single_dt; - } - } else if (timestep_method == TimestepMethod::VARIABLE) { - m_DT = this->decision_variable(1, m_num_steps + 1); - for (int i = 0; i < num_steps + 1; ++i) { -- m_DT[0, i].set_value(dt.count()); -+ m_DT(0, i).set_value(dt.count()); - } - } - -@@ -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); -- auto dt = this->dt()[0, i]; -+ auto dt = this->dt()(0, i); - callback(time, x, u, dt); - - time += dt; -@@ -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) { -- Variable h = dt()[0, i]; -+ Variable h = dt()(0, i); - - auto& f = m_dynamics; - -@@ -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); -- Variable dt = this->dt()[0, i]; -+ Variable dt = this->dt()(0, i); - - if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) { - this->subject_to( -@@ -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); -- Variable dt = this->dt()[0, i]; -+ Variable dt = this->dt()(0, i); - - 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 d3feadd577bd53147a8b07da76910e046ccbf95d..18afc996590de98050bfd3ddc629952bf0c8c2e2 100644 ---- a/include/sleipnir/optimization/problem.hpp -+++ b/include/sleipnir/optimization/problem.hpp -@@ -98,7 +98,7 @@ class Problem { - for (int row = 0; row < rows; ++row) { - for (int col = 0; col < cols; ++col) { - m_decision_variables.emplace_back(); -- vars[row, col] = m_decision_variables.back(); -+ vars(row, col) = m_decision_variables.back(); - } - } - -@@ -133,8 +133,8 @@ class Problem { - for (int row = 0; row < rows; ++row) { - for (int col = 0; col <= row; ++col) { - m_decision_variables.emplace_back(); -- vars[row, col] = m_decision_variables.back(); -- vars[col, row] = m_decision_variables.back(); -+ vars(row, col) = m_decision_variables.back(); -+ vars(col, row) = m_decision_variables.back(); - } - } - diff --git a/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 1957fc0339..12d0568f62 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,9 +4,8 @@ #include -#include - -#include "sleipnir/util/unreachable.hpp" +#include +#include namespace slp { @@ -28,16 +27,14 @@ enum class ExpressionType : uint8_t { } // namespace slp -// @cond Suppress Doxygen - /// Formatter for ExpressionType. template <> -struct fmt::formatter { +struct std::formatter { /// Parse format string. /// /// @param ctx Format parse context. /// @return Format parse context iterator. - constexpr auto parse(fmt::format_parse_context& ctx) { + constexpr auto parse(std::format_parse_context& ctx) { return m_underlying.parse(ctx); } @@ -48,8 +45,7 @@ struct fmt::formatter { /// @param ctx Format context. /// @return Format context iterator. template - constexpr auto format(const slp::ExpressionType& type, - FmtContext& ctx) const { + auto format(const slp::ExpressionType& type, FmtContext& ctx) const { using enum slp::ExpressionType; switch (type) { @@ -64,12 +60,10 @@ struct fmt::formatter { case NONLINEAR: return m_underlying.format("nonlinear", ctx); default: - slp::unreachable(); + std::unreachable(); } } private: - fmt::formatter m_underlying; + std::formatter m_underlying; }; - -// @endcond 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 ee42cd3fc2..56100d882f 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 @@ -111,9 +111,9 @@ class Hessian { auto grad = detail::gradient_tree(m_top_lists[row], m_wrt); for (int col = 0; col < m_wrt.rows(); ++col) { if (grad[col].expr != nullptr) { - result(row, col) = std::move(grad[col]); + result[row, col] = std::move(grad[col]); } else { - result(row, col) = Variable{Scalar(0)}; + result[row, col] = Variable{Scalar(0)}; } } } 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 b202f776d0..f97b5737c8 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 @@ -114,9 +114,9 @@ class Jacobian { auto grad = detail::gradient_tree(m_top_lists[row], m_wrt); for (int col = 0; col < m_wrt.rows(); ++col) { if (grad[col].expr != nullptr) { - result(row, col) = std::move(grad[col]); + result[row, col] = std::move(grad[col]); } else { - result(row, col) = Variable{Scalar(0)}; + result[row, col] = Variable{Scalar(0)}; } } } 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 5d7b56de88..6c692cbe11 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 @@ -73,7 +73,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); } @@ -733,7 +733,11 @@ 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 - constraints.emplace_back(lhs - rhs(row, col)); + if constexpr (EigenMatrixLike) { + constraints.emplace_back(lhs - rhs(row, col)); + } else { + constraints.emplace_back(lhs - rhs[row, col]); + } } } @@ -749,7 +753,11 @@ 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 - constraints.emplace_back(lhs(row, col) - rhs); + if constexpr (EigenMatrixLike) { + constraints.emplace_back(lhs(row, col) - rhs); + } else { + constraints.emplace_back(lhs[row, col] - rhs); + } } } @@ -767,7 +775,13 @@ 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 - constraints.emplace_back(lhs(row, col) - rhs(row, col)); + if constexpr (!EigenMatrixLike && !EigenMatrixLike) { + constraints.emplace_back(lhs[row, col] - rhs[row, col]); + } else if constexpr (!EigenMatrixLike && EigenMatrixLike) { + constraints.emplace_back(lhs[row, col] - rhs(row, col)); + } else if constexpr (EigenMatrixLike && !EigenMatrixLike) { + constraints.emplace_back(lhs(row, col) - rhs[row, col]); + } } } @@ -843,7 +857,7 @@ struct InequalityConstraints { /// /// @param inequality_constraints The list of InequalityConstraints to /// concatenate. - InequalityConstraints( // NOLINT + InequalityConstraints( std::initializer_list inequality_constraints) { for (const auto& elem : inequality_constraints) { constraints.insert(constraints.end(), elem.constraints.begin(), 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 9c9e901748..cf1b2a3761 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 @@ -49,7 +49,7 @@ class VariableBlock : public SleipnirBase { for (int row = 0; row < rows(); ++row) { for (int col = 0; col < cols(); ++col) { - (*this)(row, col) = values(row, col); + (*this)[row, col] = values[row, col]; } } } @@ -80,7 +80,7 @@ class VariableBlock : public SleipnirBase { for (int row = 0; row < rows(); ++row) { for (int col = 0; col < cols(); ++col) { - (*this)(row, col) = values(row, col); + (*this)[row, col] = values[row, col]; } } } @@ -135,7 +135,7 @@ class VariableBlock : public SleipnirBase { VariableBlock& operator=(ScalarLike auto value) { slp_assert(rows() == 1 && cols() == 1); - (*this)(0, 0) = value; + (*this)[0, 0] = value; return *this; } @@ -148,7 +148,7 @@ class VariableBlock : public SleipnirBase { void set_value(Scalar value) { slp_assert(rows() == 1 && cols() == 1); - (*this)(0, 0).set_value(value); + (*this)[0, 0].set_value(value); } /// 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) { - (*this)(row, col) = values(row, col); + (*this)[row, col] = values(row, col); } } @@ -178,7 +178,7 @@ class VariableBlock : public SleipnirBase { for (int row = 0; row < rows(); ++row) { for (int col = 0; col < cols(); ++col) { - (*this)(row, col).set_value(values(row, col)); + (*this)[row, col].set_value(values(row, col)); } } } @@ -192,7 +192,7 @@ class VariableBlock : public SleipnirBase { for (int row = 0; row < rows(); ++row) { for (int col = 0; col < cols(); ++col) { - (*this)(row, col) = values(row, col); + (*this)[row, col] = values[row, col]; } } return *this; @@ -207,7 +207,7 @@ class VariableBlock : public SleipnirBase { for (int row = 0; row < rows(); ++row) { for (int col = 0; col < cols(); ++col) { - (*this)(row, col) = std::move(values(row, col)); + (*this)[row, col] = std::move(values[row, col]); } } return *this; @@ -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) { slp_assert(row >= 0 && row < rows()); slp_assert(col >= 0 && col < cols()); - return (*m_mat)(m_row_slice.start + row * m_row_slice.step, - m_col_slice.start + col * m_col_slice.step); + return (*m_mat)[m_row_slice.start + row * m_row_slice.step, + m_col_slice.start + col * m_col_slice.step]; } /// 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()); slp_assert(col >= 0 && col < cols()); - return (*m_mat)(m_row_slice.start + row * m_row_slice.step, - m_col_slice.start + col * m_col_slice.step); + return (*m_mat)[m_row_slice.start + row * m_row_slice.step, + m_col_slice.start + col * m_col_slice.step]; } /// 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()); - return (*this)(index / cols(), index % cols()); + return (*this)[index / cols(), index % cols()]; } /// 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()]; } /// 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); - return (*this)({row_offset, row_offset + block_rows, 1}, - {col_offset, col_offset + block_cols, 1}); + return (*this)[Slice{row_offset, row_offset + block_rows, 1}, + Slice{col_offset, col_offset + block_cols, 1}]; } /// 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); - return (*this)({row_offset, row_offset + block_rows, 1}, - {col_offset, col_offset + block_cols, 1}); + return (*this)[Slice{row_offset, row_offset + block_rows, 1}, + Slice{col_offset, col_offset + block_cols, 1}]; } /// 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); int col_slice_length = col_slice.adjust(m_col_slice_length); - return (*this)(row_slice, row_slice_length, col_slice, col_slice_length); + return (*this)[row_slice, row_slice_length, col_slice, col_slice_length]; } /// 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 { 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); + return (*this)[row_slice, row_slice_length, col_slice, col_slice_length]; } /// 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, @@ -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 { @@ -438,9 +438,13 @@ class VariableBlock : public SleipnirBase { for (int j = 0; j < rhs.cols(); ++j) { Variable sum{Scalar(0)}; for (int k = 0; k < cols(); ++k) { - sum += lhs_old_row[k] * rhs(k, j); + if constexpr (EigenMatrixLike) { + sum += lhs_old_row[k] * rhs(k, j); + } else { + sum += lhs_old_row[k] * rhs[k, j]; + } } - (*this)(i, j) = sum; + (*this)[i, j] = sum; } } @@ -454,7 +458,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) { - (*this)(row, col) *= rhs; + (*this)[row, col] *= rhs; } } @@ -470,7 +474,11 @@ class VariableBlock : public SleipnirBase { for (int row = 0; row < rows(); ++row) { for (int col = 0; col < cols(); ++col) { - (*this)(row, col) /= rhs(0, 0); + if constexpr (EigenMatrixLike) { + (*this)[row, col] /= rhs(0, 0); + } else { + (*this)[row, col] /= rhs[0, 0]; + } } } @@ -484,7 +492,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) { - (*this)(row, col) /= rhs; + (*this)[row, col] /= rhs; } } @@ -500,7 +508,11 @@ class VariableBlock : public SleipnirBase { for (int row = 0; row < rows(); ++row) { for (int col = 0; col < cols(); ++col) { - (*this)(row, col) += rhs(row, col); + if constexpr (EigenMatrixLike) { + (*this)[row, col] += rhs(row, col); + } else { + (*this)[row, col] += rhs[row, col]; + } } } @@ -516,7 +528,7 @@ class VariableBlock : public SleipnirBase { for (int row = 0; row < rows(); ++row) { for (int col = 0; col < cols(); ++col) { - (*this)(row, col) += rhs; + (*this)[row, col] += rhs; } } @@ -532,7 +544,11 @@ class VariableBlock : public SleipnirBase { for (int row = 0; row < rows(); ++row) { for (int col = 0; col < cols(); ++col) { - (*this)(row, col) -= rhs(row, col); + if constexpr (EigenMatrixLike) { + (*this)[row, col] -= rhs(row, col); + } else { + (*this)[row, col] -= rhs[row, col]; + } } } @@ -548,7 +564,7 @@ class VariableBlock : public SleipnirBase { for (int row = 0; row < rows(); ++row) { for (int col = 0; col < cols(); ++col) { - (*this)(row, col) -= rhs; + (*this)[row, col] -= rhs; } } @@ -559,7 +575,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. @@ -570,7 +586,7 @@ class VariableBlock : public SleipnirBase { for (int row = 0; row < rows(); ++row) { for (int col = 0; col < cols(); ++col) { - result(col, row) = (*this)(row, col); + result[col, row] = (*this)[row, col]; } } @@ -592,7 +608,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. /// @@ -630,7 +646,7 @@ class VariableBlock : public SleipnirBase { for (int row = 0; row < rows(); ++row) { for (int col = 0; col < cols(); ++col) { - result(row, col) = unary_op((*this)(row, col)); + result[row, col] = unary_op((*this)[row, col]); } } diff --git a/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 2f91d1714c..59393e2eab 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 @@ -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) { - m_storage.emplace_back(values(row, col)); + m_storage.emplace_back(values[row, col]); } } } @@ -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) { - m_storage.emplace_back(values(row, col)); + m_storage.emplace_back(values[row, col]); } } } @@ -262,7 +262,7 @@ class VariableMatrix : public SleipnirBase { for (int row = 0; row < values.rows(); ++row) { for (int col = 0; col < values.cols(); ++col) { - (*this)(row, col) = values(row, col); + (*this)[row, col] = values(row, col); } } @@ -278,7 +278,7 @@ class VariableMatrix : public SleipnirBase { VariableMatrix& operator=(ScalarLike auto value) { slp_assert(rows() == 1 && cols() == 1); - (*this)(0, 0) = value; + (*this)[0, 0] = value; return *this; } @@ -293,7 +293,7 @@ class VariableMatrix : public SleipnirBase { for (int row = 0; row < values.rows(); ++row) { for (int col = 0; col < values.cols(); ++col) { - (*this)(row, col).set_value(values(row, col)); + (*this)[row, col].set_value(values(row, col)); } } } @@ -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]; @@ -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]; @@ -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, @@ -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()); @@ -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) { @@ -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, @@ -503,17 +503,13 @@ class VariableMatrix : public SleipnirBase { VariableMatrix result(detail::empty, lhs.rows(), rhs.cols()); -#if __GNUC__ >= 12 -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wmaybe-uninitialized" -#endif for (int i = 0; i < lhs.rows(); ++i) { for (int j = 0; j < rhs.cols(); ++j) { Variable sum{Scalar(0)}; for (int k = 0; k < lhs.cols(); ++k) { - sum += lhs(i, k) * rhs(k, j); + sum += lhs(i, k) * rhs[k, j]; } - result(i, j) = sum; + result[i, j] = sum; } } @@ -534,9 +530,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) { - sum += lhs(i, k) * rhs(k, j); + sum += lhs[i, k] * rhs(k, j); } - result(i, j) = sum; + result[i, j] = sum; } } @@ -557,14 +553,11 @@ class VariableMatrix : public SleipnirBase { for (int j = 0; j < rhs.cols(); ++j) { Variable sum{Scalar(0)}; for (int k = 0; k < lhs.cols(); ++k) { - sum += lhs(i, k) * rhs(k, j); + sum += lhs[i, k] * rhs[k, j]; } - result(i, j) = sum; + result[i, j] = sum; } } -#if __GNUC__ >= 12 -#pragma GCC diagnostic pop -#endif return result; } @@ -580,7 +573,7 @@ class VariableMatrix : public SleipnirBase { for (int row = 0; row < result.rows(); ++row) { for (int col = 0; col < result.cols(); ++col) { - result(row, col) = lhs(row, col) * rhs; + result[row, col] = lhs(row, col) * rhs; } } @@ -597,7 +590,7 @@ class VariableMatrix : public SleipnirBase { for (int row = 0; row < result.rows(); ++row) { for (int col = 0; col < result.cols(); ++col) { - result(row, col) = lhs(row, col) * rhs; + result[row, col] = lhs[row, col] * rhs; } } @@ -615,7 +608,7 @@ class VariableMatrix : public SleipnirBase { for (int row = 0; row < result.rows(); ++row) { for (int col = 0; col < result.cols(); ++col) { - result(row, col) = rhs(row, col) * lhs; + result[row, col] = rhs(row, col) * lhs; } } @@ -632,7 +625,7 @@ class VariableMatrix : public SleipnirBase { for (int row = 0; row < result.rows(); ++row) { for (int col = 0; col < result.cols(); ++col) { - result(row, col) = rhs(row, col) * lhs; + result[row, col] = rhs[row, col] * lhs; } } @@ -651,9 +644,13 @@ class VariableMatrix : public SleipnirBase { for (int j = 0; j < rhs.cols(); ++j) { Variable sum{Scalar(0)}; for (int k = 0; k < cols(); ++k) { - sum += lhs_old_row[k] * rhs(k, j); + if constexpr (EigenMatrixLike) { + sum += lhs_old_row[k] * rhs(k, j); + } else { + sum += lhs_old_row[k] * rhs[k, j]; + } } - (*this)(i, j) = sum; + (*this)[i, j] = sum; } } @@ -667,7 +664,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) { - (*this)(row, col) *= rhs; + (*this)[row, col] *= rhs; } } @@ -686,7 +683,7 @@ class VariableMatrix : public SleipnirBase { for (int row = 0; row < result.rows(); ++row) { for (int col = 0; col < result.cols(); ++col) { - result(row, col) = lhs(row, col) / rhs; + result[row, col] = lhs(row, col) / rhs; } } @@ -705,7 +702,7 @@ class VariableMatrix : public SleipnirBase { for (int row = 0; row < result.rows(); ++row) { for (int col = 0; col < result.cols(); ++col) { - result(row, col) = lhs(row, col) / rhs; + result[row, col] = lhs[row, col] / rhs; } } @@ -724,7 +721,7 @@ class VariableMatrix : public SleipnirBase { for (int row = 0; row < result.rows(); ++row) { for (int col = 0; col < result.cols(); ++col) { - result(row, col) = lhs(row, col) / rhs; + result[row, col] = lhs[row, col] / rhs; } } @@ -738,7 +735,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) { - (*this)(row, col) /= rhs; + (*this)[row, col] /= rhs; } } @@ -758,7 +755,7 @@ class VariableMatrix : public SleipnirBase { for (int row = 0; row < result.rows(); ++row) { for (int col = 0; col < result.cols(); ++col) { - result(row, col) = lhs(row, col) + rhs(row, col); + result[row, col] = lhs(row, col) + rhs[row, col]; } } @@ -778,7 +775,7 @@ class VariableMatrix : public SleipnirBase { for (int row = 0; row < result.rows(); ++row) { for (int col = 0; col < result.cols(); ++col) { - result(row, col) = lhs(row, col) + rhs(row, col); + result[row, col] = lhs[row, col] + rhs(row, col); } } @@ -798,7 +795,7 @@ class VariableMatrix : public SleipnirBase { for (int row = 0; row < result.rows(); ++row) { for (int col = 0; col < result.cols(); ++col) { - result(row, col) = lhs(row, col) + rhs(row, col); + result[row, col] = lhs[row, col] + rhs[row, col]; } } @@ -814,7 +811,11 @@ class VariableMatrix : public SleipnirBase { for (int row = 0; row < rows(); ++row) { for (int col = 0; col < cols(); ++col) { - (*this)(row, col) += rhs(row, col); + if constexpr (EigenMatrixLike) { + (*this)[row, col] += rhs(row, col); + } else { + (*this)[row, col] += rhs[row, col]; + } } } @@ -830,7 +831,7 @@ class VariableMatrix : public SleipnirBase { for (int row = 0; row < rows(); ++row) { for (int col = 0; col < cols(); ++col) { - (*this)(row, col) += rhs; + (*this)[row, col] += rhs; } } @@ -850,7 +851,7 @@ class VariableMatrix : public SleipnirBase { for (int row = 0; row < result.rows(); ++row) { for (int col = 0; col < result.cols(); ++col) { - result(row, col) = lhs(row, col) - rhs(row, col); + result[row, col] = lhs(row, col) - rhs[row, col]; } } @@ -870,7 +871,7 @@ class VariableMatrix : public SleipnirBase { for (int row = 0; row < result.rows(); ++row) { for (int col = 0; col < result.cols(); ++col) { - result(row, col) = lhs(row, col) - rhs(row, col); + result[row, col] = lhs[row, col] - rhs(row, col); } } @@ -890,7 +891,7 @@ class VariableMatrix : public SleipnirBase { for (int row = 0; row < result.rows(); ++row) { for (int col = 0; col < result.cols(); ++col) { - result(row, col) = lhs(row, col) - rhs(row, col); + result[row, col] = lhs[row, col] - rhs[row, col]; } } @@ -906,7 +907,11 @@ class VariableMatrix : public SleipnirBase { for (int row = 0; row < rows(); ++row) { for (int col = 0; col < cols(); ++col) { - (*this)(row, col) -= rhs(row, col); + if constexpr (EigenMatrixLike) { + (*this)[row, col] -= rhs(row, col); + } else { + (*this)[row, col] -= rhs[row, col]; + } } } @@ -922,7 +927,7 @@ class VariableMatrix : public SleipnirBase { for (int row = 0; row < rows(); ++row) { for (int col = 0; col < cols(); ++col) { - (*this)(row, col) -= rhs; + (*this)[row, col] -= rhs; } } @@ -938,7 +943,7 @@ class VariableMatrix : public SleipnirBase { for (int row = 0; row < result.rows(); ++row) { for (int col = 0; col < result.cols(); ++col) { - result(row, col) = -lhs(row, col); + result[row, col] = -lhs[row, col]; } } @@ -949,7 +954,7 @@ class VariableMatrix : 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. @@ -960,7 +965,7 @@ class VariableMatrix : public SleipnirBase { for (int row = 0; row < rows(); ++row) { for (int col = 0; col < cols(); ++col) { - result(col, row) = (*this)(row, col); + result[col, row] = (*this)[row, col]; } } @@ -982,7 +987,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. /// @@ -1017,7 +1022,7 @@ class VariableMatrix : public SleipnirBase { for (int row = 0; row < rows(); ++row) { for (int col = 0; col < cols(); ++col) { - result(row, col) = unary_op((*this)(row, col)); + result[row, col] = unary_op((*this)[row, col]); } } @@ -1270,7 +1275,7 @@ VariableMatrix cwise_reduce( for (int row = 0; row < lhs.rows(); ++row) { for (int col = 0; col < lhs.cols(); ++col) { - result(row, col) = binary_op(lhs(row, col), rhs(row, col)); + result[row, col] = binary_op(lhs[row, col], rhs[row, col]); } } @@ -1403,17 +1408,17 @@ VariableMatrix solve(const VariableMatrix& A, if (A.rows() == 1 && A.cols() == 1) { // Compute optimal inverse instead of using Eigen's general solver - return B(0, 0) / A(0, 0); + return B[0, 0] / A[0, 0]; } else if (A.rows() == 2 && A.cols() == 2) { // Compute optimal inverse instead of using Eigen's general solver // // [a b]⁻¹ ___1___ [ d −b] // [c d] = ad − bc [−c a] - const auto& a = A(0, 0); - const auto& b = A(0, 1); - const auto& c = A(1, 0); - const auto& d = A(1, 1); + const auto& a = A[0, 0]; + const auto& b = A[0, 1]; + const auto& c = A[1, 0]; + const auto& d = A[1, 1]; VariableMatrix adj_A{{d, -b}, {-c, a}}; auto det_A = a * d - b * c; @@ -1430,15 +1435,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 - const auto& a = A(0, 0); - const auto& b = A(0, 1); - const auto& c = A(0, 2); - const auto& d = A(1, 0); - const auto& e = A(1, 1); - const auto& f = A(1, 2); - const auto& g = A(2, 0); - const auto& h = A(2, 1); - const auto& i = A(2, 2); + const auto& a = A[0, 0]; + const auto& b = A[0, 1]; + const auto& c = A[0, 2]; + const auto& d = A[1, 0]; + const auto& e = A[1, 1]; + const auto& f = A[1, 2]; + const auto& g = A[2, 0]; + const auto& h = A[2, 1]; + const auto& i = A[2, 2]; auto ae = a * e; auto af = a * f; @@ -1478,22 +1483,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 - const auto& a = A(0, 0); - const auto& b = A(0, 1); - const auto& c = A(0, 2); - const auto& d = A(0, 3); - const auto& e = A(1, 0); - const auto& f = A(1, 1); - const auto& g = A(1, 2); - const auto& h = A(1, 3); - const auto& i = A(2, 0); - const auto& j = A(2, 1); - const auto& k = A(2, 2); - const auto& l = A(2, 3); - const auto& m = A(3, 0); - const auto& n = A(3, 1); - const auto& o = A(3, 2); - const auto& p = A(3, 3); + const auto& a = A[0, 0]; + const auto& b = A[0, 1]; + const auto& c = A[0, 2]; + const auto& d = A[0, 3]; + const auto& e = A[1, 0]; + const auto& f = A[1, 1]; + const auto& g = A[1, 2]; + const auto& h = A[1, 3]; + const auto& i = A[2, 0]; + const auto& j = A[2, 1]; + const auto& k = A[2, 2]; + const auto& l = A[2, 3]; + const auto& m = A[3, 0]; + const auto& n = A[3, 1]; + const auto& o = A[3, 2]; + const auto& p = A[3, 3]; auto afk = a * f * k; auto afl = a * f * l; @@ -1624,14 +1629,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) { - eigen_A(row, col) = A(row, col); + eigen_A(row, col) = A[row, col]; } } MatrixXv eigen_B{B.rows(), B.cols()}; for (int row = 0; row < B.rows(); ++row) { for (int col = 0; col < B.cols(); ++col) { - eigen_B(row, col) = B(row, col); + eigen_B(row, col) = B[row, col]; } } @@ -1640,7 +1645,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) { - X(row, col) = eigen_X(row, col); + X[row, col] = eigen_X(row, col); } } 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 e5163f9c2d..5e20b9b8de 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 @@ -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) { - m_DT(0, i) = dt.count(); + m_DT[0, i] = dt.count(); } } else if (timestep_method == TimestepMethod::VARIABLE_SINGLE) { Variable single_dt = this->decision_variable(); @@ -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) { - m_DT(0, i) = single_dt; + m_DT[0, i] = single_dt; } } else if (timestep_method == TimestepMethod::VARIABLE) { m_DT = this->decision_variable(1, m_num_steps + 1); for (int i = 0; i < num_steps + 1; ++i) { - m_DT(0, i).set_value(dt.count()); + m_DT[0, i].set_value(dt.count()); } } @@ -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); - auto dt = this->dt()(0, i); + auto dt = this->dt()[0, i]; callback(time, x, u, dt); time += dt; @@ -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) { - Variable h = dt()(0, i); + Variable h = dt()[0, i]; auto& f = m_dynamics; @@ -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); - Variable dt = this->dt()(0, i); + Variable dt = this->dt()[0, i]; if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) { this->subject_to( @@ -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); - Variable dt = this->dt()(0, i); + Variable dt = this->dt()[0, i]; if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) { x_end = rk4, 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 18afc99659..ccff8b9425 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 @@ -15,7 +15,6 @@ #include #include -#include #include #include "sleipnir/autodiff/expression_type.hpp" @@ -38,7 +37,6 @@ #include "sleipnir/util/profiler.hpp" #include "sleipnir/util/spy.hpp" #include "sleipnir/util/symbol_exports.hpp" -#include "sleipnir/util/to_underlying.hpp" namespace slp { @@ -98,7 +96,7 @@ class Problem { for (int row = 0; row < rows; ++row) { for (int col = 0; col < cols; ++col) { m_decision_variables.emplace_back(); - vars(row, col) = m_decision_variables.back(); + vars[row, col] = m_decision_variables.back(); } } @@ -133,8 +131,8 @@ class Problem { for (int row = 0; row < rows; ++row) { for (int col = 0; col <= row; ++col) { m_decision_variables.emplace_back(); - vars(row, col) = m_decision_variables.back(); - vars(col, row) = m_decision_variables.back(); + vars[row, col] = m_decision_variables.back(); + vars[col, row] = m_decision_variables.back(); } } @@ -774,11 +772,11 @@ class Problem { // Print problem structure slp::println("\nProblem structure:"); slp::println(" ↳ {} cost function", - types[slp::to_underlying(cost_function_type())]); + types[std::to_underlying(cost_function_type())]); slp::println(" ↳ {} equality constraints", - types[slp::to_underlying(equality_constraint_type())]); + types[std::to_underlying(equality_constraint_type())]); slp::println(" ↳ {} inequality constraints", - types[slp::to_underlying(inequality_constraint_type())]); + types[std::to_underlying(inequality_constraint_type())]); if (m_decision_variables.size() == 1) { slp::print("\n1 decision variable\n"); @@ -790,13 +788,11 @@ class Problem { [](const gch::small_vector>& constraints) { std::array counts{}; for (const auto& constraint : constraints) { - ++counts[slp::to_underlying(constraint.type())]; + ++counts[std::to_underlying(constraint.type())]; } - for (size_t i = 0; i < counts.size(); ++i) { - constexpr std::array names{"empty", "constant", "linear", - "quadratic", "nonlinear"}; - const auto& count = counts[i]; - const auto& name = names[i]; + for (const auto& [count, name] : + std::views::zip(counts, std::array{"empty", "constant", "linear", + "quadratic", "nonlinear"})) { if (count > 0) { slp::println(" ↳ {} {}", count, name); } 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 d1bf7e62aa..0a48df7423 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,9 +4,8 @@ #include -#include - -#include "sleipnir/util/unreachable.hpp" +#include +#include namespace slp { @@ -46,16 +45,14 @@ enum class ExitStatus : int8_t { } // namespace slp -// @cond Suppress Doxygen - /// Formatter for ExitStatus. template <> -struct fmt::formatter { +struct std::formatter { /// Parses format string. /// /// @param ctx Format parse context. /// @return Format parse context iterator. - constexpr auto parse(fmt::format_parse_context& ctx) { + constexpr auto parse(std::format_parse_context& ctx) { return m_underlying.parse(ctx); } @@ -66,8 +63,7 @@ struct fmt::formatter { /// @param ctx Format context. /// @return Format context iterator. template - constexpr auto format(const slp::ExitStatus& exit_status, - FmtContext& ctx) const { + auto format(const slp::ExitStatus& exit_status, FmtContext& ctx) const { using enum slp::ExitStatus; switch (exit_status) { @@ -96,12 +92,10 @@ struct fmt::formatter { case TIMEOUT: return m_underlying.format("timeout", ctx); default: - slp::unreachable(); + std::unreachable(); } } private: - fmt::formatter m_underlying; + std::formatter m_underlying; }; - -// @endcond diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp index ed128e5a58..6324ddcf2d 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp @@ -96,8 +96,6 @@ compute_p_n(const Eigen::Vector& c, Scalar ρ, return {std::move(p), std::move(n)}; } -// @cond Suppress Doxygen - /// Finds the iterate that minimizes the constraint violation while not /// deviating too far from the starting point. This is a fallback procedure when /// the normal Sequential Quadratic Programming method fails to converge to a @@ -626,8 +624,6 @@ ExitStatus feasibility_restoration( } } -// @endcond - } // namespace slp #include "sleipnir/optimization/solver/interior_point.hpp" 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 2d2ce114be..49ee606f7e 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 @@ -2,10 +2,10 @@ #pragma once +#include #include #include -#include namespace slp { @@ -77,16 +77,14 @@ class Inertia { } // namespace slp -// @cond Suppress Doxygen - /// Formatter for Inertia. template <> -struct fmt::formatter { +struct std::formatter { /// Parses format string. /// /// @param ctx Format parse context. /// @return Format parse context iterator. - constexpr auto parse(fmt::format_parse_context& ctx) { + constexpr auto parse(std::format_parse_context& ctx) { return m_underlying.parse(ctx); } @@ -97,20 +95,18 @@ struct fmt::formatter { /// @param ctx Format context. /// @return Format context iterator. template - constexpr auto format(const slp::Inertia& inertia, FmtContext& ctx) const { + auto format(const slp::Inertia& inertia, FmtContext& ctx) const { auto out = ctx.out(); - out = fmt::format_to(out, "("); + out = std::format_to(out, "("); out = m_underlying.format(inertia.positive, ctx); - out = fmt::format_to(out, ", "); + out = std::format_to(out, ", "); out = m_underlying.format(inertia.negative, ctx); - out = fmt::format_to(out, ", "); + out = std::format_to(out, ", "); out = m_underlying.format(inertia.zero, ctx); - return fmt::format_to(out, ")"); + return std::format_to(out, ")"); } private: - fmt::formatter m_underlying; + std::formatter m_underlying; }; - -// @endcond 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 783a0e71a1..29dfe54165 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,17 +3,16 @@ #pragma once #ifdef SLEIPNIR_PYTHON +#include #include #include -#include - /// Throws an exception in Python. #define slp_assert(condition) \ do { \ if (!(condition)) { \ auto location = std::source_location::current(); \ - throw std::invalid_argument(fmt::format( \ + throw std::invalid_argument(std::format( \ "{}:{}: {}: Assertion `{}' failed.", location.file_name(), \ location.line(), location.function_name(), #condition)); \ } \ 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 b2920580ee..d436bf297b 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 @@ -4,48 +4,47 @@ #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS #include +#include #include #include - -#include #endif namespace slp { #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS -/// Wrapper around fmt::print() that squelches write failure exceptions. +/// Wrapper around std::print() that squelches write failure exceptions. template -void print(fmt::format_string fmt, T&&... args) { +void print(std::format_string fmt, T&&... args) { try { - fmt::print(fmt, std::forward(args)...); + std::print(fmt, std::forward(args)...); } catch (const std::system_error&) { } } -/// Wrapper around fmt::print() that squelches write failure exceptions. +/// Wrapper around std::print() that squelches write failure exceptions. template -void print(std::FILE* f, fmt::format_string fmt, T&&... args) { +void print(std::FILE* f, std::format_string fmt, T&&... args) { try { - fmt::print(f, fmt, std::forward(args)...); + std::print(f, fmt, std::forward(args)...); } catch (const std::system_error&) { } } -/// Wrapper around fmt::println() that squelches write failure exceptions. +/// Wrapper around std::println() that squelches write failure exceptions. template -void println(fmt::format_string fmt, T&&... args) { +void println(std::format_string fmt, T&&... args) { try { - fmt::println(fmt, std::forward(args)...); + std::println(fmt, std::forward(args)...); } catch (const std::system_error&) { } } -/// Wrapper around fmt::println() that squelches write failure exceptions. +/// Wrapper around std::println() that squelches write failure exceptions. template -void println(std::FILE* f, fmt::format_string fmt, T&&... args) { +void println(std::FILE* f, std::format_string fmt, T&&... args) { try { - fmt::println(f, fmt, std::forward(args)...); + std::println(f, fmt, std::forward(args)...); } catch (const std::system_error&) { } } 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 831ae3e5d1..87ccad9ef4 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 @@ -17,7 +17,6 @@ #include "sleipnir/util/print.hpp" #include "sleipnir/util/profiler.hpp" -#include "sleipnir/util/to_underlying.hpp" namespace slp { @@ -231,7 +230,7 @@ void print_iteration_diagnostics(int iterations, IterationType type, slp::println( "│{:4} {:1} {:9.3f} {:10.4e} {:11.4e} {:10.4e} {:8.2e} {:8.2e} {:<5} " "{:<5} {:8.2e} {:8.2e} {:8.2e} {:8.2e} {:2d}│", - iterations, ITERATION_TYPES[slp::to_underlying(type)], to_ms(time), error, + iterations, ITERATION_TYPES[std::to_underlying(type)], to_ms(time), error, cost, infeasibility, complementarity, μ, power_of_10(δ), power_of_10(γ), full_primal_step_inf_norm, full_dual_step_inf_norm, primal_α, dual_α, backtracks); 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 49dc97ccea..bf0925e282 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 @@ -12,7 +12,6 @@ #include #include -#include namespace slp { @@ -107,7 +106,7 @@ class Spy { /// @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); + num = std::byteswap(num); } m_file.write(reinterpret_cast(&num), sizeof(num)); } diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/to_underlying.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/to_underlying.hpp deleted file mode 100644 index 3f9b4835b9..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/to_underlying.hpp +++ /dev/null @@ -1,14 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include - -namespace slp { - -template -constexpr std::underlying_type_t to_underlying(Enum e) noexcept { - return static_cast>(e); -} - -} // namespace slp 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 deleted file mode 100644 index 2e5ec3e716..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/unreachable.hpp +++ /dev/null @@ -1,16 +0,0 @@ -// 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