Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +00:00
Code Issues Packages Projects Releases Wiki Activity
Files
2ff7033edf03e124c490ffc83e75b43015e5e2f2
allwpilib/upstream_utils/sleipnir_patches/0004-Use-wpi-SmallVector.patch

516 lines
18 KiB
Diff
Raw Normal View History

[upstream_utils] Upgrade Sleipnir and use wpi::SmallVector (#6748)
2024-06-21 11:14:19 -07:00
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Sun, 16 Jun 2024 12:08:49 -0700
Subject: [PATCH 4/4] Use wpi::SmallVector
---
include/.styleguide | 1 +
include/sleipnir/autodiff/Expression.hpp | 5 +++--
include/sleipnir/autodiff/ExpressionGraph.hpp | 15 ++++++++-------
include/sleipnir/autodiff/Hessian.hpp | 4 ++--
include/sleipnir/autodiff/Jacobian.hpp | 10 +++++-----
include/sleipnir/autodiff/VariableMatrix.hpp | 4 ++--
include/sleipnir/optimization/Constraints.hpp | 11 ++++++-----
include/sleipnir/optimization/Multistart.hpp | 7 ++++---
.../sleipnir/optimization/OptimizationProblem.hpp | 8 ++++----
include/sleipnir/util/Pool.hpp | 7 ++++---
include/sleipnir/util/Spy.hpp | 4 ++--
src/.styleguide | 1 +
src/optimization/solver/InteriorPoint.cpp | 4 ++--
.../solver/util/FeasibilityRestoration.hpp | 10 +++++-----
src/optimization/solver/util/Filter.hpp | 4 ++--
15 files changed, 51 insertions(+), 44 deletions(-)
diff --git a/include/.styleguide b/include/.styleguide
index 6a7f8ed28f9cb037c9746a7e0ef5e110481d9825..efa36cee1fb593ae810032340c64f1854fbbc523 100644
--- a/include/.styleguide
+++ b/include/.styleguide
@@ -13,4 +13,5 @@ licenseUpdateExclude {
includeOtherLibs {
^Eigen/
^fmt/
+ ^wpi/
}
diff --git a/include/sleipnir/autodiff/Expression.hpp b/include/sleipnir/autodiff/Expression.hpp
index 758433270a1cbe1b33f8b41d3e971b58de711e18..f9be904cf289ad5f5538d9c91b7f2e96ea7d3ab2 100644
--- a/include/sleipnir/autodiff/Expression.hpp
+++ b/include/sleipnir/autodiff/Expression.hpp
@@ -11,11 +11,12 @@
#include <numbers>
#include <utility>
+#include <wpi/SmallVector.h>
+
#include "sleipnir/autodiff/ExpressionType.hpp"
#include "sleipnir/util/IntrusiveSharedPtr.hpp"
#include "sleipnir/util/Pool.hpp"
#include "sleipnir/util/SymbolExports.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir::detail {
@@ -423,7 +424,7 @@ inline void IntrusiveSharedPtrDecRefCount(Expression* expr) {
// Expression destructor when expr's refcount reaches zero can cause a stack
// overflow. Instead, we iterate over its children to decrement their
// refcounts and deallocate them.
- small_vector<Expression*> stack;
+ wpi::SmallVector<Expression*> stack;
stack.emplace_back(expr);
while (!stack.empty()) {
diff --git a/include/sleipnir/autodiff/ExpressionGraph.hpp b/include/sleipnir/autodiff/ExpressionGraph.hpp
index 56fe6bd5734ce2eec43f3c4c3e70c2ed41b32a21..4683089037271df7bcf0894f5a158c6c4607fe87 100644
--- a/include/sleipnir/autodiff/ExpressionGraph.hpp
+++ b/include/sleipnir/autodiff/ExpressionGraph.hpp
@@ -4,10 +4,11 @@
#include <span>
+#include <wpi/SmallVector.h>
+
#include "sleipnir/autodiff/Expression.hpp"
#include "sleipnir/util/FunctionRef.hpp"
#include "sleipnir/util/SymbolExports.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir::detail {
@@ -36,7 +37,7 @@ class SLEIPNIR_DLLEXPORT ExpressionGraph {
// https://en.wikipedia.org/wiki/Breadth-first_search
// BFS list sorted from parent to child.
- small_vector<Expression*> stack;
+ wpi::SmallVector<Expression*> stack;
stack.emplace_back(root.Get());
@@ -119,7 +120,7 @@ class SLEIPNIR_DLLEXPORT ExpressionGraph {
*
* @param wrt Variables with respect to which to compute the gradient.
*/
- small_vector<ExpressionPtr> GenerateGradientTree(
+ wpi::SmallVector<ExpressionPtr> GenerateGradientTree(
std::span<const ExpressionPtr> wrt) const {
// Read docs/algorithms.md#Reverse_accumulation_automatic_differentiation
// for background on reverse accumulation automatic differentiation.
@@ -128,7 +129,7 @@ class SLEIPNIR_DLLEXPORT ExpressionGraph {
wrt[row]->row = row;
}
- small_vector<ExpressionPtr> grad;
+ wpi::SmallVector<ExpressionPtr> grad;
grad.reserve(wrt.size());
for (size_t row = 0; row < wrt.size(); ++row) {
grad.emplace_back(MakeExpressionPtr());
@@ -231,13 +232,13 @@ class SLEIPNIR_DLLEXPORT ExpressionGraph {
private:
// List that maps nodes to their respective row.
- small_vector<int> m_rowList;
+ wpi::SmallVector<int> m_rowList;
// List for updating adjoints
- small_vector<Expression*> m_adjointList;
+ wpi::SmallVector<Expression*> m_adjointList;
// List for updating values
- small_vector<Expression*> m_valueList;
+ wpi::SmallVector<Expression*> m_valueList;
};
} // namespace sleipnir::detail
diff --git a/include/sleipnir/autodiff/Hessian.hpp b/include/sleipnir/autodiff/Hessian.hpp
index 4563aa234bd7b0ec22e12d2fc0b6f4569bee7f39..2e60d89e95280bdac422b0d7dab955ba111b0059 100644
--- a/include/sleipnir/autodiff/Hessian.hpp
+++ b/include/sleipnir/autodiff/Hessian.hpp
@@ -6,6 +6,7 @@
#include <Eigen/Core>
#include <Eigen/SparseCore>
+#include <wpi/SmallVector.h>
#include "sleipnir/autodiff/ExpressionGraph.hpp"
#include "sleipnir/autodiff/Jacobian.hpp"
@@ -13,7 +14,6 @@
#include "sleipnir/autodiff/Variable.hpp"
#include "sleipnir/autodiff/VariableMatrix.hpp"
#include "sleipnir/util/SymbolExports.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir {
@@ -36,7 +36,7 @@ class SLEIPNIR_DLLEXPORT Hessian {
Hessian(Variable variable, const VariableMatrix& wrt) noexcept
: m_jacobian{
[&] {
- small_vector<detail::ExpressionPtr> wrtVec;
+ wpi::SmallVector<detail::ExpressionPtr> wrtVec;
wrtVec.reserve(wrt.size());
for (auto& elem : wrt) {
wrtVec.emplace_back(elem.expr);
diff --git a/include/sleipnir/autodiff/Jacobian.hpp b/include/sleipnir/autodiff/Jacobian.hpp
index ac00c11ef8c947cbe95c58081bdbfb42bf901051..0c660156c80f94539383b8f0d01d7853e41e0297 100644
--- a/include/sleipnir/autodiff/Jacobian.hpp
+++ b/include/sleipnir/autodiff/Jacobian.hpp
@@ -5,13 +5,13 @@
#include <utility>
#include <Eigen/SparseCore>
+#include <wpi/SmallVector.h>
#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"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir {
@@ -81,7 +81,7 @@ class SLEIPNIR_DLLEXPORT Jacobian {
VariableMatrix Get() const {
VariableMatrix result{m_variables.Rows(), m_wrt.Rows()};
- small_vector<detail::ExpressionPtr> wrtVec;
+ wpi::SmallVector<detail::ExpressionPtr> wrtVec;
wrtVec.reserve(m_wrt.size());
for (auto& elem : m_wrt) {
wrtVec.emplace_back(elem.expr);
@@ -138,16 +138,16 @@ class SLEIPNIR_DLLEXPORT Jacobian {
VariableMatrix m_variables;
VariableMatrix m_wrt;
- small_vector<detail::ExpressionGraph> m_graphs;
+ wpi::SmallVector<detail::ExpressionGraph> m_graphs;
Eigen::SparseMatrix<double> m_J{m_variables.Rows(), m_wrt.Rows()};
// Cached triplets for gradients of linear rows
- small_vector<Eigen::Triplet<double>> m_cachedTriplets;
+ wpi::SmallVector<Eigen::Triplet<double>> m_cachedTriplets;
// List of row indices for nonlinear rows whose graients will be computed in
// Value()
- small_vector<int> m_nonlinearRows;
+ wpi::SmallVector<int> m_nonlinearRows;
Profiler m_profiler;
};
diff --git a/include/sleipnir/autodiff/VariableMatrix.hpp b/include/sleipnir/autodiff/VariableMatrix.hpp
index f080a877a482d8a45a56ff38a0b969ef81b5809d..e691e209afed43d9384c2828243972d41e59ed59 100644
--- a/include/sleipnir/autodiff/VariableMatrix.hpp
+++ b/include/sleipnir/autodiff/VariableMatrix.hpp
@@ -11,13 +11,13 @@
#include <vector>
#include <Eigen/Core>
+#include <wpi/SmallVector.h>
#include "sleipnir/autodiff/Variable.hpp"
#include "sleipnir/autodiff/VariableBlock.hpp"
#include "sleipnir/util/Assert.hpp"
#include "sleipnir/util/FunctionRef.hpp"
#include "sleipnir/util/SymbolExports.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir {
@@ -884,7 +884,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
}
private:
- small_vector<Variable> m_storage;
+ wpi::SmallVector<Variable> m_storage;
int m_rows = 0;
int m_cols = 0;
};
diff --git a/include/sleipnir/optimization/Constraints.hpp b/include/sleipnir/optimization/Constraints.hpp
index 3b7315a851eff59dabc9a11334d09ef0b7337881..a33039b8996ded92014fa96dfacc9c80fd612c79 100644
--- a/include/sleipnir/optimization/Constraints.hpp
+++ b/include/sleipnir/optimization/Constraints.hpp
@@ -6,11 +6,12 @@
#include <concepts>
#include <type_traits>
+#include <wpi/SmallVector.h>
+
#include "sleipnir/autodiff/Variable.hpp"
#include "sleipnir/util/Assert.hpp"
#include "sleipnir/util/Concepts.hpp"
#include "sleipnir/util/SymbolExports.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir {
@@ -30,8 +31,8 @@ template <typename LHS, typename RHS>
(ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
(!std::same_as<std::decay_t<LHS>, double> ||
!std::same_as<std::decay_t<RHS>, double>)
-small_vector<Variable> MakeConstraints(LHS&& lhs, RHS&& rhs) {
- small_vector<Variable> constraints;
+wpi::SmallVector<Variable> MakeConstraints(LHS&& lhs, RHS&& rhs) {
+ wpi::SmallVector<Variable> constraints;
if constexpr (ScalarLike<std::decay_t<LHS>> &&
ScalarLike<std::decay_t<RHS>>) {
@@ -119,7 +120,7 @@ small_vector<Variable> MakeConstraints(LHS&& lhs, RHS&& rhs) {
*/
struct SLEIPNIR_DLLEXPORT EqualityConstraints {
/// A vector of scalar equality constraints.
- small_vector<Variable> constraints;
+ wpi::SmallVector<Variable> constraints;
/**
* Constructs an equality constraint from a left and right side.
@@ -153,7 +154,7 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints {
*/
struct SLEIPNIR_DLLEXPORT InequalityConstraints {
/// A vector of scalar inequality constraints.
- small_vector<Variable> constraints;
+ wpi::SmallVector<Variable> constraints;
/**
* Constructs an inequality constraint from a left and right side.
diff --git a/include/sleipnir/optimization/Multistart.hpp b/include/sleipnir/optimization/Multistart.hpp
index d905cafd5fce57dc53665caf50997ada00db4ebe..6935590a75db6eb3f1ecbd35c805cb7849bc3d9c 100644
--- a/include/sleipnir/optimization/Multistart.hpp
+++ b/include/sleipnir/optimization/Multistart.hpp
@@ -6,9 +6,10 @@
#include <future>
#include <span>
+#include <wpi/SmallVector.h>
+
#include "sleipnir/optimization/SolverStatus.hpp"
#include "sleipnir/util/FunctionRef.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir {
@@ -43,14 +44,14 @@ MultistartResult<DecisionVariables> Multistart(
function_ref<MultistartResult<DecisionVariables>(const DecisionVariables&)>
solve,
std::span<const DecisionVariables> initialGuesses) {
- small_vector<std::future<MultistartResult<DecisionVariables>>> futures;
+ wpi::SmallVector<std::future<MultistartResult<DecisionVariables>>> futures;
futures.reserve(initialGuesses.size());
for (const auto& initialGuess : initialGuesses) {
futures.emplace_back(std::async(std::launch::async, solve, initialGuess));
}
- small_vector<MultistartResult<DecisionVariables>> results;
+ wpi::SmallVector<MultistartResult<DecisionVariables>> results;
results.reserve(futures.size());
for (auto& future : futures) {
diff --git a/include/sleipnir/optimization/OptimizationProblem.hpp b/include/sleipnir/optimization/OptimizationProblem.hpp
index 32ef0bf6fb0cbcba84c82031644e935a44815ed2..7dc4c50b035b1848b9b0177d10a4445a021a744e 100644
--- a/include/sleipnir/optimization/OptimizationProblem.hpp
+++ b/include/sleipnir/optimization/OptimizationProblem.hpp
@@ -12,6 +12,7 @@
#include <utility>
#include <Eigen/Core>
+#include <wpi/SmallVector.h>
#include "sleipnir/autodiff/Variable.hpp"
#include "sleipnir/autodiff/VariableMatrix.hpp"
@@ -23,7 +24,6 @@
#include "sleipnir/optimization/solver/InteriorPoint.hpp"
#include "sleipnir/util/Print.hpp"
#include "sleipnir/util/SymbolExports.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir {
@@ -360,16 +360,16 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem {
private:
// The list of decision variables, which are the root of the problem's
// expression tree
- small_vector<Variable> m_decisionVariables;
+ wpi::SmallVector<Variable> m_decisionVariables;
// The cost function: f(x)
std::optional<Variable> m_f;
// The list of equality constraints: cₑ(x) = 0
- small_vector<Variable> m_equalityConstraints;
+ wpi::SmallVector<Variable> m_equalityConstraints;
// The list of inequality constraints: cᵢ(x) ≥ 0
- small_vector<Variable> m_inequalityConstraints;
+ wpi::SmallVector<Variable> m_inequalityConstraints;
// The user callback
std::function<bool(const SolverIterationInfo&)> m_callback =
diff --git a/include/sleipnir/util/Pool.hpp b/include/sleipnir/util/Pool.hpp
index 02d8e190178973a1e2edc94225754fb6e2199bf4..95b4d2e97e05b5983720e4abd87f1dbb973dd7f4 100644
--- a/include/sleipnir/util/Pool.hpp
+++ b/include/sleipnir/util/Pool.hpp
@@ -5,8 +5,9 @@
#include <cstddef>
#include <memory>
+#include <wpi/SmallVector.h>
+
#include "sleipnir/util/SymbolExports.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir {
@@ -76,8 +77,8 @@ class SLEIPNIR_DLLEXPORT PoolResource {
}
private:
- small_vector<std::unique_ptr<std::byte[]>> m_buffer;
- small_vector<void*> m_freeList;
+ wpi::SmallVector<std::unique_ptr<std::byte[]>> m_buffer;
+ wpi::SmallVector<void*> m_freeList;
size_t blocksPerChunk;
/**
diff --git a/include/sleipnir/util/Spy.hpp b/include/sleipnir/util/Spy.hpp
index cb9b4e191545e96c2bade5f8f99b0bec376b656b..7f526a2d9968e76b385a0ddfb2edf5bab7274fb0 100644
--- a/include/sleipnir/util/Spy.hpp
+++ b/include/sleipnir/util/Spy.hpp
@@ -7,9 +7,9 @@
#include <string_view>
#include <Eigen/SparseCore>
+#include <wpi/SmallVector.h>
#include "sleipnir/util/SymbolExports.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir {
@@ -32,7 +32,7 @@ SLEIPNIR_DLLEXPORT inline void Spy(std::ostream& file,
const int cells_width = mat.cols() + 1;
const int cells_height = mat.rows();
- small_vector<uint8_t> cells;
+ wpi::SmallVector<uint8_t> cells;
// Allocate space for matrix of characters plus trailing newlines
cells.reserve(cells_width * cells_height);
diff --git a/src/.styleguide b/src/.styleguide
index f3b2f0cf9e60b3a86b9654ff2b381f9c48734ff6..ad739cea6dce6f6cb586f538d1d30b92503484c1 100644
--- a/src/.styleguide
+++ b/src/.styleguide
@@ -8,4 +8,5 @@ cppSrcFileInclude {
includeOtherLibs {
^Eigen/
+ ^wpi/
}
diff --git a/src/optimization/solver/InteriorPoint.cpp b/src/optimization/solver/InteriorPoint.cpp
index 0d3c8fa23b113a8ae42ae17dc3a8daa5b017cfb2..06187b7260182f02860b1419727b2ca71ff48c9a 100644
--- a/src/optimization/solver/InteriorPoint.cpp
+++ b/src/optimization/solver/InteriorPoint.cpp
@@ -9,6 +9,7 @@
#include <limits>
#include <Eigen/SparseCholesky>
+#include <wpi/SmallVector.h>
#include "optimization/RegularizedLDLT.hpp"
#include "optimization/solver/util/ErrorEstimate.hpp"
@@ -23,7 +24,6 @@
#include "sleipnir/optimization/SolverExitCondition.hpp"
#include "sleipnir/util/Print.hpp"
#include "sleipnir/util/Spy.hpp"
-#include "sleipnir/util/small_vector.hpp"
#include "util/ScopeExit.hpp"
#include "util/ToMilliseconds.hpp"
@@ -226,7 +226,7 @@ void InteriorPoint(std::span<Variable> decisionVariables,
};
// Kept outside the loop so its storage can be reused
- small_vector<Eigen::Triplet<double>> triplets;
+ wpi::SmallVector<Eigen::Triplet<double>> triplets;
RegularizedLDLT solver;
diff --git a/src/optimization/solver/util/FeasibilityRestoration.hpp b/src/optimization/solver/util/FeasibilityRestoration.hpp
index 37adf64ea483b1acd4a4d5db8697e1b2fc701e72..76af452517d75ae67e0511584c89f99c552989af 100644
--- a/src/optimization/solver/util/FeasibilityRestoration.hpp
+++ b/src/optimization/solver/util/FeasibilityRestoration.hpp
@@ -8,6 +8,7 @@
#include <span>
#include <Eigen/Core>
+#include <wpi/SmallVector.h>
#include "sleipnir/autodiff/Variable.hpp"
#include "sleipnir/autodiff/VariableMatrix.hpp"
@@ -16,7 +17,6 @@
#include "sleipnir/optimization/SolverStatus.hpp"
#include "sleipnir/optimization/solver/InteriorPoint.hpp"
#include "sleipnir/util/FunctionRef.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir {
@@ -66,7 +66,7 @@ inline void FeasibilityRestoration(
constexpr double ρ = 1000.0;
- small_vector<Variable> fr_decisionVariables;
+ wpi::SmallVector<Variable> fr_decisionVariables;
fr_decisionVariables.reserve(decisionVariables.size() +
2 * equalityConstraints.size() +
2 * inequalityConstraints.size());
@@ -82,7 +82,7 @@ inline void FeasibilityRestoration(
fr_decisionVariables.emplace_back();
}
- auto it = fr_decisionVariables.cbegin();
+ auto it = fr_decisionVariables.begin();
VariableMatrix xAD{std::span{it, it + decisionVariables.size()}};
it += decisionVariables.size();
@@ -158,7 +158,7 @@ inline void FeasibilityRestoration(
}
// cₑ(x) - pₑ + nₑ = 0
- small_vector<Variable> fr_equalityConstraints;
+ wpi::SmallVector<Variable> fr_equalityConstraints;
fr_equalityConstraints.assign(equalityConstraints.begin(),
equalityConstraints.end());
for (size_t row = 0; row < fr_equalityConstraints.size(); ++row) {
@@ -167,7 +167,7 @@ inline void FeasibilityRestoration(
}
// cᵢ(x) - s - pᵢ + nᵢ = 0
- small_vector<Variable> fr_inequalityConstraints;
+ wpi::SmallVector<Variable> fr_inequalityConstraints;
fr_inequalityConstraints.assign(inequalityConstraints.begin(),
inequalityConstraints.end());
for (size_t row = 0; row < fr_inequalityConstraints.size(); ++row) {
diff --git a/src/optimization/solver/util/Filter.hpp b/src/optimization/solver/util/Filter.hpp
index 3fbb849edc4a6b3336f94b5af9e018a37b07b123..02bdb5a8db5c80dd86d17ea4421ec564d7e0a2c7 100644
--- a/src/optimization/solver/util/Filter.hpp
+++ b/src/optimization/solver/util/Filter.hpp
@@ -8,9 +8,9 @@
#include <utility>
#include <Eigen/Core>
+#include <wpi/SmallVector.h>
#include "sleipnir/autodiff/Variable.hpp"
-#include "sleipnir/util/small_vector.hpp"
namespace sleipnir {
@@ -182,7 +182,7 @@ class Filter {
private:
Variable* m_f = nullptr;
double m_μ = 0.0;
- small_vector<FilterEntry> m_filter;
+ wpi::SmallVector<FilterEntry> m_filter;
};
} // namespace sleipnir
Reference in New Issue Copy Permalink