[upstream_utils] Upgrade to Sleipnir 0.6.0 (#8923)

upstream_utils/sleipnir.py

@@ -46,7 +46,7 @@ using small_vector = wpi::util::SmallVector<T>;
 def main():
     name = "sleipnir"
     url = "https://github.com/SleipnirGroup/Sleipnir"
-    tag = "v0.5.2"
+    tag = "v0.6.0"
 
     sleipnir = Lib(name, url, tag, copy_upstream_src)
     sleipnir.main()

upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch

@@ -7,9 +7,10 @@ Subject: [PATCH 01/10] Use fmtlib
  include/sleipnir/autodiff/expression_type.hpp | 16 +++++++----
  include/sleipnir/optimization/problem.hpp     |  1 +
  .../optimization/solver/exit_status.hpp       | 16 +++++++----
+ .../optimization/solver/util/inertia.hpp      | 22 ++++++++-------
  include/sleipnir/util/assert.hpp              |  5 ++--
  include/sleipnir/util/print.hpp               | 27 ++++++++++---------
- 5 files changed, 40 insertions(+), 25 deletions(-)
+ 6 files changed, 53 insertions(+), 34 deletions(-)
 
 diff --git a/include/sleipnir/autodiff/expression_type.hpp b/include/sleipnir/autodiff/expression_type.hpp
 index 12d0568f628d5b97c0c2f5291851d4b20921f9d3..d06d32dac6c7b6faeedefeaa107cedac8446a3ab 100644
@@ -66,7 +67,7 @@ index 12d0568f628d5b97c0c2f5291851d4b20921f9d3..d06d32dac6c7b6faeedefeaa107cedac
 +
 +// @endcond
 diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp
-index a9553ffbcfed568c48f7d789d8b127790dfddb91..3de6d5bf89a65fe07784350c3b1a4691dfc0c822 100644
+index ccff8b9425b45fd1c790bc4da87b81f501a70d9c..41b44578b1e2783dbceb300b7c72ee0c8d1202df 100644
 --- a/include/sleipnir/optimization/problem.hpp
 +++ b/include/sleipnir/optimization/problem.hpp
 @@ -15,6 +15,7 @@
@@ -131,6 +132,68 @@ index 0a48df7423b5a3dccd8e611e91befd32487fafdc..8786d6d64ac44ac88133df65a79636ec
  };
 +
 +// @endcond
