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

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

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

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

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());
 

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.

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

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"

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