diff --git a/upstream_utils/sleipnir.py b/upstream_utils/sleipnir.py index c1b0de9a87..8600d0be13 100755 --- a/upstream_utils/sleipnir.py +++ b/upstream_utils/sleipnir.py @@ -52,8 +52,8 @@ using small_vector = wpi::SmallVector; def main(): name = "sleipnir" url = "https://github.com/SleipnirGroup/Sleipnir" - # main on 2025-05-18 - tag = "2cc18ff6d25ee0a9bd0f9993a0a41a61a28bda3e" + # main on 2025-09-19 + tag = "7f89d5547702a09e3617bc31fe5bafe6add04fab" sleipnir = Lib(name, url, tag, copy_upstream_src) sleipnir.main() diff --git a/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch b/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch index 2baae9b2ab..3677503197 100644 --- a/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch +++ b/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch @@ -4,10 +4,12 @@ Date: Wed, 29 May 2024 16:29:55 -0700 Subject: [PATCH 1/8] Use fmtlib --- - include/.styleguide | 1 + - include/sleipnir/util/print.hpp | 31 ++++++++++++++++++------------- - src/optimization/problem.cpp | 2 +- - 3 files changed, 20 insertions(+), 14 deletions(-) + include/.styleguide | 1 + + include/sleipnir/util/assert.hpp | 5 +++-- + include/sleipnir/util/print.hpp | 31 ++++++++++++++++++------------- + src/.styleguide | 1 + + src/optimization/problem.cpp | 1 + + 5 files changed, 24 insertions(+), 15 deletions(-) diff --git a/include/.styleguide b/include/.styleguide index 1b6652d3d5886cf8c9eca0d855c21031775bad7c..4f4c76204071f90bf49eddb8c2aceb583b5e09ba 100644 @@ -20,6 +22,31 @@ index 1b6652d3d5886cf8c9eca0d855c21031775bad7c..4f4c76204071f90bf49eddb8c2aceb58 + ^fmt/ ^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 + #include + #include ++ ++#include + /** + * 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 index fe430352dabf4cd6a890dc8007237c7a261dfd4b..055d5c9fa246201f1d8ae7ddca00b1159aeb2a57 100644 --- a/include/sleipnir/util/print.hpp @@ -99,16 +126,26 @@ index fe430352dabf4cd6a890dc8007237c7a261dfd4b..055d5c9fa246201f1d8ae7ddca00b115 } 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 -index 31115490867146ec166604bcc61731d7891a9f22..81863808d329a53d4162ce0624a3b8e8afc32dfc 100644 +index c3331197e2365934273f57422b79fa18c2b78a5b..09828cdb6d7cddff692b9d17603dc0c11cd5a3ec 100644 --- a/src/optimization/problem.cpp +++ b/src/optimization/problem.cpp -@@ -335,7 +335,7 @@ void Problem::print_exit_conditions([[maybe_unused]] const Options& options) { - slp::println(" ↳ executed {} iterations", options.max_iterations); - } - if (std::isfinite(options.timeout.count())) { -- slp::println(" ↳ {} elapsed", options.timeout); -+ slp::println(" ↳ {} elapsed", options.timeout.count()); - } - } +@@ -11,6 +11,7 @@ + #include + #include ++#include + #include + + #include "optimization/bounds.hpp" diff --git a/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch b/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch index df21b64a10..63dde99b18 100644 --- a/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch +++ b/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch @@ -10,7 +10,7 @@ Subject: [PATCH 2/8] Use wpi::SmallVector 3 files changed, 6 insertions(+), 7 deletions(-) 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 +++ b/include/sleipnir/autodiff/expression.hpp @@ -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. -@@ -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. */ @@ -32,7 +32,7 @@ index 873e1c27559d92eb1b3a217890ca41bdc65af122..1c5f84d22a0bed70869121acabd52782 // 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 14eb1d3b95069e143699e1488f3081c4cd9de07c..9f79a82763213dc712cce4c2a322289d57645032 100644 +index f60236811eba45c67a9638e90d5101d877ecc2d0..264f0950f293c67d6e6c7e729887090c050e40e2 100644 --- a/include/sleipnir/autodiff/variable.hpp +++ b/include/sleipnir/autodiff/variable.hpp @@ -47,7 +47,7 @@ class SLEIPNIR_DLLEXPORT Variable { @@ -55,7 +55,7 @@ index 14eb1d3b95069e143699e1488f3081c4cd9de07c..9f79a82763213dc712cce4c2a322289d /** * Assignment operator for double. 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 +++ b/include/sleipnir/autodiff/variable_matrix.hpp @@ -1120,14 +1120,14 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { diff --git a/upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch b/upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch index 1b5f998cf8..f4d84bae89 100644 --- a/upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch +++ b/upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch @@ -9,7 +9,7 @@ Subject: [PATCH 4/8] Replace std::to_underlying() 2 files changed, 7 insertions(+), 8 deletions(-) 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 +++ b/src/optimization/problem.cpp @@ -7,7 +7,6 @@ @@ -20,7 +20,7 @@ index 81863808d329a53d4162ce0624a3b8e8afc32dfc..c3319fc0a927cf452871a2db08d5edff #include #include -@@ -346,11 +345,11 @@ void Problem::print_problem_analysis() { +@@ -350,11 +349,11 @@ void Problem::print_problem_analysis() { // Print problem structure slp::println("\nProblem structure:"); slp::println(" ↳ {} cost function", @@ -35,7 +35,7 @@ index 81863808d329a53d4162ce0624a3b8e8afc32dfc..c3319fc0a927cf452871a2db08d5edff if (m_decision_variables.size() == 1) { 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& constraints) { std::array counts{}; for (const auto& constraint : constraints) { diff --git a/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch b/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch index 2885d850de..f1fd7a7c59 100644 --- a/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch +++ b/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch @@ -9,10 +9,10 @@ Subject: [PATCH 5/8] Replace std::views::zip() 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 -index 4b4f3303faed766d3ac39829870514f50d9a582f..4576e19c9695caf4407fbbb592afe32d8252a0db 100644 +index 33b6eee615141a1d6472f116842d62052ef54dd9..b333aebd3e59fa23eed6046c13d736c3d2eccac7 100644 --- a/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 { @@ -25,7 +25,7 @@ index 4b4f3303faed766d3ac39829870514f50d9a582f..4576e19c9695caf4407fbbb592afe32d if (col != -1 && node->adjoint != 0.0) { triplets.emplace_back(row, col, node->adjoint); 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 +++ b/src/optimization/problem.cpp @@ -6,7 +6,6 @@ @@ -36,7 +36,7 @@ index c3319fc0a927cf452871a2db08d5edff87ac8eea..5532b3962409e2140132e79241da4fba #include #include -@@ -363,9 +362,11 @@ void Problem::print_problem_analysis() { +@@ -367,9 +366,11 @@ void Problem::print_problem_analysis() { for (const auto& constraint : constraints) { ++counts[static_cast(constraint.type())]; } diff --git a/upstream_utils/sleipnir_patches/0006-Suppress-clang-tidy-false-positives.patch b/upstream_utils/sleipnir_patches/0006-Suppress-clang-tidy-false-positives.patch index c71e612919..109126b697 100644 --- a/upstream_utils/sleipnir_patches/0006-Suppress-clang-tidy-false-positives.patch +++ b/upstream_utils/sleipnir_patches/0006-Suppress-clang-tidy-false-positives.patch @@ -8,10 +8,10 @@ Subject: [PATCH 6/8] Suppress clang-tidy false positives 1 file changed, 1 insertion(+), 1 deletion(-) 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 +++ 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 * concatenate. */ diff --git a/upstream_utils/sleipnir_patches/0007-Suppress-GCC-12-warning-false-positive.patch b/upstream_utils/sleipnir_patches/0007-Suppress-GCC-12-warning-false-positive.patch index a4841664ed..aed8ccc605 100644 --- a/upstream_utils/sleipnir_patches/0007-Suppress-GCC-12-warning-false-positive.patch +++ b/upstream_utils/sleipnir_patches/0007-Suppress-GCC-12-warning-false-positive.patch @@ -8,7 +8,7 @@ Subject: [PATCH 7/8] Suppress GCC 12 warning false positive 1 file changed, 7 insertions(+) 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 +++ b/include/sleipnir/autodiff/variable_matrix.hpp @@ -573,6 +573,10 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { diff --git a/upstream_utils/sleipnir_patches/0008-Revert-Use-multidimensional-array-subscript-operator.patch b/upstream_utils/sleipnir_patches/0008-Revert-Use-multidimensional-array-subscript-operator.patch index d31ba4bcc7..b04d79f5bc 100644 --- a/upstream_utils/sleipnir_patches/0008-Revert-Use-multidimensional-array-subscript-operator.patch +++ b/upstream_utils/sleipnir_patches/0008-Revert-Use-multidimensional-array-subscript-operator.patch @@ -11,16 +11,16 @@ This reverts commit f9b2c450bbbf6f14b194b8b81708d032a6431ee0. include/sleipnir/autodiff/variable.hpp | 26 +---- include/sleipnir/autodiff/variable_block.hpp | 70 +++++------ 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 +- src/autodiff/variable_matrix.cpp | 66 +++++------ 8 files changed, 118 insertions(+), 182 deletions(-) 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 +++ 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); for (int col = 0; col < m_wrt.rows(); ++col) { 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 -index 787fca8ccd3fd6e46c5d31ab980704e6a5e99402..7e7e1340d065d35412f43b27fac7d8a719b7e5b5 100644 +index 4515076cde12a2112e1b5711acc3092bd807e250..3662b5e49b93f63b5ccac0e732149bd9178f1aae 100644 --- a/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); for (int col = 0; col < m_wrt.rows(); ++col) { 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 -index 17e7eb7cc2c7c7599eaba97d8ec80972524c1599..03b929c778c03186cc5b461a2e855da23034457a 100644 +index 62135a5539308ae69f6b45a64d9337c4c3e96d7b..2fc2119d2dedaa5b4c941ce449b7fb113c641635 100644 --- a/include/sleipnir/autodiff/variable.hpp +++ b/include/sleipnir/autodiff/variable.hpp -@@ -505,11 +505,7 @@ gch::small_vector make_constraints(LHS&& lhs, RHS&& rhs) { +@@ -512,11 +512,7 @@ gch::small_vector 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 @@ -65,7 +65,7 @@ index 17e7eb7cc2c7c7599eaba97d8ec80972524c1599..03b929c778c03186cc5b461a2e855da2 } } } else if constexpr (MatrixLike && ScalarLike) { -@@ -518,11 +514,7 @@ gch::small_vector make_constraints(LHS&& lhs, RHS&& rhs) { +@@ -525,11 +521,7 @@ gch::small_vector 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 @@ -78,7 +78,7 @@ index 17e7eb7cc2c7c7599eaba97d8ec80972524c1599..03b929c778c03186cc5b461a2e855da2 } } } else if constexpr (MatrixLike && MatrixLike) { -@@ -532,19 +524,7 @@ gch::small_vector make_constraints(LHS&& lhs, RHS&& rhs) { +@@ -539,19 +531,7 @@ gch::small_vector 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 @@ -376,7 +376,7 @@ index f1c1ca0dc3fde663c3e74f6fca4b89b119cf377d..632d44beb5b3dae29b9829c52a6168fe } 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 +++ b/include/sleipnir/autodiff/variable_matrix.hpp @@ -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 -index 282520fb852d8588b96846eb5b4952bf47d1309f..d9174426669281e68a5c09d298cfd5bcd3be3776 100644 ---- a/include/sleipnir/control/ocp.hpp -+++ b/include/sleipnir/control/ocp.hpp -@@ -180,7 +180,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { - if (m_timestep_method == TimestepMethod::FIXED) { +diff --git a/include/sleipnir/optimization/ocp.hpp b/include/sleipnir/optimization/ocp.hpp +index 124224cf5ba6e54c141086e3a21389530198449f..74492a0d756a9d587df6158c7e2ef8548ae22be4 100644 +--- a/include/sleipnir/optimization/ocp.hpp ++++ b/include/sleipnir/optimization/ocp.hpp +@@ -122,7 +122,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { + if (timestep_method == TimestepMethod::FIXED) { m_DT = VariableMatrix{1, m_num_steps + 1}; for (int i = 0; i < num_steps + 1; ++i) { -- m_DT[0, i] = m_dt.count(); -+ m_DT(0, i) = m_dt.count(); +- m_DT[0, i] = dt.count(); ++ m_DT(0, i) = dt.count(); } - } else if (m_timestep_method == TimestepMethod::VARIABLE_SINGLE) { - Variable dt = decision_variable(); -@@ -189,12 +189,12 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { + } else if (timestep_method == TimestepMethod::VARIABLE_SINGLE) { + Variable single_dt = decision_variable(); +@@ -131,12 +131,12 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { // Set the member variable matrix to track the decision variable m_DT = VariableMatrix{1, m_num_steps + 1}; for (int i = 0; i < num_steps + 1; ++i) { -- m_DT[0, i] = dt; -+ m_DT(0, i) = dt; +- m_DT[0, i] = single_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); for (int i = 0; i < num_steps + 1; ++i) { -- m_DT[0, i].set_value(m_dt.count()); -+ m_DT(0, i).set_value(m_dt.count()); +- m_DT[0, i].set_value(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) { auto x = X().col(i); auto u = U().col(i); @@ -745,16 +745,16 @@ index 282520fb852d8588b96846eb5b4952bf47d1309f..d9174426669281e68a5c09d298cfd5bc callback(time, x, u, 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/ 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; -@@ -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_end = X().col(i + 1); auto u = U().col(i); @@ -762,8 +762,8 @@ index 282520fb852d8588b96846eb5b4952bf47d1309f..d9174426669281e68a5c09d298cfd5bc + Variable dt = this->dt()(0, i); if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) { - subject_to(x_end == rk4dt()(0, i); if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) { - x_end = rk4(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(std::cbrt(x->val)); + } + } + + return make_expression_ptr(x); +} + /** * 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. * diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_graph.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_graph.hpp index a1d9c33539..1e32cb07c2 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_graph.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_graph.hpp @@ -21,8 +21,7 @@ inline gch::small_vector topological_sort( const ExpressionPtr& root) { gch::small_vector list; - // If the root type is a constant, Update() is a no-op, so there's no work - // to do + // If the root type is constant, updates are a no-op, so return an empty list if (root == nullptr || root->type() == ExpressionType::CONSTANT) { return list; } diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient.hpp index 728259d3f5..e9944652d8 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient.hpp @@ -15,8 +15,8 @@ namespace slp { /** - * This class calculates the gradient of a a variable with respect to a vector - * of variables. + * This class calculates the gradient of a variable with respect to a vector of + * variables. * * The gradient is only recomputed if the variable expression is quadratic or * higher order. @@ -29,7 +29,7 @@ class SLEIPNIR_DLLEXPORT Gradient { * @param variable Variable of which to compute the gradient. * @param wrt Variable with respect to which to compute the gradient. */ - Gradient(Variable variable, Variable wrt) noexcept + Gradient(Variable variable, Variable 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 * gradient. */ - Gradient(Variable variable, SleipnirMatrixLike auto wrt) noexcept + Gradient(Variable variable, SleipnirMatrixLike auto 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. */ const Eigen::SparseVector& value() { - m_g = m_jacobian.value(); + m_g = m_jacobian.value().transpose(); return m_g; } diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/hessian.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/hessian.hpp index 8b048ab3ba..4f093b7b39 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/hessian.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/hessian.hpp @@ -10,6 +10,7 @@ #include "sleipnir/autodiff/adjoint_expression_graph.hpp" #include "sleipnir/autodiff/variable.hpp" #include "sleipnir/autodiff/variable_matrix.hpp" +#include "sleipnir/util/assert.hpp" #include "sleipnir/util/concepts.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 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)}} {} /** @@ -44,10 +45,12 @@ class SLEIPNIR_DLLEXPORT Hessian { * @param wrt Vector of variables with respect to which to compute the * Hessian. */ - Hessian(Variable variable, SleipnirMatrixLike auto wrt) noexcept + Hessian(Variable variable, SleipnirMatrixLike auto wrt) : m_variables{detail::AdjointExpressionGraph{variable} .generate_gradient_tree(wrt)}, m_wrt{wrt} { + slp_assert(m_wrt.cols() == 1); + // Initialize column each expression's adjoint occupies in the Jacobian for (size_t col = 0; col < m_wrt.size(); ++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); } - if (!triplets.empty()) { - m_H.setFromTriplets(triplets.begin(), triplets.end()); - if constexpr (UpLo == Eigen::Lower) { - m_H = m_H.triangularView(); - } - } else { - // setFromTriplets() is a no-op on empty triplets, so explicitly zero out - // the storage - m_H.setZero(); + m_H.setFromTriplets(triplets.begin(), triplets.end()); + if constexpr (UpLo == Eigen::Lower) { + m_H = m_H.triangularView(); } return m_H; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/jacobian.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/jacobian.hpp index 7e7e1340d0..3662b5e49b 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/jacobian.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/jacobian.hpp @@ -10,6 +10,7 @@ #include "sleipnir/autodiff/adjoint_expression_graph.hpp" #include "sleipnir/autodiff/variable.hpp" #include "sleipnir/autodiff/variable_matrix.hpp" +#include "sleipnir/util/assert.hpp" #include "sleipnir/util/concepts.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 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)}, VariableMatrix{std::move(wrt)}} {} @@ -41,8 +42,11 @@ class SLEIPNIR_DLLEXPORT Jacobian { * @param wrt Vector of variables with respect to which to compute the * Jacobian. */ - Jacobian(VariableMatrix variables, SleipnirMatrixLike auto wrt) noexcept + Jacobian(VariableMatrix variables, SleipnirMatrixLike auto 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 for (size_t col = 0; col < m_wrt.size(); ++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); } - if (!triplets.empty()) { - 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(); - } + m_J.setFromTriplets(triplets.begin(), triplets.end()); return m_J; } diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable.hpp index 03b929c778..2fc2119d2d 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable.hpp @@ -87,6 +87,7 @@ class SLEIPNIR_DLLEXPORT Variable { */ Variable& operator=(double value) { expr = detail::make_expression_ptr(value); + m_graph_initialized = false; return *this; } @@ -97,22 +98,18 @@ class SLEIPNIR_DLLEXPORT Variable { * @param value The value of the Variable. */ void set_value(double value) { - if (expr->is_constant(0.0)) { - expr = detail::make_expression_ptr(value); - } else { #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS - // We only need to check the first argument since unary and binary - // operators both use it - if (expr->args[0] != nullptr) { - auto location = std::source_location::current(); - slp::println( - stderr, - "WARNING: {}:{}: {}: Modified the value of a dependent variable", - location.file_name(), location.line(), location.function_name()); - } -#endif - expr->val = value; + // We only need to check the first argument since unary and binary operators + // both use it + if (expr->args[0] != nullptr) { + auto location = std::source_location::current(); + slp::println( + stderr, + "WARNING: {}:{}: {}: Modified the value of a dependent variable", + location.file_name(), location.line(), location.function_name()); } +#endif + expr->val = value; } /** @@ -266,6 +263,7 @@ class SLEIPNIR_DLLEXPORT Variable { friend SLEIPNIR_DLLEXPORT Variable atan(const Variable& x); friend SLEIPNIR_DLLEXPORT Variable atan2(const Variable& y, const Variable& x); + friend SLEIPNIR_DLLEXPORT Variable cbrt(const Variable& x); friend SLEIPNIR_DLLEXPORT Variable cos(const Variable& x); friend SLEIPNIR_DLLEXPORT Variable cosh(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)}; } +/** + * 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. * diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_matrix.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_matrix.hpp index 4dc2cea00c..2ed997819e 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_matrix.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_matrix.hpp @@ -1149,7 +1149,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { SLEIPNIR_DLLEXPORT inline VariableMatrix cwise_reduce( const VariableMatrix& lhs, const VariableMatrix& rhs, function_ref 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()}; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/ocp.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp.hpp similarity index 74% rename from wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/ocp.hpp rename to wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp.hpp index d917442666..74492a0d75 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/ocp.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp.hpp @@ -8,6 +8,9 @@ #include #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/util/assert.hpp" #include "sleipnir/util/concepts.hpp" @@ -16,64 +19,6 @@ 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 -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 * problem (OCP) in a variety of ways. @@ -117,7 +62,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { * - State transition: xₖ₊₁ = f(xₖ, uₖ) * @param dynamics_type The type of system evolution function. * @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 dt, int num_steps, @@ -126,7 +71,8 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { dynamics, DynamicsType dynamics_type = DynamicsType::EXPLICIT_ODE, TimestepMethod timestep_method = TimestepMethod::FIXED, - TranscriptionMethod method = TranscriptionMethod::DIRECT_TRANSCRIPTION) + TranscriptionMethod transcription_method = + TranscriptionMethod::DIRECT_TRANSCRIPTION) : OCP{num_states, num_inputs, dt, @@ -139,7 +85,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { }, dynamics_type, timestep_method, - method} {} + transcription_method} {} /** * 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) * @param dynamics_type The type of system evolution function. * @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 dt, int num_steps, @@ -165,50 +111,46 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { dynamics, DynamicsType dynamics_type = DynamicsType::EXPLICIT_ODE, TimestepMethod timestep_method = TimestepMethod::FIXED, - TranscriptionMethod method = TranscriptionMethod::DIRECT_TRANSCRIPTION) - : m_num_states{num_states}, - m_num_inputs{num_inputs}, - m_dt{dt}, - m_num_steps{num_steps}, - m_transcription_method{method}, - m_dynamics_type{dynamics_type}, - m_dynamics_function{std::move(dynamics)}, - m_timestep_method{timestep_method} { + TranscriptionMethod transcription_method = + TranscriptionMethod::DIRECT_TRANSCRIPTION) + : m_num_steps{num_steps}, + m_dynamics{std::move(dynamics)}, + m_dynamics_type{dynamics_type} { // 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}; 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) { - Variable dt = decision_variable(); - dt.set_value(m_dt.count()); + } else if (timestep_method == TimestepMethod::VARIABLE_SINGLE) { + Variable single_dt = decision_variable(); + single_dt.set_value(dt.count()); // Set the member variable matrix to track the decision variable m_DT = VariableMatrix{1, m_num_steps + 1}; for (int i = 0; i < num_steps + 1; ++i) { - m_DT(0, i) = 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); 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) { - m_X = decision_variable(m_num_states, m_num_steps + 1); + if (transcription_method == TranscriptionMethod::DIRECT_TRANSCRIPTION) { + m_X = decision_variable(num_states, m_num_steps + 1); constrain_direct_transcription(); - } else if (m_transcription_method == + } else if (transcription_method == 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(); - } 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 // 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(); } } @@ -370,6 +312,40 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { VariableMatrix final_state() { return m_X.col(m_num_steps); } private: + int m_num_steps; + + function_ref + 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 + 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() { 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) { Variable h = dt()(0, i); - auto& f = m_dynamics_function; + auto& f = m_dynamics; auto t_begin = time; 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() { Variable time = 0.0; @@ -415,17 +394,20 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { Variable dt = this->dt()(0, i); if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) { - subject_to(x_end == rk4( - m_dynamics_function, x_begin, u, time, dt)); + subject_to(x_end == rk4(m_dynamics, x_begin, + u, time, dt)); } 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; } } + /** + * Apply single shooting dynamics constraints. + */ void constrain_single_shooting() { Variable time = 0.0; @@ -436,34 +418,15 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem { Variable dt = this->dt()(0, i); if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) { - x_end = rk4(m_dynamics_function, x_begin, u, - time, dt); + x_end = rk4(m_dynamics, x_begin, u, time, dt); } 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; } } - - int m_num_states; - int m_num_inputs; - std::chrono::duration m_dt; - int m_num_steps; - TranscriptionMethod m_transcription_method; - - DynamicsType m_dynamics_type; - - function_ref - m_dynamics_function; - - TimestepMethod m_timestep_method; - - VariableMatrix m_X; - VariableMatrix m_U; - VariableMatrix m_DT; }; } // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/dynamics_type.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/dynamics_type.hpp new file mode 100644 index 0000000000..835a12eef8 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/dynamics_type.hpp @@ -0,0 +1,19 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +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 diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/timestep_method.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/timestep_method.hpp new file mode 100644 index 0000000000..ef1cf88a08 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/timestep_method.hpp @@ -0,0 +1,22 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +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 diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/transcription_method.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/transcription_method.hpp new file mode 100644 index 0000000000..e0513eef89 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/transcription_method.hpp @@ -0,0 +1,24 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +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 diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp index b484ec08d6..c996b37231 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp @@ -73,7 +73,7 @@ class SLEIPNIR_DLLEXPORT Problem { VariableMatrix decision_variable(int rows, int cols = 1) { 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 col = 0; col < cols; ++col) { @@ -108,7 +108,7 @@ class SLEIPNIR_DLLEXPORT Problem { m_decision_variables.reserve(m_decision_variables.size() + (rows * rows + rows) / 2); - VariableMatrix vars{rows, rows}; + VariableMatrix vars{VariableMatrix::empty, rows, rows}; for (int row = 0; row < rows; ++row) { for (int col = 0; col <= row; ++col) { @@ -317,6 +317,24 @@ class SLEIPNIR_DLLEXPORT Problem { */ 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 + requires requires(F callback, const IterationInfo& info) { + { callback(info) } -> std::same_as; + } + void add_persistent_callback(F&& callback) { + m_persistent_iteration_callbacks.emplace_back(std::forward(callback)); + } + private: // The list of decision variables, which are the root of the problem's // expression tree @@ -334,6 +352,8 @@ class SLEIPNIR_DLLEXPORT Problem { // The iteration callbacks gch::small_vector> m_iteration_callbacks; + gch::small_vector> + m_persistent_iteration_callbacks; void print_exit_conditions([[maybe_unused]] const Options& options); void print_problem_analysis(); diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/assert.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/assert.hpp index 75d8ffca32..53de01928b 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/assert.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/assert.hpp @@ -3,9 +3,10 @@ #pragma once #ifdef JORMUNGANDR -#include #include #include + +#include /** * 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/wpimath/src/main/native/thirdparty/sleipnir/src/.styleguide b/wpimath/src/main/native/thirdparty/sleipnir/src/.styleguide index 1b6652d3d5..4f4c762040 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/.styleguide +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/.styleguide @@ -8,5 +8,6 @@ cppSrcFileInclude { includeOtherLibs { ^Eigen/ + ^fmt/ ^gch/ } diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/variable_matrix.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/variable_matrix.cpp index 71f8153d34..d9619a39d5 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/variable_matrix.cpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/variable_matrix.cpp @@ -24,7 +24,7 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) { const auto& c = A(1, 0); 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; return adj_A / det_A * B; } 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_A20 = dh - eg; - slp::VariableMatrix adj_A{{adj_A00, ch - bi, bf - ce}, - {adj_A10, ai - cg, cd - af}, - {adj_A20, bg - ah, ae - bd}}; + VariableMatrix adj_A{{adj_A00, ch - bi, bf - ce}, + {adj_A10, ai - cg, cd - af}, + {adj_A20, bg - ah, ae - bd}}; auto det_A = a * adj_A00 + b * adj_A10 + c * adj_A20; return adj_A / det_A * B; } 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_A33 = afk - agj - bek + bgi + cej - cfi; - slp::VariableMatrix adj_A{{adj_A00, adj_A01, adj_A02, adj_A03}, - {adj_A10, adj_A11, adj_A12, adj_A13}, - {adj_A20, adj_A21, adj_A22, adj_A23}, - {adj_A30, adj_A31, adj_A32, adj_A33}}; + VariableMatrix adj_A{{adj_A00, adj_A01, adj_A02, adj_A03}, + {adj_A10, adj_A11, adj_A12, adj_A13}, + {adj_A20, adj_A21, adj_A22, adj_A23}, + {adj_A30, adj_A31, adj_A32, adj_A33}}; auto det_A = a * adj_A00 + b * adj_A10 + c * adj_A20 + d * adj_A30; return adj_A / det_A * B; } else { @@ -245,7 +245,7 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& 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 col = 0; col < X.cols(); ++col) { X(row, col) = eigen_X(row, col); diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/problem.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/problem.cpp index 5532b39624..e32481e931 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/problem.cpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/problem.cpp @@ -9,6 +9,7 @@ #include #include +#include #include #include "optimization/bounds.hpp" @@ -79,6 +80,14 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) { [[maybe_unused]] int num_inequality_constraints = m_inequality_constraints.size(); + gch::small_vector> 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 ExitStatus status; 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( "H.spy", "Hessian", "Decision variables", "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); return false; }); @@ -123,7 +132,7 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) { x_ad.set_value(x); return H.value(); }}, - m_iteration_callbacks, options, x); + callbacks, options, x); } else if (m_inequality_constraints.empty()) { if (options.diagnostics) { slp::println("\nInvoking SQP solver\n"); @@ -160,7 +169,7 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) { "Constraints", "Decision variables", num_equality_constraints, num_decision_variables); - m_iteration_callbacks.push_back([&](const IterationInfo& info) -> bool { + callbacks.push_back([&](const IterationInfo& info) -> bool { H_spy->add(info.H); A_e_spy->add(info.A_e); return false; @@ -193,7 +202,7 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) { x_ad.set_value(x); return A_e.value(); }}, - m_iteration_callbacks, options, x); + callbacks, options, x); } else { if (options.diagnostics) { 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", "Decision variables", num_inequality_constraints, num_decision_variables); - m_iteration_callbacks.push_back([&](const IterationInfo& info) -> bool { + callbacks.push_back([&](const IterationInfo& info) -> bool { H_spy->add(info.H); A_e_spy->add(info.A_e); 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); return A_i.value(); }}, - m_iteration_callbacks, options, + callbacks, options, #ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION bound_constraint_mask, #endif x); } -#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS - if (spy) { - m_iteration_callbacks.pop_back(); - } -#endif - if (options.diagnostics) { print_autodiff_diagnostics(ad_setup_profilers); slp::println("\nExit: {}", to_message(status)); @@ -326,14 +329,15 @@ void Problem::print_exit_conditions([[maybe_unused]] const Options& options) { // Print possible exit conditions slp::println("User-configured exit conditions:"); 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"); } if (std::isfinite(options.max_iterations)) { slp::println(" ↳ executed {} iterations", options.max_iterations); } if (std::isfinite(options.timeout.count())) { - slp::println(" ↳ {} elapsed", options.timeout.count()); + slp::println(" ↳ {} elapsed", options.timeout); } } diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/interior_point.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/interior_point.cpp index b1421bf961..f85b765875 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/interior_point.cpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/interior_point.cpp @@ -327,8 +327,7 @@ ExitStatus interior_point( Eigen::SparseMatrix lhs( num_decision_variables + num_equality_constraints, num_decision_variables + num_equality_constraints); - lhs.setFromSortedTriplets(triplets.begin(), triplets.end(), - [](const auto&, const auto& b) { return b; }); + lhs.setFromSortedTriplets(triplets.begin(), triplets.end()); // rhs = −[∇f − Aₑᵀy − Aᵢᵀ(−Σcᵢ + μS⁻¹e + z)] // [ cₑ ] diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/newton.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/newton.cpp index 98ed6c941f..535ca1f57b 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/newton.cpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/newton.cpp @@ -2,7 +2,6 @@ #include "sleipnir/optimization/solver/newton.hpp" -#include #include #include #include diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/sqp.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/sqp.cpp index 04447f8892..3b90ca7e9d 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/sqp.cpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/sqp.cpp @@ -2,7 +2,6 @@ #include "sleipnir/optimization/solver/sqp.hpp" -#include #include #include #include @@ -232,8 +231,7 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, Eigen::SparseMatrix lhs( num_decision_variables + num_equality_constraints, num_decision_variables + num_equality_constraints); - lhs.setFromSortedTriplets(triplets.begin(), triplets.end(), - [](const auto&, const auto& b) { return b; }); + lhs.setFromSortedTriplets(triplets.begin(), triplets.end()); // rhs = −[∇f − Aₑᵀy] // [ cₑ ] @@ -407,7 +405,6 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, trial_x = x + α_max * step.p_x; trial_y = y + α_max * step.p_y; - trial_f = matrices.f(trial_x); trial_c_e = matrices.c_e(trial_x); double next_kkt_error = kkt_error(