From 79dfdb9dc5e54d4f3e02fb222106c292e85f3859 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 4 Aug 2024 06:26:05 -0700 Subject: [PATCH] [upstream_utils] Upgrade Sleipnir to support Eigen type specializations (#6924) --- upstream_utils/sleipnir.py | 4 +- .../0001-Remove-using-enum-declarations.patch | 90 ++--- .../0003-Remove-unsupported-constexpr.patch | 8 +- .../0004-Use-wpi-SmallVector.patch | 93 ++--- ...-Suppress-clang-tidy-false-positives.patch | 14 +- .../native/thirdparty/sleipnir/.styleguide | 2 +- .../include/sleipnir/autodiff/Slice.hpp | 158 ++++++++ .../include/sleipnir/autodiff/Variable.hpp | 339 ++++++++++++++++++ .../sleipnir/autodiff/VariableBlock.hpp | 202 +++++++++-- .../sleipnir/autodiff/VariableMatrix.hpp | 72 +++- .../include/sleipnir/control/OCPSolver.hpp | 18 +- .../sleipnir/optimization/Constraints.hpp | 325 ----------------- .../optimization/OptimizationProblem.hpp | 1 - .../include/sleipnir/util/Concepts.hpp | 25 +- .../sleipnir/src/autodiff/VariableMatrix.cpp | 24 -- 15 files changed, 864 insertions(+), 511 deletions(-) create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Slice.hpp delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Constraints.hpp diff --git a/upstream_utils/sleipnir.py b/upstream_utils/sleipnir.py index 2e2d4003cb..fba7a71292 100755 --- a/upstream_utils/sleipnir.py +++ b/upstream_utils/sleipnir.py @@ -51,8 +51,8 @@ def copy_upstream_src(wpilib_root): def main(): name = "sleipnir" url = "https://github.com/SleipnirGroup/Sleipnir" - # main on 2024-07-09 - tag = "b6ffa2d4fdb99cab1bf79491f715a6a9a86633b5" + # main on 2024-08-03 + tag = "4f182964d9bbb1c703260bddbfaa0c4435097675" sleipnir = Lib(name, url, tag, copy_upstream_src) sleipnir.main() diff --git a/upstream_utils/sleipnir_patches/0001-Remove-using-enum-declarations.patch b/upstream_utils/sleipnir_patches/0001-Remove-using-enum-declarations.patch index 8a9817a58b..2a74fa6a84 100644 --- a/upstream_utils/sleipnir_patches/0001-Remove-using-enum-declarations.patch +++ b/upstream_utils/sleipnir_patches/0001-Remove-using-enum-declarations.patch @@ -9,10 +9,10 @@ Subject: [PATCH 1/5] Remove "using enum" declarations 2 files changed, 73 insertions(+), 110 deletions(-) diff --git a/include/sleipnir/autodiff/Expression.hpp b/include/sleipnir/autodiff/Expression.hpp -index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac597dee86 100644 +index dd53755ccae88e3975d1b5e6b13ac464bd4efcce..51070613e82cdf5e4105519f39632deb5d2bf19e 100644 --- a/include/sleipnir/autodiff/Expression.hpp +++ b/include/sleipnir/autodiff/Expression.hpp -@@ -191,8 +191,6 @@ struct SLEIPNIR_DLLEXPORT Expression { +@@ -203,8 +203,6 @@ struct SLEIPNIR_DLLEXPORT Expression { */ friend SLEIPNIR_DLLEXPORT ExpressionPtr operator*(const ExpressionPtr& lhs, const ExpressionPtr& rhs) { @@ -21,7 +21,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac // Prune expression if (lhs->IsConstant(0.0)) { // Return zero -@@ -207,20 +205,22 @@ struct SLEIPNIR_DLLEXPORT Expression { +@@ -219,20 +217,22 @@ struct SLEIPNIR_DLLEXPORT Expression { } // Evaluate constant @@ -50,7 +50,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac } return MakeExpressionPtr( -@@ -246,8 +246,6 @@ struct SLEIPNIR_DLLEXPORT Expression { +@@ -258,8 +258,6 @@ struct SLEIPNIR_DLLEXPORT Expression { */ friend SLEIPNIR_DLLEXPORT ExpressionPtr operator/(const ExpressionPtr& lhs, const ExpressionPtr& rhs) { @@ -59,7 +59,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac // Prune expression if (lhs->IsConstant(0.0)) { // Return zero -@@ -257,16 +255,17 @@ struct SLEIPNIR_DLLEXPORT Expression { +@@ -269,16 +267,17 @@ struct SLEIPNIR_DLLEXPORT Expression { } // Evaluate constant @@ -80,7 +80,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac } return MakeExpressionPtr( -@@ -294,8 +293,6 @@ struct SLEIPNIR_DLLEXPORT Expression { +@@ -306,8 +305,6 @@ struct SLEIPNIR_DLLEXPORT Expression { */ friend SLEIPNIR_DLLEXPORT ExpressionPtr operator+(const ExpressionPtr& lhs, const ExpressionPtr& rhs) { @@ -89,7 +89,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac // Prune expression if (lhs == nullptr || lhs->IsConstant(0.0)) { return rhs; -@@ -304,7 +301,8 @@ struct SLEIPNIR_DLLEXPORT Expression { +@@ -316,7 +313,8 @@ struct SLEIPNIR_DLLEXPORT Expression { } // Evaluate constant @@ -99,7 +99,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac return MakeExpressionPtr(lhs->value + rhs->value); } -@@ -328,8 +326,6 @@ struct SLEIPNIR_DLLEXPORT Expression { +@@ -340,8 +338,6 @@ struct SLEIPNIR_DLLEXPORT Expression { */ friend SLEIPNIR_DLLEXPORT ExpressionPtr operator-(const ExpressionPtr& lhs, const ExpressionPtr& rhs) { @@ -108,7 +108,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac // Prune expression if (lhs->IsConstant(0.0)) { if (rhs->IsConstant(0.0)) { -@@ -343,7 +339,8 @@ struct SLEIPNIR_DLLEXPORT Expression { +@@ -355,7 +351,8 @@ struct SLEIPNIR_DLLEXPORT Expression { } // Evaluate constant @@ -118,7 +118,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac return MakeExpressionPtr(lhs->value - rhs->value); } -@@ -365,8 +362,6 @@ struct SLEIPNIR_DLLEXPORT Expression { +@@ -377,8 +374,6 @@ struct SLEIPNIR_DLLEXPORT Expression { * @param lhs Operand of unary minus. */ friend SLEIPNIR_DLLEXPORT ExpressionPtr operator-(const ExpressionPtr& lhs) { @@ -127,7 +127,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac // Prune expression if (lhs->IsConstant(0.0)) { // Return zero -@@ -374,7 +369,7 @@ struct SLEIPNIR_DLLEXPORT Expression { +@@ -386,7 +381,7 @@ struct SLEIPNIR_DLLEXPORT Expression { } // Evaluate constant @@ -136,7 +136,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac return MakeExpressionPtr(-lhs->value); } -@@ -455,8 +450,6 @@ inline constexpr void IntrusiveSharedPtrDecRefCount(Expression* expr) { +@@ -469,8 +464,6 @@ inline constexpr void IntrusiveSharedPtrDecRefCount(Expression* expr) { */ SLEIPNIR_DLLEXPORT inline ExpressionPtr abs( // NOLINT const ExpressionPtr& x) { @@ -145,7 +145,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac // Prune expression if (x->IsConstant(0.0)) { // Return zero -@@ -464,12 +457,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr abs( // NOLINT +@@ -478,12 +471,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr abs( // NOLINT } // Evaluate constant @@ -160,7 +160,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac [](double x, double, double parentAdjoint) { if (x < 0.0) { return -parentAdjoint; -@@ -500,20 +493,18 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr abs( // NOLINT +@@ -514,20 +507,18 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr abs( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr acos( // NOLINT const ExpressionPtr& x) { @@ -183,7 +183,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac [](double x, double, double parentAdjoint) { return -parentAdjoint / std::sqrt(1.0 - x * x); }, -@@ -532,8 +523,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr acos( // NOLINT +@@ -546,8 +537,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr acos( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr asin( // NOLINT const ExpressionPtr& x) { @@ -192,7 +192,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac // Prune expression if (x->IsConstant(0.0)) { // Return zero -@@ -541,12 +530,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr asin( // NOLINT +@@ -555,12 +544,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr asin( // NOLINT } // Evaluate constant @@ -207,7 +207,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac [](double x, double, double parentAdjoint) { return parentAdjoint / std::sqrt(1.0 - x * x); }, -@@ -565,8 +554,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr asin( // NOLINT +@@ -579,8 +568,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr asin( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan( // NOLINT const ExpressionPtr& x) { @@ -216,7 +216,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac // Prune expression if (x->IsConstant(0.0)) { // Return zero -@@ -574,12 +561,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan( // NOLINT +@@ -588,12 +575,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan( // NOLINT } // Evaluate constant @@ -231,7 +231,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac [](double x, double, double parentAdjoint) { return parentAdjoint / (1.0 + x * x); }, -@@ -598,8 +585,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan( // NOLINT +@@ -612,8 +599,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan2( // NOLINT const ExpressionPtr& y, const ExpressionPtr& x) { @@ -240,7 +240,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac // Prune expression if (y->IsConstant(0.0)) { // Return zero -@@ -609,12 +594,14 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan2( // NOLINT +@@ -623,12 +608,14 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan2( // NOLINT } // Evaluate constant @@ -257,7 +257,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac [](double y, double x, double parentAdjoint) { return parentAdjoint * x / (y * y + x * x); }, -@@ -639,20 +626,18 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan2( // NOLINT +@@ -653,20 +640,18 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr atan2( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr cos( // NOLINT const ExpressionPtr& x) { @@ -280,7 +280,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac [](double x, double, double parentAdjoint) { return -parentAdjoint * std::sin(x); }, -@@ -670,20 +655,18 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr cos( // NOLINT +@@ -684,20 +669,18 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr cos( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr cosh( // NOLINT const ExpressionPtr& x) { @@ -303,7 +303,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac [](double x, double, double parentAdjoint) { return parentAdjoint * std::sinh(x); }, -@@ -701,8 +684,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr cosh( // NOLINT +@@ -715,8 +698,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr cosh( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr erf( // NOLINT const ExpressionPtr& x) { @@ -312,7 +312,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac // Prune expression if (x->IsConstant(0.0)) { // Return zero -@@ -710,12 +691,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr erf( // NOLINT +@@ -724,12 +705,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr erf( // NOLINT } // Evaluate constant @@ -327,7 +327,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac [](double x, double, double parentAdjoint) { return parentAdjoint * 2.0 * std::numbers::inv_sqrtpi * std::exp(-x * x); -@@ -736,20 +717,18 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr erf( // NOLINT +@@ -750,20 +731,18 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr erf( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr exp( // NOLINT const ExpressionPtr& x) { @@ -350,7 +350,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac [](double x, double, double parentAdjoint) { return parentAdjoint * std::exp(x); }, -@@ -768,8 +747,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr exp( // NOLINT +@@ -782,8 +761,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr exp( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr hypot( // NOLINT const ExpressionPtr& x, const ExpressionPtr& y) { @@ -359,7 +359,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac // Prune expression if (x->IsConstant(0.0)) { return y; -@@ -778,12 +755,14 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr hypot( // NOLINT +@@ -792,12 +769,14 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr hypot( // NOLINT } // Evaluate constant @@ -376,7 +376,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac [](double x, double y, double parentAdjoint) { return parentAdjoint * x / std::hypot(x, y); }, -@@ -808,8 +787,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr hypot( // NOLINT +@@ -822,8 +801,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr hypot( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr log( // NOLINT const ExpressionPtr& x) { @@ -385,7 +385,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac // Prune expression if (x->IsConstant(0.0)) { // Return zero -@@ -817,12 +794,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr log( // NOLINT +@@ -831,12 +808,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr log( // NOLINT } // Evaluate constant @@ -400,7 +400,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac [](double x, double, double parentAdjoint) { return parentAdjoint / x; }, [](const ExpressionPtr& x, const ExpressionPtr&, const ExpressionPtr& parentAdjoint) { return parentAdjoint / x; }, -@@ -836,8 +813,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr log( // NOLINT +@@ -850,8 +827,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr log( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr log10( // NOLINT const ExpressionPtr& x) { @@ -409,7 +409,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac // Prune expression if (x->IsConstant(0.0)) { // Return zero -@@ -845,12 +820,13 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr log10( // NOLINT +@@ -859,12 +834,13 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr log10( // NOLINT } // Evaluate constant @@ -425,7 +425,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac [](double x, double, double parentAdjoint) { return parentAdjoint / (std::numbers::ln10 * x); }, -@@ -869,8 +845,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr log10( // NOLINT +@@ -883,8 +859,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr log10( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr pow( // NOLINT const ExpressionPtr& base, const ExpressionPtr& power) { @@ -434,7 +434,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac // Prune expression if (base->IsConstant(0.0)) { // Return zero -@@ -885,12 +859,15 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr pow( // NOLINT +@@ -899,12 +873,15 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr pow( // NOLINT } // Evaluate constant @@ -452,7 +452,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac [](double base, double power) { return std::pow(base, power); }, [](double base, double power, double parentAdjoint) { return parentAdjoint * std::pow(base, power - 1) * power; -@@ -931,10 +908,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr pow( // NOLINT +@@ -945,10 +922,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr pow( // NOLINT * @param x The argument. */ SLEIPNIR_DLLEXPORT inline ExpressionPtr sign(const ExpressionPtr& x) { @@ -464,7 +464,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac if (x->value < 0.0) { return MakeExpressionPtr(-1.0); } else if (x->value == 0.0) { -@@ -946,7 +921,7 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sign(const ExpressionPtr& x) { +@@ -960,7 +935,7 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sign(const ExpressionPtr& x) { } return MakeExpressionPtr( @@ -473,7 +473,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac [](double x, double) { if (x < 0.0) { return -1.0; -@@ -971,8 +946,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sign(const ExpressionPtr& x) { +@@ -985,8 +960,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sign(const ExpressionPtr& x) { */ SLEIPNIR_DLLEXPORT inline ExpressionPtr sin( // NOLINT const ExpressionPtr& x) { @@ -482,7 +482,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac // Prune expression if (x->IsConstant(0.0)) { // Return zero -@@ -980,12 +953,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sin( // NOLINT +@@ -994,12 +967,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sin( // NOLINT } // Evaluate constant @@ -497,7 +497,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac [](double x, double, double parentAdjoint) { return parentAdjoint * std::cos(x); }, -@@ -1002,8 +975,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sin( // NOLINT +@@ -1016,8 +989,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sin( // NOLINT * @param x The argument. */ SLEIPNIR_DLLEXPORT inline ExpressionPtr sinh(const ExpressionPtr& x) { @@ -506,7 +506,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac // Prune expression if (x->IsConstant(0.0)) { // Return zero -@@ -1011,12 +982,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sinh(const ExpressionPtr& x) { +@@ -1025,12 +996,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sinh(const ExpressionPtr& x) { } // Evaluate constant @@ -521,7 +521,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac [](double x, double, double parentAdjoint) { return parentAdjoint * std::cosh(x); }, -@@ -1034,10 +1005,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sinh(const ExpressionPtr& x) { +@@ -1048,10 +1019,8 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sinh(const ExpressionPtr& x) { */ SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt( // NOLINT const ExpressionPtr& x) { @@ -533,7 +533,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac if (x->value == 0.0) { // Return zero return x; -@@ -1049,7 +1018,7 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt( // NOLINT +@@ -1063,7 +1032,7 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt( // NOLINT } return MakeExpressionPtr( @@ -542,7 +542,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac [](double x, double, double parentAdjoint) { return parentAdjoint / (2.0 * std::sqrt(x)); }, -@@ -1068,8 +1037,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt( // NOLINT +@@ -1082,8 +1051,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt( // NOLINT */ SLEIPNIR_DLLEXPORT inline ExpressionPtr tan( // NOLINT const ExpressionPtr& x) { @@ -551,7 +551,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac // Prune expression if (x->IsConstant(0.0)) { // Return zero -@@ -1077,12 +1044,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr tan( // NOLINT +@@ -1091,12 +1058,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr tan( // NOLINT } // Evaluate constant @@ -566,7 +566,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac [](double x, double, double parentAdjoint) { return parentAdjoint / (std::cos(x) * std::cos(x)); }, -@@ -1100,8 +1067,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr tan( // NOLINT +@@ -1114,8 +1081,6 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr tan( // NOLINT * @param x The argument. */ SLEIPNIR_DLLEXPORT inline ExpressionPtr tanh(const ExpressionPtr& x) { @@ -575,7 +575,7 @@ index 6c4ae6269f13b7d1b5c9b0281de1a4b0dc890baf..065b28e790db32234042bcc94d9a1dac // Prune expression if (x->IsConstant(0.0)) { // Return zero -@@ -1109,12 +1074,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr tanh(const ExpressionPtr& x) { +@@ -1123,12 +1088,12 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr tanh(const ExpressionPtr& x) { } // Evaluate constant diff --git a/upstream_utils/sleipnir_patches/0003-Remove-unsupported-constexpr.patch b/upstream_utils/sleipnir_patches/0003-Remove-unsupported-constexpr.patch index c3efbbd39f..853c4d2988 100644 --- a/upstream_utils/sleipnir_patches/0003-Remove-unsupported-constexpr.patch +++ b/upstream_utils/sleipnir_patches/0003-Remove-unsupported-constexpr.patch @@ -8,10 +8,10 @@ Subject: [PATCH 3/5] Remove unsupported constexpr 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/include/sleipnir/autodiff/Expression.hpp b/include/sleipnir/autodiff/Expression.hpp -index 065b28e790db32234042bcc94d9a1dac597dee86..e65b689559d01324fc4218c26144521832719025 100644 +index 51070613e82cdf5e4105519f39632deb5d2bf19e..dff8e2a6ef24413e3e6356bf0ec57286e50654cf 100644 --- a/include/sleipnir/autodiff/Expression.hpp +++ b/include/sleipnir/autodiff/Expression.hpp -@@ -21,8 +21,8 @@ namespace sleipnir::detail { +@@ -29,8 +29,8 @@ inline constexpr bool kUsePoolAllocator = true; struct SLEIPNIR_DLLEXPORT Expression; @@ -22,7 +22,7 @@ index 065b28e790db32234042bcc94d9a1dac597dee86..e65b689559d01324fc4218c261445218 /** * Typedef for intrusive shared pointer to Expression. -@@ -401,7 +401,7 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt(const ExpressionPtr& x); +@@ -413,7 +413,7 @@ SLEIPNIR_DLLEXPORT inline ExpressionPtr sqrt(const ExpressionPtr& x); * * @param expr The shared pointer's managed object. */ @@ -31,7 +31,7 @@ index 065b28e790db32234042bcc94d9a1dac597dee86..e65b689559d01324fc4218c261445218 ++expr->refCount; } -@@ -410,7 +410,7 @@ inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr) { +@@ -422,7 +422,7 @@ inline constexpr void IntrusiveSharedPtrIncRefCount(Expression* expr) { * * @param expr The shared pointer's managed object. */ diff --git a/upstream_utils/sleipnir_patches/0004-Use-wpi-SmallVector.patch b/upstream_utils/sleipnir_patches/0004-Use-wpi-SmallVector.patch index 222a38d0c5..7dcbdf5eb6 100644 --- a/upstream_utils/sleipnir_patches/0004-Use-wpi-SmallVector.patch +++ b/upstream_utils/sleipnir_patches/0004-Use-wpi-SmallVector.patch @@ -9,8 +9,8 @@ Subject: [PATCH 4/5] Use wpi::SmallVector include/sleipnir/autodiff/ExpressionGraph.hpp | 15 ++++++++------- include/sleipnir/autodiff/Hessian.hpp | 4 ++-- include/sleipnir/autodiff/Jacobian.hpp | 10 +++++----- + include/sleipnir/autodiff/Variable.hpp | 10 +++++----- include/sleipnir/autodiff/VariableMatrix.hpp | 4 ++-- - include/sleipnir/optimization/Constraints.hpp | 11 ++++++----- include/sleipnir/optimization/Multistart.hpp | 7 ++++--- .../sleipnir/optimization/OptimizationProblem.hpp | 8 ++++---- include/sleipnir/util/Pool.hpp | 7 ++++--- @@ -19,7 +19,7 @@ Subject: [PATCH 4/5] Use wpi::SmallVector src/optimization/solver/InteriorPoint.cpp | 4 ++-- .../solver/util/FeasibilityRestoration.hpp | 10 +++++----- src/optimization/solver/util/Filter.hpp | 4 ++-- - 15 files changed, 51 insertions(+), 44 deletions(-) + 15 files changed, 50 insertions(+), 44 deletions(-) diff --git a/include/.styleguide b/include/.styleguide index 6a7f8ed28f9cb037c9746a7e0ef5e110481d9825..efa36cee1fb593ae810032340c64f1854fbbc523 100644 @@ -32,7 +32,7 @@ index 6a7f8ed28f9cb037c9746a7e0ef5e110481d9825..efa36cee1fb593ae810032340c64f185 + ^wpi/ } diff --git a/include/sleipnir/autodiff/Expression.hpp b/include/sleipnir/autodiff/Expression.hpp -index e65b689559d01324fc4218c26144521832719025..2be666e7c87731d2633576d4a066efba4906502d 100644 +index dff8e2a6ef24413e3e6356bf0ec57286e50654cf..bdbeb4730223f88cfdc0c424a8f7b852b34742f7 100644 --- a/include/sleipnir/autodiff/Expression.hpp +++ b/include/sleipnir/autodiff/Expression.hpp @@ -11,11 +11,12 @@ @@ -49,7 +49,7 @@ index e65b689559d01324fc4218c26144521832719025..2be666e7c87731d2633576d4a066efba namespace sleipnir::detail { -@@ -415,7 +416,7 @@ inline void IntrusiveSharedPtrDecRefCount(Expression* expr) { +@@ -427,7 +428,7 @@ inline void IntrusiveSharedPtrDecRefCount(Expression* expr) { // Expression destructor when expr's refcount reaches zero can cause a stack // overflow. Instead, we iterate over its children to decrement their // refcounts and deallocate them. @@ -196,53 +196,27 @@ index ac00c11ef8c947cbe95c58081bdbfb42bf901051..0c660156c80f94539383b8f0d01d7853 Profiler m_profiler; }; -diff --git a/include/sleipnir/autodiff/VariableMatrix.hpp b/include/sleipnir/autodiff/VariableMatrix.hpp -index a7e89e5d7a24fb1295eb3ff04d8f22824c0884f1..bac9fe4f5ecf08bb4c0e3dfa8822125bcf854803 100644 ---- a/include/sleipnir/autodiff/VariableMatrix.hpp -+++ b/include/sleipnir/autodiff/VariableMatrix.hpp -@@ -11,13 +11,13 @@ +diff --git a/include/sleipnir/autodiff/Variable.hpp b/include/sleipnir/autodiff/Variable.hpp +index c04d629f482efe59497570ca1fd9b09a4af2ae1e..d192fb96e7984b7c0ca30262668c41e5e84ca34e 100644 +--- a/include/sleipnir/autodiff/Variable.hpp ++++ b/include/sleipnir/autodiff/Variable.hpp +@@ -10,6 +10,7 @@ #include #include +#include - #include "sleipnir/autodiff/Variable.hpp" - #include "sleipnir/autodiff/VariableBlock.hpp" - #include "sleipnir/util/Assert.hpp" - #include "sleipnir/util/FunctionRef.hpp" - #include "sleipnir/util/SymbolExports.hpp" --#include "sleipnir/util/small_vector.hpp" - - namespace sleipnir { - -@@ -884,7 +884,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { - } - - private: -- small_vector m_storage; -+ wpi::SmallVector m_storage; - int m_rows = 0; - int m_cols = 0; - }; -diff --git a/include/sleipnir/optimization/Constraints.hpp b/include/sleipnir/optimization/Constraints.hpp -index 80da66bfef55bfc54cfdcd0478432658f60f7610..b940fcc4deb76c282b27564332db5e5935fbfbc6 100644 ---- a/include/sleipnir/optimization/Constraints.hpp -+++ b/include/sleipnir/optimization/Constraints.hpp -@@ -8,11 +8,12 @@ - #include - #include - -+#include -+ - #include "sleipnir/autodiff/Variable.hpp" - #include "sleipnir/util/Assert.hpp" + #include "sleipnir/autodiff/Expression.hpp" + #include "sleipnir/autodiff/ExpressionGraph.hpp" +@@ -17,7 +18,6 @@ #include "sleipnir/util/Concepts.hpp" + #include "sleipnir/util/Print.hpp" #include "sleipnir/util/SymbolExports.hpp" -#include "sleipnir/util/small_vector.hpp" namespace sleipnir { -@@ -32,8 +33,8 @@ template +@@ -445,8 +445,8 @@ template (ScalarLike> || MatrixLike>) && (!std::same_as, double> || !std::same_as, double>) @@ -253,7 +227,7 @@ index 80da66bfef55bfc54cfdcd0478432658f60f7610..b940fcc4deb76c282b27564332db5e59 if constexpr (ScalarLike> && ScalarLike>) { -@@ -121,7 +122,7 @@ small_vector MakeConstraints(LHS&& lhs, RHS&& rhs) { +@@ -534,7 +534,7 @@ small_vector MakeConstraints(LHS&& lhs, RHS&& rhs) { */ struct SLEIPNIR_DLLEXPORT EqualityConstraints { /// A vector of scalar equality constraints. @@ -262,7 +236,7 @@ index 80da66bfef55bfc54cfdcd0478432658f60f7610..b940fcc4deb76c282b27564332db5e59 /** * Concatenates multiple equality constraints. -@@ -183,7 +184,7 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints { +@@ -596,7 +596,7 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints { */ struct SLEIPNIR_DLLEXPORT InequalityConstraints { /// A vector of scalar inequality constraints. @@ -271,6 +245,35 @@ index 80da66bfef55bfc54cfdcd0478432658f60f7610..b940fcc4deb76c282b27564332db5e59 /** * Concatenates multiple inequality constraints. +diff --git a/include/sleipnir/autodiff/VariableMatrix.hpp b/include/sleipnir/autodiff/VariableMatrix.hpp +index 47452b8988b3a1a96a78d28644200b1c4cdc89c9..57b09913d15e9590873ad7bf62e2baff9fbc5df9 100644 +--- a/include/sleipnir/autodiff/VariableMatrix.hpp ++++ b/include/sleipnir/autodiff/VariableMatrix.hpp +@@ -11,6 +11,7 @@ + #include + + #include ++#include + + #include "sleipnir/autodiff/Slice.hpp" + #include "sleipnir/autodiff/Variable.hpp" +@@ -18,7 +19,6 @@ + #include "sleipnir/util/Assert.hpp" + #include "sleipnir/util/FunctionRef.hpp" + #include "sleipnir/util/SymbolExports.hpp" +-#include "sleipnir/util/small_vector.hpp" + + namespace sleipnir { + +@@ -946,7 +946,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { + } + + private: +- small_vector m_storage; ++ wpi::SmallVector m_storage; + int m_rows = 0; + int m_cols = 0; + }; diff --git a/include/sleipnir/optimization/Multistart.hpp b/include/sleipnir/optimization/Multistart.hpp index 8055713a2492a9c8473f047a2fb9fe7ca57753c3..09b1b2f3bf33c35ae0aeddb9b5d47c0d80c68cec 100644 --- a/include/sleipnir/optimization/Multistart.hpp @@ -305,7 +308,7 @@ index 8055713a2492a9c8473f047a2fb9fe7ca57753c3..09b1b2f3bf33c35ae0aeddb9b5d47c0d for (auto& future : futures) { diff --git a/include/sleipnir/optimization/OptimizationProblem.hpp b/include/sleipnir/optimization/OptimizationProblem.hpp -index 7d387e02af386a3cbfaf0294d5be78e73fb05628..b78b3761898088235cf335bf5cc111f4cafbe29a 100644 +index 28b2943f5842229335e79eae14abbda6ff5b7000..e812fa5e3454903d4dfb8572539ebf1b506bdf11 100644 --- a/include/sleipnir/optimization/OptimizationProblem.hpp +++ b/include/sleipnir/optimization/OptimizationProblem.hpp @@ -11,6 +11,7 @@ @@ -316,7 +319,7 @@ index 7d387e02af386a3cbfaf0294d5be78e73fb05628..b78b3761898088235cf335bf5cc111f4 #include "sleipnir/autodiff/Variable.hpp" #include "sleipnir/autodiff/VariableMatrix.hpp" -@@ -22,7 +23,6 @@ +@@ -21,7 +22,6 @@ #include "sleipnir/optimization/solver/InteriorPoint.hpp" #include "sleipnir/util/Print.hpp" #include "sleipnir/util/SymbolExports.hpp" @@ -324,7 +327,7 @@ index 7d387e02af386a3cbfaf0294d5be78e73fb05628..b78b3761898088235cf335bf5cc111f4 namespace sleipnir { -@@ -359,16 +359,16 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem { +@@ -358,16 +358,16 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem { private: // The list of decision variables, which are the root of the problem's // expression tree diff --git a/upstream_utils/sleipnir_patches/0005-Suppress-clang-tidy-false-positives.patch b/upstream_utils/sleipnir_patches/0005-Suppress-clang-tidy-false-positives.patch index b65bf6824d..a7f3de198e 100644 --- a/upstream_utils/sleipnir_patches/0005-Suppress-clang-tidy-false-positives.patch +++ b/upstream_utils/sleipnir_patches/0005-Suppress-clang-tidy-false-positives.patch @@ -4,14 +4,14 @@ Date: Wed, 26 Jun 2024 12:13:33 -0700 Subject: [PATCH 5/5] Suppress clang-tidy false positives --- - include/sleipnir/optimization/Constraints.hpp | 4 ++-- + include/sleipnir/autodiff/Variable.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) -diff --git a/include/sleipnir/optimization/Constraints.hpp b/include/sleipnir/optimization/Constraints.hpp -index b940fcc4deb76c282b27564332db5e5935fbfbc6..f3eb8a276ded38cd3918c18810ddaa61590625c9 100644 ---- a/include/sleipnir/optimization/Constraints.hpp -+++ b/include/sleipnir/optimization/Constraints.hpp -@@ -129,7 +129,7 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints { +diff --git a/include/sleipnir/autodiff/Variable.hpp b/include/sleipnir/autodiff/Variable.hpp +index d192fb96e7984b7c0ca30262668c41e5e84ca34e..f25c6d153310a01700ee2390ecf35ffa8af7df11 100644 +--- a/include/sleipnir/autodiff/Variable.hpp ++++ b/include/sleipnir/autodiff/Variable.hpp +@@ -541,7 +541,7 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints { * * @param equalityConstraints The list of EqualityConstraints to concatenate. */ @@ -20,7 +20,7 @@ index b940fcc4deb76c282b27564332db5e5935fbfbc6..f3eb8a276ded38cd3918c18810ddaa61 std::initializer_list equalityConstraints) { for (const auto& elem : equalityConstraints) { constraints.insert(constraints.end(), elem.constraints.begin(), -@@ -192,7 +192,7 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints { +@@ -604,7 +604,7 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints { * @param inequalityConstraints The list of InequalityConstraints to * concatenate. */ diff --git a/wpimath/src/main/native/thirdparty/sleipnir/.styleguide b/wpimath/src/main/native/thirdparty/sleipnir/.styleguide index fc51b044a6..2cf272a115 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/.styleguide +++ b/wpimath/src/main/native/thirdparty/sleipnir/.styleguide @@ -17,6 +17,6 @@ modifiableFileExclude { includeOtherLibs { ^Eigen/ ^catch2/ - ^pybind11/ + ^nanobind/ ^sleipnir/ } diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Slice.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Slice.hpp new file mode 100644 index 0000000000..2f2b18993b --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Slice.hpp @@ -0,0 +1,158 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include +#include + +#include "sleipnir/util/Assert.hpp" +#include "sleipnir/util/SymbolExports.hpp" + +namespace sleipnir { + +namespace slicing { + +struct none_t {}; +static inline constexpr none_t _; + +} // namespace slicing + +class SLEIPNIR_DLLEXPORT Slice { + public: + /// Start index (inclusive). + int start = 0; + + /// Stop index (exclusive). + int stop = 0; + + /// Step. + int step = 1; + + /** + * Constructs a Slice. + */ + constexpr Slice() = default; + + /** + * Constructs a slice. + * + * @param stop Slice stop index (exclusive). + */ + template + requires std::same_as || + std::convertible_to + constexpr Slice(Stop stop) // NOLINT + : Slice(slicing::_, std::move(stop), 1) {} + + /** + * Constructs a slice. + * + * @param start Slice start index (inclusive). + * @param stop Slice stop index (exclusive). + */ + template + requires(std::same_as || + std::convertible_to) && + (std::same_as || + std::convertible_to) + constexpr Slice(Start start, Stop stop) + : Slice(std::move(start), std::move(stop), 1) {} + + /** + * Constructs a slice. + * + * @param start Slice start index (inclusive). + * @param stop Slice stop index (exclusive). + * @param step Slice step. + */ + template + requires(std::same_as || + std::convertible_to) && + (std::same_as || + std::convertible_to) && + (std::same_as || + std::convertible_to) + constexpr Slice(Start start, Stop stop, Step step) { + if constexpr (std::same_as) { + this->step = 1; + } else { + Assert(step != 0); + + this->step = step; + } + + // Avoid UB for step = -step if step is INT_MIN + if (this->step == std::numeric_limits::min()) { + this->step = -std::numeric_limits::max(); + } + + if constexpr (std::same_as) { + if (this->step < 0) { + this->start = std::numeric_limits::max(); + } else { + this->start = 0; + } + } else { + this->start = start; + } + + if constexpr (std::same_as) { + if (this->step < 0) { + this->stop = std::numeric_limits::min(); + } else { + this->stop = std::numeric_limits::max(); + } + } else { + this->stop = stop; + } + } + + /** + * Adjusts start and end slice indices assuming a sequence of the specified + * length. + * + * @param length The sequence length. + * @return The slice length. + */ + constexpr int Adjust(int length) { + Assert(step != 0); + Assert(step >= -std::numeric_limits::max()); + + if (start < 0) { + start += length; + + if (start < 0) { + start = (step < 0) ? -1 : 0; + } + } else if (start >= length) { + start = (step < 0) ? length - 1 : length; + } + + if (stop < 0) { + stop += length; + + if (stop < 0) { + stop = (step < 0) ? -1 : 0; + } + } else if (stop >= length) { + stop = (step < 0) ? length - 1 : length; + } + + if (step < 0) { + if (stop < start) { + return (start - stop - 1) / -step + 1; + } else { + return 0; + } + } else { + if (start < stop) { + return (stop - start - 1) / step + 1; + } else { + return 0; + } + } + } +}; + +} // namespace sleipnir 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 b176d91dfe..f25c6d1533 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 @@ -2,10 +2,20 @@ #pragma once +#include +#include +#include +#include #include +#include + +#include +#include #include "sleipnir/autodiff/Expression.hpp" #include "sleipnir/autodiff/ExpressionGraph.hpp" +#include "sleipnir/util/Assert.hpp" +#include "sleipnir/util/Concepts.hpp" #include "sleipnir/util/Print.hpp" #include "sleipnir/util/SymbolExports.hpp" @@ -419,4 +429,333 @@ SLEIPNIR_DLLEXPORT inline Variable hypot(const Variable& x, const Variable& y, sleipnir::pow(z, 2))}; } +/** + * Make a list of constraints. + * + * The standard form for equality constraints is c(x) = 0, and the standard form + * for inequality constraints is c(x) ≥ 0. This function takes constraints of + * the form lhs = rhs or lhs ≥ rhs and converts them to lhs - rhs = 0 or + * lhs - rhs ≥ 0. + * + * @param lhs Left-hand side. + * @param rhs Right-hand side. + */ +template + requires(ScalarLike> || MatrixLike>) && + (ScalarLike> || MatrixLike>) && + (!std::same_as, double> || + !std::same_as, double>) +wpi::SmallVector MakeConstraints(LHS&& lhs, RHS&& rhs) { + wpi::SmallVector constraints; + + if constexpr (ScalarLike> && + ScalarLike>) { + constraints.emplace_back(lhs - rhs); + } else if constexpr (ScalarLike> && + MatrixLike>) { + int rows; + int cols; + if constexpr (EigenMatrixLike>) { + rows = rhs.rows(); + cols = rhs.cols(); + } else { + rows = rhs.Rows(); + cols = rhs.Cols(); + } + + constraints.reserve(rows * cols); + + for (int row = 0; row < rows; ++row) { + for (int col = 0; col < cols; ++col) { + // Make right-hand side zero + constraints.emplace_back(lhs - rhs(row, col)); + } + } + } else if constexpr (MatrixLike> && + ScalarLike>) { + int rows; + int cols; + if constexpr (EigenMatrixLike>) { + rows = lhs.rows(); + cols = lhs.cols(); + } else { + rows = lhs.Rows(); + cols = lhs.Cols(); + } + + constraints.reserve(rows * cols); + + for (int row = 0; row < rows; ++row) { + for (int col = 0; col < cols; ++col) { + // Make right-hand side zero + constraints.emplace_back(lhs(row, col) - rhs); + } + } + } else if constexpr (MatrixLike> && + MatrixLike>) { + int lhsRows; + int lhsCols; + if constexpr (EigenMatrixLike>) { + lhsRows = lhs.rows(); + lhsCols = lhs.cols(); + } else { + lhsRows = lhs.Rows(); + lhsCols = lhs.Cols(); + } + + [[maybe_unused]] + int rhsRows; + [[maybe_unused]] + int rhsCols; + if constexpr (EigenMatrixLike>) { + rhsRows = rhs.rows(); + rhsCols = rhs.cols(); + } else { + rhsRows = rhs.Rows(); + rhsCols = rhs.Cols(); + } + + Assert(lhsRows == rhsRows && lhsCols == rhsCols); + constraints.reserve(lhsRows * lhsCols); + + for (int row = 0; row < lhsRows; ++row) { + for (int col = 0; col < lhsCols; ++col) { + // Make right-hand side zero + constraints.emplace_back(lhs(row, col) - rhs(row, col)); + } + } + } + + return constraints; +} + +/** + * A vector of equality constraints of the form cₑ(x) = 0. + */ +struct SLEIPNIR_DLLEXPORT EqualityConstraints { + /// A vector of scalar equality constraints. + wpi::SmallVector constraints; + + /** + * Concatenates multiple equality constraints. + * + * @param equalityConstraints The list of EqualityConstraints to concatenate. + */ + EqualityConstraints( // NOLINT + std::initializer_list equalityConstraints) { + for (const auto& elem : equalityConstraints) { + constraints.insert(constraints.end(), elem.constraints.begin(), + elem.constraints.end()); + } + } + + /** + * Concatenates multiple equality constraints. + * + * This overload is for Python bindings only. + * + * @param equalityConstraints The list of EqualityConstraints to concatenate. + */ + explicit EqualityConstraints( + const std::vector& equalityConstraints) { + for (const auto& elem : equalityConstraints) { + constraints.insert(constraints.end(), elem.constraints.begin(), + elem.constraints.end()); + } + } + + /** + * Constructs an equality constraint from a left and right side. + * + * The standard form for equality constraints is c(x) = 0. This function takes + * a constraint of the form lhs = rhs and converts it to lhs - rhs = 0. + * + * @param lhs Left-hand side. + * @param rhs Right-hand side. + */ + template + requires(ScalarLike> || MatrixLike>) && + (ScalarLike> || MatrixLike>) && + (!std::same_as, double> || + !std::same_as, double>) + EqualityConstraints(LHS&& lhs, RHS&& rhs) + : constraints{MakeConstraints(lhs, rhs)} {} + + /** + * Implicit conversion operator to bool. + */ + operator bool() { // NOLINT + return std::all_of( + constraints.begin(), constraints.end(), + [](auto& constraint) { return constraint.Value() == 0.0; }); + } +}; + +/** + * A vector of inequality constraints of the form cᵢ(x) ≥ 0. + */ +struct SLEIPNIR_DLLEXPORT InequalityConstraints { + /// A vector of scalar inequality constraints. + wpi::SmallVector constraints; + + /** + * Concatenates multiple inequality constraints. + * + * @param inequalityConstraints The list of InequalityConstraints to + * concatenate. + */ + InequalityConstraints( // NOLINT + std::initializer_list inequalityConstraints) { + for (const auto& elem : inequalityConstraints) { + constraints.insert(constraints.end(), elem.constraints.begin(), + elem.constraints.end()); + } + } + + /** + * Concatenates multiple inequality constraints. + * + * This overload is for Python bindings only. + * + * @param inequalityConstraints The list of InequalityConstraints to + * concatenate. + */ + explicit InequalityConstraints( + const std::vector& inequalityConstraints) { + for (const auto& elem : inequalityConstraints) { + constraints.insert(constraints.end(), elem.constraints.begin(), + elem.constraints.end()); + } + } + + /** + * Constructs an inequality constraint from a left and right side. + * + * The standard form for inequality constraints is c(x) ≥ 0. This function + * takes a constraints of the form lhs ≥ rhs and converts it to lhs - rhs ≥ 0. + * + * @param lhs Left-hand side. + * @param rhs Right-hand side. + */ + template + requires(ScalarLike> || MatrixLike>) && + (ScalarLike> || MatrixLike>) && + (!std::same_as, double> || + !std::same_as, double>) + InequalityConstraints(LHS&& lhs, RHS&& rhs) + : constraints{MakeConstraints(lhs, rhs)} {} + + /** + * Implicit conversion operator to bool. + */ + operator bool() { // NOLINT + return std::all_of( + constraints.begin(), constraints.end(), + [](auto& constraint) { return constraint.Value() >= 0.0; }); + } +}; + +/** + * Equality operator that returns an equality constraint for two Variables. + * + * @param lhs Left-hand side. + * @param rhs Left-hand side. + */ +template + requires(ScalarLike> || MatrixLike>) && + (ScalarLike> || MatrixLike>) && + (!std::same_as, double> || + !std::same_as, double>) +EqualityConstraints operator==(LHS&& lhs, RHS&& rhs) { + return EqualityConstraints{lhs, rhs}; +} + +/** + * Less-than comparison operator that returns an inequality constraint for two + * Variables. + * + * @param lhs Left-hand side. + * @param rhs Left-hand side. + */ +template + requires(ScalarLike> || MatrixLike>) && + (ScalarLike> || MatrixLike>) && + (!std::same_as, double> || + !std::same_as, double>) +InequalityConstraints operator<(LHS&& lhs, RHS&& rhs) { + return rhs >= lhs; +} + +/** + * Less-than-or-equal-to comparison operator that returns an inequality + * constraint for two Variables. + * + * @param lhs Left-hand side. + * @param rhs Left-hand side. + */ +template + requires(ScalarLike> || MatrixLike>) && + (ScalarLike> || MatrixLike>) && + (!std::same_as, double> || + !std::same_as, double>) +InequalityConstraints operator<=(LHS&& lhs, RHS&& rhs) { + return rhs >= lhs; +} + +/** + * Greater-than comparison operator that returns an inequality constraint for + * two Variables. + * + * @param lhs Left-hand side. + * @param rhs Left-hand side. + */ +template + requires(ScalarLike> || MatrixLike>) && + (ScalarLike> || MatrixLike>) && + (!std::same_as, double> || + !std::same_as, double>) +InequalityConstraints operator>(LHS&& lhs, RHS&& rhs) { + return lhs >= rhs; +} + +/** + * Greater-than-or-equal-to comparison operator that returns an inequality + * constraint for two Variables. + * + * @param lhs Left-hand side. + * @param rhs Left-hand side. + */ +template + requires(ScalarLike> || MatrixLike>) && + (ScalarLike> || MatrixLike>) && + (!std::same_as, double> || + !std::same_as, double>) +InequalityConstraints operator>=(LHS&& lhs, RHS&& rhs) { + return InequalityConstraints{lhs, rhs}; +} + } // namespace sleipnir + +namespace Eigen { + +/** + * NumTraits specialization that allows instantiating Eigen types with Variable. + */ +template <> +struct NumTraits : NumTraits { + using Real = sleipnir::Variable; + using NonInteger = sleipnir::Variable; + using Nested = sleipnir::Variable; + + enum { + IsComplex = 0, + IsInteger = 0, + IsSigned = 1, + RequireInitialization = 1, + ReadCost = 1, + AddCost = 3, + MulCost = 3 + }; +}; + +} // namespace Eigen diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableBlock.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableBlock.hpp index 03a1eb65dc..72bb235917 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableBlock.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableBlock.hpp @@ -8,6 +8,7 @@ #include +#include "sleipnir/autodiff/Slice.hpp" #include "sleipnir/autodiff/Variable.hpp" #include "sleipnir/util/Assert.hpp" #include "sleipnir/util/FunctionRef.hpp" @@ -36,10 +37,10 @@ class VariableBlock { if (m_mat == nullptr) { m_mat = values.m_mat; - m_rowOffset = values.m_rowOffset; - m_colOffset = values.m_colOffset; - m_blockRows = values.m_blockRows; - m_blockCols = values.m_blockCols; + m_rowSlice = values.m_rowSlice; + m_rowSliceLength = values.m_rowSliceLength; + m_colSlice = values.m_colSlice; + m_colSliceLength = values.m_colSliceLength; } else { Assert(Rows() == values.Rows()); Assert(Cols() == values.Cols()); @@ -68,10 +69,10 @@ class VariableBlock { if (m_mat == nullptr) { m_mat = values.m_mat; - m_rowOffset = values.m_rowOffset; - m_colOffset = values.m_colOffset; - m_blockRows = values.m_blockRows; - m_blockCols = values.m_blockCols; + m_rowSlice = values.m_rowSlice; + m_rowSliceLength = values.m_rowSliceLength; + m_colSlice = values.m_colSlice; + m_colSliceLength = values.m_colSliceLength; } else { Assert(Rows() == values.Rows()); Assert(Cols() == values.Cols()); @@ -92,7 +93,11 @@ class VariableBlock { * @param mat The matrix to which to point. */ VariableBlock(Mat& mat) // NOLINT - : m_mat{&mat}, m_blockRows{mat.Rows()}, m_blockCols{mat.Cols()} {} + : m_mat{&mat}, + m_rowSlice{0, mat.Rows(), 1}, + m_rowSliceLength{m_rowSlice.Adjust(mat.Rows())}, + m_colSlice{0, mat.Cols(), 1}, + m_colSliceLength{m_colSlice.Adjust(mat.Cols())} {} /** * Constructs a Variable block pointing to a subset of the given matrix. @@ -106,10 +111,29 @@ class VariableBlock { VariableBlock(Mat& mat, int rowOffset, int colOffset, int blockRows, int blockCols) : m_mat{&mat}, - m_rowOffset{rowOffset}, - m_colOffset{colOffset}, - m_blockRows{blockRows}, - m_blockCols{blockCols} {} + m_rowSlice{rowOffset, rowOffset + blockRows, 1}, + m_rowSliceLength{m_rowSlice.Adjust(mat.Rows())}, + m_colSlice{colOffset, colOffset + blockCols, 1}, + m_colSliceLength{m_colSlice.Adjust(mat.Cols())} {} + + /** + * Constructs a Variable block pointing to a subset of the given matrix. + * + * Note that the slices are taken as is rather than adjusted. + * + * @param mat The matrix to which to point. + * @param rowSlice The block's row slice. + * @param rowSliceLength The block's row length. + * @param colSlice The block's column slice. + * @param colSliceLength The block's column length. + */ + VariableBlock(Mat& mat, Slice rowSlice, int rowSliceLength, Slice colSlice, + int colSliceLength) + : m_mat{&mat}, + m_rowSlice{std::move(rowSlice)}, + m_rowSliceLength{rowSliceLength}, + m_colSlice{std::move(colSlice)}, + m_colSliceLength{colSliceLength} {} /** * Assigns a double to the block. @@ -219,7 +243,8 @@ class VariableBlock { { Assert(row >= 0 && row < Rows()); Assert(col >= 0 && col < Cols()); - return (*m_mat)(m_rowOffset + row, m_colOffset + col); + return (*m_mat)(m_rowSlice.start + row * m_rowSlice.step, + m_colSlice.start + col * m_colSlice.step); } /** @@ -231,7 +256,8 @@ class VariableBlock { const Variable& operator()(int row, int col) const { Assert(row >= 0 && row < Rows()); Assert(col >= 0 && col < Cols()); - return (*m_mat)(m_rowOffset + row, m_colOffset + col); + return (*m_mat)(m_rowSlice.start + row * m_rowSlice.step, + m_colSlice.start + col * m_colSlice.step); } /** @@ -257,7 +283,7 @@ class VariableBlock { } /** - * Returns a block slice of the variable matrix. + * Returns a block of the variable matrix. * * @param rowOffset The row offset of the block selection. * @param colOffset The column offset of the block selection. @@ -270,8 +296,8 @@ class VariableBlock { Assert(colOffset >= 0 && colOffset <= Cols()); Assert(blockRows >= 0 && blockRows <= Rows() - rowOffset); Assert(blockCols >= 0 && blockCols <= Cols() - colOffset); - return VariableBlock{*m_mat, m_rowOffset + rowOffset, - m_colOffset + colOffset, blockRows, blockCols}; + return (*this)({rowOffset, rowOffset + blockRows, 1}, + {colOffset, colOffset + blockCols, 1}); } /** @@ -288,8 +314,118 @@ class VariableBlock { Assert(colOffset >= 0 && colOffset <= Cols()); Assert(blockRows >= 0 && blockRows <= Rows() - rowOffset); Assert(blockCols >= 0 && blockCols <= Cols() - colOffset); - return VariableBlock{*m_mat, m_rowOffset + rowOffset, - m_colOffset + colOffset, blockRows, blockCols}; + return (*this)({rowOffset, rowOffset + blockRows, 1}, + {colOffset, colOffset + blockCols, 1}); + } + + /** + * Returns a slice of the variable matrix. + * + * @param rowSlice The row slice. + * @param colSlice The column slice. + */ + VariableBlock operator()(Slice rowSlice, Slice colSlice) { + int rowSliceLength = rowSlice.Adjust(m_rowSliceLength); + int colSliceLength = colSlice.Adjust(m_colSliceLength); + return VariableBlock{ + *m_mat, + {m_rowSlice.start + rowSlice.start * m_rowSlice.step, + m_rowSlice.start + rowSlice.stop, m_rowSlice.step * rowSlice.step}, + rowSliceLength, + {m_colSlice.start + colSlice.start * m_colSlice.step, + m_colSlice.start + colSlice.stop, m_colSlice.step * colSlice.step}, + colSliceLength}; + } + + /** + * Returns a slice of the variable matrix. + * + * @param rowSlice The row slice. + * @param colSlice The column slice. + */ + const VariableBlock operator()(Slice rowSlice, + Slice colSlice) const { + int rowSliceLength = rowSlice.Adjust(m_rowSliceLength); + int colSliceLength = colSlice.Adjust(m_colSliceLength); + return VariableBlock{ + *m_mat, + {m_rowSlice.start + rowSlice.start * m_rowSlice.step, + m_rowSlice.start + rowSlice.stop, m_rowSlice.step * rowSlice.step}, + rowSliceLength, + {m_colSlice.start + colSlice.start * m_colSlice.step, + m_colSlice.start + colSlice.stop, m_colSlice.step * colSlice.step}, + colSliceLength}; + } + + /** + * Returns a slice of the variable matrix. + * + * The given slices aren't adjusted. This overload is for Python bindings + * only. + * + * @param rowSlice The row slice. + * @param rowSliceLength The row slice length. + * @param colSlice The column slice. + * @param colSliceLength The column slice length. + */ + VariableBlock operator()(Slice rowSlice, int rowSliceLength, + Slice colSlice, int colSliceLength) { + return VariableBlock{ + *m_mat, + {m_rowSlice.start + rowSlice.start * m_rowSlice.step, + m_rowSlice.start + rowSlice.stop, m_rowSlice.step * rowSlice.step}, + rowSliceLength, + {m_colSlice.start + colSlice.start * m_colSlice.step, + m_colSlice.start + colSlice.stop, m_colSlice.step * colSlice.step}, + colSliceLength}; + } + + /** + * Returns a slice of the variable matrix. + * + * The given slices aren't adjusted. This overload is for Python bindings + * only. + * + * @param rowSlice The row slice. + * @param rowSliceLength The row slice length. + * @param colSlice The column slice. + * @param colSliceLength The column slice length. + */ + const VariableBlock operator()(Slice rowSlice, int rowSliceLength, + Slice colSlice, + int colSliceLength) const { + return VariableBlock{ + *m_mat, + {m_rowSlice.start + rowSlice.start * m_rowSlice.step, + m_rowSlice.start + rowSlice.stop, m_rowSlice.step * rowSlice.step}, + rowSliceLength, + {m_colSlice.start + colSlice.start * m_colSlice.step, + m_colSlice.start + colSlice.stop, m_colSlice.step * colSlice.step}, + colSliceLength}; + } + + /** + * Returns a segment of the variable vector. + * + * @param offset The offset of the segment. + * @param length The length of the segment. + */ + VariableBlock Segment(int offset, int length) { + Assert(offset >= 0 && offset < Rows() * Cols()); + Assert(length >= 0 && length <= Rows() * Cols() - offset); + return Block(offset, 0, length, 1); + } + + /** + * Returns a segment of the variable vector. + * + * @param offset The offset of the segment. + * @param length The length of the segment. + */ + const VariableBlock Segment(int offset, int length) const { + Assert(offset >= 0 && offset < Rows() * Cols()); + Assert(length >= 0 && length <= Rows() * Cols() - offset); + return Block(offset, 0, length, 1); } /** @@ -451,12 +587,12 @@ class VariableBlock { /** * Returns number of rows in the matrix. */ - int Rows() const { return m_blockRows; } + int Rows() const { return m_rowSliceLength; } /** * Returns number of columns in the matrix. */ - int Cols() const { return m_blockCols; } + int Cols() const { return m_colSliceLength; } /** * Returns an element of the variable matrix. @@ -467,7 +603,9 @@ class VariableBlock { double Value(int row, int col) { Assert(row >= 0 && row < Rows()); Assert(col >= 0 && col < Cols()); - return (*m_mat)(m_rowOffset + row, m_colOffset + col).Value(); + return (*m_mat)(m_rowSlice.start + row * m_rowSlice.step, + m_colSlice.start + col * m_colSlice.step) + .Value(); } /** @@ -477,9 +615,7 @@ class VariableBlock { */ double Value(int index) { Assert(index >= 0 && index < Rows() * Cols()); - return (*m_mat)(m_rowOffset + index / m_blockCols, - m_colOffset + index % m_blockCols) - .Value(); + return Value(index / Cols(), index % Cols()); } /** @@ -503,7 +639,7 @@ class VariableBlock { * @param unaryOp The unary operator to use for the transform operation. */ std::remove_cv_t CwiseTransform( - function_ref unaryOp) const { + function_ref unaryOp) const { std::remove_cv_t result{Rows(), Cols()}; for (int row = 0; row < Rows(); ++row) { @@ -614,14 +750,16 @@ class VariableBlock { /** * Returns number of elements in matrix. */ - size_t size() const { return m_blockRows * m_blockCols; } + size_t size() const { return Rows() * Cols(); } private: Mat* m_mat = nullptr; - int m_rowOffset = 0; - int m_colOffset = 0; - int m_blockRows = 0; - int m_blockCols = 0; + + Slice m_rowSlice; + int m_rowSliceLength = 0; + + Slice m_colSlice; + int m_colSliceLength = 0; }; } // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableMatrix.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableMatrix.hpp index 66107af1dc..57b09913d1 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableMatrix.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableMatrix.hpp @@ -13,6 +13,7 @@ #include #include +#include "sleipnir/autodiff/Slice.hpp" #include "sleipnir/autodiff/Variable.hpp" #include "sleipnir/autodiff/VariableBlock.hpp" #include "sleipnir/util/Assert.hpp" @@ -334,7 +335,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { } /** - * Returns a block slice of the variable matrix. + * Returns a block of the variable matrix. * * @param rowOffset The row offset of the block selection. * @param colOffset The column offset of the block selection. @@ -351,7 +352,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { } /** - * Returns a block slice of the variable matrix. + * Returns a block of the variable matrix. * * @param rowOffset The row offset of the block selection. * @param colOffset The column offset of the block selection. @@ -368,6 +369,69 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { return VariableBlock{*this, rowOffset, colOffset, blockRows, blockCols}; } + /** + * Returns a slice of the variable matrix. + * + * @param rowSlice The row slice. + * @param colSlice The column slice. + */ + VariableBlock operator()(Slice rowSlice, Slice colSlice) { + int rowSliceLength = rowSlice.Adjust(Rows()); + int colSliceLength = colSlice.Adjust(Cols()); + return VariableBlock{*this, std::move(rowSlice), rowSliceLength, + std::move(colSlice), colSliceLength}; + } + + /** + * Returns a slice of the variable matrix. + * + * @param rowSlice The row slice. + * @param colSlice The column slice. + */ + const VariableBlock operator()(Slice rowSlice, + Slice colSlice) const { + int rowSliceLength = rowSlice.Adjust(Rows()); + int colSliceLength = colSlice.Adjust(Cols()); + return VariableBlock{*this, std::move(rowSlice), rowSliceLength, + std::move(colSlice), colSliceLength}; + } + + /** + * Returns a slice of the variable matrix. + * + * The given slices aren't adjusted. This overload is for Python bindings + * only. + * + * @param rowSlice The row slice. + * @param rowSliceLength The row slice length. + * @param colSlice The column slice. + * @param colSliceLength The column slice length. + * + */ + VariableBlock operator()(Slice rowSlice, int rowSliceLength, + Slice colSlice, int colSliceLength) { + return VariableBlock{*this, std::move(rowSlice), rowSliceLength, + std::move(colSlice), colSliceLength}; + } + + /** + * Returns a slice of the variable matrix. + * + * The given slices aren't adjusted. This overload is for Python bindings + * only. + * + * @param rowSlice The row slice. + * @param rowSliceLength The row slice length. + * @param colSlice The column slice. + * @param colSliceLength The column slice length. + */ + const VariableBlock operator()( + Slice rowSlice, int rowSliceLength, Slice colSlice, + int colSliceLength) const { + return VariableBlock{*this, std::move(rowSlice), rowSliceLength, + std::move(colSlice), colSliceLength}; + } + /** * Returns a segment of the variable vector. * @@ -736,7 +800,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { * @param unaryOp The unary operator to use for the transform operation. */ VariableMatrix CwiseTransform( - function_ref unaryOp) const { + function_ref unaryOp) const { VariableMatrix result{Rows(), Cols()}; for (int row = 0; row < Rows(); ++row) { @@ -896,7 +960,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { */ SLEIPNIR_DLLEXPORT inline VariableMatrix CwiseReduce( const VariableMatrix& lhs, const VariableMatrix& rhs, - function_ref binaryOp) { + function_ref binaryOp) { Assert(lhs.Rows() == rhs.Rows()); Assert(lhs.Rows() == rhs.Rows()); diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/OCPSolver.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/OCPSolver.hpp index adfca44997..663e81d9ba 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/OCPSolver.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/OCPSolver.hpp @@ -307,15 +307,6 @@ class SLEIPNIR_DLLEXPORT OCPSolver : public OptimizationProblem { } } - /** - * Convenience function to set an upper bound on the timestep. - * - * @param maxTimestep The maximum timestep. - */ - void SetMaxTimestep(std::chrono::duration maxTimestep) { - SubjectTo(DT() <= maxTimestep.count()); - } - /** * Convenience function to set a lower bound on the timestep. * @@ -325,6 +316,15 @@ class SLEIPNIR_DLLEXPORT OCPSolver : public OptimizationProblem { SubjectTo(DT() >= minTimestep.count()); } + /** + * Convenience function to set an upper bound on the timestep. + * + * @param maxTimestep The maximum timestep. + */ + void SetMaxTimestep(std::chrono::duration maxTimestep) { + SubjectTo(DT() <= maxTimestep.count()); + } + /** * Get the state variables. After the problem is solved, this will contain the * optimized trajectory. diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Constraints.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Constraints.hpp deleted file mode 100644 index f3eb8a276d..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Constraints.hpp +++ /dev/null @@ -1,325 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include -#include -#include -#include -#include - -#include - -#include "sleipnir/autodiff/Variable.hpp" -#include "sleipnir/util/Assert.hpp" -#include "sleipnir/util/Concepts.hpp" -#include "sleipnir/util/SymbolExports.hpp" - -namespace sleipnir { - -/** - * Make a list of constraints. - * - * The standard form for equality constraints is c(x) = 0, and the standard form - * for inequality constraints is c(x) ≥ 0. This function takes constraints of - * the form lhs = rhs or lhs ≥ rhs and converts them to lhs - rhs = 0 or - * lhs - rhs ≥ 0. - * - * @param lhs Left-hand side. - * @param rhs Right-hand side. - */ -template - requires(ScalarLike> || MatrixLike>) && - (ScalarLike> || MatrixLike>) && - (!std::same_as, double> || - !std::same_as, double>) -wpi::SmallVector MakeConstraints(LHS&& lhs, RHS&& rhs) { - wpi::SmallVector constraints; - - if constexpr (ScalarLike> && - ScalarLike>) { - constraints.emplace_back(lhs - rhs); - } else if constexpr (ScalarLike> && - MatrixLike>) { - int rows; - int cols; - if constexpr (EigenMatrixLike>) { - rows = rhs.rows(); - cols = rhs.cols(); - } else { - rows = rhs.Rows(); - cols = rhs.Cols(); - } - - constraints.reserve(rows * cols); - - for (int row = 0; row < rows; ++row) { - for (int col = 0; col < cols; ++col) { - // Make right-hand side zero - constraints.emplace_back(lhs - rhs(row, col)); - } - } - } else if constexpr (MatrixLike> && - ScalarLike>) { - int rows; - int cols; - if constexpr (EigenMatrixLike>) { - rows = lhs.rows(); - cols = lhs.cols(); - } else { - rows = lhs.Rows(); - cols = lhs.Cols(); - } - - constraints.reserve(rows * cols); - - for (int row = 0; row < rows; ++row) { - for (int col = 0; col < cols; ++col) { - // Make right-hand side zero - constraints.emplace_back(lhs(row, col) - rhs); - } - } - } else if constexpr (MatrixLike> && - MatrixLike>) { - int lhsRows; - int lhsCols; - if constexpr (EigenMatrixLike>) { - lhsRows = lhs.rows(); - lhsCols = lhs.cols(); - } else { - lhsRows = lhs.Rows(); - lhsCols = lhs.Cols(); - } - - [[maybe_unused]] - int rhsRows; - [[maybe_unused]] - int rhsCols; - if constexpr (EigenMatrixLike>) { - rhsRows = rhs.rows(); - rhsCols = rhs.cols(); - } else { - rhsRows = rhs.Rows(); - rhsCols = rhs.Cols(); - } - - Assert(lhsRows == rhsRows && lhsCols == rhsCols); - constraints.reserve(lhsRows * lhsCols); - - for (int row = 0; row < lhsRows; ++row) { - for (int col = 0; col < lhsCols; ++col) { - // Make right-hand side zero - constraints.emplace_back(lhs(row, col) - rhs(row, col)); - } - } - } - - return constraints; -} - -/** - * A vector of equality constraints of the form cₑ(x) = 0. - */ -struct SLEIPNIR_DLLEXPORT EqualityConstraints { - /// A vector of scalar equality constraints. - wpi::SmallVector constraints; - - /** - * Concatenates multiple equality constraints. - * - * @param equalityConstraints The list of EqualityConstraints to concatenate. - */ - EqualityConstraints( // NOLINT - std::initializer_list equalityConstraints) { - for (const auto& elem : equalityConstraints) { - constraints.insert(constraints.end(), elem.constraints.begin(), - elem.constraints.end()); - } - } - - /** - * Concatenates multiple equality constraints. - * - * This overload is for Python bindings only. - * - * @param equalityConstraints The list of EqualityConstraints to concatenate. - */ - explicit EqualityConstraints( - const std::vector& equalityConstraints) { - for (const auto& elem : equalityConstraints) { - constraints.insert(constraints.end(), elem.constraints.begin(), - elem.constraints.end()); - } - } - - /** - * Constructs an equality constraint from a left and right side. - * - * The standard form for equality constraints is c(x) = 0. This function takes - * a constraint of the form lhs = rhs and converts it to lhs - rhs = 0. - * - * @param lhs Left-hand side. - * @param rhs Right-hand side. - */ - template - requires(ScalarLike> || MatrixLike>) && - (ScalarLike> || MatrixLike>) && - (!std::same_as, double> || - !std::same_as, double>) - EqualityConstraints(LHS&& lhs, RHS&& rhs) - : constraints{MakeConstraints(lhs, rhs)} {} - - /** - * Implicit conversion operator to bool. - */ - operator bool() { // NOLINT - return std::all_of( - constraints.begin(), constraints.end(), - [](auto& constraint) { return constraint.Value() == 0.0; }); - } -}; - -/** - * A vector of inequality constraints of the form cᵢ(x) ≥ 0. - */ -struct SLEIPNIR_DLLEXPORT InequalityConstraints { - /// A vector of scalar inequality constraints. - wpi::SmallVector constraints; - - /** - * Concatenates multiple inequality constraints. - * - * @param inequalityConstraints The list of InequalityConstraints to - * concatenate. - */ - InequalityConstraints( // NOLINT - std::initializer_list inequalityConstraints) { - for (const auto& elem : inequalityConstraints) { - constraints.insert(constraints.end(), elem.constraints.begin(), - elem.constraints.end()); - } - } - - /** - * Concatenates multiple inequality constraints. - * - * This overload is for Python bindings only. - * - * @param inequalityConstraints The list of InequalityConstraints to - * concatenate. - */ - explicit InequalityConstraints( - const std::vector& inequalityConstraints) { - for (const auto& elem : inequalityConstraints) { - constraints.insert(constraints.end(), elem.constraints.begin(), - elem.constraints.end()); - } - } - - /** - * Constructs an inequality constraint from a left and right side. - * - * The standard form for inequality constraints is c(x) ≥ 0. This function - * takes a constraints of the form lhs ≥ rhs and converts it to lhs - rhs ≥ 0. - * - * @param lhs Left-hand side. - * @param rhs Right-hand side. - */ - template - requires(ScalarLike> || MatrixLike>) && - (ScalarLike> || MatrixLike>) && - (!std::same_as, double> || - !std::same_as, double>) - InequalityConstraints(LHS&& lhs, RHS&& rhs) - : constraints{MakeConstraints(lhs, rhs)} {} - - /** - * Implicit conversion operator to bool. - */ - operator bool() { // NOLINT - return std::all_of( - constraints.begin(), constraints.end(), - [](auto& constraint) { return constraint.Value() >= 0.0; }); - } -}; - -/** - * Equality operator that returns an equality constraint for two Variables. - * - * @param lhs Left-hand side. - * @param rhs Left-hand side. - */ -template - requires(ScalarLike> || MatrixLike>) && - (ScalarLike> || MatrixLike>) && - (!std::same_as, double> || - !std::same_as, double>) -EqualityConstraints operator==(LHS&& lhs, RHS&& rhs) { - return EqualityConstraints{lhs, rhs}; -} - -/** - * Less-than comparison operator that returns an inequality constraint for two - * Variables. - * - * @param lhs Left-hand side. - * @param rhs Left-hand side. - */ -template - requires(ScalarLike> || MatrixLike>) && - (ScalarLike> || MatrixLike>) && - (!std::same_as, double> || - !std::same_as, double>) -InequalityConstraints operator<(LHS&& lhs, RHS&& rhs) { - return rhs >= lhs; -} - -/** - * Less-than-or-equal-to comparison operator that returns an inequality - * constraint for two Variables. - * - * @param lhs Left-hand side. - * @param rhs Left-hand side. - */ -template - requires(ScalarLike> || MatrixLike>) && - (ScalarLike> || MatrixLike>) && - (!std::same_as, double> || - !std::same_as, double>) -InequalityConstraints operator<=(LHS&& lhs, RHS&& rhs) { - return rhs >= lhs; -} - -/** - * Greater-than comparison operator that returns an inequality constraint for - * two Variables. - * - * @param lhs Left-hand side. - * @param rhs Left-hand side. - */ -template - requires(ScalarLike> || MatrixLike>) && - (ScalarLike> || MatrixLike>) && - (!std::same_as, double> || - !std::same_as, double>) -InequalityConstraints operator>(LHS&& lhs, RHS&& rhs) { - return lhs >= rhs; -} - -/** - * Greater-than-or-equal-to comparison operator that returns an inequality - * constraint for two Variables. - * - * @param lhs Left-hand side. - * @param rhs Left-hand side. - */ -template - requires(ScalarLike> || MatrixLike>) && - (ScalarLike> || MatrixLike>) && - (!std::same_as, double> || - !std::same_as, double>) -InequalityConstraints operator>=(LHS&& lhs, RHS&& rhs) { - return InequalityConstraints{lhs, rhs}; -} - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp index b78b376189..e812fa5e34 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp @@ -15,7 +15,6 @@ #include "sleipnir/autodiff/Variable.hpp" #include "sleipnir/autodiff/VariableMatrix.hpp" -#include "sleipnir/optimization/Constraints.hpp" #include "sleipnir/optimization/SolverConfig.hpp" #include "sleipnir/optimization/SolverExitCondition.hpp" #include "sleipnir/optimization/SolverIterationInfo.hpp" diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Concepts.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Concepts.hpp index 653200e708..723284117e 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Concepts.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Concepts.hpp @@ -6,27 +6,28 @@ #include -#include "sleipnir/autodiff/Variable.hpp" -#include "sleipnir/autodiff/VariableMatrix.hpp" - namespace sleipnir { template -concept ScalarLike = std::same_as || std::same_as || - std::same_as; +concept ScalarLike = requires(T t) { + t + 1.0; + t = 1.0; +}; template -concept SleipnirMatrixLike = std::same_as || - std::same_as>; - -template -concept EigenMatrixLike = - std::derived_from>; +concept SleipnirMatrixLike = requires(T t, int rows, int cols) { + t.Rows(); + t.Cols(); + t(rows, cols); +}; template -concept EigenSolver = requires(T t) { t.solve(Eigen::VectorXd{}); }; +concept EigenMatrixLike = std::derived_from>; template concept MatrixLike = SleipnirMatrixLike || EigenMatrixLike; +template +concept EigenSolver = requires(T t) { t.solve(Eigen::VectorXd{}); }; + } // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/VariableMatrix.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/VariableMatrix.cpp index 811062a1ea..589ff8822a 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/VariableMatrix.cpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/VariableMatrix.cpp @@ -4,30 +4,6 @@ #include -namespace Eigen { - -template <> -struct NumTraits : NumTraits { - using Real = sleipnir::Variable; - using NonInteger = sleipnir::Variable; - using Nested = sleipnir::Variable; - - enum { - IsComplex = 0, - IsInteger = 0, - IsSigned = 1, - RequireInitialization = 1, - ReadCost = 1, - AddCost = 3, - MulCost = 3 - }; -}; - -} // namespace Eigen - -// For Variable equality operator -#include "sleipnir/optimization/Constraints.hpp" - namespace sleipnir { VariableMatrix Solve(const VariableMatrix& A, const VariableMatrix& B) {