Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[upstream_utils] Upgrade Sleipnir (#8235)

Browse Source
This commit is contained in:
Tyler Veness
2025-09-20 11:21:06 -07:00
committed by GitHub
parent ee3d55e848
commit 3dbdfa1839
28 changed files with 403 additions and 260 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
upstream_utils/sleipnir.py
View File

@@ -52,8 +52,8 @@ using small_vector = wpi::SmallVector<T>;
def main(): def main():
name = "sleipnir" name = "sleipnir"
url = "https://github.com/SleipnirGroup/Sleipnir" url = "https://github.com/SleipnirGroup/Sleipnir"
# main on 2025-05-18 # main on 2025-09-19
tag = "2cc18ff6d25ee0a9bd0f9993a0a41a61a28bda3e" tag = "7f89d5547702a09e3617bc31fe5bafe6add04fab"
sleipnir = Lib(name, url, tag, copy_upstream_src) sleipnir = Lib(name, url, tag, copy_upstream_src)
sleipnir.main() sleipnir.main()

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

@@ -4,10 +4,12 @@ Date: Wed, 29 May 2024 16:29:55 -0700
Subject: [PATCH 1/8] Use fmtlib Subject: [PATCH 1/8] Use fmtlib
--- ---
include/.styleguide | 1 + include/.styleguide | 1 +
include/sleipnir/util/print.hpp | 31 ++++++++++++++++++------------- include/sleipnir/util/assert.hpp | 5 +++--
src/optimization/problem.cpp | 2 +- include/sleipnir/util/print.hpp | 31 ++++++++++++++++++-------------
3 files changed, 20 insertions(+), 14 deletions(-) src/.styleguide | 1 +
src/optimization/problem.cpp | 1 +
5 files changed, 24 insertions(+), 15 deletions(-)
diff --git a/include/.styleguide b/include/.styleguide diff --git a/include/.styleguide b/include/.styleguide
index 1b6652d3d5886cf8c9eca0d855c21031775bad7c..4f4c76204071f90bf49eddb8c2aceb583b5e09ba 100644 index 1b6652d3d5886cf8c9eca0d855c21031775bad7c..4f4c76204071f90bf49eddb8c2aceb583b5e09ba 100644
@@ -20,6 +22,31 @@ index 1b6652d3d5886cf8c9eca0d855c21031775bad7c..4f4c76204071f90bf49eddb8c2aceb58
+ ^fmt/ + ^fmt/
^gch/ ^gch/
} }
diff --git a/include/sleipnir/util/assert.hpp b/include/sleipnir/util/assert.hpp
index 75d8ffca32accbf66ffce30f073de1db2f42469b..53de01928b929793fa77885ec4a6d1a928bdc5a9 100644
--- a/include/sleipnir/util/assert.hpp
+++ b/include/sleipnir/util/assert.hpp
@@ -3,9 +3,10 @@
#pragma once
#ifdef JORMUNGANDR
-#include <format>
#include <source_location>
#include <stdexcept>
+
+#include <fmt/format.h>
/**
* Throw an exception in Python.
*/
@@ -13,7 +14,7 @@
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 diff --git a/include/sleipnir/util/print.hpp b/include/sleipnir/util/print.hpp
index fe430352dabf4cd6a890dc8007237c7a261dfd4b..055d5c9fa246201f1d8ae7ddca00b1159aeb2a57 100644 index fe430352dabf4cd6a890dc8007237c7a261dfd4b..055d5c9fa246201f1d8ae7ddca00b1159aeb2a57 100644
--- a/include/sleipnir/util/print.hpp --- a/include/sleipnir/util/print.hpp
@@ -99,16 +126,26 @@ index fe430352dabf4cd6a890dc8007237c7a261dfd4b..055d5c9fa246201f1d8ae7ddca00b115
} catch (const std::system_error&) { } catch (const std::system_error&) {
} }
} }
diff --git a/src/.styleguide b/src/.styleguide
index 1b6652d3d5886cf8c9eca0d855c21031775bad7c..4f4c76204071f90bf49eddb8c2aceb583b5e09ba 100644
--- a/src/.styleguide
+++ b/src/.styleguide
@@ -8,5 +8,6 @@ cppSrcFileInclude {
includeOtherLibs {
^Eigen/
+ ^fmt/
^gch/
}
diff --git a/src/optimization/problem.cpp b/src/optimization/problem.cpp diff --git a/src/optimization/problem.cpp b/src/optimization/problem.cpp
index 31115490867146ec166604bcc61731d7891a9f22..81863808d329a53d4162ce0624a3b8e8afc32dfc 100644 index c3331197e2365934273f57422b79fa18c2b78a5b..09828cdb6d7cddff692b9d17603dc0c11cd5a3ec 100644
--- a/src/optimization/problem.cpp --- a/src/optimization/problem.cpp
+++ b/src/optimization/problem.cpp +++ b/src/optimization/problem.cpp
@@ -335,7 +335,7 @@ void Problem::print_exit_conditions([[maybe_unused]] const Options& options) { @@ -11,6 +11,7 @@
slp::println(" ↳ executed {} iterations", options.max_iterations);
}
if (std::isfinite(options.timeout.count())) {
- slp::println(" ↳ {} elapsed", options.timeout);
+ slp::println(" ↳ {} elapsed", options.timeout.count());
}
}
#include <Eigen/Core>
#include <Eigen/SparseCore>
+#include <fmt/chrono.h>
#include <gch/small_vector.hpp>
#include "optimization/bounds.hpp"

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

