Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-02 02:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[upstream_utils] Upgrade to Sleipnir 0.6.3 (#9030)

Browse Source 
Also drop patches since we're now on C++23.
This commit is contained in:
Tyler Veness
2026-06-27 12:10:39 -07:00
committed by GitHub
parent f8de482385
commit 32bf24aa19
31 changed files with 536 additions and 1853 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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<double>(k) / N));
X(1, k).set_value(
X[1, k].set_value(
std::lerp(x_initial[1], x_final[1], static_cast<double>(k) / N));
}

1
docs/build.gradle
View File

@@ -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'

2
upstream_utils/sleipnir.py
View File

@@ -46,7 +46,7 @@ using small_vector = wpi::util::SmallVector<T>;
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()

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

@@ -1,286 +0,0 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Wed, 29 May 2024 16:29:55 -0700
Subject: [PATCH 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 <stdint.h>
-#include <format>
#include <utility>
+#include <fmt/base.h>
+
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<slp::ExpressionType> {
+struct fmt::formatter<slp::ExpressionType> {
/// Parse format string.
///
/// @param ctx Format parse context.
/// @return Format parse context iterator.
- constexpr auto parse(std::format_parse_context& ctx) {
+ constexpr auto parse(fmt::format_parse_context& ctx) {
return m_underlying.parse(ctx);
}
@@ -45,7 +48,8 @@ struct std::formatter<slp::ExpressionType> {
/// @param ctx Format context.
/// @return Format context iterator.
template <typename FmtContext>
- 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<slp::ExpressionType> {
}
private:
- std::formatter<const char*> m_underlying;
+ fmt::formatter<const char*> 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 <Eigen/Core>
#include <Eigen/SparseCore>
+#include <fmt/chrono.h>
#include <gch/small_vector.hpp>
#include "sleipnir/autodiff/expression_type.hpp"
diff --git a/include/sleipnir/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 <stdint.h>
-#include <format>
#include <utility>
+#include <fmt/base.h>
+
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<slp::ExitStatus> {
+struct fmt::formatter<slp::ExitStatus> {
/// 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<slp::ExitStatus> {
/// @param ctx Format context.
/// @return Format context iterator.
template <typename FmtContext>
- 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<slp::ExitStatus> {
}
private:
- std::formatter<const char*> m_underlying;
+ fmt::formatter<const char*> 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 <format>
#include <limits>
#include <Eigen/Core>
+#include <fmt/format.h>
namespace slp {
@@ -77,14 +77,16 @@ class Inertia {
} // namespace slp
+// @cond Suppress Doxygen
+
/// Formatter for Inertia.
template <>
-struct std::formatter<slp::Inertia> {
+struct fmt::formatter<slp::Inertia> {
/// 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<slp::Inertia> {
/// @param ctx Format context.
/// @return Format context iterator.
template <typename FmtContext>
- 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<int> m_underlying;
+ fmt::formatter<int> 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 <format>
#include <source_location>
#include <stdexcept>
+#include <fmt/format.h>
+
/// 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 <cstdio>
-#include <print>
#include <system_error>
#include <utility>
+
+#include <fmt/base.h>
#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 <typename... T>
-void print(std::format_string<T...> fmt, T&&... args) {
+void print(fmt::format_string<T...> fmt, T&&... args) {
try {
- std::print(fmt, std::forward<T>(args)...);
+ fmt::print(fmt, std::forward<T>(args)...);
} catch (const std::system_error&) {
}
}
-/// Wrapper around std::print() that squelches write failure exceptions.
+/// Wrapper around fmt::print() that squelches write failure exceptions.
template <typename... T>
-void print(std::FILE* f, std::format_string<T...> fmt, T&&... args) {
+void print(std::FILE* f, fmt::format_string<T...> fmt, T&&... args) {
try {
- std::print(f, fmt, std::forward<T>(args)...);
+ fmt::print(f, fmt, std::forward<T>(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 <typename... T>
-void println(std::format_string<T...> fmt, T&&... args) {
+void println(fmt::format_string<T...> fmt, T&&... args) {
try {
- std::println(fmt, std::forward<T>(args)...);
+ fmt::println(fmt, std::forward<T>(args)...);
} catch (const std::system_error&) {
}
}
-/// Wrapper around std::println() that squelches write failure exceptions.
+/// Wrapper around fmt::println() that squelches write failure exceptions.
template <typename... T>
-void println(std::FILE* f, std::format_string<T...> fmt, T&&... args) {
+void println(std::FILE* f, fmt::format_string<T...> fmt, T&&... args) {
try {
- std::println(f, fmt, std::forward<T>(args)...);
+ fmt::println(f, fmt, std::forward<T>(args)...);
} catch (const std::system_error&) {
}
}

2
upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch → upstream_utils/sleipnir_patches/0001-Use-wpi-SmallVector.patch
View File

@@ -1,7 +1,7 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Sun, 16 Jun 2024 12:08:49 -0700
Subject: [PATCH 02/10] Use wpi::SmallVector
Subject: [PATCH 1/2] Use wpi::SmallVector
---
include/sleipnir/autodiff/expression.hpp | 4 ++--

307
upstream_utils/sleipnir_patches/0002-Downgrade-to-Eigen-5.0.1.patch Normal file
View File

@@ -0,0 +1,307 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
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<RHS>) {
+ 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<LHS>) {
+ 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<LHS> && !EigenMatrixLike<RHS>) {
+ constraints.emplace_back(lhs[row, col] - rhs[row, col]);
+ } else if constexpr (!EigenMatrixLike<LHS> && EigenMatrixLike<RHS>) {
+ constraints.emplace_back(lhs[row, col] - rhs(row, col));
+ } else if constexpr (EigenMatrixLike<LHS> && !EigenMatrixLike<RHS>) {
+ 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<decltype(rhs)>) {
+ 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<decltype(rhs)>) {
+ (*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<decltype(rhs)>) {
+ (*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<decltype(rhs)>) {
+ (*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<decltype(rhs)>) {
+ 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<decltype(rhs)>) {
+ (*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<decltype(rhs)>) {
+ (*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<Scalar> solve(const VariableMatrix<Scalar>& A,
MatrixXv eigen_A{A.rows(), A.cols()};
for (int row = 0; row < A.rows(); ++row) {
for (int col = 0; col < A.cols(); ++col) {
- 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<Scalar> solve(const VariableMatrix<Scalar>& A,
VariableMatrix<Scalar> X{detail::empty, A.cols(), B.cols()};
for (int row = 0; row < X.rows(); ++row) {
for (int col = 0; col < X.cols(); ++col) {
- X[row, col] = eigen_X[row, col];
+ X[row, col] = eigen_X(row, col);
}
}

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

@@ -1,30 +0,0 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Tue, 28 Jan 2025 22:19:14 -0800
Subject: [PATCH 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 <string_view>
#include <Eigen/SparseCore>
+#include <wpi/util/bit.hpp>
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<char*>(&num), sizeof(num));
}

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

@@ -1,89 +0,0 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Tue, 28 Jan 2025 22:19:31 -0800
Subject: [PATCH 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<Variable<Scalar>>& constraints) {
std::array<size_t, 5> 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 <type_traits>
+
+namespace slp {
+
+template <typename Enum>
+constexpr std::underlying_type_t<Enum> to_underlying(Enum e) noexcept {
+ return static_cast<std::underlying_type_t<Enum>>(e);
+}
+
+} // namespace slp

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

@@ -1,28 +0,0 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Sat, 8 Feb 2025 13:42:36 -0800
Subject: [PATCH 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);
}

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

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

22
upstream_utils/sleipnir_patches/0007-Suppress-clang-tidy-false-positives.patch
View File

@@ -1,22 +0,0 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Mon, 10 Feb 2025 11:37:02 -0800
Subject: [PATCH 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<InequalityConstraints> inequality_constraints) {
for (const auto& elem : inequality_constraints) {
constraints.insert(constraints.end(), elem.constraints.begin(),

34
upstream_utils/sleipnir_patches/0008-Suppress-GCC-12-warning-false-positive.patch
View File

@@ -1,34 +0,0 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Mon, 24 Feb 2025 15:12:03 -0800
Subject: [PATCH 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<Scalar> 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;
}

31
upstream_utils/sleipnir_patches/0009-Suppress-Doxygen-warning-false-positives.patch
View File

@@ -1,31 +0,0 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: 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<Scalar, Eigen::Dynamic>& 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"

996
upstream_utils/sleipnir_patches/0010-Use-operator-instead-of-multidimensional-array-subsc.patch
View File

@@ -1,996 +0,0 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Sat, 12 Apr 2025 16:28:47 -0700
Subject: [PATCH 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<Scalar> auto value) : expr{value[0, 0].expr} {
+ Variable(SleipnirMatrixLike<Scalar> auto value) : expr{value(0, 0).expr} {
slp_assert(value.rows() == 1 && value.cols() == 1);
}
@@ -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<Mat>& 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<Scalar>& operator[](int row, int col)
+ Variable<Scalar>& operator()(int row, int col)
requires(!std::is_const_v<Mat>)
{
slp_assert(row >= 0 && row < rows());
slp_assert(col >= 0 && col < cols());
- return (*m_mat)[m_row_slice.start + row * m_row_slice.step,
- m_col_slice.start + col * m_col_slice.step];
+ return (*m_mat)(m_row_slice.start + row * m_row_slice.step,
+ m_col_slice.start + col * m_col_slice.step);
}
/// Returns a scalar subblock at the given row and column.
@@ -232,11 +232,11 @@ class VariableBlock : public SleipnirBase {
/// @param row The scalar subblock's row.
/// @param col The scalar subblock's column.
/// @return A scalar subblock at the given row and column.
- const Variable<Scalar>& operator[](int row, int col) const {
+ const Variable<Scalar>& operator()(int row, int col) const {
slp_assert(row >= 0 && row < rows());
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<Mat>)
{
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<Scalar>& 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<Mat> operator[](Slice row_slice, Slice col_slice) {
+ VariableBlock<Mat> operator()(Slice row_slice, Slice col_slice) {
int row_slice_length = row_slice.adjust(m_row_slice_length);
int col_slice_length = col_slice.adjust(m_col_slice_length);
- return (*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<const Mat> operator[](Slice row_slice,
+ const VariableBlock<const Mat> operator()(Slice row_slice,
Slice col_slice) const {
int row_slice_length = row_slice.adjust(m_row_slice_length);
int col_slice_length = col_slice.adjust(m_col_slice_length);
- 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<Mat> operator[](Slice row_slice, int row_slice_length,
+ VariableBlock<Mat> operator()(Slice row_slice, int row_slice_length,
Slice col_slice, int col_slice_length) {
return VariableBlock{
*m_mat,
@@ -350,7 +350,7 @@ class VariableBlock : public SleipnirBase {
/// @param col_slice The column slice.
/// @param col_slice_length The column slice length.
/// @return A slice of the variable matrix.
- const VariableBlock<const Mat> operator[](Slice row_slice,
+ const VariableBlock<const Mat> operator()(Slice row_slice,
int row_slice_length,
Slice col_slice,
int col_slice_length) const {
@@ -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<Mat>& operator*=(const ScalarLike auto& rhs) {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] *= rhs;
+ (*this)(row, col) *= rhs;
}
}
@@ -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<Mat>& operator/=(const ScalarLike auto& rhs) {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
- (*this)[row, col] /= rhs;
+ (*this)(row, col) /= rhs;
}
}
@@ -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<Scalar>() 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<Scalar>& operator[](int row, int col) {
+ Variable<Scalar>& operator()(int row, int col) {
slp_assert(row >= 0 && row < rows());
slp_assert(col >= 0 && col < cols());
return m_storage[row * cols() + col];
@@ -314,7 +314,7 @@ class VariableMatrix : public SleipnirBase {
/// @param row The row.
/// @param col The column.
/// @return The element at the given row and column.
- const Variable<Scalar>& operator[](int row, int col) const {
+ const Variable<Scalar>& operator()(int row, int col) const {
slp_assert(row >= 0 && row < rows());
slp_assert(col >= 0 && col < cols());
return m_storage[row * cols() + col];
@@ -377,7 +377,7 @@ class VariableMatrix : public SleipnirBase {
/// @param row_slice The row slice.
/// @param col_slice The column slice.
/// @return A slice of the variable matrix.
- VariableBlock<VariableMatrix> operator[](Slice row_slice, Slice col_slice) {
+ VariableBlock<VariableMatrix> operator()(Slice row_slice, Slice col_slice) {
int row_slice_length = row_slice.adjust(rows());
int col_slice_length = col_slice.adjust(cols());
return VariableBlock{*this, std::move(row_slice), row_slice_length,
@@ -389,7 +389,7 @@ class VariableMatrix : public SleipnirBase {
/// @param row_slice The row slice.
/// @param col_slice The column slice.
/// @return A slice of the variable matrix.
- const VariableBlock<const VariableMatrix> operator[](Slice row_slice,
+ const VariableBlock<const VariableMatrix> operator()(Slice row_slice,
Slice col_slice) const {
int row_slice_length = row_slice.adjust(rows());
int col_slice_length = col_slice.adjust(cols());
@@ -407,7 +407,7 @@ class VariableMatrix : public SleipnirBase {
/// @param col_slice The column slice.
/// @param col_slice_length The column slice length.
/// @return A slice of the variable matrix.
- VariableBlock<VariableMatrix> operator[](Slice row_slice,
+ VariableBlock<VariableMatrix> operator()(Slice row_slice,
int row_slice_length,
Slice col_slice,
int col_slice_length) {
@@ -425,7 +425,7 @@ class VariableMatrix : public SleipnirBase {
/// @param col_slice The column slice.
/// @param col_slice_length The column slice length.
/// @return A slice of the variable matrix.
- const VariableBlock<const VariableMatrix> operator[](
+ const VariableBlock<const VariableMatrix> operator()(
Slice row_slice, int row_slice_length, Slice col_slice,
int col_slice_length) const {
return VariableBlock{*this, std::move(row_slice), row_slice_length,
@@ -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<Scalar>() 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<Scalar> 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<Scalar> solve(const VariableMatrix<Scalar>& 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<Scalar> solve(const VariableMatrix<Scalar>& A,
//
// https://www.wolframalpha.com/input?i=inverse+%7B%7Ba%2C+b%2C+c%7D%2C+%7Bd%2C+e%2C+f%7D%2C+%7Bg%2C+h%2C+i%7D%7D
- 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<Scalar> solve(const VariableMatrix<Scalar>& A,
//
// https://www.wolframalpha.com/input?i=inverse+%7B%7Ba%2C+b%2C+c%2C+d%7D%2C+%7Be%2C+f%2C+g%2C+h%7D%2C+%7Bi%2C+j%2C+k%2C+l%7D%2C+%7Bm%2C+n%2C+o%2C+p%7D%7D
- 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<Scalar> solve(const VariableMatrix<Scalar>& A,
MatrixXv eigen_A{A.rows(), A.cols()};
for (int row = 0; row < A.rows(); ++row) {
for (int col = 0; col < A.cols(); ++col) {
- 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<Scalar> solve(const VariableMatrix<Scalar>& A,
VariableMatrix<Scalar> X{detail::empty, A.cols(), B.cols()};
for (int row = 0; row < X.rows(); ++row) {
for (int col = 0; col < X.cols(); ++col) {
- 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<Scalar> {
if (timestep_method == TimestepMethod::FIXED) {
m_DT = VariableMatrix<Scalar>{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<Scalar> {
// Set the member variable matrix to track the decision variable
m_DT = VariableMatrix<Scalar>{1, m_num_steps + 1};
for (int i = 0; i < num_steps + 1; ++i) {
- 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<Scalar> {
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<Scalar> {
// 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<Scalar> {
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<Scalar> {
auto x_begin = X().col(i);
auto x_end = X().col(i + 1);
auto u = U().col(i);
- Variable dt = this->dt()[0, i];
+ Variable dt = this->dt()(0, i);
if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) {
x_end = rk4<const decltype(m_dynamics)&, VariableMatrix<Scalar>,
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();
}
}

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

@@ -4,9 +4,8 @@
#include <stdint.h>
#include <fmt/base.h>
#include "sleipnir/util/unreachable.hpp"
#include <format>
#include <utility>
namespace slp {
@@ -28,16 +27,14 @@ enum class ExpressionType : uint8_t {
} // namespace slp
// @cond Suppress Doxygen
/// Formatter for ExpressionType.
template <>
struct fmt::formatter<slp::ExpressionType> {
struct std::formatter<slp::ExpressionType> {
/// 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<slp::ExpressionType> {
/// @param ctx Format context.
/// @return Format context iterator.
template <typename FmtContext>
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<slp::ExpressionType> {
case NONLINEAR:
return m_underlying.format("nonlinear", ctx);
default:
slp::unreachable();
std::unreachable();
}
}
private:
fmt::formatter<const char*> m_underlying;
std::formatter<const char*> m_underlying;
};
// @endcond

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

@@ -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)};
}
}
}

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

@@ -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)};
}
}
}

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

@@ -73,7 +73,7 @@ class Variable : public SleipnirBase {
///
/// @param value The value of the Variable.
// NOLINTNEXTLINE (google-explicit-constructor)
Variable(SleipnirMatrixLike<Scalar> auto value) : expr{value(0, 0).expr} {
Variable(SleipnirMatrixLike<Scalar> auto value) : expr{value[0, 0].expr} {
slp_assert(value.rows() == 1 && value.cols() == 1);
}
@@ -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<RHS>) {
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<LHS>) {
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<LHS> && !EigenMatrixLike<RHS>) {
constraints.emplace_back(lhs[row, col] - rhs[row, col]);
} else if constexpr (!EigenMatrixLike<LHS> && EigenMatrixLike<RHS>) {
constraints.emplace_back(lhs[row, col] - rhs(row, col));
} else if constexpr (EigenMatrixLike<LHS> && !EigenMatrixLike<RHS>) {
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<InequalityConstraints> inequality_constraints) {
for (const auto& elem : inequality_constraints) {
constraints.insert(constraints.end(), elem.constraints.begin(),

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

@@ -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<Mat>& 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<Scalar>& operator()(int row, int col)
Variable<Scalar>& operator[](int row, int col)
requires(!std::is_const_v<Mat>)
{
slp_assert(row >= 0 && row < rows());
slp_assert(col >= 0 && col < cols());
return (*m_mat)(m_row_slice.start + row * m_row_slice.step,
m_col_slice.start + col * m_col_slice.step);
return (*m_mat)[m_row_slice.start + row * m_row_slice.step,
m_col_slice.start + col * m_col_slice.step];
}
/// Returns a scalar subblock at the given row and column.
@@ -232,11 +232,11 @@ class VariableBlock : public SleipnirBase {
/// @param row The scalar subblock's row.
/// @param col The scalar subblock's column.
/// @return A scalar subblock at the given row and column.
const Variable<Scalar>& operator()(int row, int col) const {
const Variable<Scalar>& operator[](int row, int col) const {
slp_assert(row >= 0 && row < rows());
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<Mat>)
{
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<Scalar>& 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<Mat> operator()(Slice row_slice, Slice col_slice) {
VariableBlock<Mat> operator[](Slice row_slice, Slice col_slice) {
int row_slice_length = row_slice.adjust(m_row_slice_length);
int col_slice_length = col_slice.adjust(m_col_slice_length);
return (*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<const Mat> operator()(Slice row_slice,
const VariableBlock<const Mat> operator[](Slice row_slice,
Slice col_slice) const {
int row_slice_length = row_slice.adjust(m_row_slice_length);
int col_slice_length = col_slice.adjust(m_col_slice_length);
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<Mat> operator()(Slice row_slice, int row_slice_length,
VariableBlock<Mat> operator[](Slice row_slice, int row_slice_length,
Slice col_slice, int col_slice_length) {
return VariableBlock{
*m_mat,
@@ -350,7 +350,7 @@ class VariableBlock : public SleipnirBase {
/// @param col_slice The column slice.
/// @param col_slice_length The column slice length.
/// @return A slice of the variable matrix.
const VariableBlock<const Mat> operator()(Slice row_slice,
const VariableBlock<const Mat> operator[](Slice row_slice,
int row_slice_length,
Slice col_slice,
int col_slice_length) const {
@@ -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<decltype(rhs)>) {
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<Mat>& operator*=(const ScalarLike auto& rhs) {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
(*this)(row, col) *= rhs;
(*this)[row, col] *= rhs;
}
}
@@ -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<decltype(rhs)>) {
(*this)[row, col] /= rhs(0, 0);
} else {
(*this)[row, col] /= rhs[0, 0];
}
}
}
@@ -484,7 +492,7 @@ class VariableBlock : public SleipnirBase {
VariableBlock<Mat>& operator/=(const ScalarLike auto& rhs) {
for (int row = 0; row < rows(); ++row) {
for (int col = 0; col < cols(); ++col) {
(*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<decltype(rhs)>) {
(*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<decltype(rhs)>) {
(*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<Scalar>() 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]);
}
}

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

@@ -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<Scalar>& operator()(int row, int col) {
Variable<Scalar>& operator[](int row, int col) {
slp_assert(row >= 0 && row < rows());
slp_assert(col >= 0 && col < cols());
return m_storage[row * cols() + col];
@@ -314,7 +314,7 @@ class VariableMatrix : public SleipnirBase {
/// @param row The row.
/// @param col The column.
/// @return The element at the given row and column.
const Variable<Scalar>& operator()(int row, int col) const {
const Variable<Scalar>& operator[](int row, int col) const {
slp_assert(row >= 0 && row < rows());
slp_assert(col >= 0 && col < cols());
return m_storage[row * cols() + col];
@@ -377,7 +377,7 @@ class VariableMatrix : public SleipnirBase {
/// @param row_slice The row slice.
/// @param col_slice The column slice.
/// @return A slice of the variable matrix.
VariableBlock<VariableMatrix> operator()(Slice row_slice, Slice col_slice) {
VariableBlock<VariableMatrix> operator[](Slice row_slice, Slice col_slice) {
int row_slice_length = row_slice.adjust(rows());
int col_slice_length = col_slice.adjust(cols());
return VariableBlock{*this, std::move(row_slice), row_slice_length,
@@ -389,7 +389,7 @@ class VariableMatrix : public SleipnirBase {
/// @param row_slice The row slice.
/// @param col_slice The column slice.
/// @return A slice of the variable matrix.
const VariableBlock<const VariableMatrix> operator()(Slice row_slice,
const VariableBlock<const VariableMatrix> operator[](Slice row_slice,
Slice col_slice) const {
int row_slice_length = row_slice.adjust(rows());
int col_slice_length = col_slice.adjust(cols());
@@ -407,7 +407,7 @@ class VariableMatrix : public SleipnirBase {
/// @param col_slice The column slice.
/// @param col_slice_length The column slice length.
/// @return A slice of the variable matrix.
VariableBlock<VariableMatrix> operator()(Slice row_slice,
VariableBlock<VariableMatrix> operator[](Slice row_slice,
int row_slice_length,
Slice col_slice,
int col_slice_length) {
@@ -425,7 +425,7 @@ class VariableMatrix : public SleipnirBase {
/// @param col_slice The column slice.
/// @param col_slice_length The column slice length.
/// @return A slice of the variable matrix.
const VariableBlock<const VariableMatrix> operator()(
const VariableBlock<const VariableMatrix> operator[](
Slice row_slice, int row_slice_length, Slice col_slice,
int col_slice_length) const {
return VariableBlock{*this, std::move(row_slice), row_slice_length,
@@ -503,17 +503,13 @@ class VariableMatrix : public SleipnirBase {
VariableMatrix<Scalar> 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<decltype(rhs)>) {
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<decltype(rhs)>) {
(*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<decltype(rhs)>) {
(*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<Scalar>() 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<Scalar> 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<Scalar> solve(const VariableMatrix<Scalar>& 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<Scalar> solve(const VariableMatrix<Scalar>& A,
//
// https://www.wolframalpha.com/input?i=inverse+%7B%7Ba%2C+b%2C+c%7D%2C+%7Bd%2C+e%2C+f%7D%2C+%7Bg%2C+h%2C+i%7D%7D
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<Scalar> solve(const VariableMatrix<Scalar>& A,
//
// https://www.wolframalpha.com/input?i=inverse+%7B%7Ba%2C+b%2C+c%2C+d%7D%2C+%7Be%2C+f%2C+g%2C+h%7D%2C+%7Bi%2C+j%2C+k%2C+l%7D%2C+%7Bm%2C+n%2C+o%2C+p%7D%7D
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<Scalar> solve(const VariableMatrix<Scalar>& A,
MatrixXv eigen_A{A.rows(), A.cols()};
for (int row = 0; row < A.rows(); ++row) {
for (int col = 0; col < A.cols(); ++col) {
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<Scalar> solve(const VariableMatrix<Scalar>& A,
VariableMatrix<Scalar> X{detail::empty, A.cols(), B.cols()};
for (int row = 0; row < X.rows(); ++row) {
for (int col = 0; col < X.cols(); ++col) {
X(row, col) = eigen_X(row, col);
X[row, col] = eigen_X(row, col);
}
}

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

@@ -123,7 +123,7 @@ class OCP : public Problem<Scalar> {
if (timestep_method == TimestepMethod::FIXED) {
m_DT = VariableMatrix<Scalar>{1, m_num_steps + 1};
for (int i = 0; i < num_steps + 1; ++i) {
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<Scalar> {
// Set the member variable matrix to track the decision variable
m_DT = VariableMatrix<Scalar>{1, m_num_steps + 1};
for (int i = 0; i < num_steps + 1; ++i) {
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<Scalar> {
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<Scalar> {
// 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<Scalar> {
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<Scalar> {
auto x_begin = X().col(i);
auto x_end = X().col(i + 1);
auto u = U().col(i);
Variable dt = this->dt()(0, i);
Variable dt = this->dt()[0, i];
if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) {
x_end = rk4<const decltype(m_dynamics)&, VariableMatrix<Scalar>,

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

@@ -15,7 +15,6 @@
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <fmt/chrono.h>
#include <gch/small_vector.hpp>
#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<Variable<Scalar>>& constraints) {
std::array<size_t, 5> 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);
}

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

@@ -4,9 +4,8 @@
#include <stdint.h>
#include <fmt/base.h>
#include "sleipnir/util/unreachable.hpp"
#include <format>
#include <utility>
namespace slp {
@@ -46,16 +45,14 @@ enum class ExitStatus : int8_t {
} // namespace slp
// @cond Suppress Doxygen
/// Formatter for ExitStatus.
template <>
struct fmt::formatter<slp::ExitStatus> {
struct std::formatter<slp::ExitStatus> {
/// 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<slp::ExitStatus> {
/// @param ctx Format context.
/// @return Format context iterator.
template <typename FmtContext>
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<slp::ExitStatus> {
case TIMEOUT:
return m_underlying.format("timeout", ctx);
default:
slp::unreachable();
std::unreachable();
}
}
private:
fmt::formatter<const char*> m_underlying;
std::formatter<const char*> m_underlying;
};
// @endcond

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

@@ -96,8 +96,6 @@ compute_p_n(const Eigen::Vector<Scalar, Eigen::Dynamic>& 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"

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

@@ -2,10 +2,10 @@
#pragma once
#include <format>
#include <limits>
#include <Eigen/Core>
#include <fmt/format.h>
namespace slp {
@@ -77,16 +77,14 @@ class Inertia {
} // namespace slp
// @cond Suppress Doxygen
/// Formatter for Inertia.
template <>
struct fmt::formatter<slp::Inertia> {
struct std::formatter<slp::Inertia> {
/// 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<slp::Inertia> {
/// @param ctx Format context.
/// @return Format context iterator.
template <typename FmtContext>
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<int> m_underlying;
std::formatter<int> m_underlying;
};
// @endcond

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

@@ -3,17 +3,16 @@
#pragma once
#ifdef SLEIPNIR_PYTHON
#include <format>
#include <source_location>
#include <stdexcept>
#include <fmt/format.h>
/// 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)); \
} \

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

@@ -4,48 +4,47 @@
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
#include <cstdio>
#include <print>
#include <system_error>
#include <utility>
#include <fmt/base.h>
#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 <typename... T>
void print(fmt::format_string<T...> fmt, T&&... args) {
void print(std::format_string<T...> fmt, T&&... args) {
try {
fmt::print(fmt, std::forward<T>(args)...);
std::print(fmt, std::forward<T>(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 <typename... T>
void print(std::FILE* f, fmt::format_string<T...> fmt, T&&... args) {
void print(std::FILE* f, std::format_string<T...> fmt, T&&... args) {
try {
fmt::print(f, fmt, std::forward<T>(args)...);
std::print(f, fmt, std::forward<T>(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 <typename... T>
void println(fmt::format_string<T...> fmt, T&&... args) {
void println(std::format_string<T...> fmt, T&&... args) {
try {
fmt::println(fmt, std::forward<T>(args)...);
std::println(fmt, std::forward<T>(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 <typename... T>
void println(std::FILE* f, fmt::format_string<T...> fmt, T&&... args) {
void println(std::FILE* f, std::format_string<T...> fmt, T&&... args) {
try {
fmt::println(f, fmt, std::forward<T>(args)...);
std::println(f, fmt, std::forward<T>(args)...);
} catch (const std::system_error&) {
}
}

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

@@ -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);

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

@@ -12,7 +12,6 @@
#include <string_view>
#include <Eigen/SparseCore>
#include <wpi/util/bit.hpp>
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<char*>(&num), sizeof(num));
}

14
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/to_underlying.hpp vendored
View File

@@ -1,14 +0,0 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <type_traits>
namespace slp {
template <typename Enum>
constexpr std::underlying_type_t<Enum> to_underlying(Enum e) noexcept {
return static_cast<std::underlying_type_t<Enum>>(e);
}
} // namespace slp

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

@@ -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