[wpimath] Add Sleipnir (#6541)

This is useful for solving quadratic programs.

wpimath/src/main/native/thirdparty/sleipnir/.clang-format

@@ -0,0 +1,249 @@
+---
+Language:        Cpp
+BasedOnStyle:  Google
+AccessModifierOffset: -1
+AlignAfterOpenBracket: Align
+AlignArrayOfStructures: None
+AlignConsecutiveAssignments:
+  Enabled:         false
+  AcrossEmptyLines: false
+  AcrossComments:  false
+  AlignCompound:   false
+  PadOperators:    true
+AlignConsecutiveBitFields:
+  Enabled:         false
+  AcrossEmptyLines: false
+  AcrossComments:  false
+  AlignCompound:   false
+  PadOperators:    false
+AlignConsecutiveDeclarations:
+  Enabled:         false
+  AcrossEmptyLines: false
+  AcrossComments:  false
+  AlignCompound:   false
+  PadOperators:    false
+AlignConsecutiveMacros:
+  Enabled:         false
+  AcrossEmptyLines: false
+  AcrossComments:  false
+  AlignCompound:   false
+  PadOperators:    false
+AlignConsecutiveShortCaseStatements:
+  Enabled:         false
+  AcrossEmptyLines: false
+  AcrossComments:  false
+  AlignCaseColons: false
+AlignEscapedNewlines: Left
+AlignOperands:   Align
+AlignTrailingComments:
+  Kind:            Always
+  OverEmptyLines:  0
+AllowAllArgumentsOnNextLine: true
+AllowAllParametersOfDeclarationOnNextLine: true
+AllowShortBlocksOnASingleLine: Never
+AllowShortCaseLabelsOnASingleLine: false
+AllowShortEnumsOnASingleLine: true
+AllowShortFunctionsOnASingleLine: Inline
+AllowShortIfStatementsOnASingleLine: Never
+AllowShortLambdasOnASingleLine: All
+AllowShortLoopsOnASingleLine: false
+AlwaysBreakAfterDefinitionReturnType: None
+AlwaysBreakAfterReturnType: None
+AlwaysBreakBeforeMultilineStrings: true
+AlwaysBreakTemplateDeclarations: Yes
+BinPackArguments: true
+BinPackParameters: true
+BitFieldColonSpacing: Both
+BraceWrapping:
+  AfterCaseLabel:  false
+  AfterClass:      false
+  AfterControlStatement: Never
+  AfterEnum:       false
+  AfterExternBlock: false
+  AfterFunction:   false
+  AfterNamespace:  false
+  AfterObjCDeclaration: false
+  AfterStruct:     false
+  AfterUnion:      false
+  BeforeCatch:     false
+  BeforeElse:      false
+  BeforeLambdaBody: false
+  BeforeWhile:     false
+  IndentBraces:    false
+  SplitEmptyFunction: true
+  SplitEmptyRecord: true
+  SplitEmptyNamespace: true
+BreakAfterAttributes: Always
+BreakAfterJavaFieldAnnotations: false
+BreakArrays:     true
+BreakBeforeBinaryOperators: None
+BreakBeforeConceptDeclarations: Always
+BreakBeforeBraces: Attach
+BreakBeforeInlineASMColon: OnlyMultiline
+BreakBeforeTernaryOperators: true
+BreakConstructorInitializers: BeforeColon
+BreakInheritanceList: BeforeColon
+BreakStringLiterals: true
+ColumnLimit:     80
+CommentPragmas:  '^ IWYU pragma:'
+CompactNamespaces: false
+ConstructorInitializerIndentWidth: 4
+ContinuationIndentWidth: 4
+Cpp11BracedListStyle: true
+DerivePointerAlignment: false
+DisableFormat:   false
+EmptyLineAfterAccessModifier: Never
+EmptyLineBeforeAccessModifier: LogicalBlock
+ExperimentalAutoDetectBinPacking: false
+FixNamespaceComments: true
+IncludeBlocks:   Regroup
+IncludeCategories:
+  - Regex:           '^<ext/.*\.h>'
+    Priority:        2
+    SortPriority:    0
+    CaseSensitive:   false
+  - Regex:           '^<.*\.h>'
+    Priority:        1
+    SortPriority:    0
+    CaseSensitive:   false
+  - Regex:           '^<.*'
+    Priority:        2
+    SortPriority:    0
+    CaseSensitive:   false
+  - Regex:           '.*'
+    Priority:        3
+    SortPriority:    0
+    CaseSensitive:   false
+IncludeIsMainRegex: '([-_](test|unittest))?$'
+IncludeIsMainSourceRegex: ''
+IndentAccessModifiers: false
+IndentCaseBlocks: false
+IndentCaseLabels: true
+IndentExternBlock: AfterExternBlock
+IndentGotoLabels: true
+IndentPPDirectives: None
+IndentRequiresClause: true
+IndentWidth:     2
+IndentWrappedFunctionNames: false
+InsertBraces:    false
+InsertNewlineAtEOF: false
+InsertTrailingCommas: None
+IntegerLiteralSeparator:
+  Binary:          0
+  BinaryMinDigits: 0
+  Decimal:         0
+  DecimalMinDigits: 0
+  Hex:             0
+  HexMinDigits:    0
+JavaScriptQuotes: Leave
+JavaScriptWrapImports: true
+KeepEmptyLinesAtTheStartOfBlocks: false
+KeepEmptyLinesAtEOF: false
+LambdaBodyIndentation: Signature
+LineEnding:      DeriveLF
+MacroBlockBegin: ''
+MacroBlockEnd:   ''
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+ObjCBinPackProtocolList: Never
+ObjCBlockIndentWidth: 2
+ObjCBreakBeforeNestedBlockParam: true
+ObjCSpaceAfterProperty: false
+ObjCSpaceBeforeProtocolList: true
+PackConstructorInitializers: NextLine
+PenaltyBreakAssignment: 2
+PenaltyBreakBeforeFirstCallParameter: 1
+PenaltyBreakComment: 300
+PenaltyBreakFirstLessLess: 120
+PenaltyBreakOpenParenthesis: 0
+PenaltyBreakString: 1000
+PenaltyBreakTemplateDeclaration: 10
+PenaltyExcessCharacter: 1000000
+PenaltyIndentedWhitespace: 0
+PenaltyReturnTypeOnItsOwnLine: 200
+PointerAlignment: Left
+PPIndentWidth:   -1
+QualifierAlignment: Leave
+RawStringFormats:
+  - Language:        Cpp
+    Delimiters:
+      - cc
+      - CC
+      - cpp
+      - Cpp
+      - CPP
+      - 'c++'
+      - 'C++'
+    CanonicalDelimiter: ''
+    BasedOnStyle:    google
+  - Language:        TextProto
+    Delimiters:
+      - pb
+      - PB
+      - proto
+      - PROTO
+    EnclosingFunctions:
+      - EqualsProto
+      - EquivToProto
+      - PARSE_PARTIAL_TEXT_PROTO
+      - PARSE_TEST_PROTO
+      - PARSE_TEXT_PROTO
+      - ParseTextOrDie
+      - ParseTextProtoOrDie
+      - ParseTestProto
+      - ParsePartialTestProto
+    CanonicalDelimiter: pb
+    BasedOnStyle:    google
+ReferenceAlignment: Pointer
+ReflowComments:  true
+RemoveBracesLLVM: false
+RemoveParentheses: Leave
+RemoveSemicolon: false
+RequiresClausePosition: OwnLine
+RequiresExpressionIndentation: OuterScope
+SeparateDefinitionBlocks: Leave
+ShortNamespaceLines: 1
+SortIncludes:    false
+SortJavaStaticImport: Before
+SortUsingDeclarations: LexicographicNumeric
+SpaceAfterCStyleCast: false
+SpaceAfterLogicalNot: false
+SpaceAfterTemplateKeyword: true
+SpaceAroundPointerQualifiers: Default
+SpaceBeforeAssignmentOperators: true
+SpaceBeforeCaseColon: false
+SpaceBeforeCpp11BracedList: false
+SpaceBeforeCtorInitializerColon: true
+SpaceBeforeInheritanceColon: true
+SpaceBeforeJsonColon: false
+SpaceBeforeParens: ControlStatements
+SpaceBeforeParensOptions:
+  AfterControlStatements: true
+  AfterForeachMacros: true
+  AfterFunctionDefinitionName: false
+  AfterFunctionDeclarationName: false
+  AfterIfMacros:   true
+  AfterOverloadedOperator: false
+  AfterRequiresInClause: false
+  AfterRequiresInExpression: false
+  BeforeNonEmptyParentheses: false
+SpaceBeforeRangeBasedForLoopColon: true
+SpaceBeforeSquareBrackets: false
+SpaceInEmptyBlock: false
+SpacesBeforeTrailingComments: 2
+SpacesInAngles:  Never
+SpacesInContainerLiterals: true
+SpacesInLineCommentPrefix:
+  Minimum:         1
+  Maximum:         -1
+SpacesInParens:  Never
+SpacesInParensOptions:
+  InCStyleCasts:   false
+  InConditionalStatements: false
+  InEmptyParentheses: false
+  Other:           false
+SpacesInSquareBrackets: false
+Standard:        c++20
+TabWidth:        8
+UseTab:          Never
+...

wpimath/src/main/native/thirdparty/sleipnir/.clang-tidy

@@ -0,0 +1,74 @@
+Checks:
+  'bugprone-assert-side-effect,
+  bugprone-bool-pointer-implicit-conversion,
+  bugprone-copy-constructor-init,
+  bugprone-dangling-handle,
+  bugprone-dynamic-static-initializers,
+  bugprone-forward-declaration-namespace,
+  bugprone-forwarding-reference-overload,
+  bugprone-inaccurate-erase,
+  bugprone-incorrect-roundings,
+  bugprone-integer-division,
+  bugprone-lambda-function-name,
+  bugprone-misplaced-operator-in-strlen-in-alloc,
+  bugprone-misplaced-widening-cast,
+  bugprone-move-forwarding-reference,
+  bugprone-multiple-statement-macro,
+  bugprone-parent-virtual-call,
+  bugprone-posix-return,
+  bugprone-sizeof-container,
+  bugprone-sizeof-expression,
+  bugprone-spuriously-wake-up-functions,
+  bugprone-string-constructor,
+  bugprone-string-integer-assignment,
+  bugprone-string-literal-with-embedded-nul,
+  bugprone-suspicious-enum-usage,
+  bugprone-suspicious-include,
+  bugprone-suspicious-memset-usage,
+  bugprone-suspicious-missing-comma,
+  bugprone-suspicious-semicolon,
+  bugprone-suspicious-string-compare,
+  bugprone-throw-keyword-missing,
+  bugprone-too-small-loop-variable,
+  bugprone-undefined-memory-manipulation,
+  bugprone-undelegated-constructor,
+  bugprone-unhandled-self-assignment,
+  bugprone-unused-raii,
+  bugprone-virtual-near-miss,
+  cert-err52-cpp,
+  cert-err60-cpp,
+  cert-mem57-cpp,
+  cert-oop57-cpp,
+  cert-oop58-cpp,
+  clang-diagnostic-*,
+  -clang-diagnostic-deprecated-declarations,
+  -clang-diagnostic-#warnings,
+  -clang-diagnostic-pedantic,
+  clang-analyzer-*,
+  -clang-analyzer-core.uninitialized.UndefReturn,
+  -clang-analyzer-optin.cplusplus.UninitializedObject,
+  -clang-analyzer-optin.portability.UnixAPI,
+  -clang-analyzer-unix.Malloc,
+  -cppcoreguidelines-slicing,
+  google-build-namespaces,
+  google-explicit-constructor,
+  google-global-names-in-headers,
+  google-readability-avoid-underscore-in-googletest-name,
+  google-readability-casting,
+  google-runtime-operator,
+  misc-definitions-in-headers,
+  misc-misplaced-const,
+  misc-new-delete-overloads,
+  misc-non-copyable-objects,
+  modernize-avoid-bind,
+  modernize-concat-nested-namespaces,
+  modernize-make-shared,
+  modernize-make-unique,
+  modernize-pass-by-value,
+  modernize-use-default-member-init,
+  modernize-use-noexcept,
+  modernize-use-nullptr,
+  modernize-use-override,
+  modernize-use-using,
+  readability-braces-around-statements'
+FormatStyle: file

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

@@ -0,0 +1,23 @@
+cppHeaderFileInclude {
+  \.hpp$
+}
+
+cppSrcFileInclude {
+  \.cpp$
+}
+
+modifiableFileExclude {
+  \.patch$
+  \.png$
+  \.svg$
+  jormungandr/cpp/Docstrings\.hpp$
+  jormungandr/py\.typed$
+}
+
+includeOtherLibs {
+  ^Eigen/
+  ^catch2/
+  ^fmt/
+  ^pybind11/
+  ^sleipnir/
+}

wpimath/src/main/native/thirdparty/sleipnir/.styleguide-license

@@ -0,0 +1 @@
+// Copyright (c) Sleipnir contributors

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

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

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

wpimath/src/main/native/thirdparty/sleipnir/src/optimization/Inertia.hpp

@@ -0,0 +1,58 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <cstddef>
+
+#include "sleipnir/util/Concepts.hpp"
+
+namespace sleipnir {
+
+/**
+ * Represents the inertia of a matrix (the number of positive, negative, and
+ * zero eigenvalues).
+ */
+class Inertia {
+ public:
+  size_t positive = 0;
+  size_t negative = 0;
+  size_t zero = 0;
+
+  constexpr Inertia() = default;
+
+  /**
+   * Constructs the Inertia type with the given number of positive, negative,
+   * and zero eigenvalues.
+   *
+   * @param positive The number of positive eigenvalues.
+   * @param negative The number of negative eigenvalues.
+   * @param zero The number of zero eigenvalues.
+   */
+  constexpr Inertia(size_t positive, size_t negative, size_t zero)
+      : positive{positive}, negative{negative}, zero{zero} {}
+
+  /**
+   * Constructs the Inertia type with the inertia of the given LDLT
+   * decomposition.
+   *
+   * @tparam Solver Eigen sparse linear system solver.
+   * @param solver The LDLT decomposition of which to compute the inertia.
+   */
+  template <EigenSolver Solver>
+  explicit Inertia(const Solver& solver) {
+    const auto& D = solver.vectorD();
+    for (int row = 0; row < D.rows(); ++row) {
+      if (D(row) > 0.0) {
+        ++positive;
+      } else if (D(row) < 0.0) {
+        ++negative;
+      } else {
+        ++zero;
+      }
+    }
+  }
+
+  bool operator==(const Inertia&) const = default;
+};
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/src/optimization/RegularizedLDLT.hpp

@@ -0,0 +1,179 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <cmath>
+#include <cstddef>
+
+#include <Eigen/Core>
+#include <Eigen/SparseCholesky>
+#include <Eigen/SparseCore>
+
+#include "optimization/Inertia.hpp"
+
+// See docs/algorithms.md#Works_cited for citation definitions
+
+namespace sleipnir {
+
+/**
+ * Solves systems of linear equations using a regularized LDLT factorization.
+ */
+class RegularizedLDLT {
+ public:
+  using Solver = Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>,
+                                       Eigen::Lower, Eigen::AMDOrdering<int>>;
+
+  /**
+   * Constructs a RegularizedLDLT instance.
+   */
+  RegularizedLDLT() = default;
+
+  /**
+   * Reports whether previous computation was successful.
+   */
+  Eigen::ComputationInfo Info() { return m_info; }
+
+  /**
+   * Computes the regularized LDLT factorization of a matrix.
+   *
+   * @param lhs Left-hand side of the system.
+   * @param numEqualityConstraints The number of equality constraints in the
+   *   system.
+   * @param μ The barrier parameter for the current interior-point iteration.
+   */
+  void Compute(const Eigen::SparseMatrix<double>& lhs,
+               size_t numEqualityConstraints, double μ) {
+    // The regularization procedure is based on algorithm B.1 of [1]
+    m_numDecisionVariables = lhs.rows() - numEqualityConstraints;
+    m_numEqualityConstraints = numEqualityConstraints;
+
+    const Inertia idealInertia{m_numDecisionVariables, m_numEqualityConstraints,
+                               0};
+    Inertia inertia;
+
+    double δ = 0.0;
+    double γ = 0.0;
+
+    AnalyzePattern(lhs);
+    m_solver.factorize(lhs);
+
+    if (m_solver.info() == Eigen::Success) {
+      inertia = Inertia{m_solver};
+
+      // If the inertia is ideal, don't regularize the system
+      if (inertia == idealInertia) {
+        m_info = Eigen::Success;
+        return;
+      }
+    }
+
+    // If the decomposition succeeded and the inertia has some zero eigenvalues,
+    // or the decomposition failed, regularize the equality constraints
+    if ((m_solver.info() == Eigen::Success && inertia.zero > 0) ||
+        m_solver.info() != Eigen::Success) {
+      γ = 1e-8 * std::pow(μ, 0.25);
+    }
+
+    // Also regularize the Hessian. If the Hessian wasn't regularized in a
+    // previous run of Compute(), start at a small value of δ. Otherwise,
+    // attempt a δ half as big as the previous run so δ can trend downwards over
+    // time.
+    if (m_δOld == 0.0) {
+      δ = 1e-4;
+    } else {
+      δ = m_δOld / 2.0;
+    }
+
+    while (true) {
+      // Regularize lhs by adding a multiple of the identity matrix
+      //
+      // lhs = [H + AᵢᵀΣAᵢ + δI   Aₑᵀ]
+      //       [       Aₑ        −γI ]
+      Eigen::SparseMatrix<double> lhsReg = lhs + Regularization(δ, γ);
+      AnalyzePattern(lhsReg);
+      m_solver.factorize(lhsReg);
+      inertia = Inertia{m_solver};
+
+      // If the inertia is ideal, store that value of δ and return.
+      // Otherwise, increase δ by an order of magnitude and try again.
+      if (inertia == idealInertia) {
+        m_δOld = δ;
+        m_info = Eigen::Success;
+        return;
+      } else {
+        δ *= 10.0;
+
+        // If the Hessian perturbation is too high, report failure. This can
+        // happen due to a rank-deficient equality constraint Jacobian with
+        // linearly dependent constraints.
+        if (δ > 1e20) {
+          m_info = Eigen::NumericalIssue;
+          return;
+        }
+      }
+    }
+  }
+
+  /**
+   * Solve the system of equations using a regularized LDLT factorization.
+   *
+   * @param rhs Right-hand side of the system.
+   */
+  template <typename Rhs>
+  auto Solve(const Eigen::MatrixBase<Rhs>& rhs) {
+    return m_solver.solve(rhs);
+  }
+
+ private:
+  Solver m_solver;
+
+  Eigen::ComputationInfo m_info = Eigen::Success;
+
+  /// The number of decision variables in the system.
+  size_t m_numDecisionVariables = 0;
+
+  /// The number of equality constraints in the system.
+  size_t m_numEqualityConstraints = 0;
+
+  /// The value of δ from the previous run of Compute().
+  double m_δOld = 0.0;
+
+  // Number of non-zeros in LHS.
+  int m_nonZeros = -1;
+
+  /**
+   * Reanalize LHS matrix's sparsity pattern if it changed.
+   *
+   * @param lhs Matrix to analyze.
+   */
+  void AnalyzePattern(const Eigen::SparseMatrix<double>& lhs) {
+    int nonZeros = lhs.nonZeros();
+    if (m_nonZeros != nonZeros) {
+      m_solver.analyzePattern(lhs);
+      m_nonZeros = nonZeros;
+    }
+  }
+
+  /**
+   * Returns regularization matrix.
+   *
+   * @param δ The Hessian regularization factor.
+   * @param γ The equality constraint Jacobian regularization factor.
+   */
+  Eigen::SparseMatrix<double> Regularization(double δ, double γ) {
+    Eigen::VectorXd vec{m_numDecisionVariables + m_numEqualityConstraints};
+    size_t row = 0;
+    while (row < m_numDecisionVariables) {
+      vec(row) = δ;
+      ++row;
+    }
+    while (row < m_numDecisionVariables + m_numEqualityConstraints) {
+      vec(row) = -γ;
+      ++row;
+    }
+
+    return Eigen::SparseMatrix<double>{vec.asDiagonal()};
+  }
+};
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp

@@ -0,0 +1,876 @@
+// Copyright (c) Sleipnir contributors
+
+#include "sleipnir/optimization/solver/InteriorPoint.hpp"
+
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <fstream>
+#include <limits>
+#include <vector>
+
+#include <Eigen/SparseCholesky>
+
+#include "optimization/RegularizedLDLT.hpp"
+#include "optimization/solver/util/ErrorEstimate.hpp"
+#include "optimization/solver/util/FeasibilityRestoration.hpp"
+#include "optimization/solver/util/Filter.hpp"
+#include "optimization/solver/util/FractionToTheBoundaryRule.hpp"
+#include "optimization/solver/util/IsLocallyInfeasible.hpp"
+#include "optimization/solver/util/KKTError.hpp"
+#include "sleipnir/autodiff/Gradient.hpp"
+#include "sleipnir/autodiff/Hessian.hpp"
+#include "sleipnir/autodiff/Jacobian.hpp"
+#include "sleipnir/optimization/SolverExitCondition.hpp"
+#include "sleipnir/util/Print.hpp"
+#include "sleipnir/util/Spy.hpp"
+#include "util/ScopeExit.hpp"
+#include "util/ToMilliseconds.hpp"
+
+// See docs/algorithms.md#Works_cited for citation definitions.
+//
+// See docs/algorithms.md#Interior-point_method for a derivation of the
+// interior-point method formulation being used.
+
+namespace sleipnir {
+
+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) {
+  const auto solveStartTime = std::chrono::system_clock::now();
+
+  // Map decision variables and constraints to VariableMatrices for Lagrangian
+  VariableMatrix xAD{decisionVariables};
+  xAD.SetValue(x);
+  VariableMatrix c_eAD{equalityConstraints};
+  VariableMatrix c_iAD{inequalityConstraints};
+
+  // Create autodiff variables for s, y, and z for Lagrangian
+  VariableMatrix sAD(inequalityConstraints.size());
+  sAD.SetValue(s);
+  VariableMatrix yAD(equalityConstraints.size());
+  for (auto& y : yAD) {
+    y.SetValue(0.0);
+  }
+  VariableMatrix zAD(inequalityConstraints.size());
+  for (auto& z : zAD) {
+    z.SetValue(1.0);
+  }
+
+  // Lagrangian L
+  //
+  // L(xₖ, sₖ, yₖ, zₖ) = f(xₖ) − yₖᵀcₑ(xₖ) − zₖᵀ(cᵢ(xₖ) − sₖ)
+  auto L = f - (yAD.T() * c_eAD)(0) - (zAD.T() * (c_iAD - sAD))(0);
+
+  // Equality constraint Jacobian Aₑ
+  //
+  //         [∇ᵀcₑ₁(xₖ)]
+  // Aₑ(x) = [∇ᵀcₑ₂(xₖ)]
+  //         [    ⋮    ]
+  //         [∇ᵀcₑₘ(xₖ)]
+  Jacobian jacobianCe{c_eAD, xAD};
+  Eigen::SparseMatrix<double> A_e = jacobianCe.Value();
+
+  // Inequality constraint Jacobian Aᵢ
+  //
+  //         [∇ᵀcᵢ₁(xₖ)]
+  // Aᵢ(x) = [∇ᵀcᵢ₂(xₖ)]
+  //         [    ⋮    ]
+  //         [∇ᵀcᵢₘ(xₖ)]
+  Jacobian jacobianCi{c_iAD, xAD};
+  Eigen::SparseMatrix<double> A_i = jacobianCi.Value();
+
+  // Gradient of f ∇f
+  Gradient gradientF{f, xAD};
+  Eigen::SparseVector<double> g = gradientF.Value();
+
+  // Hessian of the Lagrangian H
+  //
+  // Hₖ = ∇²ₓₓL(xₖ, sₖ, yₖ, zₖ)
+  Hessian hessianL{L, xAD};
+  Eigen::SparseMatrix<double> H = hessianL.Value();
+
+  Eigen::VectorXd y = yAD.Value();
+  Eigen::VectorXd z = zAD.Value();
+  Eigen::VectorXd c_e = c_eAD.Value();
+  Eigen::VectorXd c_i = c_iAD.Value();
+
+  // Check for overconstrained problem
+  if (equalityConstraints.size() > decisionVariables.size()) {
+    if (config.diagnostics) {
+      sleipnir::println("The problem has too few degrees of freedom.");
+      sleipnir::println(
+          "Violated constraints (cₑ(x) = 0) in order of declaration:");
+      for (int row = 0; row < c_e.rows(); ++row) {
+        if (c_e(row) < 0.0) {
+          sleipnir::println("  {}/{}: {} = 0", row + 1, c_e.rows(), c_e(row));
+        }
+      }
+    }
+
+    status->exitCondition = SolverExitCondition::kTooFewDOFs;
+    return;
+  }
+
+  // Check whether initial guess has finite f(xₖ), cₑ(xₖ), and cᵢ(xₖ)
+  if (!std::isfinite(f.Value()) || !c_e.allFinite() || !c_i.allFinite()) {
+    status->exitCondition =
+        SolverExitCondition::kNonfiniteInitialCostOrConstraints;
+    return;
+  }
+
+  // Sparsity pattern files written when spy flag is set in SolverConfig
+  std::ofstream H_spy;
+  std::ofstream A_e_spy;
+  std::ofstream A_i_spy;
+  if (config.spy) {
+    A_e_spy.open("A_e.spy");
+    A_i_spy.open("A_i.spy");
+    H_spy.open("H.spy");
+  }
+
+  if (config.diagnostics && !feasibilityRestoration) {
+    sleipnir::println("Error tolerance: {}\n", config.tolerance);
+  }
+
+  std::chrono::system_clock::time_point iterationsStartTime;
+
+  int iterations = 0;
+
+  // Prints final diagnostics when the solver exits
+  scope_exit exit{[&] {
+    status->cost = f.Value();
+
+    if (config.diagnostics && !feasibilityRestoration) {
+      auto solveEndTime = std::chrono::system_clock::now();
+
+      sleipnir::println("\nSolve time: {:.3f} ms",
+                        ToMilliseconds(solveEndTime - solveStartTime));
+      sleipnir::println("  ↳ {:.3f} ms (solver setup)",
+                        ToMilliseconds(iterationsStartTime - solveStartTime));
+      if (iterations > 0) {
+        sleipnir::println(
+            "  ↳ {:.3f} ms ({} solver iterations; {:.3f} ms average)",
+            ToMilliseconds(solveEndTime - iterationsStartTime), iterations,
+            ToMilliseconds((solveEndTime - iterationsStartTime) / iterations));
+      }
+      sleipnir::println("");
+
+      sleipnir::println("{:^8}   {:^10}   {:^14}   {:^6}", "autodiff",
+                        "setup (ms)", "avg solve (ms)", "solves");
+      sleipnir::println("{:=^47}", "");
+      constexpr auto format = "{:^8}   {:10.3f}   {:14.3f}   {:6}";
+      sleipnir::println(format, "∇f(x)",
+                        gradientF.GetProfiler().SetupDuration(),
+                        gradientF.GetProfiler().AverageSolveDuration(),
+                        gradientF.GetProfiler().SolveMeasurements());
+      sleipnir::println(format, "∇²ₓₓL", hessianL.GetProfiler().SetupDuration(),
+                        hessianL.GetProfiler().AverageSolveDuration(),
+                        hessianL.GetProfiler().SolveMeasurements());
+      sleipnir::println(format, "∂cₑ/∂x",
+                        jacobianCe.GetProfiler().SetupDuration(),
+                        jacobianCe.GetProfiler().AverageSolveDuration(),
+                        jacobianCe.GetProfiler().SolveMeasurements());
+      sleipnir::println(format, "∂cᵢ/∂x",
+                        jacobianCi.GetProfiler().SetupDuration(),
+                        jacobianCi.GetProfiler().AverageSolveDuration(),
+                        jacobianCi.GetProfiler().SolveMeasurements());
+      sleipnir::println("");
+    }
+  }};
+
+  // Barrier parameter minimum
+  const double μ_min = config.tolerance / 10.0;
+
+  // Barrier parameter μ
+  double μ = 0.1;
+
+  // Fraction-to-the-boundary rule scale factor minimum
+  constexpr double τ_min = 0.99;
+
+  // Fraction-to-the-boundary rule scale factor τ
+  double τ = τ_min;
+
+  Filter filter{f, μ};
+
+  // This should be run when the error estimate is below a desired threshold for
+  // the current barrier parameter
+  auto UpdateBarrierParameterAndResetFilter = [&] {
+    // Barrier parameter linear decrease power in "κ_μ μ". Range of (0, 1).
+    constexpr double κ_μ = 0.2;
+
+    // Barrier parameter superlinear decrease power in "μ^(θ_μ)". Range of (1,
+    // 2).
+    constexpr double θ_μ = 1.5;
+
+    // Update the barrier parameter.
+    //
+    //   μⱼ₊₁ = max(εₜₒₗ/10, min(κ_μ μⱼ, μⱼ^θ_μ))
+    //
+    // See equation (7) of [2].
+    μ = std::max(μ_min, std::min(κ_μ * μ, std::pow(μ, θ_μ)));
+
+    // Update the fraction-to-the-boundary rule scaling factor.
+    //
+    //   τⱼ = max(τₘᵢₙ, 1 − μⱼ)
+    //
+    // See equation (8) of [2].
+    τ = std::max(τ_min, 1.0 - μ);
+
+    // Reset the filter when the barrier parameter is updated
+    filter.Reset(μ);
+  };
+
+  // Kept outside the loop so its storage can be reused
+  std::vector<Eigen::Triplet<double>> triplets;
+
+  RegularizedLDLT solver;
+
+  // Variables for determining when a step is acceptable
+  constexpr double α_red_factor = 0.5;
+  int acceptableIterCounter = 0;
+
+  int fullStepRejectedCounter = 0;
+  int stepTooSmallCounter = 0;
+
+  // Error estimate
+  double E_0 = std::numeric_limits<double>::infinity();
+
+  iterationsStartTime = std::chrono::system_clock::now();
+
+  while (E_0 > config.tolerance &&
+         acceptableIterCounter < config.maxAcceptableIterations) {
+    auto innerIterStartTime = std::chrono::system_clock::now();
+
+    // Check for local equality constraint infeasibility
+    if (IsEqualityLocallyInfeasible(A_e, c_e)) {
+      if (config.diagnostics) {
+        sleipnir::println(
+            "The problem is locally infeasible due to violated equality "
+            "constraints.");
+        sleipnir::println(
+            "Violated constraints (cₑ(x) = 0) in order of declaration:");
+        for (int row = 0; row < c_e.rows(); ++row) {
+          if (c_e(row) < 0.0) {
+            sleipnir::println("  {}/{}: {} = 0", row + 1, c_e.rows(), c_e(row));
+          }
+        }
+      }
+
+      status->exitCondition = SolverExitCondition::kLocallyInfeasible;
+      return;
+    }
+
+    // Check for local inequality constraint infeasibility
+    if (IsInequalityLocallyInfeasible(A_i, c_i)) {
+      if (config.diagnostics) {
+        sleipnir::println(
+            "The problem is infeasible due to violated inequality "
+            "constraints.");
+        sleipnir::println(
+            "Violated constraints (cᵢ(x) ≥ 0) in order of declaration:");
+        for (int row = 0; row < c_i.rows(); ++row) {
+          if (c_i(row) < 0.0) {
+            sleipnir::println("  {}/{}: {} ≥ 0", row + 1, c_i.rows(), c_i(row));
+          }
+        }
+      }
+
+      status->exitCondition = SolverExitCondition::kLocallyInfeasible;
+      return;
+    }
+
+    // Check for diverging iterates
+    if (x.lpNorm<Eigen::Infinity>() > 1e20 || !x.allFinite() ||
+        s.lpNorm<Eigen::Infinity>() > 1e20 || !s.allFinite()) {
+      status->exitCondition = SolverExitCondition::kDivergingIterates;
+      return;
+    }
+
+    // Write out spy file contents if that's enabled
+    if (config.spy) {
+      // Gap between sparsity patterns
+      if (iterations > 0) {
+        A_e_spy << "\n";
+        A_i_spy << "\n";
+        H_spy << "\n";
+      }
+
+      Spy(H_spy, H);
+      Spy(A_e_spy, A_e);
+      Spy(A_i_spy, A_i);
+    }
+
+    // Call user callback
+    if (callback({iterations, x, s, g, H, A_e, A_i})) {
+      status->exitCondition = SolverExitCondition::kCallbackRequestedStop;
+      return;
+    }
+
+    //     [s₁ 0 ⋯ 0 ]
+    // S = [0  ⋱   ⋮ ]
+    //     [⋮    ⋱ 0 ]
+    //     [0  ⋯ 0 sₘ]
+    const auto S = s.asDiagonal();
+    Eigen::SparseMatrix<double> Sinv;
+    Sinv = s.cwiseInverse().asDiagonal();
+
+    //     [z₁ 0 ⋯ 0 ]
+    // Z = [0  ⋱   ⋮ ]
+    //     [⋮    ⋱ 0 ]
+    //     [0  ⋯ 0 zₘ]
+    const auto Z = z.asDiagonal();
+    Eigen::SparseMatrix<double> Zinv;
+    Zinv = z.cwiseInverse().asDiagonal();
+
+    // Σ = S⁻¹Z
+    const Eigen::SparseMatrix<double> Σ = Sinv * Z;
+
+    // lhs = [H + AᵢᵀΣAᵢ  Aₑᵀ]
+    //       [    Aₑ       0 ]
+    //
+    // Don't assign upper triangle because solver only uses lower triangle.
+    const Eigen::SparseMatrix<double> topLeft =
+        H.triangularView<Eigen::Lower>() +
+        (A_i.transpose() * Σ * A_i).triangularView<Eigen::Lower>();
+    triplets.clear();
+    triplets.reserve(topLeft.nonZeros() + A_e.nonZeros());
+    for (int col = 0; col < H.cols(); ++col) {
+      // Append column of H + AᵢᵀΣAᵢ lower triangle in top-left quadrant
+      for (Eigen::SparseMatrix<double>::InnerIterator it{topLeft, col}; it;
+           ++it) {
+        triplets.emplace_back(it.row(), it.col(), it.value());
+      }
+      // Append column of Aₑ in bottom-left quadrant
+      for (Eigen::SparseMatrix<double>::InnerIterator it{A_e, col}; it; ++it) {
+        triplets.emplace_back(H.rows() + it.row(), it.col(), it.value());
+      }
+    }
+    Eigen::SparseMatrix<double> lhs(
+        decisionVariables.size() + equalityConstraints.size(),
+        decisionVariables.size() + equalityConstraints.size());
+    lhs.setFromSortedTriplets(triplets.begin(), triplets.end(),
+                              [](const auto& a, const auto& b) { return b; });
+
+    const Eigen::VectorXd e = Eigen::VectorXd::Ones(s.rows());
+
+    // rhs = −[∇f − Aₑᵀy + Aᵢᵀ(S⁻¹(Zcᵢ − μe) − z)]
+    //        [                cₑ                ]
+    Eigen::VectorXd rhs{x.rows() + y.rows()};
+    rhs.segment(0, x.rows()) =
+        -(g - A_e.transpose() * y +
+          A_i.transpose() * (Sinv * (Z * c_i - μ * e) - z));
+    rhs.segment(x.rows(), y.rows()) = -c_e;
+
+    // Solve the Newton-KKT system
+    solver.Compute(lhs, equalityConstraints.size(), μ);
+    Eigen::VectorXd step{x.rows() + y.rows()};
+    if (solver.Info() == Eigen::Success) {
+      step = solver.Solve(rhs);
+    } else {
+      // The regularization procedure failed due to a rank-deficient equality
+      // constraint Jacobian with linearly dependent constraints. Set the step
+      // length to zero and let second-order corrections attempt to restore
+      // feasibility.
+      step.setZero();
+    }
+
+    // step = [ pₖˣ]
+    //        [−pₖʸ]
+    Eigen::VectorXd p_x = step.segment(0, x.rows());
+    Eigen::VectorXd p_y = -step.segment(x.rows(), y.rows());
+
+    // pₖᶻ = −Σcᵢ + μS⁻¹e − ΣAᵢpₖˣ
+    Eigen::VectorXd p_z = -Σ * c_i + μ * Sinv * e - Σ * A_i * p_x;
+
+    // pₖˢ = μZ⁻¹e − s − Z⁻¹Spₖᶻ
+    Eigen::VectorXd p_s = μ * Zinv * e - s - Zinv * S * p_z;
+
+    // αᵐᵃˣ = max(α ∈ (0, 1] : sₖ + αpₖˢ ≥ (1−τⱼ)sₖ)
+    const double α_max = FractionToTheBoundaryRule(s, p_s, τ);
+    double α = α_max;
+
+    // αₖᶻ = max(α ∈ (0, 1] : zₖ + αpₖᶻ ≥ (1−τⱼ)zₖ)
+    double α_z = FractionToTheBoundaryRule(z, p_z, τ);
+
+    // Loop until a step is accepted. If a step becomes acceptable, the loop
+    // will exit early.
+    while (1) {
+      Eigen::VectorXd trial_x = x + α * p_x;
+      Eigen::VectorXd trial_y = y + α_z * p_y;
+      Eigen::VectorXd trial_z = z + α_z * p_z;
+
+      xAD.SetValue(trial_x);
+
+      f.Update();
+
+      for (int row = 0; row < c_e.rows(); ++row) {
+        c_eAD(row).Update();
+      }
+      Eigen::VectorXd trial_c_e = c_eAD.Value();
+
+      for (int row = 0; row < c_i.rows(); ++row) {
+        c_iAD(row).Update();
+      }
+      Eigen::VectorXd trial_c_i = c_iAD.Value();
+
+      // If f(xₖ + αpₖˣ), cₑ(xₖ + αpₖˣ), or cᵢ(xₖ + αpₖˣ) aren't finite, reduce
+      // step size immediately
+      if (!std::isfinite(f.Value()) || !trial_c_e.allFinite() ||
+          !trial_c_i.allFinite()) {
+        // Reduce step size
+        α *= α_red_factor;
+        continue;
+      }
+
+      Eigen::VectorXd trial_s;
+      if (config.feasibleIPM && c_i.cwiseGreater(0.0).all()) {
+        // If the inequality constraints are all feasible, prevent them from
+        // becoming infeasible again.
+        //
+        // See equation (19.30) in [1].
+        trial_s = trial_c_i;
+      } else {
+        trial_s = s + α * p_s;
+      }
+
+      // Check whether filter accepts trial iterate
+      auto entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i);
+      if (filter.TryAdd(entry)) {
+        // Accept step
+        break;
+      }
+
+      double prevConstraintViolation = c_e.lpNorm<1>() + (c_i - s).lpNorm<1>();
+      double nextConstraintViolation =
+          trial_c_e.lpNorm<1>() + (trial_c_i - trial_s).lpNorm<1>();
+
+      // Second-order corrections
+      //
+      // If first trial point was rejected and constraint violation stayed the
+      // same or went up, apply second-order corrections
+      if (nextConstraintViolation >= prevConstraintViolation) {
+        // Apply second-order corrections. See section 2.4 of [2].
+        Eigen::VectorXd p_x_cor = p_x;
+        Eigen::VectorXd p_y_soc = p_y;
+        Eigen::VectorXd p_z_soc = p_z;
+        Eigen::VectorXd p_s_soc = p_s;
+
+        double α_soc = α;
+        Eigen::VectorXd c_e_soc = c_e;
+
+        bool stepAcceptable = false;
+        for (int soc_iteration = 0; soc_iteration < 5 && !stepAcceptable;
+             ++soc_iteration) {
+          // Rebuild Newton-KKT rhs with updated constraint values.
+          //
+          // rhs = −[∇f − Aₑᵀy + Aᵢᵀ(S⁻¹(Zcᵢ − μe) − z)]
+          //        [              cₑˢᵒᶜ               ]
+          //
+          // where cₑˢᵒᶜ = αc(xₖ) + c(xₖ + αpₖˣ)
+          c_e_soc = α_soc * c_e_soc + trial_c_e;
+          rhs.bottomRows(y.rows()) = -c_e_soc;
+
+          // Solve the Newton-KKT system
+          step = solver.Solve(rhs);
+
+          p_x_cor = step.segment(0, x.rows());
+          p_y_soc = -step.segment(x.rows(), y.rows());
+
+          // pₖᶻ = −Σcᵢ + μS⁻¹e − ΣAᵢpₖˣ
+          p_z_soc = -Σ * c_i + μ * Sinv * e - Σ * A_i * p_x_cor;
+
+          // pₖˢ = μZ⁻¹e − s − Z⁻¹Spₖᶻ
+          p_s_soc = μ * Zinv * e - s - Zinv * S * p_z_soc;
+
+          // αˢᵒᶜ = max(α ∈ (0, 1] : sₖ + αpₖˢ ≥ (1−τⱼ)sₖ)
+          α_soc = FractionToTheBoundaryRule(s, p_s_soc, τ);
+          trial_x = x + α_soc * p_x_cor;
+          trial_s = s + α_soc * p_s_soc;
+
+          // αₖᶻ = max(α ∈ (0, 1] : zₖ + αpₖᶻ ≥ (1−τⱼ)zₖ)
+          double α_z_soc = FractionToTheBoundaryRule(z, p_z_soc, τ);
+          trial_y = y + α_z_soc * p_y_soc;
+          trial_z = z + α_z_soc * p_z_soc;
+
+          xAD.SetValue(trial_x);
+
+          f.Update();
+
+          for (int row = 0; row < c_e.rows(); ++row) {
+            c_eAD(row).Update();
+          }
+          trial_c_e = c_eAD.Value();
+
+          for (int row = 0; row < c_i.rows(); ++row) {
+            c_iAD(row).Update();
+          }
+          trial_c_i = c_iAD.Value();
+
+          // Check whether filter accepts trial iterate
+          entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i);
+          if (filter.TryAdd(entry)) {
+            p_x = p_x_cor;
+            p_y = p_y_soc;
+            p_z = p_z_soc;
+            p_s = p_s_soc;
+            α = α_soc;
+            α_z = α_z_soc;
+            stepAcceptable = true;
+          }
+        }
+
+        if (stepAcceptable) {
+          // Accept step
+          break;
+        }
+      }
+
+      // If we got here and α is the full step, the full step was rejected.
+      // Increment the full-step rejected counter to keep track of how many full
+      // steps have been rejected in a row.
+      if (α == α_max) {
+        ++fullStepRejectedCounter;
+      }
+
+      // If the full step was rejected enough times in a row, reset the filter
+      // because it may be impeding progress.
+      //
+      // See section 3.2 case I of [2].
+      if (fullStepRejectedCounter >= 4 &&
+          filter.maxConstraintViolation > entry.constraintViolation / 10.0) {
+        filter.maxConstraintViolation *= 0.1;
+        filter.Reset(μ);
+        continue;
+      }
+
+      // Reduce step size
+      α *= α_red_factor;
+
+      // Safety factor for the minimal step size
+      constexpr double α_min_frac = 0.05;
+
+      // If step size hit a minimum, check if the KKT error was reduced. If it
+      // wasn't, invoke feasibility restoration.
+      if (α < α_min_frac * Filter::γConstraint) {
+        double currentKKTError = KKTError(g, A_e, c_e, A_i, c_i, s, y, z, μ);
+
+        Eigen::VectorXd trial_x = x + α_max * p_x;
+        Eigen::VectorXd trial_s = s + α_max * p_s;
+
+        Eigen::VectorXd trial_y = y + α_z * p_y;
+        Eigen::VectorXd trial_z = z + α_z * p_z;
+
+        // Upate autodiff
+        xAD.SetValue(trial_x);
+        sAD.SetValue(trial_s);
+        yAD.SetValue(trial_y);
+        zAD.SetValue(trial_z);
+
+        for (int row = 0; row < c_e.rows(); ++row) {
+          c_eAD(row).Update();
+        }
+        Eigen::VectorXd trial_c_e = c_eAD.Value();
+
+        for (int row = 0; row < c_i.rows(); ++row) {
+          c_iAD(row).Update();
+        }
+        Eigen::VectorXd trial_c_i = c_iAD.Value();
+
+        double nextKKTError = KKTError(gradientF.Value(), jacobianCe.Value(),
+                                       trial_c_e, jacobianCi.Value(), trial_c_i,
+                                       trial_s, trial_y, trial_z, μ);
+
+        // If the step using αᵐᵃˣ reduced the KKT error, accept it anyway
+        if (nextKKTError <= 0.999 * currentKKTError) {
+          α = α_max;
+
+          // Accept step
+          break;
+        }
+
+        // If the step direction was bad and feasibility restoration is
+        // already running, running it again won't help
+        if (feasibilityRestoration) {
+          status->exitCondition = SolverExitCondition::kLocallyInfeasible;
+          return;
+        }
+
+        auto initialEntry = filter.MakeEntry(s, c_e, c_i);
+
+        // Feasibility restoration phase
+        Eigen::VectorXd fr_x = x;
+        Eigen::VectorXd fr_s = s;
+        SolverStatus fr_status;
+        FeasibilityRestoration(
+            decisionVariables, equalityConstraints, inequalityConstraints, f, μ,
+            [&](const SolverIterationInfo& info) {
+              Eigen::VectorXd trial_x =
+                  info.x.segment(0, decisionVariables.size());
+              xAD.SetValue(trial_x);
+
+              Eigen::VectorXd trial_s =
+                  info.s.segment(0, inequalityConstraints.size());
+              sAD.SetValue(trial_s);
+
+              for (int row = 0; row < c_e.rows(); ++row) {
+                c_eAD(row).Update();
+              }
+              Eigen::VectorXd trial_c_e = c_eAD.Value();
+
+              for (int row = 0; row < c_i.rows(); ++row) {
+                c_iAD(row).Update();
+              }
+              Eigen::VectorXd trial_c_i = c_iAD.Value();
+
+              for (int row = 0; row < c_i.rows(); ++row) {
+                c_iAD(row).Update();
+              }
+
+              f.Update();
+
+              // If current iterate is acceptable to normal filter and
+              // constraint violation has sufficiently reduced, stop
+              // feasibility restoration
+              auto entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i);
+              if (filter.IsAcceptable(entry) &&
+                  entry.constraintViolation <
+                      0.9 * initialEntry.constraintViolation) {
+                return true;
+              }
+
+              return false;
+            },
+            config, fr_x, fr_s, &fr_status);
+
+        if (fr_status.exitCondition ==
+            SolverExitCondition::kCallbackRequestedStop) {
+          p_x = fr_x - x;
+          p_s = fr_s - s;
+
+          // Lagrange mutliplier estimates
+          //
+          //   [y] = (ÂÂᵀ)⁻¹Â[ ∇f]
+          //   [z]           [−μe]
+          //
+          //   where  = [Aₑ   0]
+          //             [Aᵢ  −S]
+          //
+          // See equation (19.37) of [1].
+          {
+            xAD.SetValue(fr_x);
+            sAD.SetValue(c_iAD.Value());
+
+            A_e = jacobianCe.Value();
+            A_i = jacobianCi.Value();
+            g = gradientF.Value();
+
+            // Â = [Aₑ   0]
+            //     [Aᵢ  −S]
+            triplets.clear();
+            triplets.reserve(A_e.nonZeros() + A_i.nonZeros() + s.rows());
+            for (int col = 0; col < A_e.cols(); ++col) {
+              // Append column of Aₑ in top-left quadrant
+              for (Eigen::SparseMatrix<double>::InnerIterator it{A_e, col}; it;
+                   ++it) {
+                triplets.emplace_back(it.row(), it.col(), it.value());
+              }
+              // Append column of Aᵢ in bottom-left quadrant
+              for (Eigen::SparseMatrix<double>::InnerIterator it{A_i, col}; it;
+                   ++it) {
+                triplets.emplace_back(A_e.rows() + it.row(), it.col(),
+                                      it.value());
+              }
+            }
+            // Append −S in bottom-right quadrant
+            for (int i = 0; i < s.rows(); ++i) {
+              triplets.emplace_back(A_e.rows() + i, A_e.cols() + i, -s(i));
+            }
+            Eigen::SparseMatrix<double> Ahat{A_e.rows() + A_i.rows(),
+                                             A_e.cols() + s.rows()};
+            Ahat.setFromSortedTriplets(
+                triplets.begin(), triplets.end(),
+                [](const auto& a, const auto& b) { return b; });
+
+            // lhs = ÂÂᵀ
+            Eigen::SparseMatrix<double> lhs = Ahat * Ahat.transpose();
+
+            // rhs = Â[ ∇f]
+            //        [−μe]
+            Eigen::VectorXd rhsTemp{g.rows() + e.rows()};
+            rhsTemp.block(0, 0, g.rows(), 1) = g;
+            rhsTemp.block(g.rows(), 0, s.rows(), 1) = -μ * e;
+            Eigen::VectorXd rhs = Ahat * rhsTemp;
+
+            Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> yzEstimator{lhs};
+            Eigen::VectorXd sol = yzEstimator.solve(rhs);
+
+            p_y = y - sol.block(0, 0, y.rows(), 1);
+            p_z = z - sol.block(y.rows(), 0, z.rows(), 1);
+          }
+
+          α = 1.0;
+          α_z = 1.0;
+
+          // Accept step
+          break;
+        } else if (fr_status.exitCondition == SolverExitCondition::kSuccess) {
+          status->exitCondition = SolverExitCondition::kLocallyInfeasible;
+          x = fr_x;
+          return;
+        } else {
+          status->exitCondition =
+              SolverExitCondition::kFeasibilityRestorationFailed;
+          x = fr_x;
+          return;
+        }
+      }
+    }
+
+    // If full step was accepted, reset full-step rejected counter
+    if (α == α_max) {
+      fullStepRejectedCounter = 0;
+    }
+
+    // Handle very small search directions by letting αₖ = αₖᵐᵃˣ when
+    // max(|pₖˣ(i)|/(1 + |xₖ(i)|)) < 10ε_mach.
+    //
+    // See section 3.9 of [2].
+    double maxStepScaled = 0.0;
+    for (int row = 0; row < x.rows(); ++row) {
+      maxStepScaled = std::max(maxStepScaled,
+                               std::abs(p_x(row)) / (1.0 + std::abs(x(row))));
+    }
+    if (maxStepScaled < 10.0 * std::numeric_limits<double>::epsilon()) {
+      α = α_max;
+      ++stepTooSmallCounter;
+    } else {
+      stepTooSmallCounter = 0;
+    }
+
+    // xₖ₊₁ = xₖ + αₖpₖˣ
+    // sₖ₊₁ = sₖ + αₖpₖˢ
+    // yₖ₊₁ = yₖ + αₖᶻpₖʸ
+    // zₖ₊₁ = zₖ + αₖᶻpₖᶻ
+    x += α * p_x;
+    s += α * p_s;
+    y += α_z * p_y;
+    z += α_z * p_z;
+
+    // A requirement for the convergence proof is that the "primal-dual barrier
+    // term Hessian" Σₖ does not deviate arbitrarily much from the "primal
+    // Hessian" μⱼSₖ⁻². We ensure this by resetting
+    //
+    //   zₖ₊₁⁽ⁱ⁾ = max(min(zₖ₊₁⁽ⁱ⁾, κ_Σ μⱼ/sₖ₊₁⁽ⁱ⁾), μⱼ/(κ_Σ sₖ₊₁⁽ⁱ⁾))
+    //
+    // for some fixed κ_Σ ≥ 1 after each step. See equation (16) of [2].
+    {
+      // Barrier parameter scale factor for inequality constraint Lagrange
+      // multiplier safeguard
+      constexpr double κ_Σ = 1e10;
+
+      for (int row = 0; row < z.rows(); ++row) {
+        z(row) =
+            std::max(std::min(z(row), κ_Σ * μ / s(row)), μ / (κ_Σ * s(row)));
+      }
+    }
+
+    // Update autodiff for Jacobians and Hessian
+    xAD.SetValue(x);
+    sAD.SetValue(s);
+    yAD.SetValue(y);
+    zAD.SetValue(z);
+    A_e = jacobianCe.Value();
+    A_i = jacobianCi.Value();
+    g = gradientF.Value();
+    H = hessianL.Value();
+
+    // Update cₑ
+    for (int row = 0; row < c_e.rows(); ++row) {
+      c_eAD(row).Update();
+    }
+    c_e = c_eAD.Value();
+
+    // Update cᵢ
+    for (int row = 0; row < c_i.rows(); ++row) {
+      c_iAD(row).Update();
+    }
+    c_i = c_iAD.Value();
+
+    // Update the error estimate
+    E_0 = ErrorEstimate(g, A_e, c_e, A_i, c_i, s, y, z, 0.0);
+    if (E_0 < config.acceptableTolerance) {
+      ++acceptableIterCounter;
+    } else {
+      acceptableIterCounter = 0;
+    }
+
+    // Update the barrier parameter if necessary
+    if (E_0 > config.tolerance) {
+      // Barrier parameter scale factor for tolerance checks
+      constexpr double κ_ε = 10.0;
+
+      // While the error estimate is below the desired threshold for this
+      // barrier parameter value, decrease the barrier parameter further
+      double E_μ = ErrorEstimate(g, A_e, c_e, A_i, c_i, s, y, z, μ);
+      while (μ > μ_min && E_μ <= κ_ε * μ) {
+        UpdateBarrierParameterAndResetFilter();
+        E_μ = ErrorEstimate(g, A_e, c_e, A_i, c_i, s, y, z, μ);
+      }
+    }
+
+    const auto innerIterEndTime = std::chrono::system_clock::now();
+
+    // Diagnostics for current iteration
+    if (config.diagnostics) {
+      if (iterations % 20 == 0) {
+        sleipnir::println("{:^4}   {:^9}  {:^13}  {:^13}  {:^13}", "iter",
+                          "time (ms)", "error", "cost", "infeasibility");
+        sleipnir::println("{:=^61}", "");
+      }
+
+      sleipnir::println("{:4}{}  {:9.3f}  {:13e}  {:13e}  {:13e}", iterations,
+                        feasibilityRestoration ? "r" : " ",
+                        ToMilliseconds(innerIterEndTime - innerIterStartTime),
+                        E_0, f.Value(),
+                        c_e.lpNorm<1>() + (c_i - s).lpNorm<1>());
+    }
+
+    ++iterations;
+
+    // Check for max iterations
+    if (iterations >= config.maxIterations) {
+      status->exitCondition = SolverExitCondition::kMaxIterationsExceeded;
+      return;
+    }
+
+    // Check for max wall clock time
+    if (innerIterEndTime - solveStartTime > config.timeout) {
+      status->exitCondition = SolverExitCondition::kTimeout;
+      return;
+    }
+
+    // Check for solve to acceptable tolerance
+    if (E_0 > config.tolerance &&
+        acceptableIterCounter == config.maxAcceptableIterations) {
+      status->exitCondition = SolverExitCondition::kSolvedToAcceptableTolerance;
+      return;
+    }
+
+    // The search direction has been very small twice, so assume the problem has
+    // been solved as well as possible given finite precision and reduce the
+    // barrier parameter.
+    //
+    // See section 3.9 of [2].
+    if (stepTooSmallCounter >= 2 && μ > μ_min) {
+      UpdateBarrierParameterAndResetFilter();
+      continue;
+    }
+  }
+}  // NOLINT(readability/fn_size)
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/ErrorEstimate.hpp

