[upstream_utils] Upgrade Sleipnir

Author: calcmogul

benchmark/src/main/native/cpp/CartPoleBenchmark.hpp

@@ -11,8 +11,9 @@
 
 #include "wpi/math/system/NumericalIntegration.hpp"
 
-inline slp::VariableMatrix CartPoleDynamics(const slp::VariableMatrix& x,
-                                            const slp::VariableMatrix& u) {
+inline slp::VariableMatrix<double> CartPoleDynamics(
+    const slp::VariableMatrix<double>& x,
+    const slp::VariableMatrix<double>& u) {
   constexpr double m_c = 5.0;  // Cart mass (kg)
   constexpr double m_p = 0.5;  // Pole mass (kg)
   constexpr double l = 0.5;    // Pole length (m)
@@ -25,23 +26,23 @@ inline slp::VariableMatrix CartPoleDynamics(const slp::VariableMatrix& x,
 
   //        [ m_c + m_p  m_p l cosθ]
   // M(q) = [m_p l cosθ    m_p l²  ]
-  slp::VariableMatrix M{{m_c + m_p, m_p * l * cos(theta)},
-                        {m_p * l * cos(theta), m_p * std::pow(l, 2)}};
+  slp::VariableMatrix<double> M{{m_c + m_p, m_p * l * cos(theta)},
+                                {m_p * l * cos(theta), m_p * std::pow(l, 2)}};
 
   //           [0  −m_p lθ̇ sinθ]
   // C(q, q̇) = [0       0      ]
-  slp::VariableMatrix C{{0, -m_p * l * thetadot * sin(theta)}, {0, 0}};
+  slp::VariableMatrix<double> C{{0, -m_p * l * thetadot * sin(theta)}, {0, 0}};
 
   //          [     0      ]
   // τ_g(q) = [-m_p gl sinθ]
-  slp::VariableMatrix tau_g{{0}, {-m_p * g * l * sin(theta)}};
+  slp::VariableMatrix<double> tau_g{{0}, {-m_p * g * l * sin(theta)}};
 
   //     [1]
   // B = [0]
   constexpr Eigen::Matrix<double, 2, 1> B{{1}, {0}};
 
   // q̈ = M⁻¹(q)(τ_g(q) − C(q, q̇)q̇ + Bu)
-  slp::VariableMatrix qddot{4};
+  slp::VariableMatrix<double> qddot{4};
   qddot.segment(0, 2) = qdot;
   qddot.segment(2, 2) = solve(M, tau_g - C * qdot + B * u);
   return qddot;
@@ -63,7 +64,7 @@ inline void BM_CartPole(benchmark::State& state) {
     constexpr Eigen::Vector<double, 4> x_final{
         {1.0, std::numbers::pi, 0.0, 0.0}};
 
-    slp::Problem problem;
+    slp::Problem<double> problem;
 
     // x = [q, q̇]ᵀ = [x, θ, ẋ, θ̇]ᵀ
     auto X = problem.decision_variable(4, N + 1);
@@ -95,11 +96,11 @@ inline void BM_CartPole(benchmark::State& state) {
 
     // Dynamics constraints - RK4 integration
     for (int k = 0; k < N; ++k) {
-      problem.subject_to(
-          X.col(k + 1) ==
-          wpi::math::RK4<decltype(CartPoleDynamics), slp::VariableMatrix,
-                         slp::VariableMatrix>(CartPoleDynamics, X.col(k),
-                                              U.col(k), dt));
+      problem.subject_to(X.col(k + 1) ==
+                         wpi::math::RK4<decltype(CartPoleDynamics),
+                                        slp::VariableMatrix<double>,
+                                        slp::VariableMatrix<double>>(
+                             CartPoleDynamics, X.col(k), U.col(k), dt));
     }
 
     // Minimize sum squared inputs

docs/build.gradle

@@ -97,6 +97,11 @@ doxygen.sourceSets.main {
         exclude 'wpi/util/bit.hpp'
         exclude 'wpi/util/raw_ostream.hpp'
 
+        // Sleipnir
+        exclude 'sleipnir/optimization/solver/interior_point.hpp'
+        exclude 'sleipnir/optimization/solver/newton.hpp'
+        exclude 'sleipnir/optimization/solver/sqp.hpp'
+
         // apriltag
         exclude 'apriltag_pose.h'
 

upstream_utils/sleipnir.py

@@ -18,8 +18,7 @@ def copy_upstream_src(wpilib_root: Path):
 
     # Copy Sleipnir files into allwpilib
     walk_cwd_and_copy_if(
-        lambda dp, f: (has_prefix(dp, Path("include")) or has_prefix(dp, Path("src")))
-        and f not in [".styleguide", ".styleguide-license"],
+        lambda dp, f: (has_prefix(dp, Path("include")) or has_prefix(dp, Path("src"))),
         wpimath / "src/main/native/thirdparty/sleipnir",
     )
 
@@ -49,8 +48,7 @@ def copy_upstream_src(wpilib_root: Path):
 def main():
     name = "sleipnir"
     url = "https://github.com/SleipnirGroup/Sleipnir"
-    # main on 2025-09-19
-    tag = "7f89d5547702a09e3617bc31fe5bafe6add04fab"
+    tag = "v0.3.1"
 
     sleipnir = Lib(name, url, tag, copy_upstream_src)
     sleipnir.main()

upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch

@@ -4,41 +4,41 @@ Date: Wed, 29 May 2024 16:29:55 -0700
 Subject: [PATCH 1/8] Use fmtlib
 
 ---
- include/.styleguide              |  1 +
- include/sleipnir/util/assert.hpp |  5 +++--
- include/sleipnir/util/print.hpp  | 31 ++++++++++++++++++-------------
- src/.styleguide                  |  1 +
- src/optimization/problem.cpp     |  1 +
- 5 files changed, 24 insertions(+), 15 deletions(-)
+ include/sleipnir/optimization/problem.hpp |  1 +
+ include/sleipnir/util/assert.hpp          |  5 ++--
+ include/sleipnir/util/print.hpp           | 31 +++++++++++++----------
+ 3 files changed, 22 insertions(+), 15 deletions(-)
 
-diff --git a/include/.styleguide b/include/.styleguide
-index 1b6652d3d5886cf8c9eca0d855c21031775bad7c..4f4c76204071f90bf49eddb8c2aceb583b5e09ba 100644
---- a/include/.styleguide
-+++ b/include/.styleguide
-@@ -8,5 +8,6 @@ cppSrcFileInclude {
+diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp
+index 3185466605b6604068e2807e461d07d8c856c505..95a33952a5a368c7c81491dbe849a8096357dc38 100644
+--- a/include/sleipnir/optimization/problem.hpp
++++ b/include/sleipnir/optimization/problem.hpp
+@@ -15,6 +15,7 @@
 
- includeOtherLibs {
-   ^Eigen/
-+  ^fmt/
-   ^gch/
- }
+ #include <Eigen/Core>
+ #include <Eigen/SparseCore>
++#include <fmt/chrono.h>
+ #include <gch/small_vector.hpp>
+ 
+ #include "sleipnir/autodiff/expression_type.hpp"
 diff --git a/include/sleipnir/util/assert.hpp b/include/sleipnir/util/assert.hpp
-index 75d8ffca32accbf66ffce30f073de1db2f42469b..53de01928b929793fa77885ec4a6d1a928bdc5a9 100644
+index 0846928c3da7a6047a3c271dd2d377a3b755eeab..5d432608def05b6dee6b7cbdb9a0b91a6ab5e1c2 100644
 --- a/include/sleipnir/util/assert.hpp
 +++ b/include/sleipnir/util/assert.hpp
-@@ -3,9 +3,10 @@
- #pragma once
+@@ -4,10 +4,11 @@
+ 
+ #ifdef SLEIPNIR_PYTHON
 
- #ifdef JORMUNGANDR
 -#include <format>
  #include <source_location>
  #include <stdexcept>
-+
+ 
 +#include <fmt/format.h>
++
  /**
   * Throw an exception in Python.
   */
-@@ -13,7 +14,7 @@
+@@ -15,7 +16,7 @@
    do {                                                               \
      if (!(condition)) {                                              \
        auto location = std::source_location::current();               \
@@ -48,7 +48,7 @@ index 75d8ffca32accbf66ffce30f073de1db2f42469b..53de01928b929793fa77885ec4a6d1a9
            location.line(), location.function_name(), #condition));   \
      }                                                                \
 diff --git a/include/sleipnir/util/print.hpp b/include/sleipnir/util/print.hpp
-index fe430352dabf4cd6a890dc8007237c7a261dfd4b..055d5c9fa246201f1d8ae7ddca00b1159aeb2a57 100644
+index 797df849f63d960cf10eaf847415595961868ab0..a89b7d4f9864965443405a8e79cddd8dbfc54ad3 100644
 --- a/include/sleipnir/util/print.hpp
 +++ b/include/sleipnir/util/print.hpp
 @@ -4,10 +4,15 @@
@@ -76,8 +76,8 @@ index fe430352dabf4cd6a890dc8007237c7a261dfd4b..055d5c9fa246201f1d8ae7ddca00b115
 + * Wrapper around fmt::print() that squelches write failure exceptions.
   */
  template <typename... T>
--inline void print(std::format_string<T...> fmt, T&&... args) {
-+inline void print(fmt::format_string<T...> fmt, T&&... args) {
+-void print(std::format_string<T...> fmt, T&&... args) {
++void print(fmt::format_string<T...> fmt, T&&... args) {
    try {
 -    std::print(fmt, std::forward<T>(args)...);
 +    fmt::print(fmt, std::forward<T>(args)...);
@@ -90,8 +90,8 @@ index fe430352dabf4cd6a890dc8007237c7a261dfd4b..055d5c9fa246201f1d8ae7ddca00b115
 + * Wrapper around fmt::print() that squelches write failure exceptions.
   */
  template <typename... T>
--inline void print(std::FILE* f, std::format_string<T...> fmt, T&&... args) {
-+inline void print(std::FILE* f, fmt::format_string<T...> fmt, T&&... args) {
+-void print(std::FILE* f, std::format_string<T...> fmt, T&&... args) {
++void print(std::FILE* f, fmt::format_string<T...> fmt, T&&... args) {
    try {
 -    std::print(f, fmt, std::forward<T>(args)...);
 +    fmt::print(f, fmt, std::forward<T>(args)...);
@@ -104,8 +104,8 @@ index fe430352dabf4cd6a890dc8007237c7a261dfd4b..055d5c9fa246201f1d8ae7ddca00b115
 + * Wrapper around fmt::println() that squelches write failure exceptions.
   */
  template <typename... T>
--inline void println(std::format_string<T...> fmt, T&&... args) {
-+inline void println(fmt::format_string<T...> fmt, T&&... args) {
+-void println(std::format_string<T...> fmt, T&&... args) {
++void println(fmt::format_string<T...> fmt, T&&... args) {
    try {
 -    std::println(fmt, std::forward<T>(args)...);
 +    fmt::println(fmt, std::forward<T>(args)...);
@@ -118,34 +118,11 @@ index fe430352dabf4cd6a890dc8007237c7a261dfd4b..055d5c9fa246201f1d8ae7ddca00b115
 + * Wrapper around fmt::println() that squelches write failure exceptions.
   */
  template <typename... T>
--inline void println(std::FILE* f, std::format_string<T...> fmt, T&&... args) {
-+inline void println(std::FILE* f, fmt::format_string<T...> fmt, T&&... args) {
+-void println(std::FILE* f, std::format_string<T...> fmt, T&&... args) {
++void println(std::FILE* f, fmt::format_string<T...> fmt, T&&... args) {
    try {
 -    std::println(f, fmt, std::forward<T>(args)...);
 +    fmt::println(f, fmt, std::forward<T>(args)...);
    } catch (const std::system_error&) {
    }
  }
-diff --git a/src/.styleguide b/src/.styleguide
-index 1b6652d3d5886cf8c9eca0d855c21031775bad7c..4f4c76204071f90bf49eddb8c2aceb583b5e09ba 100644
---- a/src/.styleguide
-+++ b/src/.styleguide
-@@ -8,5 +8,6 @@ cppSrcFileInclude {
- 
- includeOtherLibs {
-   ^Eigen/
-+  ^fmt/
-   ^gch/
- }
-diff --git a/src/optimization/problem.cpp b/src/optimization/problem.cpp
-index c3331197e2365934273f57422b79fa18c2b78a5b..09828cdb6d7cddff692b9d17603dc0c11cd5a3ec 100644
---- a/src/optimization/problem.cpp
-+++ b/src/optimization/problem.cpp
-@@ -11,6 +11,7 @@
- 
- #include <Eigen/Core>
- #include <Eigen/SparseCore>
-+#include <fmt/chrono.h>
- #include <gch/small_vector.hpp>
- 
- #include "optimization/bounds.hpp"

upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch

@@ -5,73 +5,72 @@ Subject: [PATCH 2/8] Use wpi::SmallVector
 
 ---
  include/sleipnir/autodiff/expression.hpp      | 4 ++--
- include/sleipnir/autodiff/variable.hpp        | 5 ++---
+ include/sleipnir/autodiff/variable.hpp        | 4 ++--
  include/sleipnir/autodiff/variable_matrix.hpp | 4 ++--
- 3 files changed, 6 insertions(+), 7 deletions(-)
+ 3 files changed, 6 insertions(+), 6 deletions(-)
 
 diff --git a/include/sleipnir/autodiff/expression.hpp b/include/sleipnir/autodiff/expression.hpp
-index bb4d8c5641a5b3d633d372674e0a35f857889cd4..53a5f6d68d3153537840c4ff45fe5e5d8b0076b7 100644
+index f5919de6c9c0be044335ce7764ded545215f0486..46814576a3db9f472329b880b94b1ab98d218867 100644
 --- a/include/sleipnir/autodiff/expression.hpp
 +++ b/include/sleipnir/autodiff/expression.hpp
-@@ -30,7 +30,7 @@ inline constexpr bool USE_POOL_ALLOCATOR = true;
- struct Expression;
- 
- inline constexpr void inc_ref_count(Expression* expr);
--inline constexpr void dec_ref_count(Expression* expr);
-+inline void dec_ref_count(Expression* expr);
+@@ -33,7 +33,7 @@ struct Expression;
+ template <typename Scalar>
+ constexpr void inc_ref_count(Expression<Scalar>* expr);
+ template <typename Scalar>
+-constexpr void dec_ref_count(Expression<Scalar>* expr);
++void dec_ref_count(Expression<Scalar>* expr);
 
  /**
   * Typedef for intrusive shared pointer to Expression.
-@@ -733,7 +733,7 @@ inline constexpr void inc_ref_count(Expression* expr) {
-  *
+@@ -801,7 +801,7 @@ constexpr void inc_ref_count(Expression<Scalar>* expr) {
   * @param expr The shared pointer's managed object.
   */
--inline constexpr void dec_ref_count(Expression* expr) {
-+inline void dec_ref_count(Expression* expr) {
+ template <typename Scalar>
+-constexpr void dec_ref_count(Expression<Scalar>* expr) {
++void dec_ref_count(Expression<Scalar>* expr) {
    // If a deeply nested tree is being deallocated all at once, calling the
    // Expression destructor when expr's refcount reaches zero can cause a stack
    // overflow. Instead, we iterate over its children to decrement their
 diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp
-index f60236811eba45c67a9638e90d5101d877ecc2d0..264f0950f293c67d6e6c7e729887090c050e40e2 100644
+index c78af7224b2ef93ad50b238117583e01940c53ce..0a55b906130d7506c80eb150644ac44c222d1368 100644
 --- a/include/sleipnir/autodiff/variable.hpp
 +++ b/include/sleipnir/autodiff/variable.hpp
-@@ -47,7 +47,7 @@ class SLEIPNIR_DLLEXPORT Variable {
+@@ -61,7 +61,7 @@ class Variable : public SleipnirBase {
    /**
     * Constructs an empty Variable.
     */
 -  explicit constexpr Variable(std::nullptr_t) : expr{nullptr} {}
 +  explicit Variable(std::nullptr_t) : expr{nullptr} {}
 
    /**
-    * Constructs a Variable from a floating point type.
-@@ -77,8 +77,7 @@ class SLEIPNIR_DLLEXPORT Variable {
+    * Constructs a Variable from a scalar type.
+@@ -116,7 +116,7 @@ class Variable : public SleipnirBase {
     *
     * @param expr The autodiff variable.
     */
--  explicit constexpr Variable(detail::ExpressionPtr&& expr)
--      : expr{std::move(expr)} {}
-+  explicit Variable(detail::ExpressionPtr&& expr) : expr{std::move(expr)} {}
+-  explicit constexpr Variable(detail::ExpressionPtr<Scalar>&& expr)
++  explicit Variable(detail::ExpressionPtr<Scalar>&& expr)
+       : expr{std::move(expr)} {}
 
    /**
-    * Assignment operator for double.
 diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp
-index e1a419ca5356660b3c1c27230d1cb2a86977fb65..349a1550235516f9853609b61feded834ef2894b 100644
+index bb66bebc01413a291242886366ce329bb5f4b70a..7ddf02c0e2f66aff8da422b874cbe9772f9fd00d 100644
 --- a/include/sleipnir/autodiff/variable_matrix.hpp
 +++ b/include/sleipnir/autodiff/variable_matrix.hpp
-@@ -1120,14 +1120,14 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
+@@ -1281,14 +1281,14 @@ class VariableMatrix : public SleipnirBase {
     *
-    * @return Begin iterator.
+    * @return Const begin iterator.
     */
 -  const_iterator cbegin() const { return const_iterator{m_storage.cbegin()}; }
 +  const_iterator cbegin() const { return const_iterator{m_storage.begin()}; }
 
    /**
-    * Returns end iterator.
+    * Returns const end iterator.
     *
-    * @return End iterator.
+    * @return Const end iterator.
     */
 -  const_iterator cend() const { return const_iterator{m_storage.cend()}; }
 +  const_iterator cend() const { return const_iterator{m_storage.end()}; }
 
    /**
-    * Returns number of elements in matrix.
+    * Returns reverse begin iterator.

upstream_utils/sleipnir_patches/0003-Use-wpi-byteswap.patch

@@ -4,22 +4,11 @@ Date: Tue, 28 Jan 2025 22:19:14 -0800
 Subject: [PATCH 3/8] Use wpi::byteswap()
 
 ---
- include/.styleguide           | 1 +
  include/sleipnir/util/spy.hpp | 3 ++-
- 2 files changed, 3 insertions(+), 1 deletion(-)
+ 1 file changed, 2 insertions(+), 1 deletion(-)
 
-diff --git a/include/.styleguide b/include/.styleguide
-index 4f4c76204071f90bf49eddb8c2aceb583b5e09ba..03938557c2600a7a1f72c6b93c935602f5acb2b2 100644
---- a/include/.styleguide
-+++ b/include/.styleguide
-@@ -10,4 +10,5 @@ includeOtherLibs {
-   ^Eigen/
-   ^fmt/
-   ^gch/
-+  ^wpi/
- }
 diff --git a/include/sleipnir/util/spy.hpp b/include/sleipnir/util/spy.hpp
-index a2f94803e3744cee771669210d1af883160e9896..74dd7990b03783ce805a186920d5142caeb178c6 100644
+index f9143f2b925064e9df5c763823dcf3d435e7aa28..4b810e54a8038162e03cf08fc8eab52b67b2cdd5 100644
 --- a/include/sleipnir/util/spy.hpp
 +++ b/include/sleipnir/util/spy.hpp
 @@ -12,6 +12,7 @@
@@ -28,9 +17,9 @@ index a2f94803e3744cee771669210d1af883160e9896..74dd7990b03783ce805a186920d5142c
  #include <Eigen/SparseCore>
 +#include <wpi/util/bit.hpp>
 
- #include "sleipnir/util/symbol_exports.hpp"
+ namespace slp {
 
-@@ -115,7 +116,7 @@ class SLEIPNIR_DLLEXPORT Spy {
+@@ -114,7 +115,7 @@ class Spy {
     */
    void write32le(int32_t num) {
      if constexpr (std::endian::native != std::endian::little) {