+diff --git a/include/sleipnir/optimization/solver/util/inertia.hpp b/include/sleipnir/optimization/solver/util/inertia.hpp
+index 49ee606f7e3f02f2f5c24079eb27a412dad3055f..2d2ce114be9c40e645d685b63465b977e0b70439 100644
+--- a/include/sleipnir/optimization/solver/util/inertia.hpp
++++ b/include/sleipnir/optimization/solver/util/inertia.hpp
+@@ -2,10 +2,10 @@
+ 
+ #pragma once
+ 
+-#include <format>
+ #include <limits>
+ 
+ #include <Eigen/Core>
++#include <fmt/format.h>
+ 
+ namespace slp {
+ 
+@@ -77,14 +77,16 @@ class Inertia {
+ 
+ }  // namespace slp
+ 
++// @cond Suppress Doxygen
++
+ /// Formatter for Inertia.
+ template <>
+-struct std::formatter<slp::Inertia> {
++struct fmt::formatter<slp::Inertia> {
+   /// Parses format string.
+   ///
+   /// @param ctx Format parse context.
+   /// @return Format parse context iterator.
+-  constexpr auto parse(std::format_parse_context& ctx) {
++  constexpr auto parse(fmt::format_parse_context& ctx) {
+     return m_underlying.parse(ctx);
+   }
+ 
+@@ -95,18 +97,20 @@ struct std::formatter<slp::Inertia> {
+   /// @param ctx Format context.
+   /// @return Format context iterator.
+   template <typename FmtContext>
+-  auto format(const slp::Inertia& inertia, FmtContext& ctx) const {
++  constexpr auto format(const slp::Inertia& inertia, FmtContext& ctx) const {
+     auto out = ctx.out();
+ 
+-    out = std::format_to(out, "(");
++    out = fmt::format_to(out, "(");
+     out = m_underlying.format(inertia.positive, ctx);
+-    out = std::format_to(out, ", ");
++    out = fmt::format_to(out, ", ");
+     out = m_underlying.format(inertia.negative, ctx);
+-    out = std::format_to(out, ", ");
++    out = fmt::format_to(out, ", ");
+     out = m_underlying.format(inertia.zero, ctx);
+-    return std::format_to(out, ")");
++    return fmt::format_to(out, ")");
+   }
+ 
+  private:
+-  std::formatter<int> m_underlying;
++  fmt::formatter<int> m_underlying;
+ };
++
++// @endcond
 diff --git a/include/sleipnir/util/assert.hpp b/include/sleipnir/util/assert.hpp
 index 29dfe5416530a476417de3a4b6434d3b28d73120..783a0e71a1cb54bd1d6d17fa9d2bedd2038a8ef5 100644
 --- a/include/sleipnir/util/assert.hpp

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

@@ -4,13 +4,12 @@ Date: Sun, 16 Jun 2024 12:08:49 -0700
 Subject: [PATCH 02/10] Use wpi::SmallVector
 
 ---
- include/sleipnir/autodiff/expression.hpp      | 4 ++--
- include/sleipnir/autodiff/variable.hpp        | 4 ++--
- include/sleipnir/autodiff/variable_matrix.hpp | 4 ++--
- 3 files changed, 6 insertions(+), 6 deletions(-)
+ include/sleipnir/autodiff/expression.hpp | 4 ++--
+ include/sleipnir/autodiff/variable.hpp   | 4 ++--
+ 2 files changed, 4 insertions(+), 4 deletions(-)
 
 diff --git a/include/sleipnir/autodiff/expression.hpp b/include/sleipnir/autodiff/expression.hpp
-index 76f6d4ed534ce685194ad7fc62d9b62eb2ca1096..10e20f5e2fb0d7459fcecac2f8ba1bdcc98efc3f 100644
+index af10b505ae44ca0eb656f66f3f49172d7fc98c6d..7a9dc089a9e2fb323ecc8725f924ebee1b213af0 100644
 --- a/include/sleipnir/autodiff/expression.hpp
 +++ b/include/sleipnir/autodiff/expression.hpp
 @@ -34,7 +34,7 @@ struct Expression;
@@ -22,7 +21,7 @@ index 76f6d4ed534ce685194ad7fc62d9b62eb2ca1096..10e20f5e2fb0d7459fcecac2f8ba1bdc
  
  /// Typedef for intrusive shared pointer to Expression.
  ///
-@@ -736,7 +736,7 @@ constexpr void inc_ref_count(Expression<Scalar>* expr) {
+@@ -744,7 +744,7 @@ constexpr void inc_ref_count(Expression<Scalar>* expr) {
  /// @tparam Scalar Scalar type.
  /// @param expr The shared pointer's managed object.
  template <typename Scalar>
@@ -53,22 +52,3 @@ index 910ebcf5266bdf76711db7849dd6584549094e3b..3c4f67c1f6224226620e2ffcc5973364
        : expr{std::move(expr)} {}
  
    /// Assignment operator for scalar.
-diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp
-index 8ed39dfae69c70e0b32998eb5e0c5207383f4fa2..3eeaccac274cf38d554ea834b3ff666615939382 100644
---- a/include/sleipnir/autodiff/variable_matrix.hpp
-+++ b/include/sleipnir/autodiff/variable_matrix.hpp
-@@ -1134,12 +1134,12 @@ class VariableMatrix : public SleipnirBase {
-   /// Returns const 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 const 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 reverse begin iterator.
-   ///

upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch

@@ -11,10 +11,10 @@ Subject: [PATCH 04/10] Replace std::to_underlying()
  create mode 100644 include/sleipnir/util/to_underlying.hpp
 
 diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp
-index 3de6d5bf89a65fe07784350c3b1a4691dfc0c822..273363db2cf75c84d81f2fef36dcb2094303ee2e 100644
+index 41b44578b1e2783dbceb300b7c72ee0c8d1202df..49921b98de3452b6ca9f2e33a86daa254c0d3a01 100644
 --- a/include/sleipnir/optimization/problem.hpp
 +++ b/include/sleipnir/optimization/problem.hpp
-@@ -37,6 +37,7 @@
+@@ -38,6 +38,7 @@
  #include "sleipnir/util/profiler.hpp"
  #include "sleipnir/util/spy.hpp"
  #include "sleipnir/util/symbol_exports.hpp"
@@ -22,7 +22,7 @@ index 3de6d5bf89a65fe07784350c3b1a4691dfc0c822..273363db2cf75c84d81f2fef36dcb209
  
  namespace slp {
  
-@@ -752,11 +753,11 @@ class Problem {
+@@ -773,11 +774,11 @@ class Problem {
      // Print problem structure
      slp::println("\nProblem structure:");
      slp::println("  ↳ {} cost function",
@@ -37,7 +37,7 @@ index 3de6d5bf89a65fe07784350c3b1a4691dfc0c822..273363db2cf75c84d81f2fef36dcb209
  
      if (m_decision_variables.size() == 1) {
        slp::print("\n1 decision variable\n");
-@@ -768,7 +769,7 @@ class Problem {
+@@ -789,7 +790,7 @@ class Problem {
          [](const gch::small_vector<Variable<Scalar>>& constraints) {
            std::array<size_t, 5> counts{};
            for (const auto& constraint : constraints) {
@@ -47,7 +47,7 @@ index 3de6d5bf89a65fe07784350c3b1a4691dfc0c822..273363db2cf75c84d81f2fef36dcb209
            for (const auto& [count, name] :
                 std::views::zip(counts, std::array{"empty", "constant", "linear",
 diff --git a/include/sleipnir/util/print_diagnostics.hpp b/include/sleipnir/util/print_diagnostics.hpp
-index e16ef1b03e675b7ccb8db6781568eec7a44aace7..69cb99957cc7da5abc4162c53a229f7f642bbe4b 100644
+index 87ccad9ef4b05ee5aeaaf0e5ff9cfbc7e995d5ed..831ae3e5d1002005a8aa00fc80accd705d5cca7d 100644
 --- a/include/sleipnir/util/print_diagnostics.hpp
 +++ b/include/sleipnir/util/print_diagnostics.hpp
 @@ -17,6 +17,7 @@
@@ -58,15 +58,15 @@ index e16ef1b03e675b7ccb8db6781568eec7a44aace7..69cb99957cc7da5abc4162c53a229f7f
  
  namespace slp {
  
-@@ -239,7 +240,7 @@ void print_iteration_diagnostics(int iterations, IterationType type,
+@@ -230,7 +231,7 @@ void print_iteration_diagnostics(int iterations, IterationType type,
    slp::println(
-       "│{:4} {:4} {:9.3f} {:12e} {:13e} {:12e} {:12e} {:.2e} {:<5} {:.2e} "
-       "{:.2e} {:2d}│",
+       "│{:4} {:1} {:9.3f} {:10.4e} {:11.4e} {:10.4e} {:8.2e} {:8.2e} {:<5} "
+       "{:<5} {:8.2e} {:8.2e} {:8.2e} {:8.2e} {:2d}│",
 -      iterations, ITERATION_TYPES[std::to_underlying(type)], to_ms(time), error,
 +      iterations, ITERATION_TYPES[slp::to_underlying(type)], to_ms(time), error,
-       cost, infeasibility, complementarity, μ, power_of_10(δ), primal_α, dual_α,
+       cost, infeasibility, complementarity, μ, power_of_10(δ), power_of_10(γ),
+       full_primal_step_inf_norm, full_dual_step_inf_norm, primal_α, dual_α,
        backtracks);
- }
 diff --git a/include/sleipnir/util/to_underlying.hpp b/include/sleipnir/util/to_underlying.hpp
 new file mode 100644
 index 0000000000000000000000000000000000000000..3f9b4835b912c5a0d998c43828f255c61d0f573e

upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch

@@ -9,26 +9,26 @@ Subject: [PATCH 05/10] Replace std::views::zip()
  2 files changed, 9 insertions(+), 4 deletions(-)
 
 diff --git a/include/sleipnir/autodiff/gradient_expression_graph.hpp b/include/sleipnir/autodiff/gradient_expression_graph.hpp
-index 275c30b76d34efe7ee1608cd4eedfa54ab2dc1ec..c0e3c161343175837abcb25fb22da0c611a44799 100644
+index fda7931c686a315209acdbe6f1ac581944e2781c..d3e59068d04b3db01945c5657959b69cd4522cca 100644
 --- a/include/sleipnir/autodiff/gradient_expression_graph.hpp
 +++ b/include/sleipnir/autodiff/gradient_expression_graph.hpp
-@@ -161,7 +161,10 @@ class GradientExpressionGraph {
-         }
+@@ -139,7 +139,10 @@ class GradientExpressionGraph {
        }
-     } else {
--      for (const auto& [col, node] : std::views::zip(m_col_list, m_top_list)) {
-+      for (size_t i = 0; i < m_top_list.size(); ++i) {
-+        const auto& col = m_col_list[i];
-+        const auto& node = m_top_list[i];
+     }
+ 
+-    for (const auto& [col, node] : std::views::zip(m_col_list, m_top_list)) {
++    for (size_t i = 0; i < m_top_list.size(); ++i) {
++      const auto& col = m_col_list[i];
++      const auto& node = m_top_list[i];
 +
-         // Append adjoints of wrt to sparse matrix triplets
-         if (col != -1 && node->adjoint != Scalar(0)) {
-           triplets.emplace_back(row, col, node->adjoint);
+       // Append adjoints of wrt to sparse matrix triplets
+       if (col != -1) {
+         triplets.emplace_back(row, col, node->adjoint);
 diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp
-index 273363db2cf75c84d81f2fef36dcb2094303ee2e..c4a45f7f954c9e028b1c85c45d5034bde284474f 100644
+index 49921b98de3452b6ca9f2e33a86daa254c0d3a01..d3feadd577bd53147a8b07da76910e046ccbf95d 100644
 --- a/include/sleipnir/optimization/problem.hpp
 +++ b/include/sleipnir/optimization/problem.hpp
-@@ -771,9 +771,11 @@ class Problem {
+@@ -792,9 +792,11 @@ class Problem {
            for (const auto& constraint : constraints) {
              ++counts[slp::to_underlying(constraint.type())];
            }

upstream_utils/sleipnir_patches/0008-Suppress-GCC-12-warning-false-positive.patch

@@ -8,7 +8,7 @@ Subject: [PATCH 08/10] Suppress GCC 12 warning false positive
  1 file changed, 7 insertions(+)
 
 diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp
-index 3eeaccac274cf38d554ea834b3ff666615939382..38b90b4a1c784cfd2fd806536d001db9335ebe2d 100644
+index 2cf6e00f16f58a5847a4515f13b3a91fffbb72b8..b5d15edc5b294a2ea50dca15a7612c61e5f2680b 100644
 --- a/include/sleipnir/autodiff/variable_matrix.hpp
 +++ b/include/sleipnir/autodiff/variable_matrix.hpp
 @@ -503,6 +503,10 @@ class VariableMatrix : public SleipnirBase {

upstream_utils/sleipnir_patches/0009-Suppress-Doxygen-warning-false-positives.patch

@@ -8,7 +8,7 @@ Subject: [PATCH 09/10] Suppress Doxygen warning false positives
  1 file changed, 4 insertions(+)
 
 diff --git a/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp b/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp
-index ec87b2b8160051224de870c22f5ded6b35f6dc2d..1c042f8599e8e179c1586b2648e1ff99e5b5d32a 100644
+index 6324ddcf2d24a0cd5367420af4330e1c59b58004..ed128e5a583d51bc963f03f6b88c0a15e6440545 100644
 --- a/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp
 +++ b/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp
 @@ -96,6 +96,8 @@ compute_p_n(const Eigen::Vector<Scalar, Eigen::Dynamic>& c, Scalar ρ,
@@ -20,7 +20,7 @@ index ec87b2b8160051224de870c22f5ded6b35f6dc2d..1c042f8599e8e179c1586b2648e1ff99
  /// Finds the iterate that minimizes the constraint violation while not
  /// deviating too far from the starting point. This is a fallback procedure when
  /// the normal Sequential Quadratic Programming method fails to converge to a
-@@ -592,6 +594,8 @@ ExitStatus feasibility_restoration(
+@@ -624,6 +626,8 @@ ExitStatus feasibility_restoration(
    }
  }
  

upstream_utils/sleipnir_patches/0010-Use-operator-instead-of-multidimensional-array-subsc.patch

@@ -15,10 +15,10 @@ Subject: [PATCH 10/10] Use operator() instead of multidimensional array
  7 files changed, 135 insertions(+), 135 deletions(-)
 
 diff --git a/include/sleipnir/autodiff/hessian.hpp b/include/sleipnir/autodiff/hessian.hpp
-index 11af6033d1cf00b27cb9ff796c4a7f16b32df90a..d4cc02f54accd568d052ae5709213d45c4c8917a 100644
+index 2a8087eda23f49f8c01be35d5289568644a9742d..c48e01c7b89d80db8032f6d2e492a0ec1bfa9f42 100644
 --- a/include/sleipnir/autodiff/hessian.hpp
 +++ b/include/sleipnir/autodiff/hessian.hpp
-@@ -99,9 +99,9 @@ class Hessian {
+@@ -100,9 +100,9 @@ class Hessian {
        auto grad = m_graphs[row].generate_tree(m_wrt);
        for (int col = 0; col < m_wrt.rows(); ++col) {
          if (grad[col].expr != nullptr) {
@@ -31,7 +31,7 @@ index 11af6033d1cf00b27cb9ff796c4a7f16b32df90a..d4cc02f54accd568d052ae5709213d45
        }
      }
 diff --git a/include/sleipnir/autodiff/jacobian.hpp b/include/sleipnir/autodiff/jacobian.hpp
-index f1915a0d08c509f4eb4d032d4e6a5e6099b68ebc..edd949eefd018af1b1e091b4b2780fc1a0a492cf 100644
+index 7a5c3798dab50f14db35bee430f1d5e8e32196ff..1a5b01de99a7c011ebf8c5ed9fc64942675fcb4b 100644
 --- a/include/sleipnir/autodiff/jacobian.hpp
 +++ b/include/sleipnir/autodiff/jacobian.hpp
 @@ -104,9 +104,9 @@ class Jacobian {
@@ -388,7 +388,7 @@ index 4018606df45941b578c861caf934495f8c9e368e..cf554832b82adb17b4b1d7b56842a77d
      }
  
 diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp
-index 38b90b4a1c784cfd2fd806536d001db9335ebe2d..e9f90fb12d230e40497e8557b985ff9c82ebaaeb 100644
+index b5d15edc5b294a2ea50dca15a7612c61e5f2680b..02bccb660474d9380444f5e20124fb687e372982 100644
 --- a/include/sleipnir/autodiff/variable_matrix.hpp
 +++ b/include/sleipnir/autodiff/variable_matrix.hpp
 @@ -154,7 +154,7 @@ class VariableMatrix : public SleipnirBase {
@@ -959,10 +959,10 @@ index 5e20b9b8debc13611d5d719b589d8fd896c35a90..e5163f9c2dbbfa0a580d029e61c741e6
        if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) {
          x_end = rk4<const decltype(m_dynamics)&, VariableMatrix<Scalar>,
 diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp
-index c4a45f7f954c9e028b1c85c45d5034bde284474f..4e38a082855c07c47f1aa191613114a5a392a4e4 100644
+index d3feadd577bd53147a8b07da76910e046ccbf95d..18afc996590de98050bfd3ddc629952bf0c8c2e2 100644
 --- a/include/sleipnir/optimization/problem.hpp
 +++ b/include/sleipnir/optimization/problem.hpp
-@@ -97,7 +97,7 @@ class Problem {
+@@ -98,7 +98,7 @@ class Problem {
      for (int row = 0; row < rows; ++row) {
        for (int col = 0; col < cols; ++col) {
          m_decision_variables.emplace_back();
@@ -971,7 +971,7 @@ index c4a45f7f954c9e028b1c85c45d5034bde284474f..4e38a082855c07c47f1aa191613114a5
        }
      }
  
-@@ -132,8 +132,8 @@ class Problem {
+@@ -133,8 +133,8 @@ class Problem {
      for (int row = 0; row < rows; ++row) {
        for (int col = 0; col <= row; ++col) {
          m_decision_variables.emplace_back();