[upstream_utils] Upgrade Sleipnir to support Eigen type specializations (#6924)

2026-06-19 00:41:43 +00:00 · 2024-08-04 06:26:05 -07:00
parent 712db6711a
commit 79dfdb9dc5
15 changed files with 864 additions and 511 deletions
--- 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()
--- 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
--- 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.
  */
--- 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 <vector>
 
 #include <Eigen/Core>
 +#include <wpi/SmallVector.h>
 
- #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<Variable> m_storage;
-+  wpi::SmallVector<Variable> 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 <type_traits>
- #include <vector>
- 
-+#include <wpi/SmallVector.h>
-+
- #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 <typename LHS, typename RHS>
+@@ -445,8 +445,8 @@ template <typename LHS, typename RHS>
           (ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
           (!std::same_as<std::decay_t<LHS>, double> ||
            !std::same_as<std::decay_t<RHS>, double>)
@@ -253,7 +227,7 @@ index 80da66bfef55bfc54cfdcd0478432658f60f7610..b940fcc4deb76c282b27564332db5e59
 
   if constexpr (ScalarLike<std::decay_t<LHS>> &&
                 ScalarLike<std::decay_t<RHS>>) {
-@@ -121,7 +122,7 @@ small_vector<Variable> MakeConstraints(LHS&& lhs, RHS&& rhs) {
+@@ -534,7 +534,7 @@ small_vector<Variable> 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 <vector>
+ 
+ #include <Eigen/Core>
+#include <wpi/SmallVector.h>
+ 
+ #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<Variable> m_storage;
+  wpi::SmallVector<Variable> 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
--- 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> 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.
    */
--- 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/
 }
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Slice.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Slice.hpp
@@ -0,0 +1,158 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <concepts>
+#include <limits>
+#include <utility>
+
+#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 <typename Stop>
+    requires std::same_as<Stop, slicing::none_t> ||
+             std::convertible_to<Stop, int>
+  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 <typename Start, typename Stop>
+    requires(std::same_as<Start, slicing::none_t> ||
+             std::convertible_to<Start, int>) &&
+            (std::same_as<Stop, slicing::none_t> ||
+             std::convertible_to<Stop, int>)
+  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 <typename Start, typename Stop, typename Step>
+    requires(std::same_as<Start, slicing::none_t> ||
+             std::convertible_to<Start, int>) &&
+            (std::same_as<Stop, slicing::none_t> ||
+             std::convertible_to<Stop, int>) &&
+            (std::same_as<Step, slicing::none_t> ||
+             std::convertible_to<Step, int>)
+  constexpr Slice(Start start, Stop stop, Step step) {
+    if constexpr (std::same_as<Step, slicing::none_t>) {
+      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<int>::min()) {
+      this->step = -std::numeric_limits<int>::max();
+    }
+
+    if constexpr (std::same_as<Start, slicing::none_t>) {
+      if (this->step < 0) {
+        this->start = std::numeric_limits<int>::max();
+      } else {
+        this->start = 0;
+      }
+    } else {
+      this->start = start;
+    }
+
+    if constexpr (std::same_as<Stop, slicing::none_t>) {
+      if (this->step < 0) {
+        this->stop = std::numeric_limits<int>::min();
+      } else {
+        this->stop = std::numeric_limits<int>::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<int>::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
--- 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 <algorithm>
+#include <concepts>
+#include <initializer_list>
+#include <type_traits>
 #include <utility>
+#include <vector>
+
+#include <Eigen/Core>
+#include <wpi/SmallVector.h>

 #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 <typename LHS, typename RHS>
+  requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
+          (ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
+          (!std::same_as<std::decay_t<LHS>, double> ||
+           !std::same_as<std::decay_t<RHS>, double>)
+wpi::SmallVector<Variable> MakeConstraints(LHS&& lhs, RHS&& rhs) {
+  wpi::SmallVector<Variable> constraints;
+
+  if constexpr (ScalarLike<std::decay_t<LHS>> &&
+                ScalarLike<std::decay_t<RHS>>) {
+    constraints.emplace_back(lhs - rhs);
+  } else if constexpr (ScalarLike<std::decay_t<LHS>> &&
+                       MatrixLike<std::decay_t<RHS>>) {
+    int rows;
+    int cols;
+    if constexpr (EigenMatrixLike<std::decay_t<RHS>>) {
+      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<std::decay_t<LHS>> &&
+                       ScalarLike<std::decay_t<RHS>>) {
+    int rows;
+    int cols;
+    if constexpr (EigenMatrixLike<std::decay_t<LHS>>) {
+      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<std::decay_t<LHS>> &&
+                       MatrixLike<std::decay_t<RHS>>) {
+    int lhsRows;
+    int lhsCols;
+    if constexpr (EigenMatrixLike<std::decay_t<LHS>>) {
+      lhsRows = lhs.rows();
+      lhsCols = lhs.cols();
+    } else {
+      lhsRows = lhs.Rows();
+      lhsCols = lhs.Cols();
+    }
+
+    [[maybe_unused]]
+    int rhsRows;
+    [[maybe_unused]]
+    int rhsCols;
+    if constexpr (EigenMatrixLike<std::decay_t<RHS>>) {
+      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<Variable> constraints;
+
+  /**
+   * Concatenates multiple equality constraints.
+   *
+   * @param equalityConstraints The list of EqualityConstraints to concatenate.
+   */
+  EqualityConstraints(  // NOLINT
+      std::initializer_list<EqualityConstraints> 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>& 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 <typename LHS, typename RHS>
+    requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
+            (ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
+            (!std::same_as<std::decay_t<LHS>, double> ||
+             !std::same_as<std::decay_t<RHS>, 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<Variable> constraints;
+
+  /**
+   * Concatenates multiple inequality constraints.
+   *
+   * @param inequalityConstraints The list of InequalityConstraints to
+   * concatenate.
+   */
+  InequalityConstraints(  // NOLINT
+      std::initializer_list<InequalityConstraints> 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>& 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 <typename LHS, typename RHS>
+    requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
+            (ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
+            (!std::same_as<std::decay_t<LHS>, double> ||
+             !std::same_as<std::decay_t<RHS>, 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 <typename LHS, typename RHS>
+  requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
+          (ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
+          (!std::same_as<std::decay_t<LHS>, double> ||
+           !std::same_as<std::decay_t<RHS>, 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 <typename LHS, typename RHS>
+  requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
+          (ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
+          (!std::same_as<std::decay_t<LHS>, double> ||
+           !std::same_as<std::decay_t<RHS>, 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 <typename LHS, typename RHS>
+  requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
+          (ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
+          (!std::same_as<std::decay_t<LHS>, double> ||
+           !std::same_as<std::decay_t<RHS>, 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 <typename LHS, typename RHS>
+  requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
+          (ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
+          (!std::same_as<std::decay_t<LHS>, double> ||
+           !std::same_as<std::decay_t<RHS>, 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 <typename LHS, typename RHS>
+  requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
+          (ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
+          (!std::same_as<std::decay_t<LHS>, double> ||
+           !std::same_as<std::decay_t<RHS>, 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<sleipnir::Variable> : NumTraits<double> {
+  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
--- 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 <Eigen/Core>

+#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<Mat> 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<const Mat> 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<Mat> 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<const Mat> 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<Mat> 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<Mat> 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<Mat> CwiseTransform(
-      function_ref<Variable(const Variable&)> unaryOp) const {
+      function_ref<Variable(const Variable& x)> unaryOp) const {
    std::remove_cv_t<Mat> 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
--- 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 <Eigen/Core>
 #include <wpi/SmallVector.h>

+#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<VariableMatrix> 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<const VariableMatrix> 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<VariableMatrix> 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<const VariableMatrix> 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<Variable(const Variable&)> unaryOp) const {
+      function_ref<Variable(const Variable& x)> 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<Variable(const Variable&, const Variable&)> binaryOp) {
+    function_ref<Variable(const Variable& x, const Variable& y)> binaryOp) {
  Assert(lhs.Rows() == rhs.Rows());
  Assert(lhs.Rows() == rhs.Rows());

--- 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<double> 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<double> maxTimestep) {
+    SubjectTo(DT() <= maxTimestep.count());
+  }
+
  /**
   * Get the state variables. After the problem is solved, this will contain the
   * optimized trajectory.
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Constraints.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Constraints.hpp
@@ -1,325 +0,0 @@
-// Copyright (c) Sleipnir contributors
-
-#pragma once
-
-#include <algorithm>
-#include <concepts>
-#include <initializer_list>
-#include <type_traits>
-#include <vector>
-
-#include <wpi/SmallVector.h>
-
-#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 <typename LHS, typename RHS>
-  requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
-          (ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
-          (!std::same_as<std::decay_t<LHS>, double> ||
-           !std::same_as<std::decay_t<RHS>, double>)
-wpi::SmallVector<Variable> MakeConstraints(LHS&& lhs, RHS&& rhs) {
-  wpi::SmallVector<Variable> constraints;
-
-  if constexpr (ScalarLike<std::decay_t<LHS>> &&
-                ScalarLike<std::decay_t<RHS>>) {
-    constraints.emplace_back(lhs - rhs);
-  } else if constexpr (ScalarLike<std::decay_t<LHS>> &&
-                       MatrixLike<std::decay_t<RHS>>) {
-    int rows;
-    int cols;
-    if constexpr (EigenMatrixLike<std::decay_t<RHS>>) {
-      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<std::decay_t<LHS>> &&
-                       ScalarLike<std::decay_t<RHS>>) {
-    int rows;
-    int cols;
-    if constexpr (EigenMatrixLike<std::decay_t<LHS>>) {
-      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<std::decay_t<LHS>> &&
-                       MatrixLike<std::decay_t<RHS>>) {
-    int lhsRows;
-    int lhsCols;
-    if constexpr (EigenMatrixLike<std::decay_t<LHS>>) {
-      lhsRows = lhs.rows();
-      lhsCols = lhs.cols();
-    } else {
-      lhsRows = lhs.Rows();
-      lhsCols = lhs.Cols();
-    }
-
-    [[maybe_unused]]
-    int rhsRows;
-    [[maybe_unused]]
-    int rhsCols;
-    if constexpr (EigenMatrixLike<std::decay_t<RHS>>) {
-      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<Variable> constraints;
-
-  /**
-   * Concatenates multiple equality constraints.
-   *
-   * @param equalityConstraints The list of EqualityConstraints to concatenate.
-   */
-  EqualityConstraints(  // NOLINT
-      std::initializer_list<EqualityConstraints> 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>& 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 <typename LHS, typename RHS>
-    requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
-            (ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
-            (!std::same_as<std::decay_t<LHS>, double> ||
-             !std::same_as<std::decay_t<RHS>, 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<Variable> constraints;
-
-  /**
-   * Concatenates multiple inequality constraints.
-   *
-   * @param inequalityConstraints The list of InequalityConstraints to
-   * concatenate.
-   */
-  InequalityConstraints(  // NOLINT
-      std::initializer_list<InequalityConstraints> 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>& 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 <typename LHS, typename RHS>
-    requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
-            (ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
-            (!std::same_as<std::decay_t<LHS>, double> ||
-             !std::same_as<std::decay_t<RHS>, 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 <typename LHS, typename RHS>
-  requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
-          (ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
-          (!std::same_as<std::decay_t<LHS>, double> ||
-           !std::same_as<std::decay_t<RHS>, 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 <typename LHS, typename RHS>
-  requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
-          (ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
-          (!std::same_as<std::decay_t<LHS>, double> ||
-           !std::same_as<std::decay_t<RHS>, 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 <typename LHS, typename RHS>
-  requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
-          (ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
-          (!std::same_as<std::decay_t<LHS>, double> ||
-           !std::same_as<std::decay_t<RHS>, 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 <typename LHS, typename RHS>
-  requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
-          (ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
-          (!std::same_as<std::decay_t<LHS>, double> ||
-           !std::same_as<std::decay_t<RHS>, 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 <typename LHS, typename RHS>
-  requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
-          (ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
-          (!std::same_as<std::decay_t<LHS>, double> ||
-           !std::same_as<std::decay_t<RHS>, double>)
-InequalityConstraints operator>=(LHS&& lhs, RHS&& rhs) {
-  return InequalityConstraints{lhs, rhs};
-}
-
-}  // namespace sleipnir
--- 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"
--- 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 <Eigen/Core>

-#include "sleipnir/autodiff/Variable.hpp"
-#include "sleipnir/autodiff/VariableMatrix.hpp"
-
 namespace sleipnir {

 template <typename T>
-concept ScalarLike = std::same_as<T, double> || std::same_as<T, int> ||
-                     std::same_as<T, Variable>;
+concept ScalarLike = requires(T t) {
+  t + 1.0;
+  t = 1.0;
+};

 template <typename T>
-concept SleipnirMatrixLike = std::same_as<T, VariableMatrix> ||
-                             std::same_as<T, VariableBlock<VariableMatrix>>;
-
-template <typename Derived>
-concept EigenMatrixLike =
-    std::derived_from<Derived, Eigen::MatrixBase<Derived>>;
+concept SleipnirMatrixLike = requires(T t, int rows, int cols) {
+  t.Rows();
+  t.Cols();
+  t(rows, cols);
+};

 template <typename T>
-concept EigenSolver = requires(T t) { t.solve(Eigen::VectorXd{}); };
+concept EigenMatrixLike = std::derived_from<T, Eigen::MatrixBase<T>>;

 template <typename T>
 concept MatrixLike = SleipnirMatrixLike<T> || EigenMatrixLike<T>;

+template <typename T>
+concept EigenSolver = requires(T t) { t.solve(Eigen::VectorXd{}); };
+
 }  // namespace sleipnir
--- 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 <Eigen/QR>

-namespace Eigen {
-
-template <>
-struct NumTraits<sleipnir::Variable> : NumTraits<double> {
-  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) {