@@ -0,0 +1,80 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <algorithm>
+
+#include <Eigen/Core>
+#include <Eigen/SparseCore>
+
+// See docs/algorithms.md#Works_cited for citation definitions
+
+namespace sleipnir {
+
+/**
+ * Returns the error estimate using the KKT conditions for the interior-point
+ * method.
+ *
+ * @param g Gradient of the cost function ∇f.
+ * @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
+ *   current iterate.
+ * @param c_e The problem's equality constraints cₑ(x) evaluated at the current
+ *   iterate.
+ * @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at
+ *   the current iterate.
+ * @param c_i The problem's inequality constraints cᵢ(x) evaluated at the
+ *   current iterate.
+ * @param s Inequality constraint slack variables.
+ * @param y Equality constraint dual variables.
+ * @param z Inequality constraint dual variables.
+ * @param μ Barrier parameter.
+ */
+inline double ErrorEstimate(const Eigen::VectorXd& g,
+                            const Eigen::SparseMatrix<double>& A_e,
+                            const Eigen::VectorXd& c_e,
+                            const Eigen::SparseMatrix<double>& A_i,
+                            const Eigen::VectorXd& c_i,
+                            const Eigen::VectorXd& s, const Eigen::VectorXd& y,
+                            const Eigen::VectorXd& z, double μ) {
+  int numEqualityConstraints = A_e.rows();
+  int numInequalityConstraints = A_i.rows();
+
+  // Update the error estimate using the KKT conditions from equations (19.5a)
+  // through (19.5d) of [1].
+  //
+  //   ∇f − Aₑᵀy − Aᵢᵀz = 0
+  //   Sz − μe = 0
+  //   cₑ = 0
+  //   cᵢ − s = 0
+  //
+  // The error tolerance is the max of the following infinity norms scaled by
+  // s_d and s_c (see equation (5) of [2]).
+  //
+  //   ‖∇f − Aₑᵀy − Aᵢᵀz‖_∞ / s_d
+  //   ‖Sz − μe‖_∞ / s_c
+  //   ‖cₑ‖_∞
+  //   ‖cᵢ − s‖_∞
+
+  // s_d = max(sₘₐₓ, (‖y‖₁ + ‖z‖₁) / (m + n)) / sₘₐₓ
+  constexpr double s_max = 100.0;
+  double s_d =
+      std::max(s_max, (y.lpNorm<1>() + z.lpNorm<1>()) /
+                          (numEqualityConstraints + numInequalityConstraints)) /
+      s_max;
+
+  // s_c = max(sₘₐₓ, ‖z‖₁ / n) / sₘₐₓ
+  double s_c =
+      std::max(s_max, z.lpNorm<1>() / numInequalityConstraints) / s_max;
+
+  const auto S = s.asDiagonal();
+  const Eigen::VectorXd e = Eigen::VectorXd::Ones(s.rows());
+
+  return std::max({(g - A_e.transpose() * y - A_i.transpose() * z)
+                           .lpNorm<Eigen::Infinity>() /
+                       s_d,
+                   (S * z - μ * e).lpNorm<Eigen::Infinity>() / s_c,
+                   c_e.lpNorm<Eigen::Infinity>(),
+                   (c_i - s).lpNorm<Eigen::Infinity>()});
+}
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FeasibilityRestoration.hpp

@@ -0,0 +1,239 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <algorithm>
+#include <cmath>
+#include <iterator>
+#include <span>
+#include <vector>
+
+#include <Eigen/Core>
+
+#include "sleipnir/autodiff/Variable.hpp"
+#include "sleipnir/autodiff/VariableMatrix.hpp"
+#include "sleipnir/optimization/SolverConfig.hpp"
+#include "sleipnir/optimization/SolverIterationInfo.hpp"
+#include "sleipnir/optimization/SolverStatus.hpp"
+#include "sleipnir/optimization/solver/InteriorPoint.hpp"
+#include "sleipnir/util/FunctionRef.hpp"
+
+namespace sleipnir {
+
+/**
+ * Finds the iterate that minimizes the constraint violation while not deviating
+ * too far from the starting point. This is a fallback procedure when the normal
+ * interior-point method fails to converge to a feasible point.
+ *
+ * @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] μ Barrier parameter.
+ * @param[in] callback The user callback.
+ * @param[in] config Configuration options for the solver.
+ * @param[in,out] x The current iterate from the normal solve.
+ * @param[in,out] s The current inequality constraint slack variables from the
+ *   normal solve.
+ * @param[out] status The solver status.
+ */
+inline void FeasibilityRestoration(
+    std::span<Variable> decisionVariables,
+    std::span<Variable> equalityConstraints,
+    std::span<Variable> inequalityConstraints, Variable& f, double μ,
+    function_ref<bool(const SolverIterationInfo&)> callback,
+    const SolverConfig& config, Eigen::VectorXd& x, Eigen::VectorXd& s,
+    SolverStatus* status) {
+  // Feasibility restoration
+  //
+  //        min  ρ Σ (pₑ + nₑ + pᵢ + nᵢ) + ζ/2 (x - x_R)ᵀD_R(x - x_R)
+  //         x
+  //       pₑ,nₑ
+  //       pᵢ,nᵢ
+  //
+  //   s.t. cₑ(x) - pₑ + nₑ = 0
+  //        cᵢ(x) - s - pᵢ + nᵢ = 0
+  //        pₑ ≥ 0
+  //        nₑ ≥ 0
+  //        pᵢ ≥ 0
+  //        nᵢ ≥ 0
+  //
+  // where ρ = 1000, ζ = √μ where μ is the barrier parameter, x_R is original
+  // iterate before feasibility restoration, and D_R is a scaling matrix defined
+  // by
+  //
+  //   D_R = diag(min(1, 1/|x_R⁽¹⁾|), …, min(1, 1/|x_R|⁽ⁿ⁾)
+
+  constexpr double ρ = 1000.0;
+
+  std::vector<Variable> fr_decisionVariables;
+  fr_decisionVariables.reserve(decisionVariables.size() +
+                               2 * equalityConstraints.size() +
+                               2 * inequalityConstraints.size());
+
+  // Assign x
+  fr_decisionVariables.assign(decisionVariables.begin(),
+                              decisionVariables.end());
+
+  // Allocate pₑ, nₑ, pᵢ, and nᵢ
+  for (size_t row = 0;
+       row < 2 * equalityConstraints.size() + 2 * inequalityConstraints.size();
+       ++row) {
+    fr_decisionVariables.emplace_back();
+  }
+
+  auto it = fr_decisionVariables.cbegin();
+
+  VariableMatrix xAD{std::span{it, it + decisionVariables.size()}};
+  it += decisionVariables.size();
+
+  VariableMatrix p_e{std::span{it, it + equalityConstraints.size()}};
+  it += equalityConstraints.size();
+
+  VariableMatrix n_e{std::span{it, it + equalityConstraints.size()}};
+  it += equalityConstraints.size();
+
+  VariableMatrix p_i{std::span{it, it + inequalityConstraints.size()}};
+  it += inequalityConstraints.size();
+
+  VariableMatrix n_i{std::span{it, it + inequalityConstraints.size()}};
+
+  // Set initial values for pₑ, nₑ, pᵢ, and nᵢ.
+  //
+  //
+  // From equation (33) of [2]:
+  //                       ______________________
+  //       μ − ρ c(x)     /(μ − ρ c(x))²   μ c(x)
+  //   n = −−−−−−−−−− +  / (−−−−−−−−−−)  + −−−−−−     (1)
+  //           2ρ       √  (    2ρ    )      2ρ
+  //
+  // The quadratic formula:
+  //             ________
+  //       -b + √b² - 4ac
+  //   x = −−−−−−−−−−−−−−                             (2)
+  //             2a
+  //
+  // Rearrange (1) to fit the quadratic formula better:
+  //                     _________________________
+  //       μ - ρ c(x) + √(μ - ρ c(x))² + 2ρ μ c(x)
+  //   n = −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
+  //                         2ρ
+  //
+  // Solve for coefficients:
+  //
+  //   a = ρ                                         (3)
+  //   b = ρ c(x) - μ                                (4)
+  //
+  //   -4ac = μ c(x) 2ρ
+  //   -4(ρ)c = 2ρ μ c(x)
+  //   -4c = 2μ c(x)
+  //   c = -μ c(x)/2                                 (5)
+  //
+  //   p = c(x) + n                                  (6)
+  for (int row = 0; row < p_e.Rows(); ++row) {
+    double c_e = equalityConstraints[row].Value();
+
+    constexpr double a = 2 * ρ;
+    double b = ρ * c_e - μ;
+    double c = -μ * c_e / 2.0;
+
+    double n = -b * std::sqrt(b * b - 4.0 * a * c) / (2.0 * a);
+    double p = c_e + n;
+
+    p_e(row).SetValue(p);
+    n_e(row).SetValue(n);
+  }
+  for (int row = 0; row < p_i.Rows(); ++row) {
+    double c_i = inequalityConstraints[row].Value() - s(row);
+
+    constexpr double a = 2 * ρ;
+    double b = ρ * c_i - μ;
+    double c = -μ * c_i / 2.0;
+
+    double n = -b * std::sqrt(b * b - 4.0 * a * c) / (2.0 * a);
+    double p = c_i + n;
+
+    p_i(row).SetValue(p);
+    n_i(row).SetValue(n);
+  }
+
+  // cₑ(x) - pₑ + nₑ = 0
+  std::vector<Variable> fr_equalityConstraints;
+  fr_equalityConstraints.assign(equalityConstraints.begin(),
+                                equalityConstraints.end());
+  for (size_t row = 0; row < fr_equalityConstraints.size(); ++row) {
+    auto& constraint = fr_equalityConstraints[row];
+    constraint = constraint - p_e(row) + n_e(row);
+  }
+
+  // cᵢ(x) - s - pᵢ + nᵢ = 0
+  std::vector<Variable> fr_inequalityConstraints;
+  fr_inequalityConstraints.assign(inequalityConstraints.begin(),
+                                  inequalityConstraints.end());
+  for (size_t row = 0; row < fr_inequalityConstraints.size(); ++row) {
+    auto& constraint = fr_inequalityConstraints[row];
+    constraint = constraint - s(row) - p_i(row) + n_i(row);
+  }
+
+  // pₑ ≥ 0
+  std::copy(p_e.begin(), p_e.end(),
+            std::back_inserter(fr_inequalityConstraints));
+
+  // pᵢ ≥ 0
+  std::copy(p_i.begin(), p_i.end(),
+            std::back_inserter(fr_inequalityConstraints));
+
+  // nₑ ≥ 0
+  std::copy(n_e.begin(), n_e.end(),
+            std::back_inserter(fr_inequalityConstraints));
+
+  // nᵢ ≥ 0
+  std::copy(n_i.begin(), n_i.end(),
+            std::back_inserter(fr_inequalityConstraints));
+
+  Variable J = 0.0;
+
+  // J += ρ Σ (pₑ + nₑ + pᵢ + nᵢ)
+  for (auto& elem : p_e) {
+    J += elem;
+  }
+  for (auto& elem : p_i) {
+    J += elem;
+  }
+  for (auto& elem : n_e) {
+    J += elem;
+  }
+  for (auto& elem : n_i) {
+    J += elem;
+  }
+  J *= ρ;
+
+  // D_R = diag(min(1, 1/|x_R⁽¹⁾|), …, min(1, 1/|x_R|⁽ⁿ⁾)
+  Eigen::VectorXd D_R{x.rows()};
+  for (int row = 0; row < D_R.rows(); ++row) {
+    D_R(row) = std::min(1.0, 1.0 / std::abs(x(row)));
+  }
+
+  // J += ζ/2 (x - x_R)ᵀD_R(x - x_R)
+  for (int row = 0; row < x.rows(); ++row) {
+    J += std::sqrt(μ) / 2.0 * D_R(row) * sleipnir::pow(xAD(row) - x(row), 2);
+  }
+
+  Eigen::VectorXd fr_x = VariableMatrix{fr_decisionVariables}.Value();
+
+  // Set up initial value for inequality constraint slack variables
+  Eigen::VectorXd fr_s{fr_inequalityConstraints.size()};
+  fr_s.segment(0, inequalityConstraints.size()) = s;
+  fr_s.segment(inequalityConstraints.size(),
+               fr_s.size() - inequalityConstraints.size())
+      .setOnes();
+
+  InteriorPoint(fr_decisionVariables, fr_equalityConstraints,
+                fr_inequalityConstraints, J, callback, config, true, fr_x, fr_s,
+                status);
+
+  x = fr_x.segment(0, decisionVariables.size());
+  s = fr_s.segment(0, inequalityConstraints.size());
+}
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/Filter.hpp

@@ -0,0 +1,188 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <algorithm>
+#include <cmath>
+#include <limits>
+#include <utility>
+#include <vector>
+
+#include <Eigen/Core>
+
+#include "sleipnir/autodiff/Variable.hpp"
+
+namespace sleipnir {
+
+/**
+ * Filter entry consisting of cost and constraint violation.
+ */
+struct FilterEntry {
+  /// The cost function's value
+  double cost = 0.0;
+
+  /// The constraint violation
+  double constraintViolation = 0.0;
+
+  constexpr FilterEntry() = default;
+
+  /**
+   * Constructs a FilterEntry.
+   *
+   * @param cost The cost function's value.
+   * @param constraintViolation The constraint violation.
+   */
+  FilterEntry(double cost, double constraintViolation)
+      : cost{cost}, constraintViolation{constraintViolation} {}
+
+  /**
+   * Constructs a FilterEntry.
+   *
+   * @param f The cost function.
+   * @param μ The barrier parameter.
+   * @param s The inequality constraint slack variables.
+   * @param c_e The equality constraint values (nonzero means violation).
+   * @param c_i The inequality constraint values (negative means violation).
+   */
+  FilterEntry(const Variable& f, double μ, const Eigen::VectorXd& s,
+              const Eigen::VectorXd& c_e, const Eigen::VectorXd& c_i)
+      : cost{f.Value() - μ * s.array().log().sum()},
+        constraintViolation{c_e.lpNorm<1>() + (c_i - s).lpNorm<1>()} {}
+};
+
+/**
+ * Interior-point step filter.
+ */
+class Filter {
+ public:
+  static constexpr double γCost = 1e-8;
+  static constexpr double γConstraint = 1e-5;
+
+  double maxConstraintViolation = 1e4;
+
+  /**
+   * Construct an empty filter.
+   *
+   * @param f The cost function.
+   * @param μ The barrier parameter.
+   */
+  explicit Filter(Variable& f, double μ) {
+    m_f = &f;
+    m_μ = μ;
+
+    // Initial filter entry rejects constraint violations above max
+    m_filter.emplace_back(std::numeric_limits<double>::infinity(),
+                          maxConstraintViolation);
+  }
+
+  /**
+   * Reset the filter.
+   *
+   * @param μ The new barrier parameter.
+   */
+  void Reset(double μ) {
+    m_μ = μ;
+    m_filter.clear();
+
+    // Initial filter entry rejects constraint violations above max
+    m_filter.emplace_back(std::numeric_limits<double>::infinity(),
+                          maxConstraintViolation);
+  }
+
+  /**
+   * Creates a new filter entry.
+   *
+   * @param s The inequality constraint slack variables.
+   * @param c_e The equality constraint values (nonzero means violation).
+   * @param c_i The inequality constraint values (negative means violation).
+   */
+  FilterEntry MakeEntry(Eigen::VectorXd& s, const Eigen::VectorXd& c_e,
+                        const Eigen::VectorXd& c_i) {
+    return FilterEntry{*m_f, m_μ, s, c_e, c_i};
+  }
+
+  /**
+   * Add a new entry to the filter.
+   *
+   * @param entry The entry to add to the filter.
+   */
+  void Add(const FilterEntry& entry) {
+    // Remove dominated entries
+    std::erase_if(m_filter, [&](const auto& elem) {
+      return entry.cost <= elem.cost &&
+             entry.constraintViolation <= elem.constraintViolation;
+    });
+
+    m_filter.push_back(entry);
+  }
+
+  /**
+   * Add a new entry to the filter.
+   *
+   * @param entry The entry to add to the filter.
+   */
+  void Add(FilterEntry&& entry) {
+    // Remove dominated entries
+    std::erase_if(m_filter, [&](const auto& elem) {
+      return entry.cost <= elem.cost &&
+             entry.constraintViolation <= elem.constraintViolation;
+    });
+
+    m_filter.push_back(entry);
+  }
+
+  /**
+   * Returns true if the given iterate is accepted by the filter.
+   *
+   * @param entry The entry to attempt adding to the filter.
+   */
+  bool TryAdd(const FilterEntry& entry) {
+    if (IsAcceptable(entry)) {
+      Add(entry);
+      return true;
+    } else {
+      return false;
+    }
+  }
+
+  /**
+   * Returns true if the given iterate is accepted by the filter.
+   *
+   * @param entry The entry to attempt adding to the filter.
+   */
+  bool TryAdd(FilterEntry&& entry) {
+    if (IsAcceptable(entry)) {
+      Add(std::move(entry));
+      return true;
+    } else {
+      return false;
+    }
+  }
+
+  /**
+   * Returns true if the given entry is acceptable to the filter.
+   *
+   * @param entry The entry to check.
+   */
+  bool IsAcceptable(const FilterEntry& entry) {
+    if (!std::isfinite(entry.cost) ||
+        !std::isfinite(entry.constraintViolation)) {
+      return false;
+    }
+
+    // If current filter entry is better than all prior ones in some respect,
+    // accept it
+    return std::all_of(m_filter.begin(), m_filter.end(), [&](const auto& elem) {
+      return entry.cost <= elem.cost - γCost * elem.constraintViolation ||
+             entry.constraintViolation <=
+                 (1.0 - γConstraint) * elem.constraintViolation;
+    });
+  }
+
+ private:
+  Variable* m_f = nullptr;
+  double m_μ = 0.0;
+  std::vector<FilterEntry> m_filter;
+};
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FractionToTheBoundaryRule.hpp

@@ -0,0 +1,45 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <Eigen/Core>
+
+// See docs/algorithms.md#Works_cited for citation definitions
+
+namespace sleipnir {
+
+/**
+ * Applies fraction-to-the-boundary rule to a variable and its iterate, then
+ * returns a fraction of the iterate step size within (0, 1].
+ *
+ * @param x The variable.
+ * @param p The iterate on the variable.
+ * @param τ Fraction-to-the-boundary rule scaling factor within (0, 1].
+ * @return Fraction of the iterate step size within (0, 1].
+ */
+inline double FractionToTheBoundaryRule(
+    const Eigen::Ref<const Eigen::VectorXd>& x,
+    const Eigen::Ref<const Eigen::VectorXd>& p, double τ) {
+  // α = max(α ∈ (0, 1] : x + αp ≥ (1 − τ)x)
+  //
+  // where x and τ are positive.
+  //
+  // x + αp ≥ (1 − τ)x
+  // x + αp ≥ x − τx
+  // αp ≥ −τx
+  //
+  // If the inequality is false, p < 0 and α is too big. Find the largest value
+  // of α that makes the inequality true.
+  //
+  // α = −τ/p x
+  double α = 1.0;
+  for (int i = 0; i < x.rows(); ++i) {
+    if (α * p(i) < -τ * x(i)) {
+      α = -τ / p(i) * x(i);
+    }
+  }
+
+  return α;
+}
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/IsLocallyInfeasible.hpp

@@ -0,0 +1,63 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <Eigen/Core>
+#include <Eigen/SparseCore>
+
+// See docs/algorithms.md#Works_cited for citation definitions
+
+namespace sleipnir {
+
+/**
+ * Returns true if the problem's equality constraints are locally infeasible.
+ *
+ * @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
+ *   current iterate.
+ * @param c_e The problem's equality constraints cₑ(x) evaluated at the current
+ *   iterate.
+ */
+inline bool IsEqualityLocallyInfeasible(const Eigen::SparseMatrix<double>& A_e,
+                                        const Eigen::VectorXd& c_e) {
+  // The equality constraints are locally infeasible if
+  //
+  //   Aₑᵀcₑ → 0
+  //   ‖cₑ‖ > ε
+  //
+  // See "Infeasibility detection" in section 6 of [3].
+  return A_e.rows() > 0 && (A_e.transpose() * c_e).norm() < 1e-6 &&
+         c_e.norm() > 1e-2;
+}
+
+/**
+ * Returns true if the problem's inequality constraints are locally infeasible.
+ *
+ * @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at
+ *   the current iterate.
+ * @param c_i The problem's inequality constraints cᵢ(x) evaluated at the
+ *   current iterate.
+ */
+inline bool IsInequalityLocallyInfeasible(
+    const Eigen::SparseMatrix<double>& A_i, const Eigen::VectorXd& c_i) {
+  // The inequality constraints are locally infeasible if
+  //
+  //   Aᵢᵀcᵢ⁺ → 0
+  //   ‖cᵢ⁺‖ > ε
+  //
+  // where cᵢ⁺ = min(cᵢ, 0).
+  //
+  // See "Infeasibility detection" in section 6 of [3].
+  //
+  // cᵢ⁺ is used instead of cᵢ⁻ from the paper to follow the convention that
+  // feasible inequality constraints are ≥ 0.
+  if (A_i.rows() > 0) {
+    Eigen::VectorXd c_i_plus = c_i.cwiseMin(0.0);
+    if ((A_i.transpose() * c_i_plus).norm() < 1e-6 && c_i_plus.norm() > 1e-6) {
+      return true;
+    }
+  }
+
+  return false;
+}
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/KKTError.hpp

@@ -0,0 +1,51 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <Eigen/Core>
+#include <Eigen/SparseCore>
+
+// See docs/algorithms.md#Works_cited for citation definitions
+
+namespace sleipnir {
+
+/**
+ * Returns the KKT error for the interior-point method.
+ *
+ * @param g Gradient of the cost function ∇f.
+ * @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
+ *   current iterate.
+ * @param c_e The problem's equality constraints cₑ(x) evaluated at the current
+ *   iterate.
+ * @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at
+ *   the current iterate.
+ * @param c_i The problem's inequality constraints cᵢ(x) evaluated at the
+ *   current iterate.
+ * @param s Inequality constraint slack variables.
+ * @param y Equality constraint dual variables.
+ * @param z Inequality constraint dual variables.
+ * @param μ Barrier parameter.
+ */
+inline double KKTError(const Eigen::VectorXd& g,
+                       const Eigen::SparseMatrix<double>& A_e,
+                       const Eigen::VectorXd& c_e,
+                       const Eigen::SparseMatrix<double>& A_i,
+                       const Eigen::VectorXd& c_i, const Eigen::VectorXd& s,
+                       const Eigen::VectorXd& y, const Eigen::VectorXd& z,
+                       double μ) {
+  // Compute the KKT error as the 1-norm of the KKT conditions from equations
+  // (19.5a) through (19.5d) of [1].
+  //
+  //   ∇f − Aₑᵀy − Aᵢᵀz = 0
+  //   Sz − μe = 0
+  //   cₑ = 0
+  //   cᵢ − s = 0
+
+  const auto S = s.asDiagonal();
+  const Eigen::VectorXd e = Eigen::VectorXd::Ones(s.rows());
+
+  return (g - A_e.transpose() * y - A_i.transpose() * z).lpNorm<1>() +
+         (S * z - μ * e).lpNorm<1>() + c_e.lpNorm<1>() + (c_i - s).lpNorm<1>();
+}
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/src/util/Pool.cpp

@@ -0,0 +1,12 @@
+// Copyright (c) Sleipnir contributors
+
+#include "sleipnir/util/Pool.hpp"
+
+namespace sleipnir {
+
+PoolResource& GlobalPoolResource() {
+  thread_local PoolResource pool{16384};
+  return pool;
+}
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/src/util/ScopeExit.hpp

@@ -0,0 +1,35 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <utility>
+
+namespace sleipnir {
+
+template <typename F>
+class scope_exit {
+ public:
+  explicit scope_exit(F&& f) noexcept : m_f{std::forward<F>(f)} {}
+
+  ~scope_exit() {
+    if (m_active) {
+      m_f();
+    }
+  }
+
+  scope_exit(scope_exit&& rhs) noexcept
+      : m_f{std::move(rhs.m_f)}, m_active{rhs.m_active} {
+    rhs.release();
+  }
+
+  scope_exit(const scope_exit&) = delete;
+  scope_exit& operator=(const scope_exit&) = delete;
+
+  void release() noexcept { m_active = false; }
+
+ private:
+  F m_f;
+  bool m_active = true;
+};
+
+}  // namespace sleipnir

wpimath/src/main/native/thirdparty/sleipnir/src/util/ToMilliseconds.hpp

@@ -0,0 +1,21 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <chrono>
+
+namespace sleipnir {
+
+/**
+ * Converts std::chrono::duration to a number of milliseconds rounded to three
+ * decimals.
+ */
+template <typename Rep, typename Period = std::ratio<1>>
+constexpr double ToMilliseconds(
+    const std::chrono::duration<Rep, Period>& duration) {
+  using std::chrono::duration_cast;
+  using std::chrono::microseconds;
+  return duration_cast<microseconds>(duration).count() / 1e3;
+}
+
+}  // namespace sleipnir

wpimath/src/test/native/cpp/SleipnirTest.cpp

@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+#include <sleipnir/optimization/OptimizationProblem.hpp>
+
+TEST(SleipnirTest, Quartic) {
+  sleipnir::OptimizationProblem problem;
+
+  auto x = problem.DecisionVariable();
+  x.SetValue(20.0);
+
+  problem.Minimize(sleipnir::pow(x, 4));
+
+  problem.SubjectTo(x >= 1);
+
+  auto status = problem.Solve({.diagnostics = true});
+
+  EXPECT_EQ(status.costFunctionType, sleipnir::ExpressionType::kNonlinear);
+  EXPECT_EQ(status.equalityConstraintType, sleipnir::ExpressionType::kNone);
+  EXPECT_EQ(status.inequalityConstraintType, sleipnir::ExpressionType::kLinear);
+  EXPECT_EQ(status.exitCondition, sleipnir::SolverExitCondition::kSuccess);
+
+  EXPECT_NEAR(x.Value(), 1.0, 1e-6);
+}