diff --git a/upstream_utils/sleipnir_patches/0001-Remove-using-enum-declarations.patch b/upstream_utils/sleipnir_patches/0001-Remove-using-enum-declarations.patch index 21de50523f..674ddb16eb 100644 --- a/upstream_utils/sleipnir_patches/0001-Remove-using-enum-declarations.patch +++ b/upstream_utils/sleipnir_patches/0001-Remove-using-enum-declarations.patch @@ -9,7 +9,7 @@ Subject: [PATCH 1/4] Remove "using enum" declarations 2 files changed, 73 insertions(+), 110 deletions(-) diff --git a/include/sleipnir/autodiff/Expression.hpp b/include/sleipnir/autodiff/Expression.hpp -index 5b8dc5dca9bd685e6647e79f5fd4699bb9909853..802a8602a125251058cfe22447911999f035efad 100644 +index 99997da577f5d4de81dc0c6de3cc7225676f5aa3..0f306e3b2dccbe63367be7856d051d943a09f910 100644 --- a/include/sleipnir/autodiff/Expression.hpp +++ b/include/sleipnir/autodiff/Expression.hpp @@ -191,8 +191,6 @@ struct SLEIPNIR_DLLEXPORT Expression { diff --git a/upstream_utils/sleipnir_patches/0002-Add-implicit-typename.patch b/upstream_utils/sleipnir_patches/0002-Add-implicit-typename.patch deleted file mode 100644 index 5ba6fa5cf6..0000000000 --- a/upstream_utils/sleipnir_patches/0002-Add-implicit-typename.patch +++ /dev/null @@ -1,24 +0,0 @@ -From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 -From: Tyler Veness -Date: Mon, 29 Apr 2024 19:17:18 -0700 -Subject: [PATCH 2/4] Add implicit typename - ---- - include/sleipnir/util/SmallVector.hpp | 4 ++-- - 1 file changed, 2 insertions(+), 2 deletions(-) - -diff --git a/include/sleipnir/util/SmallVector.hpp b/include/sleipnir/util/SmallVector.hpp -index 5e93ae49e659a29c49728d337323ebbcdd8790d3..dcf02c02b7f7fcfc32197d68f885174a1ae20307 100644 ---- a/include/sleipnir/util/SmallVector.hpp -+++ b/include/sleipnir/util/SmallVector.hpp -@@ -152,8 +152,8 @@ class small_vector - }; - - template --constexpr small_vector::size_type erase_if(small_vector& c, -- Pred pred) { -+constexpr typename small_vector::size_type erase_if(small_vector& c, -+ Pred pred) { - auto it = std::remove_if(c.begin(), c.end(), pred); - auto r = c.end() - it; - c.erase(it, c.end()); diff --git a/upstream_utils/sleipnir_patches/0003-Use-fmtlib.patch b/upstream_utils/sleipnir_patches/0002-Use-fmtlib.patch similarity index 65% rename from upstream_utils/sleipnir_patches/0003-Use-fmtlib.patch rename to upstream_utils/sleipnir_patches/0002-Use-fmtlib.patch index 23f507936a..17496503ca 100644 --- a/upstream_utils/sleipnir_patches/0003-Use-fmtlib.patch +++ b/upstream_utils/sleipnir_patches/0002-Use-fmtlib.patch @@ -1,48 +1,23 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Wed, 29 May 2024 16:29:55 -0700 -Subject: [PATCH 3/4] Use fmtlib +Subject: [PATCH 2/4] Use fmtlib --- - include/.styleguide | 1 + - include/sleipnir/util/Assert.hpp | 5 +++-- - include/sleipnir/util/Print.hpp | 27 ++++++++++++++------------- - 3 files changed, 18 insertions(+), 15 deletions(-) + include/.styleguide | 1 + + include/sleipnir/util/Print.hpp | 27 ++++++++++++++------------- + 2 files changed, 15 insertions(+), 13 deletions(-) diff --git a/include/.styleguide b/include/.styleguide -index f3b2f0cf9e60b3a86b9654ff2b381f9c48734ff6..8251a490677bcf5fd316d5303195b0fa4e599b75 100644 +index 8fb61fdf9cc5ceff633d3126f0579eef25a1326f..6a7f8ed28f9cb037c9746a7e0ef5e110481d9825 100644 --- a/include/.styleguide +++ b/include/.styleguide -@@ -8,4 +8,5 @@ cppSrcFileInclude { +@@ -12,4 +12,5 @@ licenseUpdateExclude { includeOtherLibs { ^Eigen/ + ^fmt/ } -diff --git a/include/sleipnir/util/Assert.hpp b/include/sleipnir/util/Assert.hpp -index ba381ef8f48446e6d07b6d7973a8271cbda8fec9..9999544278c493f263c819811cb4fbcb407b04f1 100644 ---- a/include/sleipnir/util/Assert.hpp -+++ b/include/sleipnir/util/Assert.hpp -@@ -3,8 +3,9 @@ - #pragma once - - #ifdef JORMUNGANDR --#include - #include -+ -+#include - /** - * Throw an exception in Python. - */ -@@ -12,7 +13,7 @@ - do { \ - if (!(condition)) { \ - throw std::invalid_argument( \ -- std::format("{}:{}: {}: Assertion `{}' failed.", __FILE__, __LINE__, \ -+ fmt::format("{}:{}: {}: Assertion `{}' failed.", __FILE__, __LINE__, \ - __func__, #condition)); \ - } \ - } while (0); diff --git a/include/sleipnir/util/Print.hpp b/include/sleipnir/util/Print.hpp index 339320bce6d017ca85025060ba445b2f025bb225..fcf2e69bfb5a081cd915bdded3caa80cd9c38518 100644 --- a/include/sleipnir/util/Print.hpp diff --git a/upstream_utils/sleipnir_patches/0004-Remove-unsupported-constexpr.patch b/upstream_utils/sleipnir_patches/0003-Remove-unsupported-constexpr.patch similarity index 92% rename from upstream_utils/sleipnir_patches/0004-Remove-unsupported-constexpr.patch rename to upstream_utils/sleipnir_patches/0003-Remove-unsupported-constexpr.patch index d3907dd074..3813326ab6 100644 --- a/upstream_utils/sleipnir_patches/0004-Remove-unsupported-constexpr.patch +++ b/upstream_utils/sleipnir_patches/0003-Remove-unsupported-constexpr.patch @@ -1,14 +1,14 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Mon, 20 May 2024 09:01:54 -0700 -Subject: [PATCH 4/4] Remove unsupported constexpr +Subject: [PATCH 3/4] Remove unsupported constexpr --- include/sleipnir/autodiff/Expression.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/include/sleipnir/autodiff/Expression.hpp b/include/sleipnir/autodiff/Expression.hpp -index 802a8602a125251058cfe22447911999f035efad..7aa715a0aa5f4560b4e16cf5f0458f1385b88c7d 100644 +index 0f306e3b2dccbe63367be7856d051d943a09f910..758433270a1cbe1b33f8b41d3e971b58de711e18 100644 --- a/include/sleipnir/autodiff/Expression.hpp +++ b/include/sleipnir/autodiff/Expression.hpp @@ -21,8 +21,8 @@ namespace sleipnir::detail { diff --git a/upstream_utils/sleipnir_patches/0004-Use-wpi-SmallVector.patch b/upstream_utils/sleipnir_patches/0004-Use-wpi-SmallVector.patch new file mode 100644 index 0000000000..9569b2e0f0 --- /dev/null +++ b/upstream_utils/sleipnir_patches/0004-Use-wpi-SmallVector.patch @@ -0,0 +1,515 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Tyler Veness +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 + #include + ++#include ++ + #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 stack; ++ wpi::SmallVector 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 + ++#include ++ + #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 stack; ++ wpi::SmallVector 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 GenerateGradientTree( ++ wpi::SmallVector GenerateGradientTree( + std::span 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 grad; ++ wpi::SmallVector 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 m_rowList; ++ wpi::SmallVector m_rowList; + + // List for updating adjoints +- small_vector m_adjointList; ++ wpi::SmallVector m_adjointList; + + // List for updating values +- small_vector m_valueList; ++ wpi::SmallVector 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 + #include ++#include + + #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 wrtVec; ++ wpi::SmallVector 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 + + #include ++#include + + #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 wrtVec; ++ wpi::SmallVector 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 m_graphs; ++ wpi::SmallVector m_graphs; + + Eigen::SparseMatrix m_J{m_variables.Rows(), m_wrt.Rows()}; + + // Cached triplets for gradients of linear rows +- small_vector> m_cachedTriplets; ++ wpi::SmallVector> m_cachedTriplets; + + // List of row indices for nonlinear rows whose graients will be computed in + // Value() +- small_vector m_nonlinearRows; ++ wpi::SmallVector 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 + + #include ++#include + + #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 m_storage; ++ wpi::SmallVector 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 + #include + ++#include ++ + #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 + (ScalarLike> || MatrixLike>) && + (!std::same_as, double> || + !std::same_as, double>) +-small_vector MakeConstraints(LHS&& lhs, RHS&& rhs) { +- small_vector constraints; ++wpi::SmallVector MakeConstraints(LHS&& lhs, RHS&& rhs) { ++ wpi::SmallVector constraints; + + if constexpr (ScalarLike> && + ScalarLike>) { +@@ -119,7 +120,7 @@ small_vector MakeConstraints(LHS&& lhs, RHS&& rhs) { + */ + struct SLEIPNIR_DLLEXPORT EqualityConstraints { + /// A vector of scalar equality constraints. +- small_vector constraints; ++ wpi::SmallVector 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 constraints; ++ wpi::SmallVector 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 + #include + ++#include ++ + #include "sleipnir/optimization/SolverStatus.hpp" + #include "sleipnir/util/FunctionRef.hpp" +-#include "sleipnir/util/small_vector.hpp" + + namespace sleipnir { + +@@ -43,14 +44,14 @@ MultistartResult Multistart( + function_ref(const DecisionVariables&)> + solve, + std::span initialGuesses) { +- small_vector>> futures; ++ wpi::SmallVector>> futures; + futures.reserve(initialGuesses.size()); + + for (const auto& initialGuess : initialGuesses) { + futures.emplace_back(std::async(std::launch::async, solve, initialGuess)); + } + +- small_vector> results; ++ wpi::SmallVector> 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 + + #include ++#include + + #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 m_decisionVariables; ++ wpi::SmallVector m_decisionVariables; + + // The cost function: f(x) + std::optional m_f; + + // The list of equality constraints: cₑ(x) = 0 +- small_vector m_equalityConstraints; ++ wpi::SmallVector m_equalityConstraints; + + // The list of inequality constraints: cᵢ(x) ≥ 0 +- small_vector m_inequalityConstraints; ++ wpi::SmallVector m_inequalityConstraints; + + // The user callback + std::function 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 + #include + ++#include ++ + #include "sleipnir/util/SymbolExports.hpp" +-#include "sleipnir/util/small_vector.hpp" + + namespace sleipnir { + +@@ -76,8 +77,8 @@ class SLEIPNIR_DLLEXPORT PoolResource { + } + + private: +- small_vector> m_buffer; +- small_vector m_freeList; ++ wpi::SmallVector> m_buffer; ++ wpi::SmallVector 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 + + #include ++#include + + #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 cells; ++ wpi::SmallVector 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 + + #include ++#include + + #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 decisionVariables, + }; + + // Kept outside the loop so its storage can be reused +- small_vector> triplets; ++ wpi::SmallVector> 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 + + #include ++#include + + #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 fr_decisionVariables; ++ wpi::SmallVector 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 fr_equalityConstraints; ++ wpi::SmallVector 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 fr_inequalityConstraints; ++ wpi::SmallVector 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 + + #include ++#include + + #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 m_filter; ++ wpi::SmallVector m_filter; + }; + + } // namespace sleipnir diff --git a/upstream_utils/update_sleipnir.py b/upstream_utils/update_sleipnir.py index 942d1032e9..b6c8adda4a 100755 --- a/upstream_utils/update_sleipnir.py +++ b/upstream_utils/update_sleipnir.py @@ -15,8 +15,8 @@ from upstream_utils import ( def main(): upstream_root = clone_repo( "https://github.com/SleipnirGroup/Sleipnir", - # main on 2024-06-05 - "57005c8b740ab163d3f7ef7e0c7c7c8774f4f925", + # main on 2024-06-19 + "d5bf25acc8a28f10d7f9e85ef89b31eb0e916f6b", shallow=False, ) wpilib_root = get_repo_root() @@ -26,9 +26,9 @@ def main(): os.chdir(upstream_root) for f in [ "0001-Remove-using-enum-declarations.patch", - "0002-Add-implicit-typename.patch", - "0003-Use-fmtlib.patch", - "0004-Remove-unsupported-constexpr.patch", + "0002-Use-fmtlib.patch", + "0003-Remove-unsupported-constexpr.patch", + "0004-Use-wpi-SmallVector.patch", ]: git_am(os.path.join(wpilib_root, "upstream_utils/sleipnir_patches", f)) @@ -48,7 +48,10 @@ def main(): # Copy Sleipnir header files into allwpilib include_files = [ - os.path.join(dp, f) for dp, dn, fn in os.walk("include") for f in fn + os.path.join(dp, f) + for dp, dn, fn in os.walk("include") + for f in fn + if not f.endswith("small_vector.hpp") ] include_files = copy_to( include_files, os.path.join(wpimath, "src/main/native/thirdparty/sleipnir") diff --git a/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp b/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp index 3c677c0267..57a7f45ed4 100644 --- a/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp +++ b/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp @@ -74,14 +74,12 @@ units::volt_t ArmFeedforward::Calculate(units::unit_t currentAngle, double trial_x = x + α * p_x; xAD.SetValue(trial_x); - cost.Update(); while (cost.Value() > oldCost) { α *= 0.5; trial_x = x + α * p_x; xAD.SetValue(trial_x); - cost.Update(); } x = trial_x; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/.styleguide b/wpimath/src/main/native/thirdparty/sleipnir/include/.styleguide index 8251a49067..efa36cee1f 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/.styleguide +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/.styleguide @@ -6,7 +6,12 @@ cppSrcFileInclude { \.cpp$ } +licenseUpdateExclude { + include/sleipnir/util/small_vector\.hpp$ +} + includeOtherLibs { ^Eigen/ ^fmt/ + ^wpi/ } diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Expression.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Expression.hpp index 7aa715a0aa..f9be904cf2 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Expression.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Expression.hpp @@ -11,10 +11,11 @@ #include #include +#include + #include "sleipnir/autodiff/ExpressionType.hpp" #include "sleipnir/util/IntrusiveSharedPtr.hpp" #include "sleipnir/util/Pool.hpp" -#include "sleipnir/util/SmallVector.hpp" #include "sleipnir/util/SymbolExports.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 stack; + wpi::SmallVector stack; stack.emplace_back(expr); while (!stack.empty()) { diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/ExpressionGraph.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/ExpressionGraph.hpp index d338cfe3ab..4683089037 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/ExpressionGraph.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/ExpressionGraph.hpp @@ -4,9 +4,10 @@ #include +#include + #include "sleipnir/autodiff/Expression.hpp" #include "sleipnir/util/FunctionRef.hpp" -#include "sleipnir/util/SmallVector.hpp" #include "sleipnir/util/SymbolExports.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 stack; + wpi::SmallVector 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 GenerateGradientTree( + wpi::SmallVector GenerateGradientTree( std::span 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 grad; + wpi::SmallVector 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 m_rowList; + wpi::SmallVector m_rowList; // List for updating adjoints - small_vector m_adjointList; + wpi::SmallVector m_adjointList; // List for updating values - small_vector m_valueList; + wpi::SmallVector m_valueList; }; } // namespace sleipnir::detail diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Gradient.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Gradient.hpp index 6436726af1..cf6a4171e7 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Gradient.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Gradient.hpp @@ -59,11 +59,6 @@ class SLEIPNIR_DLLEXPORT Gradient { return m_g; } - /** - * Updates the value of the variable. - */ - void Update() { m_jacobian.Update(); } - /** * Returns the profiler. */ diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Hessian.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Hessian.hpp index b964737d60..2e60d89e95 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Hessian.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Hessian.hpp @@ -6,13 +6,13 @@ #include #include +#include #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/SmallVector.hpp" #include "sleipnir/util/SymbolExports.hpp" namespace sleipnir { @@ -36,7 +36,7 @@ class SLEIPNIR_DLLEXPORT Hessian { Hessian(Variable variable, const VariableMatrix& wrt) noexcept : m_jacobian{ [&] { - small_vector wrtVec; + wpi::SmallVector wrtVec; wrtVec.reserve(wrt.size()); for (auto& elem : wrt) { wrtVec.emplace_back(elem.expr); @@ -67,11 +67,6 @@ class SLEIPNIR_DLLEXPORT Hessian { */ const Eigen::SparseMatrix& Value() { return m_jacobian.Value(); } - /** - * Updates the values of the gradient tree. - */ - void Update() { m_jacobian.Update(); } - /** * Returns the profiler. */ diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Jacobian.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Jacobian.hpp index 4d85c2f407..0c660156c8 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Jacobian.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Jacobian.hpp @@ -5,12 +5,12 @@ #include #include +#include #include "sleipnir/autodiff/ExpressionGraph.hpp" #include "sleipnir/autodiff/Profiler.hpp" #include "sleipnir/autodiff/Variable.hpp" #include "sleipnir/autodiff/VariableMatrix.hpp" -#include "sleipnir/util/SmallVector.hpp" #include "sleipnir/util/SymbolExports.hpp" namespace sleipnir { @@ -81,7 +81,7 @@ class SLEIPNIR_DLLEXPORT Jacobian { VariableMatrix Get() const { VariableMatrix result{m_variables.Rows(), m_wrt.Rows()}; - small_vector wrtVec; + wpi::SmallVector wrtVec; wrtVec.reserve(m_wrt.size()); for (auto& elem : m_wrt) { wrtVec.emplace_back(elem.expr); @@ -107,7 +107,9 @@ class SLEIPNIR_DLLEXPORT Jacobian { m_profiler.StartSolve(); - Update(); + for (auto& graph : m_graphs) { + graph.Update(); + } // Copy the cached triplets so triplets added for the nonlinear rows are // thrown away at the end of the function @@ -127,15 +129,6 @@ class SLEIPNIR_DLLEXPORT Jacobian { return m_J; } - /** - * Updates the values of the variables. - */ - void Update() { - for (auto& graph : m_graphs) { - graph.Update(); - } - } - /** * Returns the profiler. */ @@ -145,16 +138,16 @@ class SLEIPNIR_DLLEXPORT Jacobian { VariableMatrix m_variables; VariableMatrix m_wrt; - small_vector m_graphs; + wpi::SmallVector m_graphs; Eigen::SparseMatrix m_J{m_variables.Rows(), m_wrt.Rows()}; // Cached triplets for gradients of linear rows - small_vector> m_cachedTriplets; + wpi::SmallVector> m_cachedTriplets; // List of row indices for nonlinear rows whose graients will be computed in // Value() - small_vector m_nonlinearRows; + wpi::SmallVector m_nonlinearRows; Profiler m_profiler; }; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Variable.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Variable.hpp index 6226556d32..694817f90d 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Variable.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Variable.hpp @@ -224,7 +224,13 @@ class SLEIPNIR_DLLEXPORT Variable { /** * Returns the value of this variable. */ - double Value() const { return expr->value; } + double Value() { + // Updates the value of this variable based on the values of its dependent + // variables + detail::ExpressionGraph{expr}.Update(); + + return expr->value; + } /** * Returns the type of this expression (constant, linear, quadratic, or @@ -232,16 +238,6 @@ class SLEIPNIR_DLLEXPORT Variable { */ 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 = diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableBlock.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableBlock.hpp index 3e06601aa4..faac4219de 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableBlock.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableBlock.hpp @@ -218,9 +218,9 @@ class VariableBlock { * @param row The scalar subblock's row. * @param col The scalar subblock's column. */ - template - requires(!std::is_const_v) - Variable& operator()(int row, int col) { + Variable& operator()(int row, int col) + requires(!std::is_const_v) + { Assert(row >= 0 && row < Rows()); Assert(col >= 0 && col < Cols()); return (*m_mat)(m_rowOffset + row, m_colOffset + col); @@ -243,9 +243,9 @@ class VariableBlock { * * @param row The scalar subblock's row. */ - template - requires(!std::is_const_v) - Variable& operator()(int row) { + Variable& operator()(int row) + requires(!std::is_const_v) + { Assert(row >= 0 && row < Rows() * Cols()); return (*this)(row / Cols(), row % Cols()); } @@ -468,7 +468,7 @@ class VariableBlock { * @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 { + double Value(int row, int col) { Assert(row >= 0 && row < Rows()); Assert(col >= 0 && col < Cols()); return (*m_mat)(m_rowOffset + row, m_colOffset + col).Value(); @@ -479,7 +479,7 @@ class VariableBlock { * * @param index The index of the element to return. */ - double Value(int index) const { + double Value(int index) { Assert(index >= 0 && index < Rows() * Cols()); return (*m_mat)(m_rowOffset + index / m_blockCols, m_colOffset + index % m_blockCols) @@ -489,7 +489,7 @@ class VariableBlock { /** * Returns the contents of the variable matrix. */ - Eigen::MatrixXd Value() const { + Eigen::MatrixXd Value() { Eigen::MatrixXd result{Rows(), Cols()}; for (int row = 0; row < Rows(); ++row) { diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableMatrix.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableMatrix.hpp index ecd7a82d07..e691e209af 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableMatrix.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableMatrix.hpp @@ -11,6 +11,7 @@ #include #include +#include #include "sleipnir/autodiff/Variable.hpp" #include "sleipnir/autodiff/VariableBlock.hpp" @@ -700,7 +701,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { * @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 { + double Value(int row, int col) { Assert(row >= 0 && row < Rows()); Assert(col >= 0 && col < Cols()); return m_storage[row * Cols() + col].Value(); @@ -711,7 +712,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { * * @param index The index of the element to return. */ - double Value(int index) const { + double Value(int index) { Assert(index >= 0 && index < Rows() * Cols()); return m_storage[index].Value(); } @@ -719,7 +720,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { /** * Returns the contents of the variable matrix. */ - Eigen::MatrixXd Value() const { + Eigen::MatrixXd Value() { Eigen::MatrixXd result{Rows(), Cols()}; for (int row = 0; row < Rows(); ++row) { @@ -883,7 +884,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix { } private: - std::vector m_storage; + wpi::SmallVector m_storage; int m_rows = 0; int m_cols = 0; }; @@ -1020,4 +1021,14 @@ SLEIPNIR_DLLEXPORT inline VariableMatrix Block( return result; } +/** + * Solves the VariableMatrix equation AX = B for X. + * + * @param A The left-hand side. + * @param B The right-hand side. + * @return The solution X. + */ +SLEIPNIR_DLLEXPORT VariableMatrix Solve(const VariableMatrix& A, + const VariableMatrix& B); + } // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/OCPSolver.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/OCPSolver.hpp index ef0243f46e..a3a8334129 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/OCPSolver.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/OCPSolver.hpp @@ -325,26 +325,33 @@ class SLEIPNIR_DLLEXPORT OCPSolver : public OptimizationProblem { Variable time = 0.0; + // Derivation at https://mec560sbu.github.io/2016/09/30/direct_collocation/ for (int i = 0; i < m_numSteps; ++i) { + Variable h = DT()(0, i); + + auto& f = m_dynamicsFunction; + + auto t_begin = time; + auto t_end = t_begin + h; + 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; + auto u_end = U().Col(i + 1); - time += dt; + auto xdot_begin = f(t_begin, x_begin, u_begin, h); + auto xdot_end = f(t_end, x_end, u_end, h); + auto xdot_c = + -3 / (2 * h) * (x_begin - x_end) - 0.25 * (xdot_begin + xdot_end); - // 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); + auto t_c = t_begin + 0.5 * h; + auto x_c = 0.5 * (x_begin + x_end) + h / 8 * (xdot_begin - xdot_end); + auto u_c = 0.5 * (u_begin + u_end); + + SubjectTo(xdot_c == f(t_c, x_c, u_c, h)); + + time += h; } } diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Constraints.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Constraints.hpp index 61ed70b483..a33039b899 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Constraints.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Constraints.hpp @@ -4,11 +4,13 @@ #include #include +#include + +#include #include "sleipnir/autodiff/Variable.hpp" #include "sleipnir/util/Assert.hpp" #include "sleipnir/util/Concepts.hpp" -#include "sleipnir/util/SmallVector.hpp" #include "sleipnir/util/SymbolExports.hpp" namespace sleipnir { @@ -25,18 +27,21 @@ namespace sleipnir { * @param rhs Right-hand side. */ template - requires(ScalarLike || MatrixLike) && - (ScalarLike || MatrixLike) && - (!std::same_as || !std::same_as) -small_vector MakeConstraints(const LHS& lhs, const RHS& rhs) { - small_vector constraints; + requires(ScalarLike> || MatrixLike>) && + (ScalarLike> || MatrixLike>) && + (!std::same_as, double> || + !std::same_as, double>) +wpi::SmallVector MakeConstraints(LHS&& lhs, RHS&& rhs) { + wpi::SmallVector constraints; - if constexpr (ScalarLike && ScalarLike) { + if constexpr (ScalarLike> && + ScalarLike>) { constraints.emplace_back(lhs - rhs); - } else if constexpr (ScalarLike && MatrixLike) { + } else if constexpr (ScalarLike> && + MatrixLike>) { int rows; int cols; - if constexpr (EigenMatrixLike) { + if constexpr (EigenMatrixLike>) { rows = rhs.rows(); cols = rhs.cols(); } else { @@ -52,10 +57,11 @@ small_vector MakeConstraints(const LHS& lhs, const RHS& rhs) { constraints.emplace_back(lhs - rhs(row, col)); } } - } else if constexpr (MatrixLike && ScalarLike) { + } else if constexpr (MatrixLike> && + ScalarLike>) { int rows; int cols; - if constexpr (EigenMatrixLike) { + if constexpr (EigenMatrixLike>) { rows = lhs.rows(); cols = lhs.cols(); } else { @@ -71,10 +77,11 @@ small_vector MakeConstraints(const LHS& lhs, const RHS& rhs) { constraints.emplace_back(lhs(row, col) - rhs); } } - } else if constexpr (MatrixLike && MatrixLike) { + } else if constexpr (MatrixLike> && + MatrixLike>) { int lhsRows; int lhsCols; - if constexpr (EigenMatrixLike) { + if constexpr (EigenMatrixLike>) { lhsRows = lhs.rows(); lhsCols = lhs.cols(); } else { @@ -86,7 +93,7 @@ small_vector MakeConstraints(const LHS& lhs, const RHS& rhs) { int rhsRows; [[maybe_unused]] int rhsCols; - if constexpr (EigenMatrixLike) { + if constexpr (EigenMatrixLike>) { rhsRows = rhs.rows(); rhsCols = rhs.cols(); } else { @@ -113,7 +120,7 @@ small_vector MakeConstraints(const LHS& lhs, const RHS& rhs) { */ struct SLEIPNIR_DLLEXPORT EqualityConstraints { /// A vector of scalar equality constraints. - small_vector constraints; + wpi::SmallVector constraints; /** * Constructs an equality constraint from a left and right side. @@ -125,19 +132,20 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints { * @param rhs Right-hand side. */ template - requires(ScalarLike || MatrixLike) && - (ScalarLike || MatrixLike) && - (!std::same_as || !std::same_as) - EqualityConstraints(const LHS& lhs, const RHS& rhs) + requires(ScalarLike> || MatrixLike>) && + (ScalarLike> || MatrixLike>) && + (!std::same_as, double> || + !std::same_as, double>) + EqualityConstraints(LHS&& lhs, RHS&& rhs) : constraints{MakeConstraints(lhs, rhs)} {} /** * Implicit conversion operator to bool. */ - operator bool() const { // NOLINT + operator bool() { // NOLINT return std::all_of( constraints.begin(), constraints.end(), - [](const auto& constraint) { return constraint.Value() == 0.0; }); + [](auto& constraint) { return constraint.Value() == 0.0; }); } }; @@ -146,7 +154,7 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints { */ struct SLEIPNIR_DLLEXPORT InequalityConstraints { /// A vector of scalar inequality constraints. - small_vector constraints; + wpi::SmallVector constraints; /** * Constructs an inequality constraint from a left and right side. @@ -158,19 +166,20 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints { * @param rhs Right-hand side. */ template - requires(ScalarLike || MatrixLike) && - (ScalarLike || MatrixLike) && - (!std::same_as || !std::same_as) - InequalityConstraints(const LHS& lhs, const RHS& rhs) + requires(ScalarLike> || MatrixLike>) && + (ScalarLike> || MatrixLike>) && + (!std::same_as, double> || + !std::same_as, double>) + InequalityConstraints(LHS&& lhs, RHS&& rhs) : constraints{MakeConstraints(lhs, rhs)} {} /** * Implicit conversion operator to bool. */ - operator bool() const { // NOLINT + operator bool() { // NOLINT return std::all_of( constraints.begin(), constraints.end(), - [](const auto& constraint) { return constraint.Value() >= 0.0; }); + [](auto& constraint) { return constraint.Value() >= 0.0; }); } }; @@ -181,10 +190,11 @@ struct SLEIPNIR_DLLEXPORT InequalityConstraints { * @param rhs Left-hand side. */ template - requires(ScalarLike || MatrixLike) && - (ScalarLike || MatrixLike) && - (!std::same_as || !std::same_as) -EqualityConstraints operator==(const LHS& lhs, const RHS& rhs) { + requires(ScalarLike> || MatrixLike>) && + (ScalarLike> || MatrixLike>) && + (!std::same_as, double> || + !std::same_as, double>) +EqualityConstraints operator==(LHS&& lhs, RHS&& rhs) { return EqualityConstraints{lhs, rhs}; } @@ -196,10 +206,11 @@ EqualityConstraints operator==(const LHS& lhs, const RHS& rhs) { * @param rhs Left-hand side. */ template - requires(ScalarLike || MatrixLike) && - (ScalarLike || MatrixLike) && - (!std::same_as || !std::same_as) -InequalityConstraints operator<(const LHS& lhs, const RHS& rhs) { + requires(ScalarLike> || MatrixLike>) && + (ScalarLike> || MatrixLike>) && + (!std::same_as, double> || + !std::same_as, double>) +InequalityConstraints operator<(LHS&& lhs, RHS&& rhs) { return rhs >= lhs; } @@ -211,10 +222,11 @@ InequalityConstraints operator<(const LHS& lhs, const RHS& rhs) { * @param rhs Left-hand side. */ template - requires(ScalarLike || MatrixLike) && - (ScalarLike || MatrixLike) && - (!std::same_as || !std::same_as) -InequalityConstraints operator<=(const LHS& lhs, const RHS& rhs) { + requires(ScalarLike> || MatrixLike>) && + (ScalarLike> || MatrixLike>) && + (!std::same_as, double> || + !std::same_as, double>) +InequalityConstraints operator<=(LHS&& lhs, RHS&& rhs) { return rhs >= lhs; } @@ -226,10 +238,11 @@ InequalityConstraints operator<=(const LHS& lhs, const RHS& rhs) { * @param rhs Left-hand side. */ template - requires(ScalarLike || MatrixLike) && - (ScalarLike || MatrixLike) && - (!std::same_as || !std::same_as) -InequalityConstraints operator>(const LHS& lhs, const RHS& rhs) { + requires(ScalarLike> || MatrixLike>) && + (ScalarLike> || MatrixLike>) && + (!std::same_as, double> || + !std::same_as, double>) +InequalityConstraints operator>(LHS&& lhs, RHS&& rhs) { return lhs >= rhs; } @@ -241,10 +254,11 @@ InequalityConstraints operator>(const LHS& lhs, const RHS& rhs) { * @param rhs Left-hand side. */ template - requires(ScalarLike || MatrixLike) && - (ScalarLike || MatrixLike) && - (!std::same_as || !std::same_as) -InequalityConstraints operator>=(const LHS& lhs, const RHS& rhs) { + requires(ScalarLike> || MatrixLike>) && + (ScalarLike> || MatrixLike>) && + (!std::same_as, double> || + !std::same_as, double>) +InequalityConstraints operator>=(LHS&& lhs, RHS&& rhs) { return InequalityConstraints{lhs, rhs}; } diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Multistart.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Multistart.hpp index a873062fc9..6935590a75 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Multistart.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Multistart.hpp @@ -6,9 +6,10 @@ #include #include +#include + #include "sleipnir/optimization/SolverStatus.hpp" #include "sleipnir/util/FunctionRef.hpp" -#include "sleipnir/util/SmallVector.hpp" namespace sleipnir { @@ -43,14 +44,14 @@ MultistartResult Multistart( function_ref(const DecisionVariables&)> solve, std::span initialGuesses) { - small_vector>> futures; + wpi::SmallVector>> futures; futures.reserve(initialGuesses.size()); for (const auto& initialGuess : initialGuesses) { futures.emplace_back(std::async(std::launch::async, solve, initialGuess)); } - small_vector> results; + wpi::SmallVector> results; results.reserve(futures.size()); for (auto& future : futures) { diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp index 237266ac6d..7dc4c50b03 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp @@ -12,6 +12,7 @@ #include #include +#include #include "sleipnir/autodiff/Variable.hpp" #include "sleipnir/autodiff/VariableMatrix.hpp" @@ -22,7 +23,6 @@ #include "sleipnir/optimization/SolverStatus.hpp" #include "sleipnir/optimization/solver/InteriorPoint.hpp" #include "sleipnir/util/Print.hpp" -#include "sleipnir/util/SmallVector.hpp" #include "sleipnir/util/SymbolExports.hpp" namespace sleipnir { @@ -46,185 +46,14 @@ subject to cₑ(x) = 0 * * 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 - * #include - * - * 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(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{{0.0}, {0.0}}); - * problem.SubjectTo( - * X.Col(N + 1) == Eigen::Matrix{{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: - * - *
    - *
  • none (i.e., there is no cost function or are no constraints)
    • - *
  • constant
    • - *
  • linear
    • - *
  • quadratic
    • - *
  • nonlinear
    • - *
- * - * 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: - * - *
    - *
  • the Hessian of a quadratic or lower cost function
    • - *
  • the Jacobian of linear or lower constraints
    • - *
- * - * A problem is constant if: - * - *
    - *
  • the cost function is constant or lower
    • - *
  • the equality constraints are constant or lower
    • - *
  • the inequality constraints are constant or lower
    • - *
- * - * A problem is linear if: - * - *
    - *
  • the cost function is linear
    • - *
  • the equality constraints are linear or lower
    • - *
  • the inequality constraints are linear or lower
    • - *
- * - * A problem is quadratic if: - * - *
    - *
  • the cost function is quadratic
    • - *
  • the equality constraints are linear or lower
    • - *
  • the inequality constraints are linear or lower
    • - *
- * - * All other problems are nonlinear. + * and it'll be converted for them. */ class SLEIPNIR_DLLEXPORT OptimizationProblem { public: /** * Construct the optimization problem. */ - OptimizationProblem() noexcept { - m_decisionVariables.reserve(1024); - m_equalityConstraints.reserve(1024); - m_inequalityConstraints.reserve(1024); - } + OptimizationProblem() noexcept = default; /** * Create a decision variable in the optimization problem. @@ -531,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 m_decisionVariables; + wpi::SmallVector m_decisionVariables; // The cost function: f(x) std::optional m_f; // The list of equality constraints: cₑ(x) = 0 - small_vector m_equalityConstraints; + wpi::SmallVector m_equalityConstraints; // The list of inequality constraints: cᵢ(x) ≥ 0 - small_vector m_inequalityConstraints; + wpi::SmallVector m_inequalityConstraints; // The user callback std::function m_callback = diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Assert.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Assert.hpp index 9999544278..ba381ef8f4 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Assert.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Assert.hpp @@ -3,9 +3,8 @@ #pragma once #ifdef JORMUNGANDR +#include #include - -#include /** * Throw an exception in Python. */ @@ -13,7 +12,7 @@ do { \ if (!(condition)) { \ throw std::invalid_argument( \ - fmt::format("{}:{}: {}: Assertion `{}' failed.", __FILE__, __LINE__, \ + std::format("{}:{}: {}: Assertion `{}' failed.", __FILE__, __LINE__, \ __func__, #condition)); \ } \ } while (0); diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Pool.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Pool.hpp index 87d093cec9..95b4d2e97e 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Pool.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Pool.hpp @@ -5,7 +5,8 @@ #include #include -#include "sleipnir/util/SmallVector.hpp" +#include + #include "sleipnir/util/SymbolExports.hpp" namespace sleipnir { @@ -76,8 +77,8 @@ class SLEIPNIR_DLLEXPORT PoolResource { } private: - small_vector> m_buffer; - small_vector m_freeList; + wpi::SmallVector> m_buffer; + wpi::SmallVector m_freeList; size_t blocksPerChunk; /** diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/SmallVector.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/SmallVector.hpp deleted file mode 100644 index dcf02c02b7..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/SmallVector.hpp +++ /dev/null @@ -1,163 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include -#include -#include -#include -#include -#include - -namespace sleipnir { - -template -struct small_buffer_vector_allocator { - alignas(alignof(T)) std::byte m_smallBuffer[MaxSize * sizeof(T)]; - std::allocator m_alloc; - bool m_smallBufferUsed = false; - - using value_type = T; - // we have to set this three values, as they are responsible for the correct - // handling of the move assignment operator - using propagate_on_container_move_assignment = std::false_type; - using propagate_on_container_swap = std::false_type; - using is_always_equal = std::false_type; - - constexpr small_buffer_vector_allocator() noexcept = default; - - template - constexpr small_buffer_vector_allocator( // NOLINT - const small_buffer_vector_allocator&) noexcept {} - - template - struct rebind { - using other = small_buffer_vector_allocator; - }; - - // don't copy the small buffer for the copy/move constructors, as the copying - // is done through the vector - constexpr small_buffer_vector_allocator( - const small_buffer_vector_allocator& other) noexcept - : m_smallBufferUsed(other.m_smallBufferUsed) {} - - constexpr small_buffer_vector_allocator& operator=( - const small_buffer_vector_allocator& other) noexcept { - if (this == &other) { - return *this; - } - - m_smallBufferUsed = other.m_smallBufferUsed; - return *this; - } - - constexpr small_buffer_vector_allocator( - small_buffer_vector_allocator&&) noexcept {} - - constexpr small_buffer_vector_allocator& operator=( - const small_buffer_vector_allocator&&) noexcept { - return *this; - } - - [[nodiscard]] - constexpr T* allocate(const size_t n) { - // when the allocator was rebound we don't want to use the small buffer - if constexpr (std::is_same_v) { - if (n <= MaxSize) { - m_smallBufferUsed = true; - // as long as we use less memory than the small buffer, we return a - // pointer to it - return reinterpret_cast(&m_smallBuffer); - } - } - m_smallBufferUsed = false; - // otherwise use the default allocator - return m_alloc.allocate(n); - } - - constexpr void deallocate(void* p, const size_t n) { - // we don't deallocate anything if the memory was allocated in small buffer - if (&m_smallBuffer != p) { - m_alloc.deallocate(static_cast(p), n); - } - m_smallBufferUsed = false; - } - - // according to the C++ standard when propagate_on_container_move_assignment - // is set to false, the comparision operators are used to check if two - // allocators are equal. When they are not, an element wise move is done - // instead of just taking over the memory. For our implementation this means - // the comparision has to return false, when the small buffer is active - friend constexpr bool operator==(const small_buffer_vector_allocator& lhs, - const small_buffer_vector_allocator& rhs) { - return !lhs.m_smallBufferUsed && !rhs.m_smallBufferUsed; - } - - friend constexpr bool operator!=(const small_buffer_vector_allocator& lhs, - const small_buffer_vector_allocator& rhs) { - return !(lhs == rhs); - } -}; - -template -class small_vector - : public std::vector> { - public: - using vectorT = std::vector>; - - // default initialize with the small buffer size - constexpr small_vector() noexcept { vectorT::reserve(N); } - - small_vector(const small_vector&) = default; - small_vector& operator=(const small_vector&) = default; - - small_vector(small_vector&& other) noexcept( - std::is_nothrow_move_constructible_v) { - if (other.size() <= N) { - vectorT::reserve(N); - } - vectorT::operator=(std::move(other)); - } - - small_vector& operator=(small_vector&& other) noexcept( - std::is_nothrow_move_constructible_v) { - if (other.size() <= N) { - vectorT::reserve(N); - } - vectorT::operator=(std::move(other)); - return *this; - } - - // use the default constructor first to reserve then construct the values - explicit small_vector(size_t count) : small_vector() { - vectorT::resize(count); - } - - small_vector(size_t count, const T& value) : small_vector() { - vectorT::assign(count, value); - } - - template - small_vector(InputIt first, InputIt last) : small_vector() { - vectorT::insert(vectorT::begin(), first, last); - } - - small_vector(std::initializer_list init) : small_vector() { // NOLINT - vectorT::insert(vectorT::begin(), init); - } - - friend void swap(small_vector& a, small_vector& b) noexcept { - std::swap(static_cast(a), static_cast(b)); - } -}; - -template -constexpr typename small_vector::size_type erase_if(small_vector& c, - Pred pred) { - auto it = std::remove_if(c.begin(), c.end(), pred); - auto r = c.end() - it; - c.erase(it, c.end()); - return r; -} - -} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Spy.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Spy.hpp index 67b0b0736c..7f526a2d99 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Spy.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Spy.hpp @@ -7,8 +7,8 @@ #include #include +#include -#include "sleipnir/util/SmallVector.hpp" #include "sleipnir/util/SymbolExports.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 cells; + wpi::SmallVector cells; // Allocate space for matrix of characters plus trailing newlines cells.reserve(cells_width * cells_height); diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/.styleguide b/wpimath/src/main/native/thirdparty/sleipnir/src/.styleguide index f3b2f0cf9e..ad739cea6d 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/.styleguide +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/.styleguide @@ -8,4 +8,5 @@ cppSrcFileInclude { includeOtherLibs { ^Eigen/ + ^wpi/ } diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/VariableMatrix.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/VariableMatrix.cpp new file mode 100644 index 0000000000..1917734ef8 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/VariableMatrix.cpp @@ -0,0 +1,78 @@ +// Copyright (c) Sleipnir contributors + +#include "sleipnir/autodiff/VariableMatrix.hpp" + +#include + +namespace Eigen { + +template <> +struct NumTraits : NumTraits { + using Real = sleipnir::Variable; + using NonInteger = sleipnir::Variable; + using Nested = sleipnir::Variable; + + enum { + IsComplex = 0, + IsInteger = 0, + IsSigned = 1, + RequireInitialization = 1, + ReadCost = 1, + AddCost = 3, + MulCost = 3 + }; +}; + +} // namespace Eigen + +// For Variable equality operator +#include "sleipnir/optimization/Constraints.hpp" + +namespace sleipnir { + +VariableMatrix Solve(const VariableMatrix& A, const VariableMatrix& B) { + // m x n * n x p = m x p + Assert(A.Rows() == B.Rows()); + + if (A.Rows() == 2 && A.Cols() == 2) { + // Compute optimal inverse instead of using Eigen's general solver + sleipnir::VariableMatrix Ainv{2, 2}; + Ainv(0, 0) = A(1, 1); + Ainv(0, 1) = -A(0, 1); + Ainv(1, 0) = -A(1, 0); + Ainv(1, 1) = A(0, 0); + auto detA = A(0, 0) * A(1, 1) - A(0, 1) * A(1, 0); + Ainv /= detA; + + return Ainv * B; + } else { + using MatrixXvar = Eigen::Matrix; + + MatrixXvar eigenA{A.Rows(), A.Cols()}; + for (int row = 0; row < A.Rows(); ++row) { + for (int col = 0; col < A.Cols(); ++col) { + eigenA(row, col) = A(row, col); + } + } + + MatrixXvar eigenB{B.Rows(), B.Cols()}; + for (int row = 0; row < B.Rows(); ++row) { + for (int col = 0; col < B.Cols(); ++col) { + eigenB(row, col) = B(row, col); + } + } + + MatrixXvar eigenX = eigenA.householderQr().solve(eigenB); + + VariableMatrix X{A.Cols(), B.Cols()}; + for (int row = 0; row < X.Rows(); ++row) { + for (int col = 0; col < X.Cols(); ++col) { + X(row, col) = eigenX(row, col); + } + } + + return X; + } +} + +} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp index 596988bf45..06187b7260 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp @@ -7,9 +7,9 @@ #include #include #include -#include #include +#include #include "optimization/RegularizedLDLT.hpp" #include "optimization/solver/util/ErrorEstimate.hpp" @@ -226,7 +226,7 @@ void InteriorPoint(std::span decisionVariables, }; // Kept outside the loop so its storage can be reused - std::vector> triplets; + wpi::SmallVector> triplets; RegularizedLDLT solver; @@ -406,16 +406,7 @@ void InteriorPoint(std::span decisionVariables, 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 @@ -499,16 +490,7 @@ void InteriorPoint(std::span decisionVariables, 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 @@ -571,14 +553,7 @@ void InteriorPoint(std::span decisionVariables, 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(), @@ -617,22 +592,9 @@ void InteriorPoint(std::span decisionVariables, 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 @@ -789,16 +751,7 @@ void InteriorPoint(std::span decisionVariables, 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 diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FeasibilityRestoration.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FeasibilityRestoration.hpp index 776bf8a2a9..76af452517 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FeasibilityRestoration.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FeasibilityRestoration.hpp @@ -8,6 +8,7 @@ #include #include +#include #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/SmallVector.hpp" namespace sleipnir { @@ -66,7 +66,7 @@ inline void FeasibilityRestoration( constexpr double ρ = 1000.0; - small_vector fr_decisionVariables; + wpi::SmallVector 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 fr_equalityConstraints; + wpi::SmallVector 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 fr_inequalityConstraints; + wpi::SmallVector fr_inequalityConstraints; fr_inequalityConstraints.assign(inequalityConstraints.begin(), inequalityConstraints.end()); for (size_t row = 0; row < fr_inequalityConstraints.size(); ++row) { diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/Filter.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/Filter.hpp index ee5c7c3f57..02bdb5a8db 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/Filter.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/Filter.hpp @@ -8,9 +8,9 @@ #include #include +#include #include "sleipnir/autodiff/Variable.hpp" -#include "sleipnir/util/SmallVector.hpp" namespace sleipnir { @@ -44,7 +44,7 @@ struct FilterEntry { * @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, + FilterEntry(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>()} {} @@ -182,7 +182,7 @@ class Filter { private: Variable* m_f = nullptr; double m_μ = 0.0; - small_vector m_filter; + wpi::SmallVector m_filter; }; } // namespace sleipnir