[wpimath] Add Sleipnir (#6541)

This is useful for solving quadratic programs.

wpimath/src/main/native/thirdparty/sleipnir/include/.styleguide

@@ -0,0 +1,12 @@
+cppHeaderFileInclude {
+  \.hpp$
+}
+
+cppSrcFileInclude {
+  \.cpp$
+}
+
+includeOtherLibs {
+  ^Eigen/
+  ^fmt/
+}

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/ExpressionGraph.hpp

@@ -0,0 +1,243 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <span>
+#include <vector>
+
+#include "sleipnir/autodiff/Expression.hpp"
+#include "sleipnir/util/FunctionRef.hpp"
+#include "sleipnir/util/SymbolExports.hpp"
+
+namespace sleipnir::detail {
+
+/**
+ * This class is an adaptor type that performs value updates of an expression's
+ * computational graph in a way that skips duplicates.
+ */
+class SLEIPNIR_DLLEXPORT ExpressionGraph {
+ public:
+  /**
+   * Generates the deduplicated computational graph for the given expression.
+   *
+   * @param root The root node of the expression.
+   */
+  explicit ExpressionGraph(ExpressionPtr& root) {
+    // If the root type is a constant, Update() is a no-op, so there's no work
+    // to do
+    if (root == nullptr || root->type == ExpressionType::kConstant) {
+      return;
+    }
+
+    // Breadth-first search (BFS) is used as opposed to a depth-first search
+    // (DFS) to avoid counting duplicate nodes multiple times. A list of nodes
+    // ordered from parent to child with no duplicates is generated.
+    //
+    // https://en.wikipedia.org/wiki/Breadth-first_search
+
+    // BFS list sorted from parent to child.
+    std::vector<Expression*> stack;
+
+    stack.emplace_back(root.Get());
+
+    // Initialize the number of instances of each node in the tree
+    // (Expression::duplications)
+    while (!stack.empty()) {
+      auto currentNode = stack.back();
+      stack.pop_back();
+
+      for (auto&& arg : currentNode->args) {
+        // Only continue if the node is not a constant and hasn't already been
+        // explored.
+        if (arg != nullptr && arg->type != ExpressionType::kConstant) {
+          // If this is the first instance of the node encountered (it hasn't
+          // been explored yet), add it to stack so it's recursed upon
+          if (arg->duplications == 0) {
+            stack.push_back(arg.Get());
+          }
+          ++arg->duplications;
+        }
+      }
+    }
+
+    stack.emplace_back(root.Get());
+
+    while (!stack.empty()) {
+      auto currentNode = stack.back();
+      stack.pop_back();
+
+      // BFS lists sorted from parent to child.
+      m_rowList.emplace_back(currentNode->row);
+      m_adjointList.emplace_back(currentNode);
+      if (currentNode->valueFunc != nullptr) {
+        // Constants are skipped because they have no valueFunc and don't need
+        // to be updated
+        m_valueList.emplace_back(currentNode);
+      }
+
+      for (auto&& arg : currentNode->args) {
+        // Only add node if it's not a constant and doesn't already exist in the
+        // tape.
+        if (arg != nullptr && arg->type != ExpressionType::kConstant) {
+          // Once the number of node visitations equals the number of
+          // duplications (the counter hits zero), add it to the stack. Note
+          // that this means the node is only enqueued once.
+          --arg->duplications;
+          if (arg->duplications == 0) {
+            stack.push_back(arg.Get());
+          }
+        }
+      }
+    }
+  }
+
+  /**
+   * Update the values of all nodes in this computational tree based on the
+   * values of their dependent nodes.
+   */
+  void Update() {
+    // Traverse the BFS list backward from child to parent and update the value
+    // of each node.
+    for (auto it = m_valueList.rbegin(); it != m_valueList.rend(); ++it) {
+      auto& node = *it;
+
+      auto& lhs = node->args[0];
+      auto& rhs = node->args[1];
+
+      if (lhs != nullptr) {
+        if (rhs != nullptr) {
+          node->value = node->valueFunc(lhs->value, rhs->value);
+        } else {
+          node->value = node->valueFunc(lhs->value, 0.0);
+        }
+      }
+    }
+  }
+
+  /**
+   * Returns the variable's gradient tree.
+   *
+   * @param wrt Variables with respect to which to compute the gradient.
+   */
+  std::vector<ExpressionPtr> GenerateGradientTree(
+      std::span<const ExpressionPtr> wrt) const {
+    // Read docs/algorithms.md#Reverse_accumulation_automatic_differentiation
+    // for background on reverse accumulation automatic differentiation.
+
+    for (size_t row = 0; row < wrt.size(); ++row) {
+      wrt[row]->row = row;
+    }
+
+    std::vector<ExpressionPtr> grad;
+    grad.reserve(wrt.size());
+    for (size_t row = 0; row < wrt.size(); ++row) {
+      grad.emplace_back(MakeExpressionPtr());
+    }
+
+    // Zero adjoints. The root node's adjoint is 1.0 as df/df is always 1.
+    if (m_adjointList.size() > 0) {
+      m_adjointList[0]->adjointExpr = MakeExpressionPtr(1.0);
+      for (auto it = m_adjointList.begin() + 1; it != m_adjointList.end();
+           ++it) {
+        auto& node = *it;
+        node->adjointExpr = MakeExpressionPtr();
+      }
+    }
+
+    // df/dx = (df/dy)(dy/dx). The adjoint of x is equal to the adjoint of y
+    // multiplied by dy/dx. If there are multiple "paths" from the root node to
+    // variable; the variable's adjoint is the sum of each path's adjoint
+    // contribution.
+    for (auto node : m_adjointList) {
+      auto& lhs = node->args[0];
+      auto& rhs = node->args[1];
+
+      if (lhs != nullptr && !lhs->IsConstant(0.0)) {
+        lhs->adjointExpr = lhs->adjointExpr +
+                           node->gradientFuncs[0](lhs, rhs, node->adjointExpr);
+      }
+      if (rhs != nullptr && !rhs->IsConstant(0.0)) {
+        rhs->adjointExpr = rhs->adjointExpr +
+                           node->gradientFuncs[1](lhs, rhs, node->adjointExpr);
+      }
+
+      // If variable is a leaf node, assign its adjoint to the gradient.
+      if (node->row != -1) {
+        grad[node->row] = node->adjointExpr;
+      }
+    }
+
+    // Unlink adjoints to avoid circular references between them and their
+    // parent expressions. This ensures all expressions are returned to the free
+    // list.
+    for (auto node : m_adjointList) {
+      for (auto& arg : node->args) {
+        if (arg != nullptr) {
+          arg->adjointExpr = nullptr;
+        }
+      }
+    }
+
+    for (size_t row = 0; row < wrt.size(); ++row) {
+      wrt[row]->row = -1;
+    }
+
+    return grad;
+  }
+
+  /**
+   * Updates the adjoints in the expression graph, effectively computing the
+   * gradient.
+   *
+   * @param func A function that takes two arguments: an int for the gradient
+   *   row, and a double for the adjoint (gradient value).
+   */
+  void ComputeAdjoints(function_ref<void(int, double)> func) {
+    // Zero adjoints. The root node's adjoint is 1.0 as df/df is always 1.
+    m_adjointList[0]->adjoint = 1.0;
+    for (auto it = m_adjointList.begin() + 1; it != m_adjointList.end(); ++it) {
+      auto& node = *it;
+      node->adjoint = 0.0;
+    }
+
+    // df/dx = (df/dy)(dy/dx). The adjoint of x is equal to the adjoint of y
+    // multiplied by dy/dx. If there are multiple "paths" from the root node to
+    // variable; the variable's adjoint is the sum of each path's adjoint
+    // contribution.
+    for (size_t col = 0; col < m_adjointList.size(); ++col) {
+      auto& node = m_adjointList[col];
+      auto& lhs = node->args[0];
+      auto& rhs = node->args[1];
+
+      if (lhs != nullptr) {
+        if (rhs != nullptr) {
+          lhs->adjoint += node->gradientValueFuncs[0](lhs->value, rhs->value,
+                                                      node->adjoint);
+          rhs->adjoint += node->gradientValueFuncs[1](lhs->value, rhs->value,
+                                                      node->adjoint);
+        } else {
+          lhs->adjoint +=
+              node->gradientValueFuncs[0](lhs->value, 0.0, node->adjoint);
+        }
+      }
+
+      // If variable is a leaf node, assign its adjoint to the gradient.
+      int row = m_rowList[col];
+      if (row != -1) {
+        func(row, node->adjoint);
+      }
+    }
+  }
+
+ private:
+  // List that maps nodes to their respective row.
+  std::vector<int> m_rowList;
+
+  // List for updating adjoints
+  std::vector<Expression*> m_adjointList;
+
+  // List for updating values
+  std::vector<Expression*> m_valueList;
+};
+
+}  // namespace sleipnir::detail

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/ExpressionType.hpp

@@ -0,0 +1,27 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <stdint.h>
+
+namespace sleipnir {
+
+/**
+ * Expression type.
+ *
+ * Used for autodiff caching.
+ */
+enum class ExpressionType : uint8_t {
+  /// There is no expression.
+  kNone,
+  /// The expression is a constant.
+  kConstant,
+  /// The expression is composed of linear and lower-order operators.
+  kLinear,
+  /// The expression is composed of quadratic and lower-order operators.
+  kQuadratic,
+  /// The expression is composed of nonlinear and lower-order operators.
+  kNonlinear
+};
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Gradient.hpp

@@ -0,0 +1,78 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <utility>
+
+#include <Eigen/SparseCore>
+
+#include "sleipnir/autodiff/Jacobian.hpp"
+#include "sleipnir/autodiff/Profiler.hpp"
+#include "sleipnir/autodiff/Variable.hpp"
+#include "sleipnir/autodiff/VariableMatrix.hpp"
+#include "sleipnir/util/SymbolExports.hpp"
+
+namespace sleipnir {
+
+/**
+ * This class calculates the gradient of a a variable with respect to a vector
+ * of variables.
+ *
+ * The gradient is only recomputed if the variable expression is quadratic or
+ * higher order.
+ */
+class SLEIPNIR_DLLEXPORT Gradient {
+ public:
+  /**
+   * Constructs a Gradient object.
+   *
+   * @param variable Variable of which to compute the gradient.
+   * @param wrt Variable with respect to which to compute the gradient.
+   */
+  Gradient(Variable variable, Variable wrt) noexcept
+      : Gradient{std::move(variable), VariableMatrix{wrt}} {}
+
+  /**
+   * Constructs a Gradient object.
+   *
+   * @param variable Variable of which to compute the gradient.
+   * @param wrt Vector of variables with respect to which to compute the
+   *   gradient.
+   */
+  Gradient(Variable variable, const VariableMatrix& wrt) noexcept
+      : m_jacobian{variable, wrt} {}
+
+  /**
+   * Returns the gradient as a VariableMatrix.
+   *
+   * This is useful when constructing optimization problems with derivatives in
+   * them.
+   */
+  VariableMatrix Get() const { return m_jacobian.Get().T(); }
+
+  /**
+   * Evaluates the gradient at wrt's value.
+   */
+  const Eigen::SparseVector<double>& Value() {
+    m_g = m_jacobian.Value();
+
+    return m_g;
+  }
+
+  /**
+   * Updates the value of the variable.
+   */
+  void Update() { m_jacobian.Update(); }
+
+  /**
+   * Returns the profiler.
+   */
+  Profiler& GetProfiler() { return m_jacobian.GetProfiler(); }
+
+ private:
+  Eigen::SparseVector<double> m_g;
+
+  Jacobian m_jacobian;
+};
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Hessian.hpp

@@ -0,0 +1,84 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <utility>
+#include <vector>
+
+#include <Eigen/Core>
+#include <Eigen/SparseCore>
+
+#include "sleipnir/autodiff/ExpressionGraph.hpp"
+#include "sleipnir/autodiff/Jacobian.hpp"
+#include "sleipnir/autodiff/Profiler.hpp"
+#include "sleipnir/autodiff/Variable.hpp"
+#include "sleipnir/autodiff/VariableMatrix.hpp"
+#include "sleipnir/util/SymbolExports.hpp"
+
+namespace sleipnir {
+
+/**
+ * This class calculates the Hessian of a variable with respect to a vector of
+ * variables.
+ *
+ * The gradient tree is cached so subsequent Hessian calculations are faster,
+ * and the Hessian is only recomputed if the variable expression is nonlinear.
+ */
+class SLEIPNIR_DLLEXPORT Hessian {
+ public:
+  /**
+   * Constructs a Hessian object.
+   *
+   * @param variable Variable of which to compute the Hessian.
+   * @param wrt Vector of variables with respect to which to compute the
+   *   Hessian.
+   */
+  Hessian(Variable variable, const VariableMatrix& wrt) noexcept
+      : m_jacobian{
+            [&] {
+              std::vector<detail::ExpressionPtr> wrtVec;
+              wrtVec.reserve(wrt.size());
+              for (auto& elem : wrt) {
+                wrtVec.emplace_back(elem.expr);
+              }
+
+              auto grad =
+                  detail::ExpressionGraph{variable.expr}.GenerateGradientTree(
+                      wrtVec);
+
+              VariableMatrix ret{wrt.Rows()};
+              for (int row = 0; row < ret.Rows(); ++row) {
+                ret(row) = Variable{std::move(grad[row])};
+              }
+              return ret;
+            }(),
+            wrt} {}
+
+  /**
+   * Returns the Hessian as a VariableMatrix.
+   *
+   * This is useful when constructing optimization problems with derivatives in
+   * them.
+   */
+  VariableMatrix Get() const { return m_jacobian.Get(); }
+
+  /**
+   * Evaluates the Hessian at wrt's value.
+   */
+  const Eigen::SparseMatrix<double>& Value() { return m_jacobian.Value(); }
+
+  /**
+   * Updates the values of the gradient tree.
+   */
+  void Update() { m_jacobian.Update(); }
+
+  /**
+   * Returns the profiler.
+   */
+  Profiler& GetProfiler() { return m_jacobian.GetProfiler(); }
+
+ private:
+  Jacobian m_jacobian;
+};
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Jacobian.hpp

@@ -0,0 +1,162 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <utility>
+#include <vector>
+
+#include <Eigen/SparseCore>
+
+#include "sleipnir/autodiff/ExpressionGraph.hpp"
+#include "sleipnir/autodiff/Profiler.hpp"
+#include "sleipnir/autodiff/Variable.hpp"
+#include "sleipnir/autodiff/VariableMatrix.hpp"
+#include "sleipnir/util/SymbolExports.hpp"
+
+namespace sleipnir {
+
+/**
+ * This class calculates the Jacobian of a vector of variables with respect to a
+ * vector of variables.
+ *
+ * The Jacobian is only recomputed if the variable expression is quadratic or
+ * higher order.
+ */
+class SLEIPNIR_DLLEXPORT Jacobian {
+ public:
+  /**
+   * Constructs a Jacobian object.
+   *
+   * @param variables Vector of variables of which to compute the Jacobian.
+   * @param wrt Vector of variables with respect to which to compute the
+   *   Jacobian.
+   */
+  Jacobian(const VariableMatrix& variables, const VariableMatrix& wrt) noexcept
+      : m_variables{std::move(variables)}, m_wrt{std::move(wrt)} {
+    m_profiler.StartSetup();
+
+    for (int row = 0; row < m_wrt.Rows(); ++row) {
+      m_wrt(row).expr->row = row;
+    }
+
+    for (Variable variable : m_variables) {
+      m_graphs.emplace_back(variable.expr);
+    }
+
+    // Reserve triplet space for 99% sparsity
+    m_cachedTriplets.reserve(m_variables.Rows() * m_wrt.Rows() * 0.01);
+
+    for (int row = 0; row < m_variables.Rows(); ++row) {
+      if (m_variables(row).Type() == ExpressionType::kLinear) {
+        // If the row is linear, compute its gradient once here and cache its
+        // triplets. Constant rows are ignored because their gradients have no
+        // nonzero triplets.
+        m_graphs[row].ComputeAdjoints([&](int col, double adjoint) {
+          m_cachedTriplets.emplace_back(row, col, adjoint);
+        });
+      } else if (m_variables(row).Type() > ExpressionType::kLinear) {
+        // If the row is quadratic or nonlinear, add it to the list of nonlinear
+        // rows to be recomputed in Value().
+        m_nonlinearRows.emplace_back(row);
+      }
+    }
+
+    for (int row = 0; row < m_wrt.Rows(); ++row) {
+      m_wrt(row).expr->row = -1;
+    }
+
+    if (m_nonlinearRows.empty()) {
+      m_J.setFromTriplets(m_cachedTriplets.begin(), m_cachedTriplets.end());
+    }
+
+    m_profiler.StopSetup();
+  }
+
+  /**
+   * Returns the Jacobian as a VariableMatrix.
+   *
+   * This is useful when constructing optimization problems with derivatives in
+   * them.
+   */
+  VariableMatrix Get() const {
+    VariableMatrix result{m_variables.Rows(), m_wrt.Rows()};
+
+    std::vector<detail::ExpressionPtr> wrtVec;
+    wrtVec.reserve(m_wrt.size());
+    for (auto& elem : m_wrt) {
+      wrtVec.emplace_back(elem.expr);
+    }
+
+    for (int row = 0; row < m_variables.Rows(); ++row) {
+      auto grad = m_graphs[row].GenerateGradientTree(wrtVec);
+      for (int col = 0; col < m_wrt.Rows(); ++col) {
+        result(row, col) = Variable{std::move(grad[col])};
+      }
+    }
+
+    return result;
+  }
+
+  /**
+   * Evaluates the Jacobian at wrt's value.
+   */
+  const Eigen::SparseMatrix<double>& Value() {
+    if (m_nonlinearRows.empty()) {
+      return m_J;
+    }
+
+    m_profiler.StartSolve();
+
+    Update();
+
+    // Copy the cached triplets so triplets added for the nonlinear rows are
+    // thrown away at the end of the function
+    auto triplets = m_cachedTriplets;
+
+    // Compute each nonlinear row of the Jacobian
+    for (int row : m_nonlinearRows) {
+      m_graphs[row].ComputeAdjoints([&](int col, double adjoint) {
+        triplets.emplace_back(row, col, adjoint);
+      });
+    }
+
+    m_J.setFromTriplets(triplets.begin(), triplets.end());
+
+    m_profiler.StopSolve();
+
+    return m_J;
+  }
+
+  /**
+   * Updates the values of the variables.
+   */
+  void Update() {
+    for (auto& graph : m_graphs) {
+      graph.Update();
+    }
+  }
+
+  /**
+   * Returns the profiler.
+   */
+  Profiler& GetProfiler() { return m_profiler; }
+
+ private:
+  VariableMatrix m_variables;
+  VariableMatrix m_wrt;
+
+  std::vector<detail::ExpressionGraph> m_graphs;
+
+  Eigen::SparseMatrix<double> m_J{m_variables.Rows(), m_wrt.Rows()};
+
+  // Cached triplets for gradients of linear rows
+  std::vector<Eigen::Triplet<double>> m_cachedTriplets;
+
+  // List of row indices for nonlinear rows whose graients will be computed in
+  // Value()
+  std::vector<int> m_nonlinearRows;
+
+  Profiler m_profiler;
+};
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Profiler.hpp

@@ -0,0 +1,79 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <chrono>
+
+#include "sleipnir/util/SymbolExports.hpp"
+
+namespace sleipnir {
+
+/**
+ * Records the number of profiler measurements (start/stop pairs) and the
+ * average duration between each start and stop call.
+ */
+class SLEIPNIR_DLLEXPORT Profiler {
+ public:
+  /**
+   * Tell the profiler to start measuring setup time.
+   */
+  void StartSetup() { m_setupStartTime = std::chrono::system_clock::now(); }
+
+  /**
+   * Tell the profiler to stop measuring setup time.
+   */
+  void StopSetup() {
+    m_setupDuration = std::chrono::system_clock::now() - m_setupStartTime;
+  }
+
+  /**
+   * Tell the profiler to start measuring solve time.
+   */
+  void StartSolve() { m_solveStartTime = std::chrono::system_clock::now(); }
+
+  /**
+   * Tell the profiler to stop measuring solve time, increment the number of
+   * averages, and incorporate the latest measurement into the average.
+   */
+  void StopSolve() {
+    auto now = std::chrono::system_clock::now();
+    ++m_solveMeasurements;
+    m_averageSolveDuration =
+        (m_solveMeasurements - 1.0) / m_solveMeasurements *
+            m_averageSolveDuration +
+        1.0 / m_solveMeasurements * (now - m_solveStartTime);
+  }
+
+  /**
+   * The setup duration in milliseconds as a double.
+   */
+  double SetupDuration() const {
+    using std::chrono::duration_cast;
+    using std::chrono::nanoseconds;
+    return duration_cast<nanoseconds>(m_setupDuration).count() / 1e6;
+  }
+
+  /**
+   * The number of solve measurements taken.
+   */
+  int SolveMeasurements() const { return m_solveMeasurements; }
+
+  /**
+   * The average solve duration in milliseconds as a double.
+   */
+  double AverageSolveDuration() const {
+    using std::chrono::duration_cast;
+    using std::chrono::nanoseconds;
+    return duration_cast<nanoseconds>(m_averageSolveDuration).count() / 1e6;
+  }
+
+ private:
+  std::chrono::system_clock::time_point m_setupStartTime;
+  std::chrono::duration<double> m_setupDuration{0.0};
+
+  int m_solveMeasurements = 0;
+  std::chrono::duration<double> m_averageSolveDuration{0.0};
+  std::chrono::system_clock::time_point m_solveStartTime;
+};
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Variable.hpp

@@ -0,0 +1,467 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <utility>
+
+#include "sleipnir/autodiff/Expression.hpp"
+#include "sleipnir/autodiff/ExpressionGraph.hpp"
+#include "sleipnir/util/Print.hpp"
+#include "sleipnir/util/SymbolExports.hpp"
+
+namespace sleipnir {
+
+// Forward declarations for friend declarations in Variable
+class SLEIPNIR_DLLEXPORT Hessian;
+class SLEIPNIR_DLLEXPORT Jacobian;
+
+/**
+ * An autodiff variable pointing to an expression node.
+ */
+class SLEIPNIR_DLLEXPORT Variable {
+ public:
+  /**
+   * Constructs a linear Variable with a value of zero.
+   */
+  Variable() = default;
+
+  /**
+   * Constructs a Variable from a double.
+   *
+   * @param value The value of the Variable.
+   */
+  Variable(double value) : expr{detail::MakeExpressionPtr(value)} {}  // NOLINT
+
+  /**
+   * Constructs a Variable from an int.
+   *
+   * @param value The value of the Variable.
+   */
+  Variable(int value) : expr{detail::MakeExpressionPtr(value)} {}  // NOLINT
+
+  /**
+   * Constructs a Variable pointing to the specified expression.
+   *
+   * @param expr The autodiff variable.
+   */
+  explicit Variable(const detail::ExpressionPtr& expr) : expr{expr} {}
+
+  /**
+   * Constructs a Variable pointing to the specified expression.
+   *
+   * @param expr The autodiff variable.
+   */
+  explicit Variable(detail::ExpressionPtr&& expr) : expr{std::move(expr)} {}
+
+  /**
+   * Assignment operator for double.
+   *
+   * @param value The value of the Variable.
+   */
+  Variable& operator=(double value) {
+    expr = detail::MakeExpressionPtr(value);
+
+    return *this;
+  }
+
+  /**
+   * Assignment operator for int.
+   *
+   * @param value The value of the Variable.
+   */
+  Variable& operator=(int value) {
+    expr = detail::MakeExpressionPtr(value);
+
+    return *this;
+  }
+
+  /**
+   * Sets Variable's internal value.
+   *
+   * @param value The value of the Variable.
+   */
+  Variable& SetValue(double value) {
+    if (expr->IsConstant(0.0)) {
+      expr = detail::MakeExpressionPtr(value);
+    } else {
+      // We only need to check the first argument since unary and binary
+      // operators both use it
+      if (expr->args[0] != nullptr && !expr->args[0]->IsConstant(0.0)) {
+        sleipnir::println(
+            stderr,
+            "WARNING: {}:{}: Modified the value of a dependent variable",
+            __FILE__, __LINE__);
+      }
+      expr->value = value;
+    }
+    return *this;
+  }
+
+  /**
+   * Sets Variable's internal value.
+   *
+   * @param value The value of the Variable.
+   */
+  Variable& SetValue(int value) {
+    if (expr->IsConstant(0.0)) {
+      expr = detail::MakeExpressionPtr(value);
+    } else {
+      // We only need to check the first argument since unary and binary
+      // operators both use it
+      if (expr->args[0] != nullptr && !expr->args[0]->IsConstant(0.0)) {
+        sleipnir::println(
+            stderr,
+            "WARNING: {}:{}: Modified the value of a dependent variable",
+            __FILE__, __LINE__);
+      }
+      expr->value = value;
+    }
+    return *this;
+  }
+
+  /**
+   * Variable-Variable multiplication operator.
+   *
+   * @param lhs Operator left-hand side.
+   * @param rhs Operator right-hand side.
+   */
+  friend SLEIPNIR_DLLEXPORT Variable operator*(const Variable& lhs,
+                                               const Variable& rhs) {
+    return Variable{lhs.expr * rhs.expr};
+  }
+
+  /**
+   * Variable-Variable compound multiplication operator.
+   *
+   * @param rhs Operator right-hand side.
+   */
+  Variable& operator*=(const Variable& rhs) {
+    *this = *this * rhs;
+    return *this;
+  }
+
+  /**
+   * Variable-Variable division operator.
+   *
+   * @param lhs Operator left-hand side.
+   * @param rhs Operator right-hand side.
+   */
+  friend SLEIPNIR_DLLEXPORT Variable operator/(const Variable& lhs,
+                                               const Variable& rhs) {
+    return Variable{lhs.expr / rhs.expr};
+  }
+
+  /**
+   * Variable-Variable compound division operator.
+   *
+   * @param rhs Operator right-hand side.
+   */
+  Variable& operator/=(const Variable& rhs) {
+    *this = *this / rhs;
+    return *this;
+  }
+
+  /**
+   * Variable-Variable addition operator.
+   *
+   * @param lhs Operator left-hand side.
+   * @param rhs Operator right-hand side.
+   */
+  friend SLEIPNIR_DLLEXPORT Variable operator+(const Variable& lhs,
+                                               const Variable& rhs) {
+    return Variable{lhs.expr + rhs.expr};
+  }
+
+  /**
+   * Variable-Variable compound addition operator.
+   *
+   * @param rhs Operator right-hand side.
+   */
+  Variable& operator+=(const Variable& rhs) {
+    *this = *this + rhs;
+    return *this;
+  }
+
+  /**
+   * Variable-Variable subtraction operator.
+   *
+   * @param lhs Operator left-hand side.
+   * @param rhs Operator right-hand side.
+   */
+  friend SLEIPNIR_DLLEXPORT Variable operator-(const Variable& lhs,
+                                               const Variable& rhs) {
+    return Variable{lhs.expr - rhs.expr};
+  }
+
+  /**
+   * Variable-Variable compound subtraction operator.
+   *
+   * @param rhs Operator right-hand side.
+   */
+  Variable& operator-=(const Variable& rhs) {
+    *this = *this - rhs;
+    return *this;
+  }
+
+  /**
+   * Unary minus operator.
+   *
+   * @param lhs Operand for unary minus.
+   */
+  friend SLEIPNIR_DLLEXPORT Variable operator-(const Variable& lhs) {
+    return Variable{-lhs.expr};
+  }
+
+  /**
+   * Unary plus operator.
+   *
+   * @param lhs Operand for unary plus.
+   */
+  friend SLEIPNIR_DLLEXPORT Variable operator+(const Variable& lhs) {
+    return Variable{+lhs.expr};
+  }
+
+  /**
+   * Returns the value of this variable.
+   */
+  double Value() const { return expr->value; }
+
+  /**
+   * Returns the type of this expression (constant, linear, quadratic, or
+   * nonlinear).
+   */
+  ExpressionType Type() const { return expr->type; }
+
+  /**
+   * Updates the value of this variable based on the values of its dependent
+   * variables.
+   */
+  void Update() {
+    if (!expr->IsConstant(0.0)) {
+      detail::ExpressionGraph{expr}.Update();
+    }
+  }
+
+ private:
+  /// The expression node.
+  detail::ExpressionPtr expr =
+      detail::MakeExpressionPtr(0.0, ExpressionType::kLinear);
+
+  friend SLEIPNIR_DLLEXPORT Variable abs(const Variable& x);
+  friend SLEIPNIR_DLLEXPORT Variable acos(const Variable& x);
+  friend SLEIPNIR_DLLEXPORT Variable asin(const Variable& x);
+  friend SLEIPNIR_DLLEXPORT Variable atan(const Variable& x);
+  friend SLEIPNIR_DLLEXPORT Variable atan2(const Variable& y,
+                                           const Variable& x);
+  friend SLEIPNIR_DLLEXPORT Variable cos(const Variable& x);
+  friend SLEIPNIR_DLLEXPORT Variable cosh(const Variable& x);
+  friend SLEIPNIR_DLLEXPORT Variable erf(const Variable& x);
+  friend SLEIPNIR_DLLEXPORT Variable exp(const Variable& x);
+  friend SLEIPNIR_DLLEXPORT Variable hypot(const Variable& x,
+                                           const Variable& y);
+  friend SLEIPNIR_DLLEXPORT Variable log(const Variable& x);
+  friend SLEIPNIR_DLLEXPORT Variable log10(const Variable& x);
+  friend SLEIPNIR_DLLEXPORT Variable pow(const Variable& base,
+                                         const Variable& power);
+  friend SLEIPNIR_DLLEXPORT Variable sign(const Variable& x);
+  friend SLEIPNIR_DLLEXPORT Variable sin(const Variable& x);
+  friend SLEIPNIR_DLLEXPORT Variable sinh(const Variable& x);
+  friend SLEIPNIR_DLLEXPORT Variable sqrt(const Variable& x);
+  friend SLEIPNIR_DLLEXPORT Variable tan(const Variable& x);
+  friend SLEIPNIR_DLLEXPORT Variable tanh(const Variable& x);
+  friend SLEIPNIR_DLLEXPORT Variable hypot(const Variable& x, const Variable& y,
+                                           const Variable& z);
+
+  friend class SLEIPNIR_DLLEXPORT Hessian;
+  friend class SLEIPNIR_DLLEXPORT Jacobian;
+};
+
+/**
+ * std::abs() for Variables.
+ *
+ * @param x The argument.
+ */
+SLEIPNIR_DLLEXPORT inline Variable abs(const Variable& x) {
+  return Variable{detail::abs(x.expr)};
+}
+
+/**
+ * std::acos() for Variables.
+ *
+ * @param x The argument.
+ */
+SLEIPNIR_DLLEXPORT inline Variable acos(const Variable& x) {
+  return Variable{detail::acos(x.expr)};
+}
+
+/**
+ * std::asin() for Variables.
+ *
+ * @param x The argument.
+ */
+SLEIPNIR_DLLEXPORT inline Variable asin(const Variable& x) {
+  return Variable{detail::asin(x.expr)};
+}
+
+/**
+ * std::atan() for Variables.
+ *
+ * @param x The argument.
+ */
+SLEIPNIR_DLLEXPORT inline Variable atan(const Variable& x) {
+  return Variable{detail::atan(x.expr)};
+}
+
+/**
+ * std::atan2() for Variables.
+ *
+ * @param y The y argument.
+ * @param x The x argument.
+ */
+SLEIPNIR_DLLEXPORT inline Variable atan2(const Variable& y, const Variable& x) {
+  return Variable{detail::atan2(y.expr, x.expr)};
+}
+
+/**
+ * std::cos() for Variables.
+ *
+ * @param x The argument.
+ */
+SLEIPNIR_DLLEXPORT inline Variable cos(const Variable& x) {
+  return Variable{detail::cos(x.expr)};
+}
+
+/**
+ * std::cosh() for Variables.
+ *
+ * @param x The argument.
+ */
+SLEIPNIR_DLLEXPORT inline Variable cosh(const Variable& x) {
+  return Variable{detail::cosh(x.expr)};
+}
+
+/**
+ * std::erf() for Variables.
+ *
+ * @param x The argument.
+ */
+SLEIPNIR_DLLEXPORT inline Variable erf(const Variable& x) {
+  return Variable{detail::erf(x.expr)};
+}
+
+/**
+ * std::exp() for Variables.
+ *
+ * @param x The argument.
+ */
+SLEIPNIR_DLLEXPORT inline Variable exp(const Variable& x) {
+  return Variable{detail::exp(x.expr)};
+}
+
+/**
+ * std::hypot() for Variables.
+ *
+ * @param x The x argument.
+ * @param y The y argument.
+ */
+SLEIPNIR_DLLEXPORT inline Variable hypot(const Variable& x, const Variable& y) {
+  return Variable{detail::hypot(x.expr, y.expr)};
+}
+
+/**
+ * std::pow() for Variables.
+ *
+ * @param base The base.
+ * @param power The power.
+ */
+SLEIPNIR_DLLEXPORT inline Variable pow(const Variable& base,
+                                       const Variable& power) {
+  return Variable{detail::pow(base.expr, power.expr)};
+}
+
+/**
+ * std::log() for Variables.
+ *
+ * @param x The argument.
+ */
+SLEIPNIR_DLLEXPORT inline Variable log(const Variable& x) {
+  return Variable{detail::log(x.expr)};
+}
+
+/**
+ * std::log10() for Variables.
+ *
+ * @param x The argument.
+ */
+SLEIPNIR_DLLEXPORT inline Variable log10(const Variable& x) {
+  return Variable{detail::log10(x.expr)};
+}
+
+/**
+ * sign() for Variables.
+ *
+ * @param x The argument.
+ */
+SLEIPNIR_DLLEXPORT inline Variable sign(const Variable& x) {
+  return Variable{detail::sign(x.expr)};
+}
+
+/**
+ * std::sin() for Variables.
+ *
+ * @param x The argument.
+ */
+SLEIPNIR_DLLEXPORT inline Variable sin(const Variable& x) {
+  return Variable{detail::sin(x.expr)};
+}
+
+/**
+ * std::sinh() for Variables.
+ *
+ * @param x The argument.
+ */
+SLEIPNIR_DLLEXPORT inline Variable sinh(const Variable& x) {
+  return Variable{detail::sinh(x.expr)};
+}
+
+/**
+ * std::sqrt() for Variables.
+ *
+ * @param x The argument.
+ */
+SLEIPNIR_DLLEXPORT inline Variable sqrt(const Variable& x) {
+  return Variable{detail::sqrt(x.expr)};
+}
+
+/**
+ * std::tan() for Variables.
+ *
+ * @param x The argument.
+ */
+SLEIPNIR_DLLEXPORT inline Variable tan(const Variable& x) {
+  return Variable{detail::tan(x.expr)};
+}
+
+/**
+ * std::tanh() for Variables.
+ *
+ * @param x The argument.
+ */
+SLEIPNIR_DLLEXPORT inline Variable tanh(const Variable& x) {
+  return Variable{detail::tanh(x.expr)};
+}
+
+/**
+ * std::hypot() for Variables.
+ *
+ * @param x The x argument.
+ * @param y The y argument.
+ * @param z The z argument.
+ */
+SLEIPNIR_DLLEXPORT inline Variable hypot(const Variable& x, const Variable& y,
+                                         const Variable& z) {
+  return Variable{sleipnir::sqrt(sleipnir::pow(x, 2) + sleipnir::pow(y, 2) +
+                                 sleipnir::pow(z, 2))};
+}
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableBlock.hpp

@@ -0,0 +1,617 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <concepts>
+#include <type_traits>
+#include <utility>
+
+#include <Eigen/Core>
+
+#include "sleipnir/autodiff/Variable.hpp"
+#include "sleipnir/util/Assert.hpp"
+#include "sleipnir/util/FunctionRef.hpp"
+
+namespace sleipnir {
+
+/**
+ * A submatrix of autodiff variables with reference semantics.
+ *
+ * @tparam Mat The type of the matrix whose storage this class points to.
+ */
+template <typename Mat>
+class VariableBlock {
+ public:
+  VariableBlock(const VariableBlock<Mat>& values) = default;
+
+  /**
+   * Assigns a VariableBlock to the block.
+   */
+  VariableBlock<Mat>& operator=(const VariableBlock<Mat>& values) {
+    if (this == &values) {
+      return *this;
+    }
+
+    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;
+    } else {
+      Assert(Rows() == values.Rows());
+      Assert(Cols() == values.Cols());
+
+      for (int row = 0; row < Rows(); ++row) {
+        for (int col = 0; col < Cols(); ++col) {
+          (*this)(row, col) = values(row, col);
+        }
+      }
+    }
+
+    return *this;
+  }
+
+  VariableBlock(VariableBlock<Mat>&&) = default;
+
+  /**
+   * Assigns a VariableBlock to the block.
+   */
+  VariableBlock<Mat>& operator=(VariableBlock<Mat>&& values) {
+    if (this == &values) {
+      return *this;
+    }
+
+    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;
+    } else {
+      Assert(Rows() == values.Rows());
+      Assert(Cols() == values.Cols());
+
+      for (int row = 0; row < Rows(); ++row) {
+        for (int col = 0; col < Cols(); ++col) {
+          (*this)(row, col) = values(row, col);
+        }
+      }
+    }
+
+    return *this;
+  }
+
+  /**
+   * Constructs a Variable block pointing to all of the given matrix.
+   *
+   * @param mat The matrix to which to point.
+   */
+  VariableBlock(Mat& mat)  // NOLINT
+      : m_mat{&mat}, m_blockRows{mat.Rows()}, m_blockCols{mat.Cols()} {}
+
+  /**
+   * Constructs a Variable block pointing to a subset of the given matrix.
+   *
+   * @param mat The matrix to which to point.
+   * @param rowOffset The block's row offset.
+   * @param colOffset The block's column offset.
+   * @param blockRows The number of rows in the block.
+   * @param blockCols The number of columns in the block.
+   */
+  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} {}
+
+  /**
+   * Assigns a double to the block.
+   *
+   * This only works for blocks with one row and one column.
+   */
+  VariableBlock<Mat>& operator=(double value) {
+    Assert(Rows() == 1 && Cols() == 1);
+
+    (*this)(0, 0) = value;
+
+    return *this;
+  }
+
+  /**
+   * Assigns a double to the block.
+   *
+   * This only works for blocks with one row and one column.
+   */
+  VariableBlock<Mat>& SetValue(double value) {
+    Assert(Rows() == 1 && Cols() == 1);
+
+    (*this)(0, 0).SetValue(value);
+
+    return *this;
+  }
+
+  /**
+   * Assigns an Eigen matrix to the block.
+   */
+  template <typename Derived>
+  VariableBlock<Mat>& operator=(const Eigen::MatrixBase<Derived>& values) {
+    Assert(Rows() == values.rows());
+    Assert(Cols() == values.cols());
+
+    for (int row = 0; row < Rows(); ++row) {
+      for (int col = 0; col < Cols(); ++col) {
+        (*this)(row, col) = values(row, col);
+      }
+    }
+
+    return *this;
+  }
+
+  /**
+   * Sets block's internal values.
+   */
+  template <typename Derived>
+    requires std::same_as<typename Derived::Scalar, double>
+  VariableBlock<Mat>& SetValue(const Eigen::MatrixBase<Derived>& values) {
+    Assert(Rows() == values.rows());
+    Assert(Cols() == values.cols());
+
+    for (int row = 0; row < Rows(); ++row) {
+      for (int col = 0; col < Cols(); ++col) {
+        (*this)(row, col).SetValue(values(row, col));
+      }
+    }
+
+    return *this;
+  }
+
+  /**
+   * Assigns a VariableMatrix to the block.
+   */
+  VariableBlock<Mat>& operator=(const Mat& values) {
+    Assert(Rows() == values.Rows());
+    Assert(Cols() == values.Cols());
+
+    for (int row = 0; row < Rows(); ++row) {
+      for (int col = 0; col < Cols(); ++col) {
+        (*this)(row, col) = values(row, col);
+      }
+    }
+    return *this;
+  }
+
+  /**
+   * Assigns a VariableMatrix to the block.
+   */
+  VariableBlock<Mat>& operator=(Mat&& values) {
+    Assert(Rows() == values.Rows());
+    Assert(Cols() == values.Cols());
+
+    for (int row = 0; row < Rows(); ++row) {
+      for (int col = 0; col < Cols(); ++col) {
+        (*this)(row, col) = std::move(values(row, col));
+      }
+    }
+    return *this;
+  }
+
+  /**
+   * Returns a scalar subblock at the given row and column.
+   *
+   * @param row The scalar subblock's row.
+   * @param col The scalar subblock's column.
+   */
+  template <typename Mat2 = Mat>
+    requires(!std::is_const_v<Mat2>)
+  Variable& operator()(int row, int col) {
+    Assert(row >= 0 && row < Rows());
+    Assert(col >= 0 && col < Cols());
+    return (*m_mat)(m_rowOffset + row, m_colOffset + col);
+  }
+
+  /**
+   * Returns a scalar subblock at the given row and column.
+   *
+   * @param row The scalar subblock's row.
+   * @param col The scalar subblock's column.
+   */
+  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);
+  }
+
+  /**
+   * Returns a scalar subblock at the given row.
+   *
+   * @param row The scalar subblock's row.
+   */
+  template <typename Mat2 = Mat>
+    requires(!std::is_const_v<Mat2>)
+  Variable& operator()(int row) {
+    Assert(row >= 0 && row < Rows() * Cols());
+    return (*this)(row / Cols(), row % Cols());
+  }
+
+  /**
+   * Returns a scalar subblock at the given row.
+   *
+   * @param row The scalar subblock's row.
+   */
+  const Variable& operator()(int row) const {
+    Assert(row >= 0 && row < Rows() * Cols());
+    return (*this)(row / Cols(), row % Cols());
+  }
+
+  /**
+   * Returns a block slice of the variable matrix.
+   *
+   * @param rowOffset The row offset of the block selection.
+   * @param colOffset The column offset of the block selection.
+   * @param blockRows The number of rows in the block selection.
+   * @param blockCols The number of columns in the block selection.
+   */
+  VariableBlock<Mat> Block(int rowOffset, int colOffset, int blockRows,
+                           int blockCols) {
+    Assert(rowOffset >= 0 && rowOffset <= Rows());
+    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};
+  }
+
+  /**
+   * Returns a block slice of the variable matrix.
+   *
+   * @param rowOffset The row offset of the block selection.
+   * @param colOffset The column offset of the block selection.
+   * @param blockRows The number of rows in the block selection.
+   * @param blockCols The number of columns in the block selection.
+   */
+  const VariableBlock<const Mat> Block(int rowOffset, int colOffset,
+                                       int blockRows, int blockCols) const {
+    Assert(rowOffset >= 0 && rowOffset <= Rows());
+    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};
+  }
+
+  /**
+   * Returns a row slice of the variable matrix.
+   *
+   * @param row The row to slice.
+   */
+  VariableBlock<Mat> Row(int row) {
+    Assert(row >= 0 && row < Rows());
+    return Block(row, 0, 1, Cols());
+  }
+
+  /**
+   * Returns a row slice of the variable matrix.
+   *
+   * @param row The row to slice.
+   */
+  VariableBlock<const Mat> Row(int row) const {
+    Assert(row >= 0 && row < Rows());
+    return Block(row, 0, 1, Cols());
+  }
+
+  /**
+   * Returns a column slice of the variable matrix.
+   *
+   * @param col The column to slice.
+   */
+  VariableBlock<Mat> Col(int col) {
+    Assert(col >= 0 && col < Cols());
+    return Block(0, col, Rows(), 1);
+  }
+
+  /**
+   * Returns a column slice of the variable matrix.
+   *
+   * @param col The column to slice.
+   */
+  VariableBlock<const Mat> Col(int col) const {
+    Assert(col >= 0 && col < Cols());
+    return Block(0, col, Rows(), 1);
+  }
+
+  /**
+   * Compound matrix multiplication-assignment operator.
+   *
+   * @param rhs Variable to multiply.
+   */
+  VariableBlock<Mat>& operator*=(const VariableBlock<Mat>& rhs) {
+    Assert(Cols() == rhs.Rows() && Cols() == rhs.Cols());
+
+    for (int i = 0; i < Rows(); ++i) {
+      for (int j = 0; j < rhs.Cols(); ++j) {
+        Variable sum;
+        for (int k = 0; k < Cols(); ++k) {
+          sum += (*this)(i, k) * rhs(k, j);
+        }
+        (*this)(i, j) = sum;
+      }
+    }
+
+    return *this;
+  }
+
+  /**
+   * Compound matrix multiplication-assignment operator (only enabled when lhs
+   * is a scalar).
+   *
+   * @param rhs Variable to multiply.
+   */
+  VariableBlock& operator*=(double rhs) {
+    for (int row = 0; row < Rows(); ++row) {
+      for (int col = 0; col < Cols(); ++col) {
+        (*this)(row, col) *= rhs;
+      }
+    }
+
+    return *this;
+  }
+
+  /**
+   * Compound matrix division-assignment operator (only enabled when rhs
+   * is a scalar).
+   *
+   * @param rhs Variable to divide.
+   */
+  VariableBlock<Mat>& operator/=(const VariableBlock<Mat>& rhs) {
+    Assert(rhs.Rows() == 1 && rhs.Cols() == 1);
+
+    for (int row = 0; row < Rows(); ++row) {
+      for (int col = 0; col < Cols(); ++col) {
+        (*this)(row, col) /= rhs(0, 0);
+      }
+    }
+
+    return *this;
+  }
+
+  /**
+   * Compound matrix division-assignment operator (only enabled when rhs
+   * is a scalar).
+   *
+   * @param rhs Variable to divide.
+   */
+  VariableBlock<Mat>& operator/=(double rhs) {
+    for (int row = 0; row < Rows(); ++row) {
+      for (int col = 0; col < Cols(); ++col) {
+        (*this)(row, col) /= rhs;
+      }
+    }
+
+    return *this;
+  }
+
+  /**
+   * Compound addition-assignment operator.
+   *
+   * @param rhs Variable to add.
+   */
+  VariableBlock<Mat>& operator+=(const VariableBlock<Mat>& rhs) {
+    for (int row = 0; row < Rows(); ++row) {
+      for (int col = 0; col < Cols(); ++col) {
+        (*this)(row, col) += rhs(row, col);
+      }
+    }
+
+    return *this;
+  }
+
+  /**
+   * Compound subtraction-assignment operator.
+   *
+   * @param rhs Variable to subtract.
+   */
+  VariableBlock<Mat>& operator-=(const VariableBlock<Mat>& rhs) {
+    for (int row = 0; row < Rows(); ++row) {
+      for (int col = 0; col < Cols(); ++col) {
+        (*this)(row, col) -= rhs(row, col);
+      }
+    }
+
+    return *this;
+  }
+
+  /**
+   * Returns the transpose of the variable matrix.
+   */
+  std::remove_cv_t<Mat> T() const {
+    std::remove_cv_t<Mat> result{Cols(), Rows()};
+
+    for (int row = 0; row < Rows(); ++row) {
+      for (int col = 0; col < Cols(); ++col) {
+        result(col, row) = (*this)(row, col);
+      }
+    }
+
+    return result;
+  }
+
+  /**
+   * Returns number of rows in the matrix.
+   */
+  int Rows() const { return m_blockRows; }
+
+  /**
+   * Returns number of columns in the matrix.
+   */
+  int Cols() const { return m_blockCols; }
+
+  /**
+   * Returns an element of the variable matrix.
+   *
+   * @param row The row of the element to return.
+   * @param col The column of the element to return.
+   */
+  double Value(int row, int col) const {
+    Assert(row >= 0 && row < Rows());
+    Assert(col >= 0 && col < Cols());
+    return (*m_mat)(m_rowOffset + row, m_colOffset + col).Value();
+  }
+
+  /**
+   * Returns a row of the variable column vector.
+   *
+   * @param index The index of the element to return.
+   */
+  double Value(int index) const {
+    Assert(index >= 0 && index < Rows() * Cols());
+    return (*m_mat)(m_rowOffset + index / m_blockCols,
+                    m_colOffset + index % m_blockCols)
+        .Value();
+  }
+
+  /**
+   * Returns the contents of the variable matrix.
+   */
+  Eigen::MatrixXd Value() const {
+    Eigen::MatrixXd result{Rows(), Cols()};
+
+    for (int row = 0; row < Rows(); ++row) {
+      for (int col = 0; col < Cols(); ++col) {
+        result(row, col) = Value(row, col);
+      }
+    }
+
+    return result;
+  }
+
+  /**
+   * Transforms the matrix coefficient-wise with an unary operator.
+   *
+   * @param unaryOp The unary operator to use for the transform operation.
+   */
+  std::remove_cv_t<Mat> CwiseTransform(
+      function_ref<Variable(const Variable&)> unaryOp) const {
+    std::remove_cv_t<Mat> result{Rows(), Cols()};
+
+    for (int row = 0; row < Rows(); ++row) {
+      for (int col = 0; col < Cols(); ++col) {
+        result(row, col) = unaryOp((*this)(row, col));
+      }
+    }
+
+    return result;
+  }
+
+  class iterator {
+   public:
+    using iterator_category = std::forward_iterator_tag;
+    using value_type = Variable;
+    using difference_type = std::ptrdiff_t;
+    using pointer = Variable*;
+    using reference = Variable&;
+
+    iterator(VariableBlock<Mat>* mat, int row, int col)
+        : m_mat{mat}, m_row{row}, m_col{col} {}
+
+    iterator& operator++() {
+      ++m_col;
+      if (m_col == m_mat->Cols()) {
+        m_col = 0;
+        ++m_row;
+      }
+      return *this;
+    }
+    iterator operator++(int) {
+      iterator retval = *this;
+      ++(*this);
+      return retval;
+    }
+    bool operator==(const iterator&) const = default;
+    reference operator*() { return (*m_mat)(m_row, m_col); }
+
+   private:
+    VariableBlock<Mat>* m_mat;
+    int m_row;
+    int m_col;
+  };
+
+  class const_iterator {
+   public:
+    using iterator_category = std::forward_iterator_tag;
+    using value_type = Variable;
+    using difference_type = std::ptrdiff_t;
+    using pointer = Variable*;
+    using const_reference = const Variable&;
+
+    const_iterator(const VariableBlock<Mat>* mat, int row, int col)
+        : m_mat{mat}, m_row{row}, m_col{col} {}
+
+    const_iterator& operator++() {
+      ++m_col;
+      if (m_col == m_mat->Cols()) {
+        m_col = 0;
+        ++m_row;
+      }
+      return *this;
+    }
+    const_iterator operator++(int) {
+      const_iterator retval = *this;
+      ++(*this);
+      return retval;
+    }
+    bool operator==(const const_iterator&) const = default;
+    const_reference operator*() const { return (*m_mat)(m_row, m_col); }
+
+   private:
+    const VariableBlock<Mat>* m_mat;
+    int m_row;
+    int m_col;
+  };
+
+  /**
+   * Returns begin iterator.
+   */
+  iterator begin() { return iterator(this, 0, 0); }
+
+  /**
+   * Returns end iterator.
+   */
+  iterator end() { return iterator(this, Rows(), 0); }
+
+  /**
+   * Returns begin iterator.
+   */
+  const_iterator begin() const { return const_iterator(this, 0, 0); }
+
+  /**
+   * Returns end iterator.
+   */
+  const_iterator end() const { return const_iterator(this, Rows(), 0); }
+
+  /**
+   * Returns begin iterator.
+   */
+  const_iterator cbegin() const { return const_iterator(this, 0, 0); }
+
+  /**
+   * Returns end iterator.
+   */
+  const_iterator cend() const { return const_iterator(this, Rows(), 0); }
+
+  /**
+   * Returns number of elements in matrix.
+   */
+  size_t size() const { return m_blockRows * m_blockCols; }
+
+ private:
+  Mat* m_mat = nullptr;
+  int m_rowOffset = 0;
+  int m_colOffset = 0;
+  int m_blockRows = 0;
+  int m_blockCols = 0;
+};
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/OCPSolver.hpp

@@ -0,0 +1,408 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <stdint.h>
+
+#include <chrono>
+#include <utility>
+
+#include "sleipnir/autodiff/VariableMatrix.hpp"
+#include "sleipnir/optimization/OptimizationProblem.hpp"
+#include "sleipnir/util/Assert.hpp"
+#include "sleipnir/util/Concepts.hpp"
+#include "sleipnir/util/FunctionRef.hpp"
+#include "sleipnir/util/SymbolExports.hpp"
+
+namespace sleipnir {
+
+/**
+ * Function representing an explicit or implicit ODE, or a discrete state
+ * transition function.
+ *
+ * - Explicit: dx/dt = f(t, x, u, *)
+ * - Implicit: f(t, [x dx/dt]', u, *) = 0
+ * - State transition: xₖ₊₁ = f(t, xₖ, uₖ, dt)
+ */
+using DynamicsFunction =
+    function_ref<VariableMatrix(const Variable&, const VariableMatrix&,
+                                const VariableMatrix&, const Variable&)>;
+
+/**
+ * Performs 4th order Runge-Kutta integration of dx/dt = f(t, x, u) for dt.
+ *
+ * @param f  The function to integrate. It must take two arguments x and u.
+ * @param x  The initial value of x.
+ * @param u  The value u held constant over the integration period.
+ * @param t0 The initial time.
+ * @param dt The time over which to integrate.
+ */
+template <typename F, typename State, typename Input, typename Time>
+State RK4(F&& f, State x, Input u, Time t0, Time dt) {
+  auto halfdt = dt * 0.5;
+  State k1 = f(t0, x, u, dt);
+  State k2 = f(t0 + halfdt, x + k1 * halfdt, u, dt);
+  State k3 = f(t0 + halfdt, x + k2 * halfdt, u, dt);
+  State k4 = f(t0 + dt, x + k3 * dt, u, dt);
+
+  return x + (k1 + k2 * 2.0 + k3 * 2.0 + k4) * (dt / 6.0);
+}
+
+/**
+ * Enum describing an OCP transcription method.
+ */
+enum class TranscriptionMethod : uint8_t {
+  /// Each state is a decision variable constrained to the integrated dynamics
+  /// of the previous state.
+  kDirectTranscription,
+  /// The trajectory is modeled as a series of cubic polynomials where the
+  /// centerpoint slope is constrained.
+  kDirectCollocation,
+  /// States depend explicitly as a function of all previous states and all
+  /// previous inputs.
+  kSingleShooting
+};
+
+/**
+ * Enum describing a type of system dynamics constraints.
+ */
+enum class DynamicsType : uint8_t {
+  /// The dynamics are a function in the form dx/dt = f(t, x, u).
+  kExplicitODE,
+  /// The dynamics are a function in the form xₖ₊₁ = f(t, xₖ, uₖ).
+  kDiscrete
+};
+
+/**
+ * Enum describing the type of system timestep.
+ */
+enum class TimestepMethod : uint8_t {
+  /// The timestep is a fixed constant.
+  kFixed,
+  /// The timesteps are allowed to vary as independent decision variables.
+  kVariable,
+  /// The timesteps are equal length but allowed to vary as a single decision
+  /// variable.
+  kVariableSingle
+};
+
+/**
+ * This class allows the user to pose and solve a constrained optimal control
+ * problem (OCP) in a variety of ways.
+ *
+ * The system is transcripted by one of three methods (direct transcription,
+ * direct collocation, or single-shooting) and additional constraints can be
+ * added.
+ *
+ * In direct transcription, each state is a decision variable constrained to the
+ * integrated dynamics of the previous state. In direct collocation, the
+ * trajectory is modeled as a series of cubic polynomials where the centerpoint
+ * slope is constrained. In single-shooting, states depend explicitly as a
+ * function of all previous states and all previous inputs.
+ *
+ * Explicit ODEs are integrated using RK4.
+ *
+ * For explicit ODEs, the function must be in the form dx/dt = f(t, x, u).
+ * For discrete state transition functions, the function must be in the form
+ * xₖ₊₁ = f(t, xₖ, uₖ).
+ *
+ * Direct collocation requires an explicit ODE. Direct transcription and
+ * single-shooting can use either an ODE or state transition function.
+ *
+ * https://underactuated.mit.edu/trajopt.html goes into more detail on each
+ * transcription method.
+ */
+class SLEIPNIR_DLLEXPORT OCPSolver : public OptimizationProblem {
+ public:
+  /**
+   * Build an optimization problem using a system evolution function (explicit
+   * ODE or discrete state transition function).
+   *
+   * @param numStates The number of system states.
+   * @param numInputs The number of system inputs.
+   * @param dt The timestep for fixed-step integration.
+   * @param numSteps The number of control points.
+   * @param dynamics The system evolution function, either an explicit ODE or a
+   * discrete state transition function.
+   * @param dynamicsType The type of system evolution function.
+   * @param timestepMethod The timestep method.
+   * @param method The transcription method.
+   */
+  OCPSolver(
+      int numStates, int numInputs, std::chrono::duration<double> dt,
+      int numSteps, DynamicsFunction dynamics,
+      DynamicsType dynamicsType = DynamicsType::kExplicitODE,
+      TimestepMethod timestepMethod = TimestepMethod::kFixed,
+      TranscriptionMethod method = TranscriptionMethod::kDirectTranscription)
+      : m_numStates{numStates},
+        m_numInputs{numInputs},
+        m_dt{dt},
+        m_numSteps{numSteps},
+        m_transcriptionMethod{method},
+        m_dynamicsType{dynamicsType},
+        m_dynamicsFunction{std::move(dynamics)},
+        m_timestepMethod{timestepMethod} {
+    // u is numSteps + 1 so that the final constraintFunction evaluation works
+    m_U = DecisionVariable(m_numInputs, m_numSteps + 1);
+
+    if (m_timestepMethod == TimestepMethod::kFixed) {
+      m_DT = VariableMatrix{1, m_numSteps + 1};
+      for (int i = 0; i < numSteps + 1; ++i) {
+        m_DT(0, i) = m_dt.count();
+      }
+    } else if (m_timestepMethod == TimestepMethod::kVariableSingle) {
+      Variable DT = DecisionVariable();
+      DT.SetValue(m_dt.count());
+
+      // Set the member variable matrix to track the decision variable
+      m_DT = VariableMatrix{1, m_numSteps + 1};
+      for (int i = 0; i < numSteps + 1; ++i) {
+        m_DT(0, i) = DT;
+      }
+    } else if (m_timestepMethod == TimestepMethod::kVariable) {
+      m_DT = DecisionVariable(1, m_numSteps + 1);
+      for (int i = 0; i < numSteps + 1; ++i) {
+        m_DT(0, i).SetValue(m_dt.count());
+      }
+    }
+
+    if (m_transcriptionMethod == TranscriptionMethod::kDirectTranscription) {
+      m_X = DecisionVariable(m_numStates, m_numSteps + 1);
+      ConstrainDirectTranscription();
+    } else if (m_transcriptionMethod ==
+               TranscriptionMethod::kDirectCollocation) {
+      m_X = DecisionVariable(m_numStates, m_numSteps + 1);
+      ConstrainDirectCollocation();
+    } else if (m_transcriptionMethod == TranscriptionMethod::kSingleShooting) {
+      // In single-shooting the states aren't decision variables, but instead
+      // depend on the input and previous states
+      m_X = VariableMatrix{m_numStates, m_numSteps + 1};
+      ConstrainSingleShooting();
+    }
+  }
+
+  /**
+   * Utility function to constrain the initial state.
+   *
+   * @param initialState the initial state to constrain to.
+   */
+  template <typename T>
+    requires ScalarLike<T> || MatrixLike<T>
+  void ConstrainInitialState(const T& initialState) {
+    SubjectTo(InitialState() == initialState);
+  }
+
+  /**
+   * Utility function to constrain the final state.
+   *
+   * @param finalState the final state to constrain to.
+   */
+  template <typename T>
+    requires ScalarLike<T> || MatrixLike<T>
+  void ConstrainFinalState(const T& finalState) {
+    SubjectTo(FinalState() == finalState);
+  }
+
+  /**
+   * Set the constraint evaluation function. This function is called
+   * `numSteps+1` times, with the corresponding state and input
+   * VariableMatrices.
+   *
+   * @param callback The callback f(t, x, u, dt) where t is time, x is the state
+   *   vector, u is the input vector, and dt is the timestep duration.
+   */
+  void ForEachStep(
+      const function_ref<void(const Variable&, const VariableMatrix&,
+                              const VariableMatrix&, const Variable&)>
+          callback) {
+    Variable time = 0.0;
+
+    for (int i = 0; i < m_numSteps + 1; ++i) {
+      auto x = X().Col(i);
+      auto u = U().Col(i);
+      auto dt = DT()(0, i);
+      callback(time, x, u, dt);
+
+      time += dt;
+    }
+  }
+
+  /**
+   * Convenience function to set a lower bound on the input.
+   *
+   * @param lowerBound The lower bound that inputs must always be above. Must be
+   * shaped (numInputs)x1.
+   */
+  template <typename T>
+    requires ScalarLike<T> || MatrixLike<T>
+  void SetLowerInputBound(const T& lowerBound) {
+    for (int i = 0; i < m_numSteps + 1; ++i) {
+      SubjectTo(U().Col(i) >= lowerBound);
+    }
+  }
+
+  /**
+   * Convenience function to set an upper bound on the input.
+   *
+   * @param upperBound The upper bound that inputs must always be below. Must be
+   * shaped (numInputs)x1.
+   */
+  template <typename T>
+    requires ScalarLike<T> || MatrixLike<T>
+  void SetUpperInputBound(const T& upperBound) {
+    for (int i = 0; i < m_numSteps + 1; ++i) {
+      SubjectTo(U().Col(i) <= upperBound);
+    }
+  }
+
+  /**
+   * 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.
+   *
+   * @param minTimestep The minimum timestep.
+   */
+  void SetMinTimestep(std::chrono::duration<double> minTimestep) {
+    SubjectTo(DT() >= minTimestep.count());
+  }
+
+  /**
+   * Get the state variables. After the problem is solved, this will contain the
+   * optimized trajectory.
+   *
+   * Shaped (numStates)x(numSteps+1).
+   *
+   * @returns The state variable matrix.
+   */
+  VariableMatrix& X() { return m_X; };
+
+  /**
+   * Get the input variables. After the problem is solved, this will contain the
+   * inputs corresponding to the optimized trajectory.
+   *
+   * Shaped (numInputs)x(numSteps+1), although the last input step is unused in
+   * the trajectory.
+   *
+   * @returns The input variable matrix.
+   */
+  VariableMatrix& U() { return m_U; };
+
+  /**
+   * Get the timestep variables. After the problem is solved, this will contain
+   * the timesteps corresponding to the optimized trajectory.
+   *
+   * Shaped 1x(numSteps+1), although the last timestep is unused in
+   * the trajectory.
+   *
+   * @returns The timestep variable matrix.
+   */
+  VariableMatrix& DT() { return m_DT; };
+
+  /**
+   * Convenience function to get the initial state in the trajectory.
+   *
+   * @returns The initial state of the trajectory.
+   */
+  VariableMatrix InitialState() { return m_X.Col(0); }
+
+  /**
+   * Convenience function to get the final state in the trajectory.
+   *
+   * @returns The final state of the trajectory.
+   */
+  VariableMatrix FinalState() { return m_X.Col(m_numSteps); }
+
+ private:
+  void ConstrainDirectCollocation() {
+    Assert(m_dynamicsType == DynamicsType::kExplicitODE);
+
+    Variable time = 0.0;
+
+    for (int i = 0; i < m_numSteps; ++i) {
+      auto x_begin = X().Col(i);
+      auto x_end = X().Col(i + 1);
+      auto u_begin = U().Col(i);
+      Variable dt = DT()(0, i);
+      auto t_begin = time;
+      auto t_end = time + dt;
+      auto t_c = t_begin + dt / 2.0;
+
+      time += dt;
+
+      // Use u_begin on the end point as well because we are approaching a
+      // discontinuity from the left
+      auto f_begin = m_dynamicsFunction(t_begin, x_begin, u_begin, dt);
+      auto f_end = m_dynamicsFunction(t_end, x_end, u_begin, dt);
+      auto x_c = (x_begin + x_end) / 2.0 + (f_begin - f_end) * (dt / 8.0);
+      auto xprime_c =
+          (x_begin - x_end) * (-3.0 / (2.0 * dt)) - (f_begin + f_end) / 4.0;
+      auto f_c = m_dynamicsFunction(t_c, x_c, u_begin, dt);
+      SubjectTo(f_c == xprime_c);
+    }
+  }
+
+  void ConstrainDirectTranscription() {
+    Variable time = 0.0;
+
+    for (int i = 0; i < m_numSteps; ++i) {
+      auto x_begin = X().Col(i);
+      auto x_end = X().Col(i + 1);
+      auto u = U().Col(i);
+      Variable dt = DT()(0, i);
+
+      if (m_dynamicsType == DynamicsType::kExplicitODE) {
+        SubjectTo(x_end ==
+                  RK4<const DynamicsFunction&, VariableMatrix, VariableMatrix,
+                      Variable>(m_dynamicsFunction, x_begin, u, time, dt));
+      } else if (m_dynamicsType == DynamicsType::kDiscrete) {
+        SubjectTo(x_end == m_dynamicsFunction(time, x_begin, u, dt));
+      }
+
+      time += dt;
+    }
+  }
+
+  void ConstrainSingleShooting() {
+    Variable time = 0.0;
+
+    for (int i = 0; i < m_numSteps; ++i) {
+      auto x_begin = X().Col(i);
+      auto x_end = X().Col(i + 1);
+      auto u = U().Col(i);
+      Variable dt = DT()(0, i);
+
+      if (m_dynamicsType == DynamicsType::kExplicitODE) {
+        x_end = RK4<const DynamicsFunction&, VariableMatrix, VariableMatrix,
+                    Variable>(m_dynamicsFunction, x_begin, u, time, dt);
+      } else if (m_dynamicsType == DynamicsType::kDiscrete) {
+        x_end = m_dynamicsFunction(time, x_begin, u, dt);
+      }
+
+      time += dt;
+    }
+  }
+
+  int m_numStates;
+  int m_numInputs;
+  std::chrono::duration<double> m_dt;
+  int m_numSteps;
+  TranscriptionMethod m_transcriptionMethod;
+
+  DynamicsType m_dynamicsType;
+  DynamicsFunction m_dynamicsFunction;
+
+  TimestepMethod m_timestepMethod;
+
+  VariableMatrix m_X;
+  VariableMatrix m_U;
+  VariableMatrix m_DT;
+};
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Constraints.hpp

@@ -0,0 +1,251 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <algorithm>
+#include <concepts>
+#include <vector>
+
+#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<LHS> || MatrixLike<LHS>) &&
+          (ScalarLike<RHS> || MatrixLike<RHS>) &&
+          (!std::same_as<LHS, double> || !std::same_as<RHS, double>)
+std::vector<Variable> MakeConstraints(const LHS& lhs, const RHS& rhs) {
+  std::vector<Variable> constraints;
+
+  if constexpr (ScalarLike<LHS> && ScalarLike<RHS>) {
+    constraints.emplace_back(lhs - rhs);
+  } else if constexpr (ScalarLike<LHS> && MatrixLike<RHS>) {
+    int rows;
+    int cols;
+    if constexpr (EigenMatrixLike<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<LHS> && ScalarLike<RHS>) {
+    int rows;
+    int cols;
+    if constexpr (EigenMatrixLike<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<LHS> && MatrixLike<RHS>) {
+    int lhsRows;
+    int lhsCols;
+    if constexpr (EigenMatrixLike<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<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.
+  std::vector<Variable> constraints;
+
+  /**
+   * 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<LHS> || MatrixLike<LHS>) &&
+            (ScalarLike<RHS> || MatrixLike<RHS>) &&
+            (!std::same_as<LHS, double> || !std::same_as<RHS, double>)
+  EqualityConstraints(const LHS& lhs, const RHS& rhs)
+      : constraints{MakeConstraints(lhs, rhs)} {}
+
+  /**
+   * Implicit conversion operator to bool.
+   */
+  operator bool() const {  // NOLINT
+    return std::all_of(
+        constraints.begin(), constraints.end(),
+        [](const 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.
+  std::vector<Variable> constraints;
+
+  /**
+   * 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<LHS> || MatrixLike<LHS>) &&
+            (ScalarLike<RHS> || MatrixLike<RHS>) &&
+            (!std::same_as<LHS, double> || !std::same_as<RHS, double>)
+  InequalityConstraints(const LHS& lhs, const RHS& rhs)
+      : constraints{MakeConstraints(lhs, rhs)} {}
+
+  /**
+   * Implicit conversion operator to bool.
+   */
+  operator bool() const {  // NOLINT
+    return std::all_of(
+        constraints.begin(), constraints.end(),
+        [](const 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<LHS> || MatrixLike<LHS>) &&
+          (ScalarLike<RHS> || MatrixLike<RHS>) &&
+          (!std::same_as<LHS, double> || !std::same_as<RHS, double>)
+EqualityConstraints operator==(const LHS& lhs, const 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<LHS> || MatrixLike<LHS>) &&
+          (ScalarLike<RHS> || MatrixLike<RHS>) &&
+          (!std::same_as<LHS, double> || !std::same_as<RHS, double>)
+InequalityConstraints operator<(const LHS& lhs, const 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<LHS> || MatrixLike<LHS>) &&
+          (ScalarLike<RHS> || MatrixLike<RHS>) &&
+          (!std::same_as<LHS, double> || !std::same_as<RHS, double>)
+InequalityConstraints operator<=(const LHS& lhs, const 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<LHS> || MatrixLike<LHS>) &&
+          (ScalarLike<RHS> || MatrixLike<RHS>) &&
+          (!std::same_as<LHS, double> || !std::same_as<RHS, double>)
+InequalityConstraints operator>(const LHS& lhs, const 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<LHS> || MatrixLike<LHS>) &&
+          (ScalarLike<RHS> || MatrixLike<RHS>) &&
+          (!std::same_as<LHS, double> || !std::same_as<RHS, double>)
+InequalityConstraints operator>=(const LHS& lhs, const RHS& rhs) {
+  return InequalityConstraints{lhs, rhs};
+}
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Multistart.hpp

@@ -0,0 +1,69 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <algorithm>
+#include <future>
+#include <span>
+#include <vector>
+
+#include "sleipnir/optimization/SolverStatus.hpp"
+#include "sleipnir/util/FunctionRef.hpp"
+
+namespace sleipnir {
+
+/**
+ * The result of a multistart solve.
+ *
+ * @tparam DecisionVariables The type containing the decision variable initial
+ *   guess.
+ */
+template <typename DecisionVariables>
+struct MultistartResult {
+  SolverStatus status;
+  DecisionVariables variables;
+};
+
+/**
+ * Solves an optimization problem from different starting points in parallel,
+ * then returns the solution with the lowest cost.
+ *
+ * Each solve is performed on a separate thread. Solutions from successful
+ * solves are always preferred over solutions from unsuccessful solves, and cost
+ * (lower is better) is the tiebreaker between successful solves.
+ *
+ * @tparam DecisionVariables The type containing the decision variable initial
+ *   guess.
+ * @param solve A user-provided function that takes a decision variable initial
+ *   guess and returns a MultistartResult.
+ * @param initialGuesses A list of decision variable initial guesses to try.
+ */
+template <typename DecisionVariables>
+MultistartResult<DecisionVariables> Multistart(
+    function_ref<MultistartResult<DecisionVariables>(const DecisionVariables&)>
+        solve,
+    std::span<const DecisionVariables> initialGuesses) {
+  std::vector<std::future<MultistartResult<DecisionVariables>>> futures;
+  for (const auto& initialGuess : initialGuesses) {
+    futures.emplace_back(std::async(std::launch::async, solve, initialGuess));
+  }
+
+  std::vector<MultistartResult<DecisionVariables>> results;
+  for (auto& future : futures) {
+    results.emplace_back(future.get());
+  }
+
+  return *std::min_element(
+      results.cbegin(), results.cend(), [](const auto& a, const auto& b) {
+        // Prioritize successful solve
+        if (a.status.exitCondition == SolverExitCondition::kSuccess &&
+            b.status.exitCondition != SolverExitCondition::kSuccess) {
+          return true;
+        }
+
+        // Otherwise prioritize solution with lower cost
+        return a.status.cost < b.status.cost;
+      });
+}
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp

@@ -0,0 +1,553 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <algorithm>
+#include <array>
+#include <concepts>
+#include <functional>
+#include <iterator>
+#include <optional>
+#include <type_traits>
+#include <utility>
+#include <vector>
+
+#include <Eigen/Core>
+
+#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"
+#include "sleipnir/optimization/SolverStatus.hpp"
+#include "sleipnir/optimization/solver/InteriorPoint.hpp"
+#include "sleipnir/util/Print.hpp"
+#include "sleipnir/util/SymbolExports.hpp"
+
+namespace sleipnir {
+
+/**
+ * This class allows the user to pose a constrained nonlinear optimization
+ * problem in natural mathematical notation and solve it.
+ *
+ * This class supports problems of the form:
+@verbatim
+      minₓ f(x)
+subject to cₑ(x) = 0
+           cᵢ(x) ≥ 0
+@endverbatim
+ *
+ * where f(x) is the scalar cost function, x is the vector of decision variables
+ * (variables the solver can tweak to minimize the cost function), cᵢ(x) are the
+ * inequality constraints, and cₑ(x) are the equality constraints. Constraints
+ * are equations or inequalities of the decision variables that constrain what
+ * values the solver is allowed to use when searching for an optimal solution.
+ *
+ * The nice thing about this class is users don't have to put their system in
+ * the form shown above manually; they can write it in natural mathematical form
+ * and it'll be converted for them. We'll cover some examples next.
+ *
+ * ## Double integrator minimum time
+ *
+ * A system with position and velocity states and an acceleration input is an
+ * example of a double integrator. We want to go from 0 m at rest to 10 m at
+ * rest in the minimum time while obeying the velocity limit (-1, 1) and the
+ * acceleration limit (-1, 1).
+ *
+ * The model for our double integrator is ẍ=u where x is the vector [position;
+ * velocity] and u is the acceleration. The velocity constraints are -1 ≤ x(1)
+ * ≤ 1 and the acceleration constraints are -1 ≤ u ≤ 1.
+ *
+ * ### Initializing a problem instance
+ *
+ * First, we need to make a problem instance.
+ * @code{.cpp}
+ * #include <Eigen/Core>
+ * #include <sleipnir/optimization/OptimizationProblem.hpp>
+ *
+ * int main() {
+ *   constexpr auto T = 5s;
+ *   constexpr auto dt = 5ms;
+ *   constexpr int N = T / dt;
+ *
+ *   sleipnir::OptimizationProblem problem;
+ * @endcode
+ *
+ * ### Creating decision variables
+ *
+ * First, we need to make decision variables for our state and input.
+ * @code{.cpp}
+ *   // 2x1 state vector with N + 1 timesteps (includes last state)
+ *   auto X = problem.DecisionVariable(2, N + 1);
+ *
+ *   // 1x1 input vector with N timesteps (input at last state doesn't matter)
+ *   auto U = problem.DecisionVariable(1, N);
+ * @endcode
+ * By convention, we use capital letters for the variables to designate
+ * matrices.
+ *
+ * ### Applying constraints
+ *
+ * Now, we need to apply dynamics constraints between timesteps.
+ * @code{.cpp}
+ * // Kinematics constraint assuming constant acceleration between timesteps
+ * for (int k = 0; k < N; ++k) {
+ *   constexpr double t = std::chrono::duration<double>(dt).count();
+ *   auto p_k1 = X(0, k + 1);
+ *   auto v_k1 = X(1, k + 1);
+ *   auto p_k = X(0, k);
+ *   auto v_k = X(1, k);
+ *   auto a_k = U(0, k);
+ *
+ *   // pₖ₊₁ = pₖ + vₖt
+ *   problem.SubjectTo(p_k1 == p_k + v_k * t);
+ *
+ *   // vₖ₊₁ = vₖ + aₖt
+ *   problem.SubjectTo(v_k1 == v_k + a_k * t);
+ * }
+ * @endcode
+ *
+ * Next, we'll apply the state and input constraints.
+ * @code{.cpp}
+ * // Start and end at rest
+ * problem.SubjectTo(X.Col(0) == Eigen::Matrix<double, 2, 1>{{0.0}, {0.0}});
+ * problem.SubjectTo(
+ *   X.Col(N + 1) == Eigen::Matrix<double, 2, 1>{{10.0}, {0.0}});
+ *
+ * // Limit velocity
+ * problem.SubjectTo(-1 <= X.Row(1));
+ * problem.SubjectTo(X.Row(1) <= 1);
+ *
+ * // Limit acceleration
+ * problem.SubjectTo(-1 <= U);
+ * problem.SubjectTo(U <= 1);
+ * @endcode
+ *
+ * ### Specifying a cost function
+ *
+ * Next, we'll create a cost function for minimizing position error.
+ * @code{.cpp}
+ * // Cost function - minimize position error
+ * sleipnir::Variable J = 0.0;
+ * for (int k = 0; k < N + 1; ++k) {
+ *   J += sleipnir::pow(10.0 - X(0, k), 2);
+ * }
+ * problem.Minimize(J);
+ * @endcode
+ * The cost function passed to Minimize() should produce a scalar output.
+ *
+ * ### Solving the problem
+ *
+ * Now we can solve the problem.
+ * @code{.cpp}
+ * problem.Solve();
+ * @endcode
+ *
+ * The solver will find the decision variable values that minimize the cost
+ * function while satisfying the constraints.
+ *
+ * ### Accessing the solution
+ *
+ * You can obtain the solution by querying the values of the variables like so.
+ * @code{.cpp}
+ * double position = X.Value(0, 0);
+ * double velocity = X.Value(1, 0);
+ * double acceleration = U.Value(0);
+ * @endcode
+ *
+ * ### Other applications
+ *
+ * In retrospect, the solution here seems obvious: if you want to reach the
+ * desired position in the minimum time, you just apply positive max input to
+ * accelerate to the max speed, coast for a while, then apply negative max input
+ * to decelerate to a stop at the desired position. Optimization problems can
+ * get more complex than this though. In fact, we can use this same framework to
+ * design optimal trajectories for a drivetrain while satisfying dynamics
+ * constraints, avoiding obstacles, and driving through points of interest.
+ *
+ * ## Optimizing the problem formulation
+ *
+ * Cost functions and constraints can have the following orders:
+ *
+ * <ul>
+ *   <li>none (i.e., there is no cost function or are no constraints)</li>
+ *   <li>constant</li>
+ *   <li>linear</li>
+ *   <li>quadratic</li>
+ *   <li>nonlinear</li>
+ * </ul>
+ *
+ * For nonlinear problems, the solver calculates the Hessian of the cost
+ * function and the Jacobians of the constraints at each iteration. However,
+ * problems with lower order cost functions and constraints can be solved
+ * faster. For example, the following only need to be computed once because
+ * they're constant:
+ *
+ * <ul>
+ *   <li>the Hessian of a quadratic or lower cost function</li>
+ *   <li>the Jacobian of linear or lower constraints</li>
+ * </ul>
+ *
+ * A problem is constant if:
+ *
+ * <ul>
+ *   <li>the cost function is constant or lower</li>
+ *   <li>the equality constraints are constant or lower</li>
+ *   <li>the inequality constraints are constant or lower</li>
+ * </ul>
+ *
+ * A problem is linear if:
+ *
+ * <ul>
+ *   <li>the cost function is linear</li>
+ *   <li>the equality constraints are linear or lower</li>
+ *   <li>the inequality constraints are linear or lower</li>
+ * </ul>
+ *
+ * A problem is quadratic if:
+ *
+ * <ul>
+ *   <li>the cost function is quadratic</li>
+ *   <li>the equality constraints are linear or lower</li>
+ *   <li>the inequality constraints are linear or lower</li>
+ * </ul>
+ *
+ * All other problems are nonlinear.
+ */
+class SLEIPNIR_DLLEXPORT OptimizationProblem {
+ public:
+  /**
+   * Construct the optimization problem.
+   */
+  OptimizationProblem() noexcept {
+    m_decisionVariables.reserve(1024);
+    m_equalityConstraints.reserve(1024);
+    m_inequalityConstraints.reserve(1024);
+  }
+
+  /**
+   * Create a decision variable in the optimization problem.
+   */
+  [[nodiscard]]
+  Variable DecisionVariable() {
+    m_decisionVariables.emplace_back();
+    return m_decisionVariables.back();
+  }
+
+  /**
+   * Create a matrix of decision variables in the optimization problem.
+   *
+   * @param rows Number of matrix rows.
+   * @param cols Number of matrix columns.
+   */
+  [[nodiscard]]
+  VariableMatrix DecisionVariable(int rows, int cols = 1) {
+    m_decisionVariables.reserve(m_decisionVariables.size() + rows * cols);
+
+    VariableMatrix vars{rows, cols};
+
+    for (int row = 0; row < rows; ++row) {
+      for (int col = 0; col < cols; ++col) {
+        m_decisionVariables.emplace_back();
+        vars(row, col) = m_decisionVariables.back();
+      }
+    }
+
+    return vars;
+  }
+
+  /**
+   * Create a symmetric matrix of decision variables in the optimization
+   * problem.
+   *
+   * Variable instances are reused across the diagonal, which helps reduce
+   * problem dimensionality.
+   *
+   * @param rows Number of matrix rows.
+   */
+  [[nodiscard]]
+  VariableMatrix SymmetricDecisionVariable(int rows) {
+    // We only need to store the lower triangle of an n x n symmetric matrix;
+    // the other elements are duplicates. The lower triangle has (n² + n)/2
+    // elements.
+    //
+    //   n
+    //   Σ k = (n² + n)/2
+    //  k=1
+    m_decisionVariables.reserve(m_decisionVariables.size() +
+                                (rows * rows + rows) / 2);
+
+    VariableMatrix vars{rows, rows};
+
+    for (int row = 0; row < rows; ++row) {
+      for (int col = 0; col <= row; ++col) {
+        m_decisionVariables.emplace_back();
+        vars(row, col) = m_decisionVariables.back();
+        vars(col, row) = m_decisionVariables.back();
+      }
+    }
+
+    return vars;
+  }
+
+  /**
+   * Tells the solver to minimize the output of the given cost function.
+   *
+   * Note that this is optional. If only constraints are specified, the solver
+   * will find the closest solution to the initial conditions that's in the
+   * feasible set.
+   *
+   * @param cost The cost function to minimize.
+   */
+  void Minimize(const Variable& cost) {
+    m_f = cost;
+    status.costFunctionType = m_f.value().Type();
+  }
+
+  /**
+   * Tells the solver to minimize the output of the given cost function.
+   *
+   * Note that this is optional. If only constraints are specified, the solver
+   * will find the closest solution to the initial conditions that's in the
+   * feasible set.
+   *
+   * @param cost The cost function to minimize.
+   */
+  void Minimize(Variable&& cost) {
+    m_f = std::move(cost);
+    status.costFunctionType = m_f.value().Type();
+  }
+
+  /**
+   * Tells the solver to maximize the output of the given objective function.
+   *
+   * Note that this is optional. If only constraints are specified, the solver
+   * will find the closest solution to the initial conditions that's in the
+   * feasible set.
+   *
+   * @param objective The objective function to maximize.
+   */
+  void Maximize(const Variable& objective) {
+    // Maximizing a cost function is the same as minimizing its negative
+    m_f = -objective;
+    status.costFunctionType = m_f.value().Type();
+  }
+
+  /**
+   * Tells the solver to maximize the output of the given objective function.
+   *
+   * Note that this is optional. If only constraints are specified, the solver
+   * will find the closest solution to the initial conditions that's in the
+   * feasible set.
+   *
+   * @param objective The objective function to maximize.
+   */
+  void Maximize(Variable&& objective) {
+    // Maximizing a cost function is the same as minimizing its negative
+    m_f = -std::move(objective);
+    status.costFunctionType = m_f.value().Type();
+  }
+
+  /**
+   * Tells the solver to solve the problem while satisfying the given equality
+   * constraint.
+   *
+   * @param constraint The constraint to satisfy.
+   */
+  void SubjectTo(const EqualityConstraints& constraint) {
+    // Get the highest order equality constraint expression type
+    for (const auto& c : constraint.constraints) {
+      status.equalityConstraintType =
+          std::max(status.equalityConstraintType, c.Type());
+    }
+
+    m_equalityConstraints.reserve(m_equalityConstraints.size() +
+                                  constraint.constraints.size());
+    std::copy(constraint.constraints.begin(), constraint.constraints.end(),
+              std::back_inserter(m_equalityConstraints));
+  }
+
+  /**
+   * Tells the solver to solve the problem while satisfying the given equality
+   * constraint.
+   *
+   * @param constraint The constraint to satisfy.
+   */
+  void SubjectTo(EqualityConstraints&& constraint) {
+    // Get the highest order equality constraint expression type
+    for (const auto& c : constraint.constraints) {
+      status.equalityConstraintType =
+          std::max(status.equalityConstraintType, c.Type());
+    }
+
+    m_equalityConstraints.reserve(m_equalityConstraints.size() +
+                                  constraint.constraints.size());
+    std::copy(constraint.constraints.begin(), constraint.constraints.end(),
+              std::back_inserter(m_equalityConstraints));
+  }
+
+  /**
+   * Tells the solver to solve the problem while satisfying the given inequality
+   * constraint.
+   *
+   * @param constraint The constraint to satisfy.
+   */
+  void SubjectTo(const InequalityConstraints& constraint) {
+    // Get the highest order inequality constraint expression type
+    for (const auto& c : constraint.constraints) {
+      status.inequalityConstraintType =
+          std::max(status.inequalityConstraintType, c.Type());
+    }
+
+    m_inequalityConstraints.reserve(m_inequalityConstraints.size() +
+                                    constraint.constraints.size());
+    std::copy(constraint.constraints.begin(), constraint.constraints.end(),
+              std::back_inserter(m_inequalityConstraints));
+  }
+
+  /**
+   * Tells the solver to solve the problem while satisfying the given inequality
+   * constraint.
+   *
+   * @param constraint The constraint to satisfy.
+   */
+  void SubjectTo(InequalityConstraints&& constraint) {
+    // Get the highest order inequality constraint expression type
+    for (const auto& c : constraint.constraints) {
+      status.inequalityConstraintType =
+          std::max(status.inequalityConstraintType, c.Type());
+    }
+
+    m_inequalityConstraints.reserve(m_inequalityConstraints.size() +
+                                    constraint.constraints.size());
+    std::copy(constraint.constraints.begin(), constraint.constraints.end(),
+              std::back_inserter(m_inequalityConstraints));
+  }
+
+  /**
+   * Solve the optimization problem. The solution will be stored in the original
+   * variables used to construct the problem.
+   *
+   * @param config Configuration options for the solver.
+   */
+  SolverStatus Solve(const SolverConfig& config = SolverConfig{}) {
+    // Create the initial value column vector
+    Eigen::VectorXd x{m_decisionVariables.size()};
+    for (size_t i = 0; i < m_decisionVariables.size(); ++i) {
+      x(i) = m_decisionVariables[i].Value();
+    }
+
+    status.exitCondition = SolverExitCondition::kSuccess;
+
+    // If there's no cost function, make it zero and continue
+    if (!m_f.has_value()) {
+      m_f = Variable();
+    }
+
+    if (config.diagnostics) {
+      constexpr std::array kExprTypeToName{"empty", "constant", "linear",
+                                           "quadratic", "nonlinear"};
+
+      // Print cost function and constraint expression types
+      sleipnir::println(
+          "The cost function is {}.",
+          kExprTypeToName[static_cast<int>(status.costFunctionType)]);
+      sleipnir::println(
+          "The equality constraints are {}.",
+          kExprTypeToName[static_cast<int>(status.equalityConstraintType)]);
+      sleipnir::println(
+          "The inequality constraints are {}.",
+          kExprTypeToName[static_cast<int>(status.inequalityConstraintType)]);
+      sleipnir::println("");
+
+      // Print problem dimensionality
+      sleipnir::println("Number of decision variables: {}",
+                        m_decisionVariables.size());
+      sleipnir::println("Number of equality constraints: {}",
+                        m_equalityConstraints.size());
+      sleipnir::println("Number of inequality constraints: {}\n",
+                        m_inequalityConstraints.size());
+    }
+
+    // If the problem is empty or constant, there's nothing to do
+    if (status.costFunctionType <= ExpressionType::kConstant &&
+        status.equalityConstraintType <= ExpressionType::kConstant &&
+        status.inequalityConstraintType <= ExpressionType::kConstant) {
+      return status;
+    }
+
+    // Solve the optimization problem
+    Eigen::VectorXd s = Eigen::VectorXd::Ones(m_inequalityConstraints.size());
+    InteriorPoint(m_decisionVariables, m_equalityConstraints,
+                  m_inequalityConstraints, m_f.value(), m_callback, config,
+                  false, x, s, &status);
+
+    if (config.diagnostics) {
+      sleipnir::println("Exit condition: {}", ToMessage(status.exitCondition));
+    }
+
+    // Assign the solution to the original Variable instances
+    VariableMatrix{m_decisionVariables}.SetValue(x);
+
+    return status;
+  }
+
+  /**
+   * Sets a callback to be called at each solver iteration.
+   *
+   * The callback for this overload should return void.
+   *
+   * @param callback The callback.
+   */
+  template <typename F>
+    requires std::invocable<F, const SolverIterationInfo&> &&
+             std::same_as<std::invoke_result_t<F, const SolverIterationInfo&>,
+                          void>
+  void Callback(F&& callback) {
+    m_callback = [=, callback = std::forward<F>(callback)](
+                     const SolverIterationInfo& info) {
+      callback(info);
+      return false;
+    };
+  }
+
+  /**
+   * Sets a callback to be called at each solver iteration.
+   *
+   * The callback for this overload should return bool.
+   *
+   * @param callback The callback. Returning true from the callback causes the
+   *   solver to exit early with the solution it has so far.
+   */
+  template <typename F>
+    requires std::invocable<F, const SolverIterationInfo&> &&
+             std::same_as<std::invoke_result_t<F, const SolverIterationInfo&>,
+                          bool>
+  void Callback(F&& callback) {
+    m_callback = std::forward<F>(callback);
+  }
+
+ private:
+  // The list of decision variables, which are the root of the problem's
+  // expression tree
+  std::vector<Variable> m_decisionVariables;
+
+  // The cost function: f(x)
+  std::optional<Variable> m_f;
+
+  // The list of equality constraints: cₑ(x) = 0
+  std::vector<Variable> m_equalityConstraints;
+
+  // The list of inequality constraints: cᵢ(x) ≥ 0
+  std::vector<Variable> m_inequalityConstraints;
+
+  // The user callback
+  std::function<bool(const SolverIterationInfo&)> m_callback =
+      [](const SolverIterationInfo&) { return false; };
+
+  // The solver status
+  SolverStatus status;
+};
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverConfig.hpp

@@ -0,0 +1,54 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <chrono>
+#include <limits>
+
+#include "sleipnir/util/SymbolExports.hpp"
+
+namespace sleipnir {
+
+/**
+ * Solver configuration.
+ */
+struct SLEIPNIR_DLLEXPORT SolverConfig {
+  /// The solver will stop once the error is below this tolerance.
+  double tolerance = 1e-8;
+
+  /// The maximum number of solver iterations before returning a solution.
+  int maxIterations = 5000;
+
+  /// The solver will stop once the error is below this tolerance for
+  /// `acceptableIterations` iterations. This is useful in cases where the
+  /// solver might not be able to achieve the desired level of accuracy due to
+  /// floating-point round-off.
+  double acceptableTolerance = 1e-6;
+
+  /// The solver will stop once the error is below `acceptableTolerance` for
+  /// this many iterations.
+  int maxAcceptableIterations = 15;
+
+  /// The maximum elapsed wall clock time before returning a solution.
+  std::chrono::duration<double> timeout{
+      std::numeric_limits<double>::infinity()};
+
+  /// Enables the feasible interior-point method. When the inequality
+  /// constraints are all feasible, step sizes are reduced when necessary to
+  /// prevent them becoming infeasible again. This is useful when parts of the
+  /// problem are ill-conditioned in infeasible regions (e.g., square root of a
+  /// negative value). This can slow or prevent progress toward a solution
+  /// though, so only enable it if necessary.
+  bool feasibleIPM = false;
+
+  /// Enables diagnostic prints.
+  bool diagnostics = false;
+
+  /// Enables writing sparsity patterns of H, Aₑ, and Aᵢ to files named H.spy,
+  /// A_e.spy, and A_i.spy respectively during solve.
+  ///
+  /// Use tools/spy.py to plot them.
+  bool spy = false;
+};
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverExitCondition.hpp

@@ -0,0 +1,78 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <stdint.h>
+
+#include <string_view>
+
+#include "sleipnir/util/SymbolExports.hpp"
+
+namespace sleipnir {
+
+/**
+ * Solver exit condition.
+ */
+enum class SolverExitCondition : int8_t {
+  /// Solved the problem to the desired tolerance.
+  kSuccess = 0,
+  /// Solved the problem to an acceptable tolerance, but not the desired one.
+  kSolvedToAcceptableTolerance = 1,
+  /// The solver returned its solution so far after the user requested a stop.
+  kCallbackRequestedStop = 2,
+  /// The solver determined the problem to be overconstrained and gave up.
+  kTooFewDOFs = -1,
+  /// The solver determined the problem to be locally infeasible and gave up.
+  kLocallyInfeasible = -2,
+  /// The solver failed to reach the desired tolerance, and feasibility
+  /// restoration failed to converge.
+  kFeasibilityRestorationFailed = -3,
+  /// The solver encountered nonfinite initial cost or constraints and gave up.
+  kNonfiniteInitialCostOrConstraints = -4,
+  /// The solver encountered diverging primal iterates xₖ and/or sₖ and gave up.
+  kDivergingIterates = -5,
+  /// The solver returned its solution so far after exceeding the maximum number
+  /// of iterations.
+  kMaxIterationsExceeded = -6,
+  /// The solver returned its solution so far after exceeding the maximum
+  /// elapsed wall clock time.
+  kTimeout = -7
+};
+
+/**
+ * Returns user-readable message corresponding to the exit condition.
+ *
+ * @param exitCondition Solver exit condition.
+ */
+SLEIPNIR_DLLEXPORT constexpr std::string_view ToMessage(
+    const SolverExitCondition& exitCondition) {
+  switch (exitCondition) {
+    case SolverExitCondition::kSuccess:
+      return "solved to desired tolerance";
+    case SolverExitCondition::kSolvedToAcceptableTolerance:
+      return "solved to acceptable tolerance";
+    case SolverExitCondition::kCallbackRequestedStop:
+      return "callback requested stop";
+    case SolverExitCondition::kTooFewDOFs:
+      return "problem has too few degrees of freedom";
+    case SolverExitCondition::kLocallyInfeasible:
+      return "problem is locally infeasible";
+    case SolverExitCondition::kFeasibilityRestorationFailed:
+      return "solver failed to reach the desired tolerance, and feasibility "
+             "restoration failed to converge";
+    case SolverExitCondition::kNonfiniteInitialCostOrConstraints:
+      return "solver encountered nonfinite initial cost or constraints and "
+             "gave up";
+    case SolverExitCondition::kDivergingIterates:
+      return "solver encountered diverging primal iterates xₖ and/or sₖ and "
+             "gave up";
+    case SolverExitCondition::kMaxIterationsExceeded:
+      return "solution returned after maximum iterations exceeded";
+    case SolverExitCondition::kTimeout:
+      return "solution returned after maximum wall clock time exceeded";
+    default:
+      return "unknown";
+  }
+}
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverIterationInfo.hpp

@@ -0,0 +1,36 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <Eigen/Core>
+#include <Eigen/SparseCore>
+
+namespace sleipnir {
+
+/**
+ * Solver iteration information exposed to a user callback.
+ */
+struct SolverIterationInfo {
+  /// The solver iteration.
+  int iteration;
+
+  /// The decision variables.
+  const Eigen::VectorXd& x;
+
+  /// The inequality constraint slack variables.
+  const Eigen::VectorXd& s;
+
+  /// The gradient of the cost function.
+  const Eigen::SparseVector<double>& g;
+
+  /// The Hessian of the Lagrangian.
+  const Eigen::SparseMatrix<double>& H;
+
+  /// The equality constraint Jacobian.
+  const Eigen::SparseMatrix<double>& A_e;
+
+  /// The inequality constraint Jacobian.
+  const Eigen::SparseMatrix<double>& A_i;
+};
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/SolverStatus.hpp

@@ -0,0 +1,32 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include "sleipnir/autodiff/ExpressionType.hpp"
+#include "sleipnir/optimization/SolverExitCondition.hpp"
+#include "sleipnir/util/SymbolExports.hpp"
+
+namespace sleipnir {
+
+/**
+ * Return value of OptimizationProblem::Solve() containing the cost function and
+ * constraint types and solver's exit condition.
+ */
+struct SLEIPNIR_DLLEXPORT SolverStatus {
+  /// The cost function type detected by the solver.
+  ExpressionType costFunctionType = ExpressionType::kNone;
+
+  /// The equality constraint type detected by the solver.
+  ExpressionType equalityConstraintType = ExpressionType::kNone;
+
+  /// The inequality constraint type detected by the solver.
+  ExpressionType inequalityConstraintType = ExpressionType::kNone;
+
+  /// The solver's exit condition.
+  SolverExitCondition exitCondition = SolverExitCondition::kSuccess;
+
+  /// The solution's cost.
+  double cost = 0.0;
+};
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/InteriorPoint.hpp

@@ -0,0 +1,55 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <span>
+
+#include <Eigen/Core>
+
+#include "sleipnir/autodiff/Variable.hpp"
+#include "sleipnir/optimization/SolverConfig.hpp"
+#include "sleipnir/optimization/SolverIterationInfo.hpp"
+#include "sleipnir/optimization/SolverStatus.hpp"
+#include "sleipnir/util/FunctionRef.hpp"
+#include "sleipnir/util/SymbolExports.hpp"
+
+namespace sleipnir {
+
+/**
+Finds the optimal solution to a nonlinear program using the interior-point
+method.
+
+A nonlinear program has the form:
+
+@verbatim
+     min_x f(x)
+subject to cₑ(x) = 0
+           cᵢ(x) ≥ 0
+@endverbatim
+
+where f(x) is the cost function, cₑ(x) are the equality constraints, and cᵢ(x)
+are the inequality constraints.
+
+@param[in] decisionVariables The list of decision variables.
+@param[in] equalityConstraints The list of equality constraints.
+@param[in] inequalityConstraints The list of inequality constraints.
+@param[in] f The cost function.
+@param[in] callback The user callback.
+@param[in] config Configuration options for the solver.
+@param[in] feasibilityRestoration Whether to use feasibility restoration instead
+  of the normal algorithm.
+@param[in,out] x The initial guess and output location for the decision
+  variables.
+@param[in,out] s The initial guess and output location for the inequality
+  constraint slack variables.
+@param[out] status The solver status.
+*/
+SLEIPNIR_DLLEXPORT void InteriorPoint(
+    std::span<Variable> decisionVariables,
+    std::span<Variable> equalityConstraints,
+    std::span<Variable> inequalityConstraints, Variable& f,
+    function_ref<bool(const SolverIterationInfo&)> callback,
+    const SolverConfig& config, bool feasibilityRestoration, Eigen::VectorXd& x,
+    Eigen::VectorXd& s, SolverStatus* status);
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Assert.hpp

@@ -0,0 +1,26 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#ifdef JORMUNGANDR
+#include <stdexcept>
+
+#include <fmt/format.h>
+/**
+ * Throw an exception in Python.
+ */
+#define Assert(condition)                                                      \
+  do {                                                                         \
+    if (!(condition)) {                                                        \
+      throw std::invalid_argument(                                             \
+          fmt::format("{}:{}: {}: Assertion `{}' failed.", __FILE__, __LINE__, \
+                      __func__, #condition));                                  \
+    }                                                                          \
+  } while (0);
+#else
+#include <cassert>
+/**
+ * Abort in C++.
+ */
+#define Assert(condition) assert(condition)
+#endif

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Concepts.hpp

@@ -0,0 +1,30 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <concepts>
+
+#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>;
+
+template <typename Derived>
+concept EigenMatrixLike =
+    std::derived_from<Derived, Eigen::MatrixBase<Derived>>;
+
+template <typename T>
+concept EigenSolver = requires(T t) { t.solve(Eigen::VectorXd{}); };
+
+template <typename T>
+concept MatrixLike =
+    std::same_as<T, VariableMatrix> ||
+    std::same_as<T, VariableBlock<VariableMatrix>> || EigenMatrixLike<T>;
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/FunctionRef.hpp

@@ -0,0 +1,100 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <functional>
+#include <memory>
+#include <type_traits>
+#include <utility>
+
+namespace sleipnir {
+
+/**
+ * An implementation of std::function_ref, a lightweight non-owning reference to
+ * a callable.
+ */
+template <class F>
+class function_ref;
+
+template <class R, class... Args>
+class function_ref<R(Args...)> {
+ public:
+  constexpr function_ref() noexcept = delete;
+
+  /**
+   * Creates a `function_ref` which refers to the same callable as `rhs`.
+   */
+  constexpr function_ref(const function_ref<R(Args...)>& rhs) noexcept =
+      default;
+
+  /**
+   * Constructs a `function_ref` referring to `f`.
+   */
+  template <typename F>
+    requires(!std::is_same_v<std::decay_t<F>, function_ref> &&
+             std::is_invocable_r_v<R, F &&, Args...>)
+  constexpr function_ref(F&& f) noexcept  // NOLINT(google-explicit-constructor)
+      : obj_(const_cast<void*>(
+            reinterpret_cast<const void*>(std::addressof(f)))) {
+    callback_ = [](void* obj, Args... args) -> R {
+      return std::invoke(
+          *reinterpret_cast<typename std::add_pointer<F>::type>(obj),
+          std::forward<Args>(args)...);
+    };
+  }
+
+  /**
+   * Makes `*this` refer to the same callable as `rhs`.
+   */
+  constexpr function_ref<R(Args...)>& operator=(
+      const function_ref<R(Args...)>& rhs) noexcept = default;
+
+  /**
+   * Makes `*this` refer to `f`.
+   */
+  template <typename F>
+    requires std::is_invocable_r_v<R, F&&, Args...>
+  constexpr function_ref<R(Args...)>& operator=(F&& f) noexcept {
+    obj_ = reinterpret_cast<void*>(std::addressof(f));
+    callback_ = [](void* obj, Args... args) {
+      return std::invoke(
+          *reinterpret_cast<typename std::add_pointer<F>::type>(obj),
+          std::forward<Args>(args)...);
+    };
+
+    return *this;
+  }
+
+  /**
+   * Swaps the referred callables of `*this` and `rhs`.
+   */
+  constexpr void swap(function_ref<R(Args...)>& rhs) noexcept {
+    std::swap(obj_, rhs.obj_);
+    std::swap(callback_, rhs.callback_);
+  }
+
+  /**
+   * Call the stored callable with the given arguments.
+   */
+  R operator()(Args... args) const {
+    return callback_(obj_, std::forward<Args>(args)...);
+  }
+
+ private:
+  void* obj_ = nullptr;
+  R (*callback_)(void*, Args...) = nullptr;
+};
+
+/**
+ * Swaps the referred callables of `lhs` and `rhs`.
+ */
+template <typename R, typename... Args>
+constexpr void swap(function_ref<R(Args...)>& lhs,
+                    function_ref<R(Args...)>& rhs) noexcept {
+  lhs.swap(rhs);
+}
+
+template <typename R, typename... Args>
+function_ref(R (*)(Args...)) -> function_ref<R(Args...)>;
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/IntrusiveSharedPtr.hpp

@@ -0,0 +1,216 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <cstddef>
+#include <memory>
+#include <utility>
+
+namespace sleipnir {
+
+/**
+ * A custom intrusive shared pointer implementation without thread
+ * synchronization overhead.
+ *
+ * Types used with this class should have three things:
+ *
+ * 1. A zero-initialized public counter variable that serves as the shared
+ *    pointer's reference count.
+ * 2. A free function `void IntrusiveSharedPtrIncRefCount(T*)` that increments
+ *    the reference count.
+ * 3. A free function `void IntrusiveSharedPtrDecRefCount(T*)` that decrements
+ *    the reference count and deallocates the pointed to object if the reference
+ *    count reaches zero.
+ *
+ * @tparam T The type of the object to be reference counted.
+ */
+template <typename T>
+class IntrusiveSharedPtr {
+ public:
+  /**
+   * Constructs an empty intrusive shared pointer.
+   */
+  constexpr IntrusiveSharedPtr() noexcept = default;
+
+  /**
+   * Constructs an empty intrusive shared pointer.
+   */
+  constexpr IntrusiveSharedPtr(std::nullptr_t) noexcept {}  // NOLINT
+
+  /**
+   * Constructs an intrusive shared pointer from the given pointer and takes
+   * ownership.
+   */
+  explicit constexpr IntrusiveSharedPtr(T* ptr) noexcept : m_ptr{ptr} {
+    if (m_ptr != nullptr) {
+      IntrusiveSharedPtrIncRefCount(m_ptr);
+    }
+  }
+
+  constexpr ~IntrusiveSharedPtr() {
+    if (m_ptr != nullptr) {
+      IntrusiveSharedPtrDecRefCount(m_ptr);
+    }
+  }
+
+  /**
+   * Copy constructs from the given intrusive shared pointer.
+   */
+  constexpr IntrusiveSharedPtr(const IntrusiveSharedPtr<T>& rhs) noexcept
+      : m_ptr{rhs.m_ptr} {
+    if (m_ptr != nullptr) {
+      IntrusiveSharedPtrIncRefCount(m_ptr);
+    }
+  }
+
+  /**
+   * Makes a copy of the given intrusive shared pointer.
+   */
+  constexpr IntrusiveSharedPtr<T>& operator=(  // NOLINT
+      const IntrusiveSharedPtr<T>& rhs) noexcept {
+    if (m_ptr == rhs.m_ptr) {
+      return *this;
+    }
+
+    if (m_ptr != nullptr) {
+      IntrusiveSharedPtrDecRefCount(m_ptr);
+    }
+
+    m_ptr = rhs.m_ptr;
+
+    if (m_ptr != nullptr) {
+      IntrusiveSharedPtrIncRefCount(m_ptr);
+    }
+
+    return *this;
+  }
+
+  /**
+   * Move constructs from the given intrusive shared pointer.
+   */
+  constexpr IntrusiveSharedPtr(IntrusiveSharedPtr<T>&& rhs) noexcept
+      : m_ptr{std::exchange(rhs.m_ptr, nullptr)} {}
+
+  /**
+   * Move assigns from the given intrusive shared pointer.
+   */
+  constexpr IntrusiveSharedPtr<T>& operator=(
+      IntrusiveSharedPtr<T>&& rhs) noexcept {
+    if (m_ptr == rhs.m_ptr) {
+      return *this;
+    }
+
+    std::swap(m_ptr, rhs.m_ptr);
+
+    return *this;
+  }
+
+  /**
+   * Returns the internal pointer.
+   */
+  constexpr T* Get() const noexcept { return m_ptr; }
+
+  /**
+   * Returns the object pointed to by the internal pointer.
+   */
+  constexpr T& operator*() const noexcept { return *m_ptr; }
+
+  /**
+   * Returns the internal pointer.
+   */
+  constexpr T* operator->() const noexcept { return m_ptr; }
+
+  /**
+   * Returns true if the internal pointer isn't nullptr.
+   */
+  explicit constexpr operator bool() const noexcept { return m_ptr != nullptr; }
+
+  /**
+   * Returns true if the given intrusive shared pointers point to the same
+   * object.
+   */
+  friend constexpr bool operator==(const IntrusiveSharedPtr<T>& lhs,
+                                   const IntrusiveSharedPtr<T>& rhs) noexcept {
+    return lhs.m_ptr == rhs.m_ptr;
+  }
+
+  /**
+   * Returns true if the given intrusive shared pointers point to different
+   * objects.
+   */
+  friend constexpr bool operator!=(const IntrusiveSharedPtr<T>& lhs,
+                                   const IntrusiveSharedPtr<T>& rhs) noexcept {
+    return lhs.m_ptr != rhs.m_ptr;
+  }
+
+  /**
+   * Returns true if the left-hand intrusive shared pointer points to nullptr.
+   */
+  friend constexpr bool operator==(const IntrusiveSharedPtr<T>& lhs,
+                                   std::nullptr_t) noexcept {
+    return lhs.m_ptr == nullptr;
+  }
+
+  /**
+   * Returns true if the right-hand intrusive shared pointer points to nullptr.
+   */
+  friend constexpr bool operator==(std::nullptr_t,
+                                   const IntrusiveSharedPtr<T>& rhs) noexcept {
+    return nullptr == rhs.m_ptr;
+  }
+
+  /**
+   * Returns true if the left-hand intrusive shared pointer doesn't point to
+   * nullptr.
+   */
+  friend constexpr bool operator!=(const IntrusiveSharedPtr<T>& lhs,
+                                   std::nullptr_t) noexcept {
+    return lhs.m_ptr != nullptr;
+  }
+
+  /**
+   * Returns true if the right-hand intrusive shared pointer doesn't point to
+   * nullptr.
+   */
+  friend constexpr bool operator!=(std::nullptr_t,
+                                   const IntrusiveSharedPtr<T>& rhs) noexcept {
+    return nullptr != rhs.m_ptr;
+  }
+
+ private:
+  T* m_ptr = nullptr;
+};
+
+/**
+ * Constructs an object of type T and wraps it in an intrusive shared pointer
+ * using args as the parameter list for the constructor of T.
+ *
+ * @tparam T Type of object for intrusive shared pointer.
+ * @tparam Args Types of constructor arguments.
+ * @param args Constructor arguments for T.
+ */
+template <typename T, typename... Args>
+IntrusiveSharedPtr<T> MakeIntrusiveShared(Args&&... args) {
+  return IntrusiveSharedPtr<T>{new T(std::forward<Args>(args)...)};
+}
+
+/**
+ * Constructs an object of type T and wraps it in an intrusive shared pointer
+ * using alloc as the storage allocator of T and args as the parameter list for
+ * the constructor of T.
+ *
+ * @tparam T Type of object for intrusive shared pointer.
+ * @tparam Alloc Type of allocator for T.
+ * @tparam Args Types of constructor arguments.
+ * @param alloc The allocator for T.
+ * @param args Constructor arguments for T.
+ */
+template <typename T, typename Alloc, typename... Args>
+IntrusiveSharedPtr<T> AllocateIntrusiveShared(Alloc alloc, Args&&... args) {
+  auto ptr = std::allocator_traits<Alloc>::allocate(alloc, sizeof(T));
+  std::allocator_traits<Alloc>::construct(alloc, ptr,
+                                          std::forward<Args>(args)...);
+  return IntrusiveSharedPtr<T>{ptr};
+}
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Pool.hpp

@@ -0,0 +1,159 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <cstddef>
+#include <memory>
+#include <vector>
+
+#include "sleipnir/util/SymbolExports.hpp"
+
+namespace sleipnir {
+
+/**
+ * This class implements a pool memory resource.
+ *
+ * The pool allocates chunks of memory and splits them into blocks managed by a
+ * free list. Allocations return pointers from the free list, and deallocations
+ * return pointers to the free list.
+ */
+class SLEIPNIR_DLLEXPORT PoolResource {
+ public:
+  /**
+   * Constructs a default PoolResource.
+   *
+   * @param blocksPerChunk Number of blocks per chunk of memory.
+   */
+  explicit PoolResource(size_t blocksPerChunk)
+      : blocksPerChunk{blocksPerChunk} {}
+
+  PoolResource(const PoolResource&) = delete;
+  PoolResource& operator=(const PoolResource&) = delete;
+  PoolResource(PoolResource&&) = default;
+  PoolResource& operator=(PoolResource&&) = default;
+
+  /**
+   * Returns a block of memory from the pool.
+   *
+   * @param bytes Number of bytes in the block.
+   * @param alignment Alignment of the block (unused).
+   */
+  [[nodiscard]]
+  void* allocate(size_t bytes, size_t alignment = alignof(std::max_align_t)) {
+    if (m_freeList.empty()) {
+      AddChunk(bytes);
+    }
+
+    auto ptr = m_freeList.back();
+    m_freeList.pop_back();
+    return ptr;
+  }
+
+  /**
+   * Gives a block of memory back to the pool.
+   *
+   * @param p A pointer to the block of memory.
+   * @param bytes Number of bytes in the block (unused).
+   * @param alignment Alignment of the block (unused).
+   */
+  void deallocate(void* p, size_t bytes,
+                  size_t alignment = alignof(std::max_align_t)) {
+    m_freeList.emplace_back(p);
+  }
+
+  /**
+   * Returns true if this pool resource has the same backing storage as another.
+   */
+  bool is_equal(const PoolResource& other) const noexcept {
+    return this == &other;
+  }
+
+  /**
+   * Returns the number of blocks from this pool resource that are in use.
+   */
+  size_t blocks_in_use() const noexcept {
+    return m_buffer.size() * blocksPerChunk - m_freeList.size();
+  }
+
+ private:
+  std::vector<std::unique_ptr<std::byte[]>> m_buffer;
+  std::vector<void*> m_freeList;
+  size_t blocksPerChunk;
+
+  /**
+   * Adds a memory chunk to the pool, partitions it into blocks with the given
+   * number of bytes, and appends pointers to them to the free list.
+   *
+   * @param bytesPerBlock Number of bytes in the block.
+   */
+  void AddChunk(size_t bytesPerBlock) {
+    m_buffer.emplace_back(new std::byte[bytesPerBlock * blocksPerChunk]);
+    for (int i = blocksPerChunk - 1; i >= 0; --i) {
+      m_freeList.emplace_back(m_buffer.back().get() + bytesPerBlock * i);
+    }
+  }
+};
+
+/**
+ * This class is an allocator for the pool resource.
+ *
+ * @tparam T The type of object in the pool.
+ */
+template <typename T>
+class PoolAllocator {
+ public:
+  /**
+   * The type of object in the pool.
+   */
+  using value_type = T;
+
+  /**
+   * Constructs a pool allocator with the given pool memory resource.
+   *
+   * @param r The pool resource.
+   */
+  explicit constexpr PoolAllocator(PoolResource* r) : m_memoryResource{r} {}
+
+  constexpr PoolAllocator(const PoolAllocator<T>& other) = default;
+  constexpr PoolAllocator<T>& operator=(const PoolAllocator<T>&) = default;
+
+  /**
+   * Returns a block of memory from the pool.
+   *
+   * @param n Number of bytes in the block.
+   */
+  [[nodiscard]]
+  constexpr T* allocate(size_t n) {
+    return static_cast<T*>(m_memoryResource->allocate(n));
+  }
+
+  /**
+   * Gives a block of memory back to the pool.
+   *
+   * @param p A pointer to the block of memory.
+   * @param n Number of bytes in the block.
+   */
+  constexpr void deallocate(T* p, size_t n) {
+    m_memoryResource->deallocate(p, n);
+  }
+
+ private:
+  PoolResource* m_memoryResource;
+};
+
+/**
+ * Returns a global pool memory resource.
+ */
+SLEIPNIR_DLLEXPORT PoolResource& GlobalPoolResource();
+
+/**
+ * Returns an allocator for a global pool memory resource.
+ *
+ * @tparam T The type of object in the pool.
+ */
+template <typename T>
+PoolAllocator<T> GlobalPoolAllocator() {
+  return PoolAllocator<T>{&GlobalPoolResource()};
+}
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Print.hpp

@@ -0,0 +1,56 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <system_error>
+#include <utility>
+
+#include <fmt/core.h>
+
+namespace sleipnir {
+
+/**
+ * Wrapper around fmt::print() that squelches write failure exceptions.
+ */
+template <typename... T>
+inline void print(fmt::format_string<T...> fmt, T&&... args) {
+  try {
+    fmt::print(fmt, std::forward<T>(args)...);
+  } catch (const std::system_error&) {
+  }
+}
+
+/**
+ * Wrapper around fmt::print() that squelches write failure exceptions.
+ */
+template <typename... T>
+inline void print(std::FILE* f, fmt::format_string<T...> fmt, T&&... args) {
+  try {
+    fmt::print(f, fmt, std::forward<T>(args)...);
+  } catch (const std::system_error&) {
+  }
+}
+
+/**
+ * Wrapper around fmt::println() that squelches write failure exceptions.
+ */
+template <typename... T>
+inline void println(fmt::format_string<T...> fmt, T&&... args) {
+  try {
+    fmt::println(fmt, std::forward<T>(args)...);
+  } catch (const std::system_error&) {
+  }
+}
+
+/**
+ * Wrapper around fmt::println() that squelches write failure exceptions.
+ */
+template <typename... T>
+inline void println(std::FILE* f, fmt::format_string<T...> fmt, T&&... args) {
+  try {
+    fmt::println(f, fmt, std::forward<T>(args)...);
+  } catch (const std::system_error&) {
+  }
+}
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Spy.hpp

@@ -0,0 +1,89 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <fstream>
+#include <string>
+#include <string_view>
+#include <vector>
+
+#include <Eigen/SparseCore>
+
+#include "sleipnir/util/SymbolExports.hpp"
+
+namespace sleipnir {
+
+/**
+ * Write the sparsity pattern of a sparse matrix to a file.
+ *
+ * Each character represents an element with '.' representing zero, '+'
+ * representing positive, and '-' representing negative. Here's an example for a
+ * 3x3 identity matrix.
+ *
+ * "+.."
+ * ".+."
+ * "..+"
+ *
+ * @param[out] file A file stream.
+ * @param[in] mat The sparse matrix.
+ */
+SLEIPNIR_DLLEXPORT inline void Spy(std::ostream& file,
+                                   const Eigen::SparseMatrix<double>& mat) {
+  const int cells_width = mat.cols() + 1;
+  const int cells_height = mat.rows();
+
+  std::vector<uint8_t> cells;
+
+  // Allocate space for matrix of characters plus trailing newlines
+  cells.reserve(cells_width * cells_height);
+
+  // Initialize cell array
+  for (int row = 0; row < mat.rows(); ++row) {
+    for (int col = 0; col < mat.cols(); ++col) {
+      cells.emplace_back('.');
+    }
+    cells.emplace_back('\n');
+  }
+
+  // Fill in non-sparse entries
+  for (int k = 0; k < mat.outerSize(); ++k) {
+    for (Eigen::SparseMatrix<double>::InnerIterator it{mat, k}; it; ++it) {
+      if (it.value() < 0.0) {
+        cells[it.row() * cells_width + it.col()] = '-';
+      } else if (it.value() > 0.0) {
+        cells[it.row() * cells_width + it.col()] = '+';
+      }
+    }
+  }
+
+  // Write cell array to file
+  for (const auto& c : cells) {
+    file << c;
+  }
+}
+
+/**
+ * Write the sparsity pattern of a sparse matrix to a file.
+ *
+ * Each character represents an element with "." representing zero, "+"
+ * representing positive, and "-" representing negative. Here's an example for a
+ * 3x3 identity matrix.
+ *
+ * "+.."
+ * ".+."
+ * "..+"
+ *
+ * @param[in] filename The filename.
+ * @param[in] mat The sparse matrix.
+ */
+SLEIPNIR_DLLEXPORT inline void Spy(std::string_view filename,
+                                   const Eigen::SparseMatrix<double>& mat) {
+  std::ofstream file{std::string{filename}};
+  if (!file.is_open()) {
+    return;
+  }
+
+  Spy(file, mat);
+}
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/SymbolExports.hpp

@@ -0,0 +1,192 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#ifdef _WIN32
+#ifdef _MSC_VER
+#pragma warning(disable : 4251)
+#endif
+
+#ifdef SLEIPNIR_EXPORTS
+#ifdef __GNUC__
+#define SLEIPNIR_DLLEXPORT __attribute__((dllexport))
+#else
+#define SLEIPNIR_DLLEXPORT __declspec(dllexport)
+#endif
+
+#elif defined(SLEIPNIR_IMPORTS)
+
+#ifdef __GNUC__
+#define SLEIPNIR_DLLEXPORT __attribute__((dllimport))
+#else
+#define SLEIPNIR_DLLEXPORT __declspec(dllimport)
+#endif
+
+#else
+#define SLEIPNIR_DLLEXPORT
+#endif
+
+#else  // _WIN32
+
+#ifdef SLEIPNIR_EXPORTS
+#define SLEIPNIR_DLLEXPORT __attribute__((visibility("default")))
+#else
+#define SLEIPNIR_DLLEXPORT
+#endif
+
+#endif  // _WIN32
+
+// Synopsis
+//
+// This header provides macros for using FOO_EXPORT macros with explicit
+// template instantiation declarations and definitions.
+// Generally, the FOO_EXPORT macros are used at declarations,
+// and GCC requires them to be used at explicit instantiation declarations,
+// but MSVC requires __declspec(dllexport) to be used at the explicit
+// instantiation definitions instead.
+
+// Usage
+//
+// In a header file, write:
+//
+//   extern template class EXPORT_TEMPLATE_DECLARE(FOO_EXPORT) foo<bar>;
+//
+// In a source file, write:
+//
+//   template class EXPORT_TEMPLATE_DEFINE(FOO_EXPORT) foo<bar>;
+
+// Implementation notes
+//
+// The implementation of this header uses some subtle macro semantics to
+// detect what the provided FOO_EXPORT value was defined as and then
+// to dispatch to appropriate macro definitions.  Unfortunately,
+// MSVC's C preprocessor is rather non-compliant and requires special
+// care to make it work.
+//
+// Issue 1.
+//
+//   #define F(x)
+//   F()
+//
+// MSVC emits warning C4003 ("not enough actual parameters for macro
+// 'F'), even though it's a valid macro invocation.  This affects the
+// macros below that take just an "export" parameter, because export
+// may be empty.
+//
+// As a workaround, we can add a dummy parameter and arguments:
+//
+//   #define F(x,_)
+//   F(,)
+//
+// Issue 2.
+//
+//   #define F(x) G##x
+//   #define Gj() ok
+//   F(j())
+//
+// The correct replacement for "F(j())" is "ok", but MSVC replaces it
+// with "Gj()".  As a workaround, we can pass the result to an
+// identity macro to force MSVC to look for replacements again.  (This
+// is why EXPORT_TEMPLATE_STYLE_3 exists.)
+
+#define EXPORT_TEMPLATE_DECLARE(export) \
+  EXPORT_TEMPLATE_INVOKE(DECLARE, EXPORT_TEMPLATE_STYLE(export, ), export)
+#define EXPORT_TEMPLATE_DEFINE(export) \
+  EXPORT_TEMPLATE_INVOKE(DEFINE, EXPORT_TEMPLATE_STYLE(export, ), export)
+
+// INVOKE is an internal helper macro to perform parameter replacements
+// and token pasting to chain invoke another macro.  E.g.,
+//     EXPORT_TEMPLATE_INVOKE(DECLARE, DEFAULT, FOO_EXPORT)
+// will export to call
+//     EXPORT_TEMPLATE_DECLARE_DEFAULT(FOO_EXPORT, )
+// (but with FOO_EXPORT expanded too).
+#define EXPORT_TEMPLATE_INVOKE(which, style, export) \
+  EXPORT_TEMPLATE_INVOKE_2(which, style, export)
+#define EXPORT_TEMPLATE_INVOKE_2(which, style, export) \
+  EXPORT_TEMPLATE_##which##_##style(export, )
+
+// Default style is to apply the FOO_EXPORT macro at declaration sites.
+#define EXPORT_TEMPLATE_DECLARE_DEFAULT(export, _) export
+#define EXPORT_TEMPLATE_DEFINE_DEFAULT(export, _)
+
+// The "MSVC hack" style is used when FOO_EXPORT is defined
+// as __declspec(dllexport), which MSVC requires to be used at
+// definition sites instead.
+#define EXPORT_TEMPLATE_DECLARE_MSVC_HACK(export, _)
+#define EXPORT_TEMPLATE_DEFINE_MSVC_HACK(export, _) export
+
+// EXPORT_TEMPLATE_STYLE is an internal helper macro that identifies which
+// export style needs to be used for the provided FOO_EXPORT macro definition.
+// "", "__attribute__(...)", and "__declspec(dllimport)" are mapped
+// to "DEFAULT"; while "__declspec(dllexport)" is mapped to "MSVC_HACK".
+//
+// It's implemented with token pasting to transform the __attribute__ and
+// __declspec annotations into macro invocations.  E.g., if FOO_EXPORT is
+// defined as "__declspec(dllimport)", it undergoes the following sequence of
+// macro substitutions:
+//     EXPORT_TEMPLATE_STYLE(FOO_EXPORT, )
+//     EXPORT_TEMPLATE_STYLE_2(__declspec(dllimport), )
+//     EXPORT_TEMPLATE_STYLE_3(EXPORT_TEMPLATE_STYLE_MATCH__declspec(dllimport))
+//     EXPORT_TEMPLATE_STYLE_MATCH__declspec(dllimport)
+//     EXPORT_TEMPLATE_STYLE_MATCH_DECLSPEC_dllimport
+//     DEFAULT
+#define EXPORT_TEMPLATE_STYLE(export, _) EXPORT_TEMPLATE_STYLE_2(export, )
+#define EXPORT_TEMPLATE_STYLE_2(export, _) \
+  EXPORT_TEMPLATE_STYLE_3(                 \
+      EXPORT_TEMPLATE_STYLE_MATCH_foj3FJo5StF0OvIzl7oMxA##export)
+#define EXPORT_TEMPLATE_STYLE_3(style) style
+
+// Internal helper macros for EXPORT_TEMPLATE_STYLE.
+//
+// XXX: C++ reserves all identifiers containing "__" for the implementation,
+// but "__attribute__" and "__declspec" already contain "__" and the token-paste
+// operator can only add characters; not remove them.  To minimize the risk of
+// conflict with implementations, we include "foj3FJo5StF0OvIzl7oMxA" (a random
+// 128-bit string, encoded in Base64) in the macro name.
+#define EXPORT_TEMPLATE_STYLE_MATCH_foj3FJo5StF0OvIzl7oMxA DEFAULT
+#define EXPORT_TEMPLATE_STYLE_MATCH_foj3FJo5StF0OvIzl7oMxA__attribute__(...) \
+  DEFAULT
+#define EXPORT_TEMPLATE_STYLE_MATCH_foj3FJo5StF0OvIzl7oMxA__declspec(arg) \
+  EXPORT_TEMPLATE_STYLE_MATCH_DECLSPEC_##arg
+
+// Internal helper macros for EXPORT_TEMPLATE_STYLE.
+#define EXPORT_TEMPLATE_STYLE_MATCH_DECLSPEC_dllexport MSVC_HACK
+#define EXPORT_TEMPLATE_STYLE_MATCH_DECLSPEC_dllimport DEFAULT
+
+// Sanity checks.
+//
+// EXPORT_TEMPLATE_TEST uses the same macro invocation pattern as
+// EXPORT_TEMPLATE_DECLARE and EXPORT_TEMPLATE_DEFINE do to check that they're
+// working correctly.  When they're working correctly, the sequence of macro
+// replacements should go something like:
+//
+//     EXPORT_TEMPLATE_TEST(DEFAULT, __declspec(dllimport));
+//
+//     static_assert(EXPORT_TEMPLATE_INVOKE(TEST_DEFAULT,
+//         EXPORT_TEMPLATE_STYLE(__declspec(dllimport), ),
+//         __declspec(dllimport)), "__declspec(dllimport)");
+//
+//     static_assert(EXPORT_TEMPLATE_INVOKE(TEST_DEFAULT,
+//         DEFAULT, __declspec(dllimport)), "__declspec(dllimport)");
+//
+//     static_assert(EXPORT_TEMPLATE_TEST_DEFAULT_DEFAULT(
+//         __declspec(dllimport)), "__declspec(dllimport)");
+//
+//     static_assert(true, "__declspec(dllimport)");
+//
+// When they're not working correctly, a syntax error should occur instead.
+#define EXPORT_TEMPLATE_TEST(want, export)                                 \
+  static_assert(EXPORT_TEMPLATE_INVOKE(                                    \
+                    TEST_##want, EXPORT_TEMPLATE_STYLE(export, ), export), \
+                #export)
+#define EXPORT_TEMPLATE_TEST_DEFAULT_DEFAULT(...) true
+#define EXPORT_TEMPLATE_TEST_MSVC_HACK_MSVC_HACK(...) true
+
+EXPORT_TEMPLATE_TEST(DEFAULT, );
+EXPORT_TEMPLATE_TEST(DEFAULT, __attribute__((visibility("default"))));
+EXPORT_TEMPLATE_TEST(MSVC_HACK, __declspec(dllexport));
+EXPORT_TEMPLATE_TEST(DEFAULT, __declspec(dllimport));
+
+#undef EXPORT_TEMPLATE_TEST
+#undef EXPORT_TEMPLATE_TEST_DEFAULT_DEFAULT
+#undef EXPORT_TEMPLATE_TEST_MSVC_HACK_MSVC_HACK