@@ -10,7 +10,7 @@ Subject: [PATCH 2/8] Use wpi::SmallVector
3 files changed, 6 insertions(+), 7 deletions(-) 3 files changed, 6 insertions(+), 7 deletions(-)
diff --git a/include/sleipnir/autodiff/expression.hpp b/include/sleipnir/autodiff/expression.hpp diff --git a/include/sleipnir/autodiff/expression.hpp b/include/sleipnir/autodiff/expression.hpp
index 873e1c27559d92eb1b3a217890ca41bdc65af122..1c5f84d22a0bed70869121acabd527825ba90adb 100644 index bb4d8c5641a5b3d633d372674e0a35f857889cd4..53a5f6d68d3153537840c4ff45fe5e5d8b0076b7 100644
--- a/include/sleipnir/autodiff/expression.hpp --- a/include/sleipnir/autodiff/expression.hpp
+++ b/include/sleipnir/autodiff/expression.hpp +++ b/include/sleipnir/autodiff/expression.hpp
@@ -30,7 +30,7 @@ inline constexpr bool USE_POOL_ALLOCATOR = true; @@ -30,7 +30,7 @@ inline constexpr bool USE_POOL_ALLOCATOR = true;
@@ -22,7 +22,7 @@ index 873e1c27559d92eb1b3a217890ca41bdc65af122..1c5f84d22a0bed70869121acabd52782
/** /**
* Typedef for intrusive shared pointer to Expression. * Typedef for intrusive shared pointer to Expression.
@@ -680,7 +680,7 @@ inline constexpr void inc_ref_count(Expression* expr) { @@ -733,7 +733,7 @@ inline constexpr void inc_ref_count(Expression* expr) {
* *
* @param expr The shared pointer's managed object. * @param expr The shared pointer's managed object.
*/ */
@@ -32,7 +32,7 @@ index 873e1c27559d92eb1b3a217890ca41bdc65af122..1c5f84d22a0bed70869121acabd52782
// Expression destructor when expr's refcount reaches zero can cause a stack // Expression destructor when expr's refcount reaches zero can cause a stack
// overflow. Instead, we iterate over its children to decrement their // overflow. Instead, we iterate over its children to decrement their
diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp
index 14eb1d3b95069e143699e1488f3081c4cd9de07c..9f79a82763213dc712cce4c2a322289d57645032 100644 index f60236811eba45c67a9638e90d5101d877ecc2d0..264f0950f293c67d6e6c7e729887090c050e40e2 100644
--- a/include/sleipnir/autodiff/variable.hpp --- a/include/sleipnir/autodiff/variable.hpp
+++ b/include/sleipnir/autodiff/variable.hpp +++ b/include/sleipnir/autodiff/variable.hpp
@@ -47,7 +47,7 @@ class SLEIPNIR_DLLEXPORT Variable { @@ -47,7 +47,7 @@ class SLEIPNIR_DLLEXPORT Variable {
@@ -55,7 +55,7 @@ index 14eb1d3b95069e143699e1488f3081c4cd9de07c..9f79a82763213dc712cce4c2a322289d
/** /**
* Assignment operator for double. * Assignment operator for double.
diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp
index 410f12873cfdf5d0d484653c6c3dac74ed96348a..1c6f9e8dade8bebce7aec18bbb9b5491acb1d977 100644 index e1a419ca5356660b3c1c27230d1cb2a86977fb65..349a1550235516f9853609b61feded834ef2894b 100644
--- a/include/sleipnir/autodiff/variable_matrix.hpp --- a/include/sleipnir/autodiff/variable_matrix.hpp
+++ b/include/sleipnir/autodiff/variable_matrix.hpp +++ b/include/sleipnir/autodiff/variable_matrix.hpp
@@ -1120,14 +1120,14 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { @@ -1120,14 +1120,14 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {

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

@@ -9,7 +9,7 @@ Subject: [PATCH 4/8] Replace std::to_underlying()
2 files changed, 7 insertions(+), 8 deletions(-) 2 files changed, 7 insertions(+), 8 deletions(-)
diff --git a/src/optimization/problem.cpp b/src/optimization/problem.cpp diff --git a/src/optimization/problem.cpp b/src/optimization/problem.cpp
index 81863808d329a53d4162ce0624a3b8e8afc32dfc..c3319fc0a927cf452871a2db08d5edff87ac8eea 100644 index 09828cdb6d7cddff692b9d17603dc0c11cd5a3ec..886de24cc0532d31f1e186150da79e925f212556 100644
--- a/src/optimization/problem.cpp --- a/src/optimization/problem.cpp
+++ b/src/optimization/problem.cpp +++ b/src/optimization/problem.cpp
@@ -7,7 +7,6 @@ @@ -7,7 +7,6 @@
@@ -20,7 +20,7 @@ index 81863808d329a53d4162ce0624a3b8e8afc32dfc..c3319fc0a927cf452871a2db08d5edff
#include <Eigen/Core> #include <Eigen/Core>
#include <Eigen/SparseCore> #include <Eigen/SparseCore>
@@ -346,11 +345,11 @@ void Problem::print_problem_analysis() { @@ -350,11 +349,11 @@ void Problem::print_problem_analysis() {
// Print problem structure // Print problem structure
slp::println("\nProblem structure:"); slp::println("\nProblem structure:");
slp::println(" ↳ {} cost function", slp::println(" ↳ {} cost function",
@@ -35,7 +35,7 @@ index 81863808d329a53d4162ce0624a3b8e8afc32dfc..c3319fc0a927cf452871a2db08d5edff
if (m_decision_variables.size() == 1) { if (m_decision_variables.size() == 1) {
slp::print("\n1 decision variable\n"); slp::print("\n1 decision variable\n");
@@ -362,7 +361,7 @@ void Problem::print_problem_analysis() { @@ -366,7 +365,7 @@ void Problem::print_problem_analysis() {
[](const gch::small_vector<Variable>& constraints) { [](const gch::small_vector<Variable>& constraints) {
std::array<size_t, 5> counts{}; std::array<size_t, 5> counts{};
for (const auto& constraint : constraints) { for (const auto& constraint : constraints) {

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

@@ -9,10 +9,10 @@ Subject: [PATCH 5/8] Replace std::views::zip()
2 files changed, 9 insertions(+), 5 deletions(-) 2 files changed, 9 insertions(+), 5 deletions(-)
diff --git a/include/sleipnir/autodiff/adjoint_expression_graph.hpp b/include/sleipnir/autodiff/adjoint_expression_graph.hpp diff --git a/include/sleipnir/autodiff/adjoint_expression_graph.hpp b/include/sleipnir/autodiff/adjoint_expression_graph.hpp
index 4b4f3303faed766d3ac39829870514f50d9a582f..4576e19c9695caf4407fbbb592afe32d8252a0db 100644 index 33b6eee615141a1d6472f116842d62052ef54dd9..b333aebd3e59fa23eed6046c13d736c3d2eccac7 100644
--- a/include/sleipnir/autodiff/adjoint_expression_graph.hpp --- a/include/sleipnir/autodiff/adjoint_expression_graph.hpp
+++ b/include/sleipnir/autodiff/adjoint_expression_graph.hpp +++ b/include/sleipnir/autodiff/adjoint_expression_graph.hpp
@@ -155,7 +155,10 @@ class AdjointExpressionGraph { @@ -158,7 +158,10 @@ class AdjointExpressionGraph {
} }
} }
} else { } else {
@@ -25,7 +25,7 @@ index 4b4f3303faed766d3ac39829870514f50d9a582f..4576e19c9695caf4407fbbb592afe32d
if (col != -1 && node->adjoint != 0.0) { if (col != -1 && node->adjoint != 0.0) {
triplets.emplace_back(row, col, node->adjoint); triplets.emplace_back(row, col, node->adjoint);
diff --git a/src/optimization/problem.cpp b/src/optimization/problem.cpp diff --git a/src/optimization/problem.cpp b/src/optimization/problem.cpp
index c3319fc0a927cf452871a2db08d5edff87ac8eea..5532b3962409e2140132e79241da4fba0f36bc78 100644 index 886de24cc0532d31f1e186150da79e925f212556..e32481e9314c9ef472843adb5bedbd993627d5d9 100644
--- a/src/optimization/problem.cpp --- a/src/optimization/problem.cpp
+++ b/src/optimization/problem.cpp +++ b/src/optimization/problem.cpp
@@ -6,7 +6,6 @@ @@ -6,7 +6,6 @@
@@ -36,7 +36,7 @@ index c3319fc0a927cf452871a2db08d5edff87ac8eea..5532b3962409e2140132e79241da4fba
#include <Eigen/Core> #include <Eigen/Core>
#include <Eigen/SparseCore> #include <Eigen/SparseCore>
@@ -363,9 +362,11 @@ void Problem::print_problem_analysis() { @@ -367,9 +366,11 @@ void Problem::print_problem_analysis() {
for (const auto& constraint : constraints) { for (const auto& constraint : constraints) {
++counts[static_cast<uint8_t>(constraint.type())]; ++counts[static_cast<uint8_t>(constraint.type())];
} }

4
upstream_utils/sleipnir_patches/0006-Suppress-clang-tidy-false-positives.patch
View File

@@ -8,10 +8,10 @@ Subject: [PATCH 6/8] Suppress clang-tidy false positives
1 file changed, 1 insertion(+), 1 deletion(-) 1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp
index 9f79a82763213dc712cce4c2a322289d57645032..17e7eb7cc2c7c7599eaba97d8ec80972524c1599 100644 index 264f0950f293c67d6e6c7e729887090c050e40e2..62135a5539308ae69f6b45a64d9337c4c3e96d7b 100644
--- a/include/sleipnir/autodiff/variable.hpp --- a/include/sleipnir/autodiff/variable.hpp
+++ b/include/sleipnir/autodiff/variable.hpp +++ b/include/sleipnir/autodiff/variable.hpp
@@ -626,7 +626,7 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints { @@ -633,7 +633,7 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints {
* @param inequality_constraints The list of InequalityConstraints to * @param inequality_constraints The list of InequalityConstraints to
* concatenate. * concatenate.
*/ */

2
upstream_utils/sleipnir_patches/0007-Suppress-GCC-12-warning-false-positive.patch
View File

@@ -8,7 +8,7 @@ Subject: [PATCH 7/8] Suppress GCC 12 warning false positive
1 file changed, 7 insertions(+) 1 file changed, 7 insertions(+)
diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp
index 1c6f9e8dade8bebce7aec18bbb9b5491acb1d977..dee43f926d304e1f4900bd57b99cd613e808f58e 100644 index 349a1550235516f9853609b61feded834ef2894b..70bccf4fc078a49e22b6699db1228c765430a121 100644
--- a/include/sleipnir/autodiff/variable_matrix.hpp --- a/include/sleipnir/autodiff/variable_matrix.hpp
+++ b/include/sleipnir/autodiff/variable_matrix.hpp +++ b/include/sleipnir/autodiff/variable_matrix.hpp
@@ -573,6 +573,10 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { @@ -573,6 +573,10 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {

74
upstream_utils/sleipnir_patches/0008-Revert-Use-multidimensional-array-subscript-operator.patch
View File

@@ -11,16 +11,16 @@ This reverts commit f9b2c450bbbf6f14b194b8b81708d032a6431ee0.
include/sleipnir/autodiff/variable.hpp | 26 +---- include/sleipnir/autodiff/variable.hpp | 26 +----
include/sleipnir/autodiff/variable_block.hpp | 70 +++++------ include/sleipnir/autodiff/variable_block.hpp | 70 +++++------
include/sleipnir/autodiff/variable_matrix.hpp | 110 ++++++------------ include/sleipnir/autodiff/variable_matrix.hpp | 110 ++++++------------
include/sleipnir/control/ocp.hpp | 14 +-- include/sleipnir/optimization/ocp.hpp | 14 +--
include/sleipnir/optimization/problem.hpp | 6 +- include/sleipnir/optimization/problem.hpp | 6 +-
src/autodiff/variable_matrix.cpp | 66 +++++------ src/autodiff/variable_matrix.cpp | 66 +++++------
8 files changed, 118 insertions(+), 182 deletions(-) 8 files changed, 118 insertions(+), 182 deletions(-)
diff --git a/include/sleipnir/autodiff/hessian.hpp b/include/sleipnir/autodiff/hessian.hpp diff --git a/include/sleipnir/autodiff/hessian.hpp b/include/sleipnir/autodiff/hessian.hpp
index 4ad097a8117dac47566a3c6896d281004147be70..8b048ab3ba0d671397cfdadcd137ac67bef1b441 100644 index fa6d8af0843eca8b674744f02551584dd8d79c21..4f093b7b39ea84e56c4a12ae1b6f645c4f84a1f0 100644
--- a/include/sleipnir/autodiff/hessian.hpp --- a/include/sleipnir/autodiff/hessian.hpp
+++ b/include/sleipnir/autodiff/hessian.hpp +++ b/include/sleipnir/autodiff/hessian.hpp
@@ -103,9 +103,9 @@ class SLEIPNIR_DLLEXPORT Hessian { @@ -106,9 +106,9 @@ class SLEIPNIR_DLLEXPORT Hessian {
auto grad = m_graphs[row].generate_gradient_tree(m_wrt); auto grad = m_graphs[row].generate_gradient_tree(m_wrt);
for (int col = 0; col < m_wrt.rows(); ++col) { for (int col = 0; col < m_wrt.rows(); ++col) {
if (grad[col].expr != nullptr) { if (grad[col].expr != nullptr) {
@@ -33,10 +33,10 @@ index 4ad097a8117dac47566a3c6896d281004147be70..8b048ab3ba0d671397cfdadcd137ac67
} }
} }
diff --git a/include/sleipnir/autodiff/jacobian.hpp b/include/sleipnir/autodiff/jacobian.hpp diff --git a/include/sleipnir/autodiff/jacobian.hpp b/include/sleipnir/autodiff/jacobian.hpp
index 787fca8ccd3fd6e46c5d31ab980704e6a5e99402..7e7e1340d065d35412f43b27fac7d8a719b7e5b5 100644 index 4515076cde12a2112e1b5711acc3092bd807e250..3662b5e49b93f63b5ccac0e732149bd9178f1aae 100644
--- a/include/sleipnir/autodiff/jacobian.hpp --- a/include/sleipnir/autodiff/jacobian.hpp
+++ b/include/sleipnir/autodiff/jacobian.hpp +++ b/include/sleipnir/autodiff/jacobian.hpp
@@ -95,9 +95,9 @@ class SLEIPNIR_DLLEXPORT Jacobian { @@ -99,9 +99,9 @@ class SLEIPNIR_DLLEXPORT Jacobian {
auto grad = m_graphs[row].generate_gradient_tree(m_wrt); auto grad = m_graphs[row].generate_gradient_tree(m_wrt);
for (int col = 0; col < m_wrt.rows(); ++col) { for (int col = 0; col < m_wrt.rows(); ++col) {
if (grad[col].expr != nullptr) { if (grad[col].expr != nullptr) {
@@ -49,10 +49,10 @@ index 787fca8ccd3fd6e46c5d31ab980704e6a5e99402..7e7e1340d065d35412f43b27fac7d8a7
} }
} }
diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp
index 17e7eb7cc2c7c7599eaba97d8ec80972524c1599..03b929c778c03186cc5b461a2e855da23034457a 100644 index 62135a5539308ae69f6b45a64d9337c4c3e96d7b..2fc2119d2dedaa5b4c941ce449b7fb113c641635 100644
--- a/include/sleipnir/autodiff/variable.hpp --- a/include/sleipnir/autodiff/variable.hpp
+++ b/include/sleipnir/autodiff/variable.hpp +++ b/include/sleipnir/autodiff/variable.hpp
@@ -505,11 +505,7 @@ gch::small_vector<Variable> make_constraints(LHS&& lhs, RHS&& rhs) { @@ -512,11 +512,7 @@ gch::small_vector<Variable> make_constraints(LHS&& lhs, RHS&& rhs) {
for (int row = 0; row < rhs.rows(); ++row) { for (int row = 0; row < rhs.rows(); ++row) {
for (int col = 0; col < rhs.cols(); ++col) { for (int col = 0; col < rhs.cols(); ++col) {
// Make right-hand side zero // Make right-hand side zero
@@ -65,7 +65,7 @@ index 17e7eb7cc2c7c7599eaba97d8ec80972524c1599..03b929c778c03186cc5b461a2e855da2
} }
} }
} else if constexpr (MatrixLike<LHS> && ScalarLike<RHS>) { } else if constexpr (MatrixLike<LHS> && ScalarLike<RHS>) {
@@ -518,11 +514,7 @@ gch::small_vector<Variable> make_constraints(LHS&& lhs, RHS&& rhs) { @@ -525,11 +521,7 @@ gch::small_vector<Variable> make_constraints(LHS&& lhs, RHS&& rhs) {
for (int row = 0; row < lhs.rows(); ++row) { for (int row = 0; row < lhs.rows(); ++row) {
for (int col = 0; col < lhs.cols(); ++col) { for (int col = 0; col < lhs.cols(); ++col) {
// Make right-hand side zero // Make right-hand side zero
@@ -78,7 +78,7 @@ index 17e7eb7cc2c7c7599eaba97d8ec80972524c1599..03b929c778c03186cc5b461a2e855da2
} }
} }
} else if constexpr (MatrixLike<LHS> && MatrixLike<RHS>) { } else if constexpr (MatrixLike<LHS> && MatrixLike<RHS>) {
@@ -532,19 +524,7 @@ gch::small_vector<Variable> make_constraints(LHS&& lhs, RHS&& rhs) { @@ -539,19 +531,7 @@ gch::small_vector<Variable> make_constraints(LHS&& lhs, RHS&& rhs) {
for (int row = 0; row < lhs.rows(); ++row) { for (int row = 0; row < lhs.rows(); ++row) {
for (int col = 0; col < lhs.cols(); ++col) { for (int col = 0; col < lhs.cols(); ++col) {
// Make right-hand side zero // Make right-hand side zero
@@ -376,7 +376,7 @@ index f1c1ca0dc3fde663c3e74f6fca4b89b119cf377d..632d44beb5b3dae29b9829c52a6168fe
} }
diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp
index dee43f926d304e1f4900bd57b99cd613e808f58e..4dc2cea00cb9491035a9b4795be3562186991c7a 100644 index 70bccf4fc078a49e22b6699db1228c765430a121..2ed997819e70c584ce413f639826b6da506e382b 100644
--- a/include/sleipnir/autodiff/variable_matrix.hpp --- a/include/sleipnir/autodiff/variable_matrix.hpp
+++ b/include/sleipnir/autodiff/variable_matrix.hpp +++ b/include/sleipnir/autodiff/variable_matrix.hpp
@@ -211,7 +211,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { @@ -211,7 +211,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
@@ -708,35 +708,35 @@ index dee43f926d304e1f4900bd57b99cd613e808f58e..4dc2cea00cb9491035a9b4795be35621
} }
} }
diff --git a/include/sleipnir/control/ocp.hpp b/include/sleipnir/control/ocp.hpp diff --git a/include/sleipnir/optimization/ocp.hpp b/include/sleipnir/optimization/ocp.hpp
index 282520fb852d8588b96846eb5b4952bf47d1309f..d9174426669281e68a5c09d298cfd5bcd3be3776 100644 index 124224cf5ba6e54c141086e3a21389530198449f..74492a0d756a9d587df6158c7e2ef8548ae22be4 100644
--- a/include/sleipnir/control/ocp.hpp --- a/include/sleipnir/optimization/ocp.hpp
+++ b/include/sleipnir/control/ocp.hpp +++ b/include/sleipnir/optimization/ocp.hpp
@@ -180,7 +180,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { @@ -122,7 +122,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
if (m_timestep_method == TimestepMethod::FIXED) { if (timestep_method == TimestepMethod::FIXED) {
m_DT = VariableMatrix{1, m_num_steps + 1}; m_DT = VariableMatrix{1, m_num_steps + 1};
for (int i = 0; i < num_steps + 1; ++i) { for (int i = 0; i < num_steps + 1; ++i) {
- m_DT[0, i] = m_dt.count(); - m_DT[0, i] = dt.count();
+ m_DT(0, i) = m_dt.count(); + m_DT(0, i) = dt.count();
} }
} else if (m_timestep_method == TimestepMethod::VARIABLE_SINGLE) { } else if (timestep_method == TimestepMethod::VARIABLE_SINGLE) {
Variable dt = decision_variable(); Variable single_dt = decision_variable();
@@ -189,12 +189,12 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { @@ -131,12 +131,12 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
// Set the member variable matrix to track the decision variable // Set the member variable matrix to track the decision variable
m_DT = VariableMatrix{1, m_num_steps + 1}; m_DT = VariableMatrix{1, m_num_steps + 1};
for (int i = 0; i < num_steps + 1; ++i) { for (int i = 0; i < num_steps + 1; ++i) {
- m_DT[0, i] = dt; - m_DT[0, i] = single_dt;
+ m_DT(0, i) = dt; + m_DT(0, i) = single_dt;
} }
} else if (m_timestep_method == TimestepMethod::VARIABLE) { } else if (timestep_method == TimestepMethod::VARIABLE) {
m_DT = decision_variable(1, m_num_steps + 1); m_DT = decision_variable(1, m_num_steps + 1);
for (int i = 0; i < num_steps + 1; ++i) { for (int i = 0; i < num_steps + 1; ++i) {
- m_DT[0, i].set_value(m_dt.count()); - m_DT[0, i].set_value(dt.count());
+ m_DT(0, i).set_value(m_dt.count()); + m_DT(0, i).set_value(dt.count());
} }
} }
@@ -270,7 +270,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { @@ -212,7 +212,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
for (int i = 0; i < m_num_steps + 1; ++i) { for (int i = 0; i < m_num_steps + 1; ++i) {
auto x = X().col(i); auto x = X().col(i);
auto u = U().col(i); auto u = U().col(i);
@@ -745,16 +745,16 @@ index 282520fb852d8588b96846eb5b4952bf47d1309f..d9174426669281e68a5c09d298cfd5bc
callback(time, x, u, dt); callback(time, x, u, dt);
time += dt; time += dt;
@@ -377,7 +377,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { @@ -353,7 +353,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
// Derivation at https://mec560sbu.github.io/2016/09/30/direct_collocation/ // Derivation at https://mec560sbu.github.io/2016/09/30/direct_collocation/
for (int i = 0; i < m_num_steps; ++i) { for (int i = 0; i < m_num_steps; ++i) {
- Variable h = dt()[0, i]; - Variable h = dt()[0, i];
+ Variable h = dt()(0, i); + Variable h = dt()(0, i);
auto& f = m_dynamics_function; auto& f = m_dynamics;
@@ -412,7 +412,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { @@ -391,7 +391,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
auto x_begin = X().col(i); auto x_begin = X().col(i);
auto x_end = X().col(i + 1); auto x_end = X().col(i + 1);
auto u = U().col(i); auto u = U().col(i);
@@ -762,8 +762,8 @@ index 282520fb852d8588b96846eb5b4952bf47d1309f..d9174426669281e68a5c09d298cfd5bc
+ Variable dt = this->dt()(0, i); + Variable dt = this->dt()(0, i);
if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) { if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) {
subject_to(x_end == rk4<const decltype(m_dynamics_function)&, subject_to(x_end == rk4<const decltype(m_dynamics)&, VariableMatrix,
@@ -433,7 +433,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { @@ -415,7 +415,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
auto x_begin = X().col(i); auto x_begin = X().col(i);
auto x_end = X().col(i + 1); auto x_end = X().col(i + 1);
auto u = U().col(i); auto u = U().col(i);
@@ -771,9 +771,9 @@ index 282520fb852d8588b96846eb5b4952bf47d1309f..d9174426669281e68a5c09d298cfd5bc
+ Variable dt = this->dt()(0, i); + Variable dt = this->dt()(0, i);
if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) { if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) {
x_end = rk4<const decltype(m_dynamics_function)&, VariableMatrix, x_end = rk4<const decltype(m_dynamics)&, VariableMatrix, VariableMatrix,
diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp
index b7a868657c704487049efaf6b3972b1f7b72bfb4..b484ec08d6c50bf42fbaa1d5b4c66a20cb11a922 100644 index efde2006397fb7d8ca24651e9a84b47fc879ee15..c996b372311f708153f8c89ef15fa35a097a6171 100644
--- a/include/sleipnir/optimization/problem.hpp --- a/include/sleipnir/optimization/problem.hpp
+++ b/include/sleipnir/optimization/problem.hpp +++ b/include/sleipnir/optimization/problem.hpp
@@ -78,7 +78,7 @@ class SLEIPNIR_DLLEXPORT Problem { @@ -78,7 +78,7 @@ class SLEIPNIR_DLLEXPORT Problem {
@@ -797,7 +797,7 @@ index b7a868657c704487049efaf6b3972b1f7b72bfb4..b484ec08d6c50bf42fbaa1d5b4c66a20
} }
diff --git a/src/autodiff/variable_matrix.cpp b/src/autodiff/variable_matrix.cpp diff --git a/src/autodiff/variable_matrix.cpp b/src/autodiff/variable_matrix.cpp
index decdc70809189d309708774ec60603fe73c50ecc..71f8153d345750d79fa41cf7af14ac766fcad2a4 100644 index 6c3a040e08bdc5009885e762402a8b44434024c3..d9619a39d583e1a29c46602ba61e881531f57e09 100644
--- a/src/autodiff/variable_matrix.cpp --- a/src/autodiff/variable_matrix.cpp
+++ b/src/autodiff/variable_matrix.cpp +++ b/src/autodiff/variable_matrix.cpp
@@ -12,17 +12,17 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) { @@ -12,17 +12,17 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
@@ -821,7 +821,7 @@ index decdc70809189d309708774ec60603fe73c50ecc..71f8153d345750d79fa41cf7af14ac76
+ const auto& c = A(1, 0); + const auto& c = A(1, 0);
+ const auto& d = A(1, 1); + const auto& d = A(1, 1);
slp::VariableMatrix adj_A{{d, -b}, {-c, a}}; VariableMatrix adj_A{{d, -b}, {-c, a}};
auto det_A = a * d - b * c; auto det_A = a * d - b * c;
@@ -39,15 +39,15 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) { @@ -39,15 +39,15 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
// //
@@ -905,7 +905,7 @@ index decdc70809189d309708774ec60603fe73c50ecc..71f8153d345750d79fa41cf7af14ac76
} }
@@ -248,7 +248,7 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) { @@ -248,7 +248,7 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
VariableMatrix X{A.cols(), B.cols()}; VariableMatrix X{VariableMatrix::empty, A.cols(), B.cols()};
for (int row = 0; row < X.rows(); ++row) { for (int row = 0; row < X.rows(); ++row) {
for (int col = 0; col < X.cols(); ++col) { for (int col = 0; col < X.cols(); ++col) {
- X[row, col] = eigen_X(row, col); - X[row, col] = eigen_X(row, col);

3
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/adjoint_expression_graph.hpp vendored
View File

@@ -11,6 +11,7 @@
#include "sleipnir/autodiff/expression_graph.hpp" #include "sleipnir/autodiff/expression_graph.hpp"
#include "sleipnir/autodiff/variable.hpp" #include "sleipnir/autodiff/variable.hpp"
#include "sleipnir/autodiff/variable_matrix.hpp" #include "sleipnir/autodiff/variable_matrix.hpp"
#include "sleipnir/util/assert.hpp"
namespace slp::detail { namespace slp::detail {
@@ -50,6 +51,8 @@ class AdjointExpressionGraph {
* @return The variable's gradient tree. * @return The variable's gradient tree.
*/ */
VariableMatrix generate_gradient_tree(const VariableMatrix& wrt) const { VariableMatrix generate_gradient_tree(const VariableMatrix& wrt) const {
slp_assert(wrt.cols() == 1);
// Read docs/algorithms.md#Reverse_accumulation_automatic_differentiation // Read docs/algorithms.md#Reverse_accumulation_automatic_differentiation
// for background on reverse accumulation automatic differentiation. // for background on reverse accumulation automatic differentiation.

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

@@ -422,6 +422,12 @@ struct Expression {
} }
}; };
inline ExpressionPtr cbrt(const ExpressionPtr& x);
inline ExpressionPtr exp(const ExpressionPtr& x);
inline ExpressionPtr sin(const ExpressionPtr& x);
inline ExpressionPtr sinh(const ExpressionPtr& x);
inline ExpressionPtr sqrt(const ExpressionPtr& x);
/** /**
* Derived expression type for binary minus operator. * Derived expression type for binary minus operator.
* *
@@ -504,6 +510,58 @@ struct BinaryPlusExpression final : Expression {
} }
}; };
/**
* Derived expression type for std::cbrt().
*/
struct CbrtExpression final : Expression {
/**
* Constructs an unary expression (an operator with one argument).
*
* @param lhs Unary operator's operand.
*/
explicit constexpr CbrtExpression(ExpressionPtr lhs)
: Expression{std::move(lhs)} {}
double value(double x, double) const override { return std::cbrt(x); }
ExpressionType type() const override { return ExpressionType::NONLINEAR; }
double grad_l(double x, double, double parent_adjoint) const override {
double c = std::cbrt(x);
return parent_adjoint / (3.0 * c * c);
}
ExpressionPtr grad_expr_l(
const ExpressionPtr& x, const ExpressionPtr&,
const ExpressionPtr& parent_adjoint) const override {
auto c = slp::detail::cbrt(x);
return parent_adjoint / (make_expression_ptr<ConstExpression>(3.0) * c * c);
}
};
/**
* std::cbrt() for Expressions.
*
* @param x The argument.
*/
inline ExpressionPtr cbrt(const ExpressionPtr& x) {
using enum ExpressionType;
// Evaluate constant
if (x->type() == CONSTANT) {
if (x->val == 0.0) {
// Return zero
return x;
} else if (x->val == -1.0 || x->val == 1.0) {
return x;
} else {
return make_expression_ptr<ConstExpression>(std::cbrt(x->val));
}
}
return make_expression_ptr<CbrtExpression>(x);
}
/** /**
* Derived expression type for constant. * Derived expression type for constant.
*/ */
@@ -661,11 +719,6 @@ struct UnaryMinusExpression final : Expression {
} }
}; };
inline ExpressionPtr exp(const ExpressionPtr& x);
inline ExpressionPtr sin(const ExpressionPtr& x);
inline ExpressionPtr sinh(const ExpressionPtr& x);
inline ExpressionPtr sqrt(const ExpressionPtr& x);
/** /**
* Refcount increment for intrusive shared pointer. * Refcount increment for intrusive shared pointer.
* *

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

@@ -21,8 +21,7 @@ inline gch::small_vector<Expression*> topological_sort(
const ExpressionPtr& root) { const ExpressionPtr& root) {
gch::small_vector<Expression*> list; gch::small_vector<Expression*> list;
// If the root type is a constant, Update() is a no-op, so there's no work // If the root type is constant, updates are a no-op, so return an empty list
// to do
if (root == nullptr || root->type() == ExpressionType::CONSTANT) { if (root == nullptr || root->type() == ExpressionType::CONSTANT) {
return list; return list;
} }

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

@@ -15,8 +15,8 @@
namespace slp { namespace slp {
/** /**
* This class calculates the gradient of a a variable with respect to a vector * This class calculates the gradient of a variable with respect to a vector of
* of variables. * variables.
* *
* The gradient is only recomputed if the variable expression is quadratic or * The gradient is only recomputed if the variable expression is quadratic or
* higher order. * higher order.
@@ -29,7 +29,7 @@ class SLEIPNIR_DLLEXPORT Gradient {
* @param variable Variable of which to compute the gradient. * @param variable Variable of which to compute the gradient.
* @param wrt Variable with respect to which to compute the gradient. * @param wrt Variable with respect to which to compute the gradient.
*/ */
Gradient(Variable variable, Variable wrt) noexcept Gradient(Variable variable, Variable wrt)
: m_jacobian{std::move(variable), std::move(wrt)} {} : m_jacobian{std::move(variable), std::move(wrt)} {}
/** /**
@@ -39,7 +39,7 @@ class SLEIPNIR_DLLEXPORT Gradient {
* @param wrt Vector of variables with respect to which to compute the * @param wrt Vector of variables with respect to which to compute the
* gradient. * gradient.
*/ */
Gradient(Variable variable, SleipnirMatrixLike auto wrt) noexcept Gradient(Variable variable, SleipnirMatrixLike auto wrt)
: m_jacobian{VariableMatrix{std::move(variable)}, std::move(wrt)} {} : m_jacobian{VariableMatrix{std::move(variable)}, std::move(wrt)} {}
/** /**
@@ -58,7 +58,7 @@ class SLEIPNIR_DLLEXPORT Gradient {
* @return The gradient at wrt's value. * @return The gradient at wrt's value.
*/ */
const Eigen::SparseVector<double>& value() { const Eigen::SparseVector<double>& value() {
m_g = m_jacobian.value(); m_g = m_jacobian.value().transpose();
return m_g; return m_g;
} }

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

@@ -10,6 +10,7 @@
#include "sleipnir/autodiff/adjoint_expression_graph.hpp" #include "sleipnir/autodiff/adjoint_expression_graph.hpp"
#include "sleipnir/autodiff/variable.hpp" #include "sleipnir/autodiff/variable.hpp"
#include "sleipnir/autodiff/variable_matrix.hpp" #include "sleipnir/autodiff/variable_matrix.hpp"
#include "sleipnir/util/assert.hpp"
#include "sleipnir/util/concepts.hpp" #include "sleipnir/util/concepts.hpp"
#include "sleipnir/util/symbol_exports.hpp" #include "sleipnir/util/symbol_exports.hpp"
@@ -34,7 +35,7 @@ class SLEIPNIR_DLLEXPORT Hessian {
* @param variable Variable of which to compute the Hessian. * @param variable Variable of which to compute the Hessian.
* @param wrt Variable with respect to which to compute the Hessian. * @param wrt Variable with respect to which to compute the Hessian.
*/ */
Hessian(Variable variable, Variable wrt) noexcept Hessian(Variable variable, Variable wrt)
: Hessian{std::move(variable), VariableMatrix{std::move(wrt)}} {} : Hessian{std::move(variable), VariableMatrix{std::move(wrt)}} {}
/** /**
@@ -44,10 +45,12 @@ class SLEIPNIR_DLLEXPORT Hessian {
* @param wrt Vector of variables with respect to which to compute the * @param wrt Vector of variables with respect to which to compute the
* Hessian. * Hessian.
*/ */
Hessian(Variable variable, SleipnirMatrixLike auto wrt) noexcept Hessian(Variable variable, SleipnirMatrixLike auto wrt)
: m_variables{detail::AdjointExpressionGraph{variable} : m_variables{detail::AdjointExpressionGraph{variable}
.generate_gradient_tree(wrt)}, .generate_gradient_tree(wrt)},
m_wrt{wrt} { m_wrt{wrt} {
slp_assert(m_wrt.cols() == 1);
// Initialize column each expression's adjoint occupies in the Jacobian // Initialize column each expression's adjoint occupies in the Jacobian
for (size_t col = 0; col < m_wrt.size(); ++col) { for (size_t col = 0; col < m_wrt.size(); ++col) {
m_wrt[col].expr->col = col; m_wrt[col].expr->col = col;
@@ -136,15 +139,9 @@ class SLEIPNIR_DLLEXPORT Hessian {
m_graphs[row].append_adjoint_triplets(triplets, row, m_wrt); m_graphs[row].append_adjoint_triplets(triplets, row, m_wrt);
} }
if (!triplets.empty()) { m_H.setFromTriplets(triplets.begin(), triplets.end());
m_H.setFromTriplets(triplets.begin(), triplets.end()); if constexpr (UpLo == Eigen::Lower) {
if constexpr (UpLo == Eigen::Lower) { m_H = m_H.triangularView<Eigen::Lower>();
m_H = m_H.triangularView<Eigen::Lower>();
}
} else {
// setFromTriplets() is a no-op on empty triplets, so explicitly zero out
// the storage
m_H.setZero();
} }
return m_H; return m_H;

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

@@ -10,6 +10,7 @@
#include "sleipnir/autodiff/adjoint_expression_graph.hpp" #include "sleipnir/autodiff/adjoint_expression_graph.hpp"
#include "sleipnir/autodiff/variable.hpp" #include "sleipnir/autodiff/variable.hpp"
#include "sleipnir/autodiff/variable_matrix.hpp" #include "sleipnir/autodiff/variable_matrix.hpp"
#include "sleipnir/util/assert.hpp"
#include "sleipnir/util/concepts.hpp" #include "sleipnir/util/concepts.hpp"
#include "sleipnir/util/symbol_exports.hpp" #include "sleipnir/util/symbol_exports.hpp"
@@ -30,7 +31,7 @@ class SLEIPNIR_DLLEXPORT Jacobian {
* @param variable Variable of which to compute the Jacobian. * @param variable Variable of which to compute the Jacobian.
* @param wrt Variable with respect to which to compute the Jacobian. * @param wrt Variable with respect to which to compute the Jacobian.
*/ */
Jacobian(Variable variable, Variable wrt) noexcept Jacobian(Variable variable, Variable wrt)
: Jacobian{VariableMatrix{std::move(variable)}, : Jacobian{VariableMatrix{std::move(variable)},
VariableMatrix{std::move(wrt)}} {} VariableMatrix{std::move(wrt)}} {}
@@ -41,8 +42,11 @@ class SLEIPNIR_DLLEXPORT Jacobian {
* @param wrt Vector of variables with respect to which to compute the * @param wrt Vector of variables with respect to which to compute the
* Jacobian. * Jacobian.
*/ */
Jacobian(VariableMatrix variables, SleipnirMatrixLike auto wrt) noexcept Jacobian(VariableMatrix variables, SleipnirMatrixLike auto wrt)
: m_variables{std::move(variables)}, m_wrt{std::move(wrt)} { : m_variables{std::move(variables)}, m_wrt{std::move(wrt)} {
slp_assert(m_variables.cols() == 1);
slp_assert(m_wrt.cols() == 1);
// Initialize column each expression's adjoint occupies in the Jacobian // Initialize column each expression's adjoint occupies in the Jacobian
for (size_t col = 0; col < m_wrt.size(); ++col) { for (size_t col = 0; col < m_wrt.size(); ++col) {
m_wrt[col].expr->col = col; m_wrt[col].expr->col = col;
@@ -128,13 +132,7 @@ class SLEIPNIR_DLLEXPORT Jacobian {
m_graphs[row].append_adjoint_triplets(triplets, row, m_wrt); m_graphs[row].append_adjoint_triplets(triplets, row, m_wrt);
} }
if (!triplets.empty()) { m_J.setFromTriplets(triplets.begin(), triplets.end());
m_J.setFromTriplets(triplets.begin(), triplets.end());
} else {
// setFromTriplets() is a no-op on empty triplets, so explicitly zero out
// the storage
m_J.setZero();
}
return m_J; return m_J;
} }

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

@@ -87,6 +87,7 @@ class SLEIPNIR_DLLEXPORT Variable {
*/ */
Variable& operator=(double value) { Variable& operator=(double value) {
expr = detail::make_expression_ptr<detail::ConstExpression>(value); expr = detail::make_expression_ptr<detail::ConstExpression>(value);
m_graph_initialized = false;
return *this; return *this;
} }
@@ -97,22 +98,18 @@ class SLEIPNIR_DLLEXPORT Variable {
* @param value The value of the Variable. * @param value The value of the Variable.
*/ */
void set_value(double value) { void set_value(double value) {
if (expr->is_constant(0.0)) {
expr = detail::make_expression_ptr<detail::ConstExpression>(value);
} else {
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
// We only need to check the first argument since unary and binary // We only need to check the first argument since unary and binary operators
// operators both use it // both use it
if (expr->args[0] != nullptr) { if (expr->args[0] != nullptr) {
auto location = std::source_location::current(); auto location = std::source_location::current();
slp::println( slp::println(
stderr, stderr,
"WARNING: {}:{}: {}: Modified the value of a dependent variable", "WARNING: {}:{}: {}: Modified the value of a dependent variable",
location.file_name(), location.line(), location.function_name()); location.file_name(), location.line(), location.function_name());
}
#endif
expr->val = value;
} }
#endif
expr->val = value;
} }
/** /**
@@ -266,6 +263,7 @@ class SLEIPNIR_DLLEXPORT Variable {
friend SLEIPNIR_DLLEXPORT Variable atan(const Variable& x); friend SLEIPNIR_DLLEXPORT Variable atan(const Variable& x);
friend SLEIPNIR_DLLEXPORT Variable atan2(const Variable& y, friend SLEIPNIR_DLLEXPORT Variable atan2(const Variable& y,
const Variable& x); const Variable& x);
friend SLEIPNIR_DLLEXPORT Variable cbrt(const Variable& x);
friend SLEIPNIR_DLLEXPORT Variable cos(const Variable& x); friend SLEIPNIR_DLLEXPORT Variable cos(const Variable& x);
friend SLEIPNIR_DLLEXPORT Variable cosh(const Variable& x); friend SLEIPNIR_DLLEXPORT Variable cosh(const Variable& x);
friend SLEIPNIR_DLLEXPORT Variable erf(const Variable& x); friend SLEIPNIR_DLLEXPORT Variable erf(const Variable& x);
@@ -338,6 +336,15 @@ SLEIPNIR_DLLEXPORT inline Variable atan2(const Variable& y, const Variable& x) {
return Variable{detail::atan2(y.expr, x.expr)}; return Variable{detail::atan2(y.expr, x.expr)};
} }
/**
* std::cbrt() for Variables.
*
* @param x The argument.
*/
SLEIPNIR_DLLEXPORT inline Variable cbrt(const Variable& x) {
return Variable{detail::cbrt(x.expr)};
}
/** /**
* std::cos() for Variables. * std::cos() for Variables.
* *

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

@@ -1149,7 +1149,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
SLEIPNIR_DLLEXPORT inline VariableMatrix cwise_reduce( SLEIPNIR_DLLEXPORT inline VariableMatrix cwise_reduce(
const VariableMatrix& lhs, const VariableMatrix& rhs, const VariableMatrix& lhs, const VariableMatrix& rhs,
function_ref<Variable(const Variable& x, const Variable& y)> binary_op) { function_ref<Variable(const Variable& x, const Variable& y)> binary_op) {
slp_assert(lhs.rows() == rhs.rows() && lhs.rows() == rhs.rows()); slp_assert(lhs.rows() == rhs.rows() && lhs.cols() == rhs.cols());
VariableMatrix result{VariableMatrix::empty, lhs.rows(), lhs.cols()}; VariableMatrix result{VariableMatrix::empty, lhs.rows(), lhs.cols()};

189
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/ocp.hpp → wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp.hpp vendored
View File

@@ -8,6 +8,9 @@
#include <utility> #include <utility>
#include "sleipnir/autodiff/variable_matrix.hpp" #include "sleipnir/autodiff/variable_matrix.hpp"
#include "sleipnir/optimization/ocp/dynamics_type.hpp"
#include "sleipnir/optimization/ocp/timestep_method.hpp"
#include "sleipnir/optimization/ocp/transcription_method.hpp"
#include "sleipnir/optimization/problem.hpp" #include "sleipnir/optimization/problem.hpp"
#include "sleipnir/util/assert.hpp" #include "sleipnir/util/assert.hpp"
#include "sleipnir/util/concepts.hpp" #include "sleipnir/util/concepts.hpp"
@@ -16,64 +19,6 @@
namespace slp { namespace slp {
/**
* Performs 4th order Runge-Kutta integration of dx/dt = f(t, x, u) for dt.
*
* @param f The function to integrate. It must take two arguments x and u.
* @param x The initial value of x.
* @param u The value u held constant over the integration period.
* @param t0 The initial time.
* @param dt The time over which to integrate.
*/
template <typename F, typename State, typename Input, typename Time>
State rk4(F&& f, State x, Input u, Time t0, Time dt) {
auto halfdt = dt * 0.5;
State k1 = f(t0, x, u, dt);
State k2 = f(t0 + halfdt, x + k1 * halfdt, u, dt);
State k3 = f(t0 + halfdt, x + k2 * halfdt, u, dt);
State k4 = f(t0 + dt, x + k3 * dt, u, dt);
return x + (k1 + k2 * 2.0 + k3 * 2.0 + k4) * (dt / 6.0);
}
/**
* Enum describing an OCP transcription method.
*/
enum class TranscriptionMethod : uint8_t {
/// Each state is a decision variable constrained to the integrated dynamics
/// of the previous state.
DIRECT_TRANSCRIPTION,
/// The trajectory is modeled as a series of cubic polynomials where the
/// centerpoint slope is constrained.
DIRECT_COLLOCATION,
/// States depend explicitly as a function of all previous states and all
/// previous inputs.
SINGLE_SHOOTING
};
/**
* Enum describing a type of system dynamics constraints.
*/
enum class DynamicsType : uint8_t {
/// The dynamics are a function in the form dx/dt = f(t, x, u).
EXPLICIT_ODE,
/// The dynamics are a function in the form xₖ₊₁ = f(t, xₖ, uₖ).
DISCRETE
};
/**
* Enum describing the type of system timestep.
*/
enum class TimestepMethod : uint8_t {
/// The timestep is a fixed constant.
FIXED,
/// The timesteps are allowed to vary as independent decision variables.
VARIABLE,
/// The timesteps are equal length but allowed to vary as a single decision
/// variable.
VARIABLE_SINGLE
};
/** /**
* This class allows the user to pose and solve a constrained optimal control * This class allows the user to pose and solve a constrained optimal control
* problem (OCP) in a variety of ways. * problem (OCP) in a variety of ways.
@@ -117,7 +62,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
* - State transition: xₖ₊₁ = f(xₖ, uₖ) * - State transition: xₖ₊₁ = f(xₖ, uₖ)
* @param dynamics_type The type of system evolution function. * @param dynamics_type The type of system evolution function.
* @param timestep_method The timestep method. * @param timestep_method The timestep method.
* @param method The transcription method. * @param transcription_method The transcription method.
*/ */
OCP(int num_states, int num_inputs, std::chrono::duration<double> dt, OCP(int num_states, int num_inputs, std::chrono::duration<double> dt,
int num_steps, int num_steps,
@@ -126,7 +71,8 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
dynamics, dynamics,
DynamicsType dynamics_type = DynamicsType::EXPLICIT_ODE, DynamicsType dynamics_type = DynamicsType::EXPLICIT_ODE,
TimestepMethod timestep_method = TimestepMethod::FIXED, TimestepMethod timestep_method = TimestepMethod::FIXED,
TranscriptionMethod method = TranscriptionMethod::DIRECT_TRANSCRIPTION) TranscriptionMethod transcription_method =
TranscriptionMethod::DIRECT_TRANSCRIPTION)
: OCP{num_states, : OCP{num_states,
num_inputs, num_inputs,
dt, dt,
@@ -139,7 +85,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
}, },
dynamics_type, dynamics_type,
timestep_method, timestep_method,
method} {} transcription_method} {}
/** /**
* Build an optimization problem using a system evolution function (explicit * Build an optimization problem using a system evolution function (explicit
@@ -156,7 +102,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
* - State transition: xₖ₊₁ = f(t, xₖ, uₖ, dt) * - State transition: xₖ₊₁ = f(t, xₖ, uₖ, dt)
* @param dynamics_type The type of system evolution function. * @param dynamics_type The type of system evolution function.
* @param timestep_method The timestep method. * @param timestep_method The timestep method.
* @param method The transcription method. * @param transcription_method The transcription method.
*/ */
OCP(int num_states, int num_inputs, std::chrono::duration<double> dt, OCP(int num_states, int num_inputs, std::chrono::duration<double> dt,
int num_steps, int num_steps,
@@ -165,50 +111,46 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
dynamics, dynamics,
DynamicsType dynamics_type = DynamicsType::EXPLICIT_ODE, DynamicsType dynamics_type = DynamicsType::EXPLICIT_ODE,
TimestepMethod timestep_method = TimestepMethod::FIXED, TimestepMethod timestep_method = TimestepMethod::FIXED,
TranscriptionMethod method = TranscriptionMethod::DIRECT_TRANSCRIPTION) TranscriptionMethod transcription_method =
: m_num_states{num_states}, TranscriptionMethod::DIRECT_TRANSCRIPTION)
m_num_inputs{num_inputs}, : m_num_steps{num_steps},
m_dt{dt}, m_dynamics{std::move(dynamics)},
m_num_steps{num_steps}, m_dynamics_type{dynamics_type} {
m_transcription_method{method},
m_dynamics_type{dynamics_type},
m_dynamics_function{std::move(dynamics)},
m_timestep_method{timestep_method} {
// u is num_steps + 1 so that the final constraint function evaluation works // u is num_steps + 1 so that the final constraint function evaluation works
m_U = decision_variable(m_num_inputs, m_num_steps + 1); m_U = decision_variable(num_inputs, m_num_steps + 1);
if (m_timestep_method == TimestepMethod::FIXED) { if (timestep_method == TimestepMethod::FIXED) {
m_DT = VariableMatrix{1, m_num_steps + 1}; m_DT = VariableMatrix{1, m_num_steps + 1};
for (int i = 0; i < num_steps + 1; ++i) { for (int i = 0; i < num_steps + 1; ++i) {
m_DT(0, i) = m_dt.count(); m_DT(0, i) = dt.count();
} }
} else if (m_timestep_method == TimestepMethod::VARIABLE_SINGLE) { } else if (timestep_method == TimestepMethod::VARIABLE_SINGLE) {
Variable dt = decision_variable(); Variable single_dt = decision_variable();
dt.set_value(m_dt.count()); single_dt.set_value(dt.count());
// Set the member variable matrix to track the decision variable // Set the member variable matrix to track the decision variable
m_DT = VariableMatrix{1, m_num_steps + 1}; m_DT = VariableMatrix{1, m_num_steps + 1};
for (int i = 0; i < num_steps + 1; ++i) { for (int i = 0; i < num_steps + 1; ++i) {
m_DT(0, i) = dt; m_DT(0, i) = single_dt;
} }
} else if (m_timestep_method == TimestepMethod::VARIABLE) { } else if (timestep_method == TimestepMethod::VARIABLE) {
m_DT = decision_variable(1, m_num_steps + 1); m_DT = decision_variable(1, m_num_steps + 1);
for (int i = 0; i < num_steps + 1; ++i) { for (int i = 0; i < num_steps + 1; ++i) {
m_DT(0, i).set_value(m_dt.count()); m_DT(0, i).set_value(dt.count());
} }
} }
if (m_transcription_method == TranscriptionMethod::DIRECT_TRANSCRIPTION) { if (transcription_method == TranscriptionMethod::DIRECT_TRANSCRIPTION) {
m_X = decision_variable(m_num_states, m_num_steps + 1); m_X = decision_variable(num_states, m_num_steps + 1);
constrain_direct_transcription(); constrain_direct_transcription();
} else if (m_transcription_method == } else if (transcription_method ==
TranscriptionMethod::DIRECT_COLLOCATION) { TranscriptionMethod::DIRECT_COLLOCATION) {
m_X = decision_variable(m_num_states, m_num_steps + 1); m_X = decision_variable(num_states, m_num_steps + 1);
constrain_direct_collocation(); constrain_direct_collocation();
} else if (m_transcription_method == TranscriptionMethod::SINGLE_SHOOTING) { } else if (transcription_method == TranscriptionMethod::SINGLE_SHOOTING) {
// In single-shooting the states aren't decision variables, but instead // In single-shooting the states aren't decision variables, but instead
// depend on the input and previous states // depend on the input and previous states
m_X = VariableMatrix{m_num_states, m_num_steps + 1}; m_X = VariableMatrix{num_states, m_num_steps + 1};
constrain_single_shooting(); constrain_single_shooting();
} }
} }
@@ -370,6 +312,40 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
VariableMatrix final_state() { return m_X.col(m_num_steps); } VariableMatrix final_state() { return m_X.col(m_num_steps); }
private: private:
int m_num_steps;
function_ref<VariableMatrix(const Variable& t, const VariableMatrix& x,
const VariableMatrix& u, const Variable& dt)>
m_dynamics;
DynamicsType m_dynamics_type;
VariableMatrix m_X;
VariableMatrix m_U;
VariableMatrix m_DT;
/**
* Performs 4th order Runge-Kutta integration of dx/dt = f(t, x, u) for dt.
*
* @param f The function to integrate. It must take two arguments x and u.
* @param x The initial value of x.
* @param u The value u held constant over the integration period.
* @param t0 The initial time.
* @param dt The time over which to integrate.
*/
template <typename F, typename State, typename Input, typename Time>
State rk4(F&& f, State x, Input u, Time t0, Time dt) {
auto halfdt = dt * 0.5;
State k1 = f(t0, x, u, dt);
State k2 = f(t0 + halfdt, x + k1 * halfdt, u, dt);
State k3 = f(t0 + halfdt, x + k2 * halfdt, u, dt);
State k4 = f(t0 + dt, x + k3 * dt, u, dt);
return x + (k1 + k2 * 2.0 + k3 * 2.0 + k4) * (dt / 6.0);
}
/**
* Apply direct collocation dynamics constraints.
*/
void constrain_direct_collocation() { void constrain_direct_collocation() {
slp_assert(m_dynamics_type == DynamicsType::EXPLICIT_ODE); slp_assert(m_dynamics_type == DynamicsType::EXPLICIT_ODE);
@@ -379,7 +355,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
for (int i = 0; i < m_num_steps; ++i) { for (int i = 0; i < m_num_steps; ++i) {
Variable h = dt()(0, i); Variable h = dt()(0, i);
auto& f = m_dynamics_function; auto& f = m_dynamics;
auto t_begin = time; auto t_begin = time;
auto t_end = t_begin + h; auto t_end = t_begin + h;
@@ -405,6 +381,9 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
} }
} }
/**
* Apply direct transcription dynamics constraints.
*/
void constrain_direct_transcription() { void constrain_direct_transcription() {
Variable time = 0.0; Variable time = 0.0;
@@ -415,17 +394,20 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
Variable dt = this->dt()(0, i); Variable dt = this->dt()(0, i);
if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) { if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) {
subject_to(x_end == rk4<const decltype(m_dynamics_function)&, subject_to(x_end == rk4<const decltype(m_dynamics)&, VariableMatrix,
VariableMatrix, VariableMatrix, Variable>( VariableMatrix, Variable>(m_dynamics, x_begin,
m_dynamics_function, x_begin, u, time, dt)); u, time, dt));
} else if (m_dynamics_type == DynamicsType::DISCRETE) { } else if (m_dynamics_type == DynamicsType::DISCRETE) {
subject_to(x_end == m_dynamics_function(time, x_begin, u, dt)); subject_to(x_end == m_dynamics(time, x_begin, u, dt));
} }
time += dt; time += dt;
} }
} }
/**
* Apply single shooting dynamics constraints.
*/
void constrain_single_shooting() { void constrain_single_shooting() {
Variable time = 0.0; Variable time = 0.0;
@@ -436,34 +418,15 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
Variable dt = this->dt()(0, i); Variable dt = this->dt()(0, i);
if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) { if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) {
x_end = rk4<const decltype(m_dynamics_function)&, VariableMatrix, x_end = rk4<const decltype(m_dynamics)&, VariableMatrix, VariableMatrix,
VariableMatrix, Variable>(m_dynamics_function, x_begin, u, Variable>(m_dynamics, x_begin, u, time, dt);
time, dt);
} else if (m_dynamics_type == DynamicsType::DISCRETE) { } else if (m_dynamics_type == DynamicsType::DISCRETE) {
x_end = m_dynamics_function(time, x_begin, u, dt); x_end = m_dynamics(time, x_begin, u, dt);
} }
time += dt; time += dt;
} }
} }
int m_num_states;
int m_num_inputs;
std::chrono::duration<double> m_dt;
int m_num_steps;
TranscriptionMethod m_transcription_method;
DynamicsType m_dynamics_type;
function_ref<VariableMatrix(const Variable& t, const VariableMatrix& x,
const VariableMatrix& u, const Variable& dt)>
m_dynamics_function;
TimestepMethod m_timestep_method;
VariableMatrix m_X;
VariableMatrix m_U;
VariableMatrix m_DT;
}; };
} // namespace slp } // namespace slp

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

@@ -0,0 +1,19 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <stdint.h>
namespace slp {
/**
* Enum describing a type of system dynamics constraints.
*/
enum class DynamicsType : uint8_t {
/// The dynamics are a function in the form dx/dt = f(t, x, u).
EXPLICIT_ODE,
/// The dynamics are a function in the form xₖ₊₁ = f(t, xₖ, uₖ).
DISCRETE
};
} // namespace slp

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

@@ -0,0 +1,22 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <stdint.h>
namespace slp {
/**
* Enum describing the type of system timestep.
*/
enum class TimestepMethod : uint8_t {
/// The timestep is a fixed constant.
FIXED,
/// The timesteps are allowed to vary as independent decision variables.
VARIABLE,
/// The timesteps are equal length but allowed to vary as a single decision
/// variable.
VARIABLE_SINGLE
};
} // namespace slp

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

@@ -0,0 +1,24 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <stdint.h>
namespace slp {
/**
* Enum describing an OCP transcription method.
*/
enum class TranscriptionMethod : uint8_t {
/// Each state is a decision variable constrained to the integrated dynamics
/// of the previous state.
DIRECT_TRANSCRIPTION,
/// The trajectory is modeled as a series of cubic polynomials where the
/// centerpoint slope is constrained.
DIRECT_COLLOCATION,
/// States depend explicitly as a function of all previous states and all
/// previous inputs.
SINGLE_SHOOTING
};
} // namespace slp

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

@@ -73,7 +73,7 @@ class SLEIPNIR_DLLEXPORT Problem {
VariableMatrix decision_variable(int rows, int cols = 1) { VariableMatrix decision_variable(int rows, int cols = 1) {
m_decision_variables.reserve(m_decision_variables.size() + rows * cols); m_decision_variables.reserve(m_decision_variables.size() + rows * cols);
VariableMatrix vars{rows, cols}; VariableMatrix vars{VariableMatrix::empty, rows, cols};
for (int row = 0; row < rows; ++row) { for (int row = 0; row < rows; ++row) {
for (int col = 0; col < cols; ++col) { for (int col = 0; col < cols; ++col) {
@@ -108,7 +108,7 @@ class SLEIPNIR_DLLEXPORT Problem {
m_decision_variables.reserve(m_decision_variables.size() + m_decision_variables.reserve(m_decision_variables.size() +
(rows * rows + rows) / 2); (rows * rows + rows) / 2);
VariableMatrix vars{rows, rows}; VariableMatrix vars{VariableMatrix::empty, rows, rows};
for (int row = 0; row < rows; ++row) { for (int row = 0; row < rows; ++row) {
for (int col = 0; col <= row; ++col) { for (int col = 0; col <= row; ++col) {
@@ -317,6 +317,24 @@ class SLEIPNIR_DLLEXPORT Problem {
*/ */
void clear_callbacks() { m_iteration_callbacks.clear(); } void clear_callbacks() { m_iteration_callbacks.clear(); }
/**
* Adds a callback to be called at the beginning of each solver iteration.
*
* Language bindings should call this in the Problem constructor to register
* callbacks that shouldn't be removed by clear_callbacks(). Persistent
* callbacks run after non-persistent callbacks.
*
* @param callback The callback. Returning true from the callback causes the
* solver to exit early with the solution it has so far.
*/
template <typename F>
requires requires(F callback, const IterationInfo& info) {
{ callback(info) } -> std::same_as<bool>;
}
void add_persistent_callback(F&& callback) {
m_persistent_iteration_callbacks.emplace_back(std::forward<F>(callback));
}
private: private:
// The list of decision variables, which are the root of the problem's // The list of decision variables, which are the root of the problem's
// expression tree // expression tree
@@ -334,6 +352,8 @@ class SLEIPNIR_DLLEXPORT Problem {
// The iteration callbacks // The iteration callbacks
gch::small_vector<std::function<bool(const IterationInfo& info)>> gch::small_vector<std::function<bool(const IterationInfo& info)>>
m_iteration_callbacks; m_iteration_callbacks;
gch::small_vector<std::function<bool(const IterationInfo& info)>>
m_persistent_iteration_callbacks;
void print_exit_conditions([[maybe_unused]] const Options& options); void print_exit_conditions([[maybe_unused]] const Options& options);
void print_problem_analysis(); void print_problem_analysis();

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

@@ -3,9 +3,10 @@
#pragma once #pragma once
#ifdef JORMUNGANDR #ifdef JORMUNGANDR
#include <format>
#include <source_location> #include <source_location>
#include <stdexcept> #include <stdexcept>
#include <fmt/format.h>
/** /**
* Throw an exception in Python. * Throw an exception in Python.
*/ */
@@ -13,7 +14,7 @@
do { \ do { \
if (!(condition)) { \ if (!(condition)) { \
auto location = std::source_location::current(); \ auto location = std::source_location::current(); \
throw std::invalid_argument(std::format( \ throw std::invalid_argument(fmt::format( \
"{}:{}: {}: Assertion `{}' failed.", location.file_name(), \ "{}:{}: {}: Assertion `{}' failed.", location.file_name(), \
location.line(), location.function_name(), #condition)); \ location.line(), location.function_name(), #condition)); \
} \ } \

1
wpimath/src/main/native/thirdparty/sleipnir/src/.styleguide vendored
View File

@@ -8,5 +8,6 @@ cppSrcFileInclude {
includeOtherLibs { includeOtherLibs {
^Eigen/ ^Eigen/
^fmt/
^gch/ ^gch/
} }

18
wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/variable_matrix.cpp vendored
View File

@@ -24,7 +24,7 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
const auto& c = A(1, 0); const auto& c = A(1, 0);
const auto& d = A(1, 1); const auto& d = A(1, 1);
slp::VariableMatrix adj_A{{d, -b}, {-c, a}}; VariableMatrix adj_A{{d, -b}, {-c, a}};
auto det_A = a * d - b * c; auto det_A = a * d - b * c;
return adj_A / det_A * B; return adj_A / det_A * B;
} else if (A.rows() == 3 && A.cols() == 3) { } else if (A.rows() == 3 && A.cols() == 3) {
@@ -72,9 +72,9 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
auto adj_A10 = fg - di; auto adj_A10 = fg - di;
auto adj_A20 = dh - eg; auto adj_A20 = dh - eg;
slp::VariableMatrix adj_A{{adj_A00, ch - bi, bf - ce}, VariableMatrix adj_A{{adj_A00, ch - bi, bf - ce},
{adj_A10, ai - cg, cd - af}, {adj_A10, ai - cg, cd - af},
{adj_A20, bg - ah, ae - bd}}; {adj_A20, bg - ah, ae - bd}};
auto det_A = a * adj_A00 + b * adj_A10 + c * adj_A20; auto det_A = a * adj_A00 + b * adj_A10 + c * adj_A20;
return adj_A / det_A * B; return adj_A / det_A * B;
} else if (A.rows() == 4 && A.cols() == 4) { } else if (A.rows() == 4 && A.cols() == 4) {
@@ -220,10 +220,10 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
auto adj_A32 = -afo + agn + beo - bgm - cen + cfm; auto adj_A32 = -afo + agn + beo - bgm - cen + cfm;
auto adj_A33 = afk - agj - bek + bgi + cej - cfi; auto adj_A33 = afk - agj - bek + bgi + cej - cfi;
slp::VariableMatrix adj_A{{adj_A00, adj_A01, adj_A02, adj_A03}, VariableMatrix adj_A{{adj_A00, adj_A01, adj_A02, adj_A03},
{adj_A10, adj_A11, adj_A12, adj_A13}, {adj_A10, adj_A11, adj_A12, adj_A13},
{adj_A20, adj_A21, adj_A22, adj_A23}, {adj_A20, adj_A21, adj_A22, adj_A23},
{adj_A30, adj_A31, adj_A32, adj_A33}}; {adj_A30, adj_A31, adj_A32, adj_A33}};
auto det_A = a * adj_A00 + b * adj_A10 + c * adj_A20 + d * adj_A30; auto det_A = a * adj_A00 + b * adj_A10 + c * adj_A20 + d * adj_A30;
return adj_A / det_A * B; return adj_A / det_A * B;
} else { } else {
@@ -245,7 +245,7 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
MatrixXv eigen_X = eigen_A.householderQr().solve(eigen_B); MatrixXv eigen_X = eigen_A.householderQr().solve(eigen_B);
VariableMatrix X{A.cols(), B.cols()}; VariableMatrix X{VariableMatrix::empty, A.cols(), B.cols()};
for (int row = 0; row < X.rows(); ++row) { for (int row = 0; row < X.rows(); ++row) {
for (int col = 0; col < X.cols(); ++col) { for (int col = 0; col < X.cols(); ++col) {
X(row, col) = eigen_X(row, col); X(row, col) = eigen_X(row, col);

32
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/problem.cpp vendored
View File

@@ -9,6 +9,7 @@
#include <Eigen/Core> #include <Eigen/Core>
#include <Eigen/SparseCore> #include <Eigen/SparseCore>
#include <fmt/chrono.h>
#include <gch/small_vector.hpp> #include <gch/small_vector.hpp>
#include "optimization/bounds.hpp" #include "optimization/bounds.hpp"
@@ -79,6 +80,14 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) {
[[maybe_unused]] [[maybe_unused]]
int num_inequality_constraints = m_inequality_constraints.size(); int num_inequality_constraints = m_inequality_constraints.size();
gch::small_vector<std::function<bool(const IterationInfo& info)>> callbacks;
for (const auto& callback : m_iteration_callbacks) {
callbacks.emplace_back(callback);
}
for (const auto& callback : m_persistent_iteration_callbacks) {
callbacks.emplace_back(callback);
}
// Solve the optimization problem // Solve the optimization problem
ExitStatus status; ExitStatus status;
if (m_equality_constraints.empty() && m_inequality_constraints.empty()) { if (m_equality_constraints.empty() && m_inequality_constraints.empty()) {
@@ -101,7 +110,7 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) {
H_spy = std::make_unique<Spy>( H_spy = std::make_unique<Spy>(
"H.spy", "Hessian", "Decision variables", "Decision variables", "H.spy", "Hessian", "Decision variables", "Decision variables",
num_decision_variables, num_decision_variables); num_decision_variables, num_decision_variables);
m_iteration_callbacks.push_back([&](const IterationInfo& info) -> bool { callbacks.push_back([&](const IterationInfo& info) -> bool {
H_spy->add(info.H); H_spy->add(info.H);
return false; return false;
}); });
@@ -123,7 +132,7 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) {
x_ad.set_value(x); x_ad.set_value(x);
return H.value(); return H.value();
}}, }},
m_iteration_callbacks, options, x); callbacks, options, x);
} else if (m_inequality_constraints.empty()) { } else if (m_inequality_constraints.empty()) {
if (options.diagnostics) { if (options.diagnostics) {
slp::println("\nInvoking SQP solver\n"); slp::println("\nInvoking SQP solver\n");
@@ -160,7 +169,7 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) {
"Constraints", "Decision variables", "Constraints", "Decision variables",
num_equality_constraints, num_equality_constraints,
num_decision_variables); num_decision_variables);
m_iteration_callbacks.push_back([&](const IterationInfo& info) -> bool { callbacks.push_back([&](const IterationInfo& info) -> bool {
H_spy->add(info.H); H_spy->add(info.H);
A_e_spy->add(info.A_e); A_e_spy->add(info.A_e);
return false; return false;
@@ -193,7 +202,7 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) {
x_ad.set_value(x); x_ad.set_value(x);
return A_e.value(); return A_e.value();
}}, }},
m_iteration_callbacks, options, x); callbacks, options, x);
} else { } else {
if (options.diagnostics) { if (options.diagnostics) {
slp::println("\nInvoking IPM solver...\n"); slp::println("\nInvoking IPM solver...\n");
@@ -242,7 +251,7 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) {
"A_i.spy", "Inequality constraint Jacobian", "Constraints", "A_i.spy", "Inequality constraint Jacobian", "Constraints",
"Decision variables", num_inequality_constraints, "Decision variables", num_inequality_constraints,
num_decision_variables); num_decision_variables);
m_iteration_callbacks.push_back([&](const IterationInfo& info) -> bool { callbacks.push_back([&](const IterationInfo& info) -> bool {
H_spy->add(info.H); H_spy->add(info.H);
A_e_spy->add(info.A_e); A_e_spy->add(info.A_e);
A_i_spy->add(info.A_i); A_i_spy->add(info.A_i);
@@ -298,19 +307,13 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) {
x_ad.set_value(x); x_ad.set_value(x);
return A_i.value(); return A_i.value();
}}, }},
m_iteration_callbacks, options, callbacks, options,
#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION #ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
bound_constraint_mask, bound_constraint_mask,
#endif #endif
x); x);
} }
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
if (spy) {
m_iteration_callbacks.pop_back();
}
#endif
if (options.diagnostics) { if (options.diagnostics) {
print_autodiff_diagnostics(ad_setup_profilers); print_autodiff_diagnostics(ad_setup_profilers);
slp::println("\nExit: {}", to_message(status)); slp::println("\nExit: {}", to_message(status));
@@ -326,14 +329,15 @@ void Problem::print_exit_conditions([[maybe_unused]] const Options& options) {
// Print possible exit conditions // Print possible exit conditions
slp::println("User-configured exit conditions:"); slp::println("User-configured exit conditions:");
slp::println(" ↳ error below {}", options.tolerance); slp::println(" ↳ error below {}", options.tolerance);
if (!m_iteration_callbacks.empty()) { if (!m_iteration_callbacks.empty() ||
!m_persistent_iteration_callbacks.empty()) {
slp::println(" ↳ iteration callback requested stop"); slp::println(" ↳ iteration callback requested stop");
} }
if (std::isfinite(options.max_iterations)) { if (std::isfinite(options.max_iterations)) {
slp::println(" ↳ executed {} iterations", options.max_iterations); slp::println(" ↳ executed {} iterations", options.max_iterations);
} }
if (std::isfinite(options.timeout.count())) { if (std::isfinite(options.timeout.count())) {
slp::println(" ↳ {} elapsed", options.timeout.count()); slp::println(" ↳ {} elapsed", options.timeout);
} }
} }

3
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/interior_point.cpp vendored
View File

@@ -327,8 +327,7 @@ ExitStatus interior_point(
Eigen::SparseMatrix<double> lhs( Eigen::SparseMatrix<double> lhs(
num_decision_variables + num_equality_constraints, num_decision_variables + num_equality_constraints,
num_decision_variables + num_equality_constraints); num_decision_variables + num_equality_constraints);
lhs.setFromSortedTriplets(triplets.begin(), triplets.end(), lhs.setFromSortedTriplets(triplets.begin(), triplets.end());
[](const auto&, const auto& b) { return b; });
// rhs = −[∇f − Aₑᵀy − Aᵢᵀ(−Σcᵢ + μS⁻¹e + z)] // rhs = −[∇f − Aₑᵀy − Aᵢᵀ(−Σcᵢ + μS⁻¹e + z)]
// [ cₑ ] // [ cₑ ]

1
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/newton.cpp vendored
View File

@@ -2,7 +2,6 @@
#include "sleipnir/optimization/solver/newton.hpp" #include "sleipnir/optimization/solver/newton.hpp"
#include <algorithm>
#include <chrono> #include <chrono>
#include <cmath> #include <cmath>
#include <functional> #include <functional>

5
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/sqp.cpp vendored
View File

@@ -2,7 +2,6 @@
#include "sleipnir/optimization/solver/sqp.hpp" #include "sleipnir/optimization/solver/sqp.hpp"
#include <algorithm>
#include <chrono> #include <chrono>
#include <cmath> #include <cmath>
#include <functional> #include <functional>
@@ -232,8 +231,7 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks,
Eigen::SparseMatrix<double> lhs( Eigen::SparseMatrix<double> lhs(
num_decision_variables + num_equality_constraints, num_decision_variables + num_equality_constraints,
num_decision_variables + num_equality_constraints); num_decision_variables + num_equality_constraints);
lhs.setFromSortedTriplets(triplets.begin(), triplets.end(), lhs.setFromSortedTriplets(triplets.begin(), triplets.end());
[](const auto&, const auto& b) { return b; });
// rhs = −[∇f − Aₑᵀy] // rhs = −[∇f − Aₑᵀy]
// [ cₑ ] // [ cₑ ]
@@ -407,7 +405,6 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks,
trial_x = x + α_max * step.p_x; trial_x = x + α_max * step.p_x;
trial_y = y + α_max * step.p_y; trial_y = y + α_max * step.p_y;
trial_f = matrices.f(trial_x);
trial_c_e = matrices.c_e(trial_x); trial_c_e = matrices.c_e(trial_x);
double next_kkt_error = kkt_error( double next_kkt_error = kkt_error(