mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
[upstream_utils] Upgrade to Sleipnir 0.5.0 (#8711)
This commit is contained in:
@@ -46,7 +46,7 @@ using small_vector = wpi::util::SmallVector<T>;
|
||||
def main():
|
||||
name = "sleipnir"
|
||||
url = "https://github.com/SleipnirGroup/Sleipnir"
|
||||
tag = "v0.3.3"
|
||||
tag = "v0.5.0"
|
||||
|
||||
sleipnir = Lib(name, url, tag, copy_upstream_src)
|
||||
sleipnir.main()
|
||||
|
||||
@@ -1,18 +1,18 @@
|
||||
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
|
||||
From: Tyler Veness <calcmogul@gmail.com>
|
||||
Date: Wed, 29 May 2024 16:29:55 -0700
|
||||
Subject: [PATCH 1/9] Use fmtlib
|
||||
Subject: [PATCH 01/10] Use fmtlib
|
||||
|
||||
---
|
||||
include/sleipnir/autodiff/expression_type.hpp | 9 ++++---
|
||||
include/sleipnir/autodiff/expression_type.hpp | 16 +++++++----
|
||||
include/sleipnir/optimization/problem.hpp | 1 +
|
||||
.../optimization/solver/exit_status.hpp | 9 ++++---
|
||||
.../optimization/solver/exit_status.hpp | 16 +++++++----
|
||||
include/sleipnir/util/assert.hpp | 5 ++--
|
||||
include/sleipnir/util/print.hpp | 27 ++++++++++---------
|
||||
5 files changed, 28 insertions(+), 23 deletions(-)
|
||||
5 files changed, 40 insertions(+), 25 deletions(-)
|
||||
|
||||
diff --git a/include/sleipnir/autodiff/expression_type.hpp b/include/sleipnir/autodiff/expression_type.hpp
|
||||
index 200324a2c38c58dbe07e96c061658c9e65aacce6..2fde6bdf3cf72f46fb633b72e29e66209e9f6a83 100644
|
||||
index 12d0568f628d5b97c0c2f5291851d4b20921f9d3..d06d32dac6c7b6faeedefeaa107cedac8446a3ab 100644
|
||||
--- a/include/sleipnir/autodiff/expression_type.hpp
|
||||
+++ b/include/sleipnir/autodiff/expression_type.hpp
|
||||
@@ -4,9 +4,10 @@
|
||||
@@ -27,8 +27,12 @@ index 200324a2c38c58dbe07e96c061658c9e65aacce6..2fde6bdf3cf72f46fb633b72e29e6620
|
||||
namespace slp {
|
||||
|
||||
/// Expression type.
|
||||
@@ -29,12 +30,12 @@ enum class ExpressionType : uint8_t {
|
||||
@@ -27,14 +28,16 @@ enum class ExpressionType : uint8_t {
|
||||
|
||||
} // namespace slp
|
||||
|
||||
+// @cond Suppress Doxygen
|
||||
+
|
||||
/// Formatter for ExpressionType.
|
||||
template <>
|
||||
-struct std::formatter<slp::ExpressionType> {
|
||||
@@ -42,15 +46,27 @@ index 200324a2c38c58dbe07e96c061658c9e65aacce6..2fde6bdf3cf72f46fb633b72e29e6620
|
||||
return m_underlying.parse(ctx);
|
||||
}
|
||||
|
||||
@@ -65,5 +66,5 @@ struct std::formatter<slp::ExpressionType> {
|
||||
@@ -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 59acc732dbc5bfe520745089abd156da9a8a2e51..ba45ead1500b45616b74e1d108959127484cf4d6 100644
|
||||
index 42e572b6c13bba6d5f7592d2b7ffbe1879b1d99b..bb0bb21bb932f77f95c7779241784c4c54d6fe93 100644
|
||||
--- a/include/sleipnir/optimization/problem.hpp
|
||||
+++ b/include/sleipnir/optimization/problem.hpp
|
||||
@@ -15,6 +15,7 @@
|
||||
@@ -62,7 +78,7 @@ index 59acc732dbc5bfe520745089abd156da9a8a2e51..ba45ead1500b45616b74e1d108959127
|
||||
|
||||
#include "sleipnir/autodiff/expression_type.hpp"
|
||||
diff --git a/include/sleipnir/optimization/solver/exit_status.hpp b/include/sleipnir/optimization/solver/exit_status.hpp
|
||||
index e17f4490cc0f6704f0b5d29340ec6dfbbcbb8aee..c1e4d319c197bdad2ace19500fb3a0426060e87b 100644
|
||||
index 01dddd0d4967268cbbb2c808f7973fafc6be081d..8a7904dd5664dcd66d1332f38902df2252387a58 100644
|
||||
--- a/include/sleipnir/optimization/solver/exit_status.hpp
|
||||
+++ b/include/sleipnir/optimization/solver/exit_status.hpp
|
||||
@@ -4,9 +4,10 @@
|
||||
@@ -77,13 +93,17 @@ index e17f4490cc0f6704f0b5d29340ec6dfbbcbb8aee..c1e4d319c197bdad2ace19500fb3a042
|
||||
namespace slp {
|
||||
|
||||
/// Solver exit status. Negative values indicate failure.
|
||||
@@ -43,12 +44,12 @@ enum class ExitStatus : int8_t {
|
||||
@@ -42,14 +43,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> {
|
||||
/// Parse format string.
|
||||
/// Parses format string.
|
||||
///
|
||||
/// @param ctx Format parse context.
|
||||
/// @return Format parse context iterator.
|
||||
@@ -92,15 +112,27 @@ index e17f4490cc0f6704f0b5d29340ec6dfbbcbb8aee..c1e4d319c197bdad2ace19500fb3a042
|
||||
return m_underlying.parse(ctx);
|
||||
}
|
||||
|
||||
@@ -92,5 +93,5 @@ struct std::formatter<slp::ExitStatus> {
|
||||
@@ -60,7 +63,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) {
|
||||
@@ -93,5 +97,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/util/assert.hpp b/include/sleipnir/util/assert.hpp
|
||||
index dd479f11080d67b03dbe72475be974908e96fc2f..4f0203a4b8df2f86e1e15cf1102a51c340665722 100644
|
||||
index 29dfe5416530a476417de3a4b6434d3b28d73120..783a0e71a1cb54bd1d6d17fa9d2bedd2038a8ef5 100644
|
||||
--- a/include/sleipnir/util/assert.hpp
|
||||
+++ b/include/sleipnir/util/assert.hpp
|
||||
@@ -3,16 +3,17 @@
|
||||
@@ -113,7 +145,7 @@ index dd479f11080d67b03dbe72475be974908e96fc2f..4f0203a4b8df2f86e1e15cf1102a51c3
|
||||
|
||||
+#include <fmt/format.h>
|
||||
+
|
||||
/// Throw an exception in Python.
|
||||
/// Throws an exception in Python.
|
||||
#define slp_assert(condition) \
|
||||
do { \
|
||||
if (!(condition)) { \
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
|
||||
From: Tyler Veness <calcmogul@gmail.com>
|
||||
Date: Sun, 16 Jun 2024 12:08:49 -0700
|
||||
Subject: [PATCH 2/9] Use wpi::SmallVector
|
||||
Subject: [PATCH 02/10] Use wpi::SmallVector
|
||||
|
||||
---
|
||||
include/sleipnir/autodiff/expression.hpp | 4 ++--
|
||||
@@ -10,7 +10,7 @@ Subject: [PATCH 2/9] Use wpi::SmallVector
|
||||
3 files changed, 6 insertions(+), 6 deletions(-)
|
||||
|
||||
diff --git a/include/sleipnir/autodiff/expression.hpp b/include/sleipnir/autodiff/expression.hpp
|
||||
index 36d7e7248a77aaa843f02b262d3865711097a2cb..0e6f31bf77beb6759b8907da9a05e8405c5c5cca 100644
|
||||
index 76f6d4ed534ce685194ad7fc62d9b62eb2ca1096..10e20f5e2fb0d7459fcecac2f8ba1bdcc98efc3f 100644
|
||||
--- a/include/sleipnir/autodiff/expression.hpp
|
||||
+++ b/include/sleipnir/autodiff/expression.hpp
|
||||
@@ -34,7 +34,7 @@ struct Expression;
|
||||
@@ -32,7 +32,7 @@ index 36d7e7248a77aaa843f02b262d3865711097a2cb..0e6f31bf77beb6759b8907da9a05e840
|
||||
// Expression destructor when expr's refcount reaches zero can cause a stack
|
||||
// overflow. Instead, we iterate over its children to decrement their
|
||||
diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp
|
||||
index cca246d6c33f4a7e3d0a89f5d9bd21a3f7916aeb..40d18f2a680d6537d698c1c774d6996c65a173d8 100644
|
||||
index 910ebcf5266bdf76711db7849dd6584549094e3b..3c4f67c1f6224226620e2ffcc597336435943ce8 100644
|
||||
--- a/include/sleipnir/autodiff/variable.hpp
|
||||
+++ b/include/sleipnir/autodiff/variable.hpp
|
||||
@@ -53,7 +53,7 @@ class Variable : public SleipnirBase {
|
||||
@@ -54,7 +54,7 @@ index cca246d6c33f4a7e3d0a89f5d9bd21a3f7916aeb..40d18f2a680d6537d698c1c774d6996c
|
||||
|
||||
/// Assignment operator for scalar.
|
||||
diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp
|
||||
index 4c5a98311b1ea542877f38db086b51fd0b7c00f1..88122498f384a68b2537b24e57c4a291a951e6dd 100644
|
||||
index 8ed39dfae69c70e0b32998eb5e0c5207383f4fa2..3eeaccac274cf38d554ea834b3ff666615939382 100644
|
||||
--- a/include/sleipnir/autodiff/variable_matrix.hpp
|
||||
+++ b/include/sleipnir/autodiff/variable_matrix.hpp
|
||||
@@ -1134,12 +1134,12 @@ class VariableMatrix : public SleipnirBase {
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
|
||||
From: Tyler Veness <calcmogul@gmail.com>
|
||||
Date: Tue, 28 Jan 2025 22:19:14 -0800
|
||||
Subject: [PATCH 3/9] Use wpi::byteswap()
|
||||
Subject: [PATCH 03/10] Use wpi::byteswap()
|
||||
|
||||
---
|
||||
include/sleipnir/util/spy.hpp | 3 ++-
|
||||
|
||||
@@ -1,55 +1,89 @@
|
||||
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
|
||||
From: Tyler Veness <calcmogul@gmail.com>
|
||||
Date: Tue, 28 Jan 2025 22:19:31 -0800
|
||||
Subject: [PATCH 4/9] Replace std::to_underlying()
|
||||
Subject: [PATCH 04/10] Replace std::to_underlying()
|
||||
|
||||
---
|
||||
include/sleipnir/optimization/problem.hpp | 8 ++++----
|
||||
include/sleipnir/util/print_diagnostics.hpp | 6 +++---
|
||||
2 files changed, 7 insertions(+), 7 deletions(-)
|
||||
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 ba45ead1500b45616b74e1d108959127484cf4d6..22e7cdaa945648e2b9effcc30533d6602c839c22 100644
|
||||
index bb0bb21bb932f77f95c7779241784c4c54d6fe93..61c5de59d8c5f4d582eff4f650c66c8d50e181a0 100644
|
||||
--- a/include/sleipnir/optimization/problem.hpp
|
||||
+++ b/include/sleipnir/optimization/problem.hpp
|
||||
@@ -657,11 +657,11 @@ class Problem {
|
||||
@@ -37,6 +37,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 {
|
||||
|
||||
@@ -696,11 +697,11 @@ class Problem {
|
||||
// Print problem structure
|
||||
slp::println("\nProblem structure:");
|
||||
slp::println(" ↳ {} cost function",
|
||||
- types[std::to_underlying(cost_function_type())]);
|
||||
+ types[static_cast<uint8_t>(cost_function_type())]);
|
||||
+ types[slp::to_underlying(cost_function_type())]);
|
||||
slp::println(" ↳ {} equality constraints",
|
||||
- types[std::to_underlying(equality_constraint_type())]);
|
||||
+ types[static_cast<uint8_t>(equality_constraint_type())]);
|
||||
+ types[slp::to_underlying(equality_constraint_type())]);
|
||||
slp::println(" ↳ {} inequality constraints",
|
||||
- types[std::to_underlying(inequality_constraint_type())]);
|
||||
+ types[static_cast<uint8_t>(inequality_constraint_type())]);
|
||||
+ types[slp::to_underlying(inequality_constraint_type())]);
|
||||
|
||||
if (m_decision_variables.size() == 1) {
|
||||
slp::print("\n1 decision variable\n");
|
||||
@@ -673,7 +673,7 @@ class Problem {
|
||||
@@ -712,7 +713,7 @@ class Problem {
|
||||
[](const gch::small_vector<Variable<Scalar>>& constraints) {
|
||||
std::array<size_t, 5> counts{};
|
||||
for (const auto& constraint : constraints) {
|
||||
- ++counts[std::to_underlying(constraint.type())];
|
||||
+ ++counts[static_cast<uint8_t>(constraint.type())];
|
||||
+ ++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 54277542c46e9490a9ef01c43dc8572e9367c97e..c50d7e171bc90ea47108de90471e7d778885f6af 100644
|
||||
index f44663c6d283e6b1658a330c79e39de0dc9b428e..698a8df43d4236bb6740e8cf7872d2e1f5df8c31 100644
|
||||
--- a/include/sleipnir/util/print_diagnostics.hpp
|
||||
+++ b/include/sleipnir/util/print_diagnostics.hpp
|
||||
@@ -238,9 +238,9 @@ void print_iteration_diagnostics(int iterations, IterationType type,
|
||||
@@ -17,6 +17,7 @@
|
||||
|
||||
#include "sleipnir/util/print.hpp"
|
||||
#include "sleipnir/util/profiler.hpp"
|
||||
+#include "sleipnir/util/to_underlying.hpp"
|
||||
|
||||
namespace slp {
|
||||
|
||||
@@ -239,7 +240,7 @@ void print_iteration_diagnostics(int iterations, IterationType type,
|
||||
slp::println(
|
||||
"│{:4} {:4} {:9.3f} {:12e} {:13e} {:12e} {:12e} {:.2e} {:<5} {:.2e} "
|
||||
"{:.2e} {:2d}│",
|
||||
- iterations, ITERATION_TYPES[std::to_underlying(type)], to_ms(time), error,
|
||||
- cost, infeasibility, complementarity, μ, power_of_10(δ), primal_α, dual_α,
|
||||
- backtracks);
|
||||
+ iterations, ITERATION_TYPES[static_cast<uint8_t>(type)], to_ms(time),
|
||||
+ error, cost, infeasibility, complementarity, μ, power_of_10(δ), primal_α,
|
||||
+ dual_α, backtracks);
|
||||
+ iterations, ITERATION_TYPES[slp::to_underlying(type)], to_ms(time), error,
|
||||
cost, infeasibility, complementarity, μ, power_of_10(δ), primal_α, dual_α,
|
||||
backtracks);
|
||||
}
|
||||
#else
|
||||
#define print_iteration_diagnostics(...)
|
||||
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
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
|
||||
From: Tyler Veness <calcmogul@gmail.com>
|
||||
Date: Sat, 8 Feb 2025 13:42:36 -0800
|
||||
Subject: [PATCH 5/9] Replace std::views::zip()
|
||||
Subject: [PATCH 05/10] Replace std::views::zip()
|
||||
|
||||
---
|
||||
include/sleipnir/autodiff/gradient_expression_graph.hpp | 5 ++++-
|
||||
@@ -9,7 +9,7 @@ Subject: [PATCH 5/9] Replace std::views::zip()
|
||||
2 files changed, 9 insertions(+), 4 deletions(-)
|
||||
|
||||
diff --git a/include/sleipnir/autodiff/gradient_expression_graph.hpp b/include/sleipnir/autodiff/gradient_expression_graph.hpp
|
||||
index 414b98971580a6e3209eabfffd082de1dd5711c7..d21c1b5ad1b2f79ac4ec4e525e9b7fe7789a0461 100644
|
||||
index 275c30b76d34efe7ee1608cd4eedfa54ab2dc1ec..c0e3c161343175837abcb25fb22da0c611a44799 100644
|
||||
--- a/include/sleipnir/autodiff/gradient_expression_graph.hpp
|
||||
+++ b/include/sleipnir/autodiff/gradient_expression_graph.hpp
|
||||
@@ -161,7 +161,10 @@ class GradientExpressionGraph {
|
||||
@@ -25,12 +25,12 @@ index 414b98971580a6e3209eabfffd082de1dd5711c7..d21c1b5ad1b2f79ac4ec4e525e9b7fe7
|
||||
if (col != -1 && node->adjoint != Scalar(0)) {
|
||||
triplets.emplace_back(row, col, node->adjoint);
|
||||
diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp
|
||||
index 22e7cdaa945648e2b9effcc30533d6602c839c22..70955fdc7148e5af737d3094a5602024df790b3d 100644
|
||||
index 61c5de59d8c5f4d582eff4f650c66c8d50e181a0..6acd2f655a4b35d087ff365c2a2917b015b03b32 100644
|
||||
--- a/include/sleipnir/optimization/problem.hpp
|
||||
+++ b/include/sleipnir/optimization/problem.hpp
|
||||
@@ -675,9 +675,11 @@ class Problem {
|
||||
@@ -715,9 +715,11 @@ class Problem {
|
||||
for (const auto& constraint : constraints) {
|
||||
++counts[static_cast<uint8_t>(constraint.type())];
|
||||
++counts[slp::to_underlying(constraint.type())];
|
||||
}
|
||||
- for (const auto& [count, name] :
|
||||
- std::views::zip(counts, std::array{"empty", "constant", "linear",
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
|
||||
From: Tyler Veness <calcmogul@gmail.com>
|
||||
Date: Wed, 3 Dec 2025 23:38:53 -0800
|
||||
Subject: [PATCH 6/9] Replace std::unreachable()
|
||||
Subject: [PATCH 06/10] Replace std::unreachable()
|
||||
|
||||
---
|
||||
include/sleipnir/autodiff/expression_type.hpp | 6 +++---
|
||||
@@ -11,7 +11,7 @@ Subject: [PATCH 6/9] Replace std::unreachable()
|
||||
create mode 100644 include/sleipnir/util/unreachable.hpp
|
||||
|
||||
diff --git a/include/sleipnir/autodiff/expression_type.hpp b/include/sleipnir/autodiff/expression_type.hpp
|
||||
index 2fde6bdf3cf72f46fb633b72e29e66209e9f6a83..8a2ae457808b1ae505c19c8f65a5c68e6e11a082 100644
|
||||
index d06d32dac6c7b6faeedefeaa107cedac8446a3ab..1957fc0339b5538fbdc56a8ea2d8503c89ed4b59 100644
|
||||
--- a/include/sleipnir/autodiff/expression_type.hpp
|
||||
+++ b/include/sleipnir/autodiff/expression_type.hpp
|
||||
@@ -4,10 +4,10 @@
|
||||
@@ -27,7 +27,7 @@ index 2fde6bdf3cf72f46fb633b72e29e66209e9f6a83..8a2ae457808b1ae505c19c8f65a5c68e
|
||||
namespace slp {
|
||||
|
||||
/// Expression type.
|
||||
@@ -61,7 +61,7 @@ struct fmt::formatter<slp::ExpressionType> {
|
||||
@@ -64,7 +64,7 @@ struct fmt::formatter<slp::ExpressionType> {
|
||||
case NONLINEAR:
|
||||
return m_underlying.format("nonlinear", ctx);
|
||||
default:
|
||||
@@ -37,7 +37,7 @@ index 2fde6bdf3cf72f46fb633b72e29e66209e9f6a83..8a2ae457808b1ae505c19c8f65a5c68e
|
||||
}
|
||||
|
||||
diff --git a/include/sleipnir/optimization/solver/exit_status.hpp b/include/sleipnir/optimization/solver/exit_status.hpp
|
||||
index c1e4d319c197bdad2ace19500fb3a0426060e87b..4d43cea244afd6c8b0235b84e9140359451fbf2f 100644
|
||||
index 8a7904dd5664dcd66d1332f38902df2252387a58..ee9bfbc7630513a33297dbeaf39fd695509b5a97 100644
|
||||
--- a/include/sleipnir/optimization/solver/exit_status.hpp
|
||||
+++ b/include/sleipnir/optimization/solver/exit_status.hpp
|
||||
@@ -4,10 +4,10 @@
|
||||
@@ -53,7 +53,7 @@ index c1e4d319c197bdad2ace19500fb3a0426060e87b..4d43cea244afd6c8b0235b84e9140359
|
||||
namespace slp {
|
||||
|
||||
/// Solver exit status. Negative values indicate failure.
|
||||
@@ -88,7 +88,7 @@ struct fmt::formatter<slp::ExitStatus> {
|
||||
@@ -92,7 +92,7 @@ struct fmt::formatter<slp::ExitStatus> {
|
||||
case TIMEOUT:
|
||||
return m_underlying.format("timeout", ctx);
|
||||
default:
|
||||
|
||||
@@ -1,17 +1,17 @@
|
||||
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 7/9] Suppress clang-tidy false positives
|
||||
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 40d18f2a680d6537d698c1c774d6996c65a173d8..2091f339744f8f8980ccb1e44b0b54b06eb32418 100644
|
||||
index 3c4f67c1f6224226620e2ffcc597336435943ce8..3d3b355f9fef7d5b6b57521317f67041e0fb1ad6 100644
|
||||
--- a/include/sleipnir/autodiff/variable.hpp
|
||||
+++ b/include/sleipnir/autodiff/variable.hpp
|
||||
@@ -745,7 +745,7 @@ struct InequalityConstraints {
|
||||
@@ -835,7 +835,7 @@ struct InequalityConstraints {
|
||||
///
|
||||
/// @param inequality_constraints The list of InequalityConstraints to
|
||||
/// concatenate.
|
||||
|
||||
@@ -1,14 +1,14 @@
|
||||
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 8/9] Suppress GCC 12 warning false positive
|
||||
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 88122498f384a68b2537b24e57c4a291a951e6dd..10b3a4cc5ab22e93fe4e0b44d4664adce9228867 100644
|
||||
index 3eeaccac274cf38d554ea834b3ff666615939382..38b90b4a1c784cfd2fd806536d001db9335ebe2d 100644
|
||||
--- a/include/sleipnir/autodiff/variable_matrix.hpp
|
||||
+++ b/include/sleipnir/autodiff/variable_matrix.hpp
|
||||
@@ -503,6 +503,10 @@ class VariableMatrix : public SleipnirBase {
|
||||
|
||||
@@ -0,0 +1,31 @@
|
||||
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 ec87b2b8160051224de870c22f5ded6b35f6dc2d..1c042f8599e8e179c1586b2648e1ff99e5b5d32a 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
|
||||
@@ -592,6 +594,8 @@ ExitStatus feasibility_restoration(
|
||||
}
|
||||
}
|
||||
|
||||
+// @endcond
|
||||
+
|
||||
} // namespace slp
|
||||
|
||||
#include "sleipnir/optimization/solver/interior_point.hpp"
|
||||
@@ -1,7 +1,7 @@
|
||||
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 9/9] Use operator() instead of multidimensional array
|
||||
Subject: [PATCH 10/10] Use operator() instead of multidimensional array
|
||||
subscript operator
|
||||
|
||||
---
|
||||
@@ -15,10 +15,10 @@ Subject: [PATCH 9/9] Use operator() instead of multidimensional array
|
||||
7 files changed, 135 insertions(+), 135 deletions(-)
|
||||
|
||||
diff --git a/include/sleipnir/autodiff/hessian.hpp b/include/sleipnir/autodiff/hessian.hpp
|
||||
index 5c8d18b41c4025f4492e7216f94299785a513552..b52371b182b755f87f4d038dbeb1795704ab730e 100644
|
||||
index 11af6033d1cf00b27cb9ff796c4a7f16b32df90a..d4cc02f54accd568d052ae5709213d45c4c8917a 100644
|
||||
--- a/include/sleipnir/autodiff/hessian.hpp
|
||||
+++ b/include/sleipnir/autodiff/hessian.hpp
|
||||
@@ -98,9 +98,9 @@ class Hessian {
|
||||
@@ -99,9 +99,9 @@ class Hessian {
|
||||
auto grad = m_graphs[row].generate_tree(m_wrt);
|
||||
for (int col = 0; col < m_wrt.rows(); ++col) {
|
||||
if (grad[col].expr != nullptr) {
|
||||
@@ -31,7 +31,7 @@ index 5c8d18b41c4025f4492e7216f94299785a513552..b52371b182b755f87f4d038dbeb17957
|
||||
}
|
||||
}
|
||||
diff --git a/include/sleipnir/autodiff/jacobian.hpp b/include/sleipnir/autodiff/jacobian.hpp
|
||||
index bb3a0319880bd240497e1e27ca26f0105b50b803..082a6cf6782f376cbb6ce05809e62e8021feb321 100644
|
||||
index f1915a0d08c509f4eb4d032d4e6a5e6099b68ebc..edd949eefd018af1b1e091b4b2780fc1a0a492cf 100644
|
||||
--- a/include/sleipnir/autodiff/jacobian.hpp
|
||||
+++ b/include/sleipnir/autodiff/jacobian.hpp
|
||||
@@ -104,9 +104,9 @@ class Jacobian {
|
||||
@@ -47,7 +47,7 @@ index bb3a0319880bd240497e1e27ca26f0105b50b803..082a6cf6782f376cbb6ce05809e62e80
|
||||
}
|
||||
}
|
||||
diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp
|
||||
index 2091f339744f8f8980ccb1e44b0b54b06eb32418..7616c02b456c755a56f68de86a755d29cd55d91d 100644
|
||||
index 3d3b355f9fef7d5b6b57521317f67041e0fb1ad6..130a4f057ea08ab0565b54f8a3556bd81fbc6825 100644
|
||||
--- a/include/sleipnir/autodiff/variable.hpp
|
||||
+++ b/include/sleipnir/autodiff/variable.hpp
|
||||
@@ -68,7 +68,7 @@ class Variable : public SleipnirBase {
|
||||
@@ -59,7 +59,7 @@ index 2091f339744f8f8980ccb1e44b0b54b06eb32418..7616c02b456c755a56f68de86a755d29
|
||||
slp_assert(value.rows() == 1 && value.cols() == 1);
|
||||
}
|
||||
|
||||
@@ -635,7 +635,7 @@ auto make_constraints(LHS&& lhs, RHS&& rhs) {
|
||||
@@ -725,7 +725,7 @@ auto make_constraints(LHS&& lhs, RHS&& rhs) {
|
||||
for (int row = 0; row < rhs.rows(); ++row) {
|
||||
for (int col = 0; col < rhs.cols(); ++col) {
|
||||
// Make right-hand side zero
|
||||
@@ -68,7 +68,7 @@ index 2091f339744f8f8980ccb1e44b0b54b06eb32418..7616c02b456c755a56f68de86a755d29
|
||||
}
|
||||
}
|
||||
|
||||
@@ -651,7 +651,7 @@ auto make_constraints(LHS&& lhs, RHS&& rhs) {
|
||||
@@ -741,7 +741,7 @@ auto make_constraints(LHS&& lhs, RHS&& rhs) {
|
||||
for (int row = 0; row < lhs.rows(); ++row) {
|
||||
for (int col = 0; col < lhs.cols(); ++col) {
|
||||
// Make right-hand side zero
|
||||
@@ -77,7 +77,7 @@ index 2091f339744f8f8980ccb1e44b0b54b06eb32418..7616c02b456c755a56f68de86a755d29
|
||||
}
|
||||
}
|
||||
|
||||
@@ -669,7 +669,7 @@ auto make_constraints(LHS&& lhs, RHS&& rhs) {
|
||||
@@ -759,7 +759,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
|
||||
@@ -388,7 +388,7 @@ index 4018606df45941b578c861caf934495f8c9e368e..cf554832b82adb17b4b1d7b56842a77d
|
||||
}
|
||||
|
||||
diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp
|
||||
index 10b3a4cc5ab22e93fe4e0b44d4664adce9228867..d90645910d1db545fb34a726aa801e08739318cc 100644
|
||||
index 38b90b4a1c784cfd2fd806536d001db9335ebe2d..e9f90fb12d230e40497e8557b985ff9c82ebaaeb 100644
|
||||
--- a/include/sleipnir/autodiff/variable_matrix.hpp
|
||||
+++ b/include/sleipnir/autodiff/variable_matrix.hpp
|
||||
@@ -154,7 +154,7 @@ class VariableMatrix : public SleipnirBase {
|
||||
@@ -772,7 +772,7 @@ index 10b3a4cc5ab22e93fe4e0b44d4664adce9228867..d90645910d1db545fb34a726aa801e08
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1253,7 +1253,7 @@ VariableMatrix<Scalar> cwise_reduce(
|
||||
@@ -1269,7 +1269,7 @@ VariableMatrix<Scalar> cwise_reduce(
|
||||
|
||||
for (int row = 0; row < lhs.rows(); ++row) {
|
||||
for (int col = 0; col < lhs.cols(); ++col) {
|
||||
@@ -781,7 +781,7 @@ index 10b3a4cc5ab22e93fe4e0b44d4664adce9228867..d90645910d1db545fb34a726aa801e08
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1386,17 +1386,17 @@ VariableMatrix<Scalar> solve(const VariableMatrix<Scalar>& A,
|
||||
@@ -1402,17 +1402,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
|
||||
@@ -804,7 +804,7 @@ index 10b3a4cc5ab22e93fe4e0b44d4664adce9228867..d90645910d1db545fb34a726aa801e08
|
||||
|
||||
VariableMatrix adj_A{{d, -b}, {-c, a}};
|
||||
auto det_A = a * d - b * c;
|
||||
@@ -1413,15 +1413,15 @@ VariableMatrix<Scalar> solve(const VariableMatrix<Scalar>& A,
|
||||
@@ -1429,15 +1429,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
|
||||
|
||||
@@ -829,7 +829,7 @@ index 10b3a4cc5ab22e93fe4e0b44d4664adce9228867..d90645910d1db545fb34a726aa801e08
|
||||
|
||||
auto ae = a * e;
|
||||
auto af = a * f;
|
||||
@@ -1461,22 +1461,22 @@ VariableMatrix<Scalar> solve(const VariableMatrix<Scalar>& A,
|
||||
@@ -1477,22 +1477,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
|
||||
|
||||
@@ -868,7 +868,7 @@ index 10b3a4cc5ab22e93fe4e0b44d4664adce9228867..d90645910d1db545fb34a726aa801e08
|
||||
|
||||
auto afk = a * f * k;
|
||||
auto afl = a * f * l;
|
||||
@@ -1607,14 +1607,14 @@ VariableMatrix<Scalar> solve(const VariableMatrix<Scalar>& A,
|
||||
@@ -1623,14 +1623,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) {
|
||||
@@ -885,7 +885,7 @@ index 10b3a4cc5ab22e93fe4e0b44d4664adce9228867..d90645910d1db545fb34a726aa801e08
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1623,7 +1623,7 @@ VariableMatrix<Scalar> solve(const VariableMatrix<Scalar>& A,
|
||||
@@ -1639,7 +1639,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) {
|
||||
@@ -895,7 +895,7 @@ index 10b3a4cc5ab22e93fe4e0b44d4664adce9228867..d90645910d1db545fb34a726aa801e08
|
||||
}
|
||||
|
||||
diff --git a/include/sleipnir/optimization/ocp.hpp b/include/sleipnir/optimization/ocp.hpp
|
||||
index 091d985234ac8be5c177b9269c7608036f0daddb..e906c194e93a9e26b9cb5c20b720b87828463e80 100644
|
||||
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> {
|
||||
@@ -959,10 +959,10 @@ index 091d985234ac8be5c177b9269c7608036f0daddb..e906c194e93a9e26b9cb5c20b720b878
|
||||
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 70955fdc7148e5af737d3094a5602024df790b3d..4243d13c9d04eeee948f8e0e7a21d72b6ef0eaa6 100644
|
||||
index 6acd2f655a4b35d087ff365c2a2917b015b03b32..e05ef004e999787a923860b2d173702e3ca4045a 100644
|
||||
--- a/include/sleipnir/optimization/problem.hpp
|
||||
+++ b/include/sleipnir/optimization/problem.hpp
|
||||
@@ -92,7 +92,7 @@ class Problem {
|
||||
@@ -97,7 +97,7 @@ class Problem {
|
||||
for (int row = 0; row < rows; ++row) {
|
||||
for (int col = 0; col < cols; ++col) {
|
||||
m_decision_variables.emplace_back();
|
||||
@@ -971,7 +971,7 @@ index 70955fdc7148e5af737d3094a5602024df790b3d..4243d13c9d04eeee948f8e0e7a21d72b
|
||||
}
|
||||
}
|
||||
|
||||
@@ -125,8 +125,8 @@ class Problem {
|
||||
@@ -132,8 +132,8 @@ class Problem {
|
||||
for (int row = 0; row < rows; ++row) {
|
||||
for (int col = 0; col <= row; ++col) {
|
||||
m_decision_variables.emplace_back();
|
||||
Reference in New Issue
Block a user