Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[upstream_utils] Upgrade Sleipnir and use wpi::SmallVector (#6748)

Browse Source
This commit is contained in:
Tyler Veness
2024-06-21 11:14:19 -07:00
committed by GitHub
parent e2893fc1a3
commit 25865759f4
29 changed files with 780 additions and 596 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
wpimath/src/main/native/thirdparty/sleipnir/include/.styleguide vendored
View File

@@ -6,7 +6,12 @@ cppSrcFileInclude {
\.cpp$
}
licenseUpdateExclude {
include/sleipnir/util/small_vector\.hpp$
}
includeOtherLibs {
^Eigen/
^fmt/
^wpi/
}

5
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Expression.hpp vendored
View File

@@ -11,10 +11,11 @@
#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/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<Expression*> stack;
wpi::SmallVector<Expression*> stack;
stack.emplace_back(expr);
while (!stack.empty()) {

15
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/ExpressionGraph.hpp vendored
View File

@@ -4,9 +4,10 @@
#include <span>
#include <wpi/SmallVector.h>
#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<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

5
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Gradient.hpp vendored
View File

@@ -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.
*/

9
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Hessian.hpp vendored
View File

@@ -6,13 +6,13 @@
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <wpi/SmallVector.h>
#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<detail::ExpressionPtr> wrtVec;
wpi::SmallVector<detail::ExpressionPtr> 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<double>& Value() { return m_jacobian.Value(); }
/**
* Updates the values of the gradient tree.
*/
void Update() { m_jacobian.Update(); }
/**
* Returns the profiler.
*/

23
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Jacobian.hpp vendored
View File

@@ -5,12 +5,12 @@
#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/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<detail::ExpressionPtr> wrtVec;
wpi::SmallVector<detail::ExpressionPtr> 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<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;
};

18
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/Variable.hpp vendored
View File

@@ -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 =

18
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableBlock.hpp vendored
View File

@@ -218,9 +218,9 @@ class VariableBlock {
* @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) {
Variable& operator()(int row, int col)
requires(!std::is_const_v<Mat>)
{
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 <typename Mat2 = Mat>
requires(!std::is_const_v<Mat2>)
Variable& operator()(int row) {
Variable& operator()(int row)
requires(!std::is_const_v<Mat>)
{
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) {

19
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/VariableMatrix.hpp vendored
View File

@@ -11,6 +11,7 @@
#include <vector>
#include <Eigen/Core>
#include <wpi/SmallVector.h>
#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<Variable> m_storage;
wpi::SmallVector<Variable> 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

35
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/OCPSolver.hpp vendored
View File

@@ -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;
}
}

110
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Constraints.hpp vendored
View File

@@ -4,11 +4,13 @@
#include <algorithm>
#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/SmallVector.hpp"
#include "sleipnir/util/SymbolExports.hpp"
namespace sleipnir {
@@ -25,18 +27,21 @@ namespace sleipnir {
* @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>)
small_vector<Variable> MakeConstraints(const LHS& lhs, const RHS& rhs) {
small_vector<Variable> constraints;
requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
(ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
(!std::same_as<std::decay_t<LHS>, double> ||
!std::same_as<std::decay_t<RHS>, double>)
wpi::SmallVector<Variable> MakeConstraints(LHS&& lhs, RHS&& rhs) {
wpi::SmallVector<Variable> constraints;
if constexpr (ScalarLike<LHS> && ScalarLike<RHS>) {
if constexpr (ScalarLike<std::decay_t<LHS>> &&
ScalarLike<std::decay_t<RHS>>) {
constraints.emplace_back(lhs - rhs);
} else if constexpr (ScalarLike<LHS> && MatrixLike<RHS>) {
} else if constexpr (ScalarLike<std::decay_t<LHS>> &&
MatrixLike<std::decay_t<RHS>>) {
int rows;
int cols;
if constexpr (EigenMatrixLike<RHS>) {
if constexpr (EigenMatrixLike<std::decay_t<RHS>>) {
rows = rhs.rows();
cols = rhs.cols();
} else {
@@ -52,10 +57,11 @@ small_vector<Variable> MakeConstraints(const LHS& lhs, const RHS& rhs) {
constraints.emplace_back(lhs - rhs(row, col));
}
}
} else if constexpr (MatrixLike<LHS> && ScalarLike<RHS>) {
} else if constexpr (MatrixLike<std::decay_t<LHS>> &&
ScalarLike<std::decay_t<RHS>>) {
int rows;
int cols;
if constexpr (EigenMatrixLike<LHS>) {
if constexpr (EigenMatrixLike<std::decay_t<LHS>>) {
rows = lhs.rows();
cols = lhs.cols();
} else {
@@ -71,10 +77,11 @@ small_vector<Variable> MakeConstraints(const LHS& lhs, const RHS& rhs) {
constraints.emplace_back(lhs(row, col) - rhs);
}
}
} else if constexpr (MatrixLike<LHS> && MatrixLike<RHS>) {
} else if constexpr (MatrixLike<std::decay_t<LHS>> &&
MatrixLike<std::decay_t<RHS>>) {
int lhsRows;
int lhsCols;
if constexpr (EigenMatrixLike<LHS>) {
if constexpr (EigenMatrixLike<std::decay_t<LHS>>) {
lhsRows = lhs.rows();
lhsCols = lhs.cols();
} else {
@@ -86,7 +93,7 @@ small_vector<Variable> MakeConstraints(const LHS& lhs, const RHS& rhs) {
int rhsRows;
[[maybe_unused]]
int rhsCols;
if constexpr (EigenMatrixLike<RHS>) {
if constexpr (EigenMatrixLike<std::decay_t<RHS>>) {
rhsRows = rhs.rows();
rhsCols = rhs.cols();
} else {
@@ -113,7 +120,7 @@ small_vector<Variable> MakeConstraints(const LHS& lhs, const 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.
@@ -125,19 +132,20 @@ struct SLEIPNIR_DLLEXPORT EqualityConstraints {
* @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)
requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
(ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
(!std::same_as<std::decay_t<LHS>, double> ||
!std::same_as<std::decay_t<RHS>, double>)
EqualityConstraints(LHS&& lhs, RHS&& rhs)
: constraints{MakeConstraints(lhs, rhs)} {}
/**
* Implicit conversion operator to bool.
*/
operator bool() 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<Variable> constraints;
wpi::SmallVector<Variable> 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 <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)
requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
(ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
(!std::same_as<std::decay_t<LHS>, double> ||
!std::same_as<std::decay_t<RHS>, double>)
InequalityConstraints(LHS&& lhs, RHS&& rhs)
: constraints{MakeConstraints(lhs, rhs)} {}
/**
* Implicit conversion operator to bool.
*/
operator bool() 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 <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) {
requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
(ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
(!std::same_as<std::decay_t<LHS>, double> ||
!std::same_as<std::decay_t<RHS>, double>)
EqualityConstraints operator==(LHS&& lhs, RHS&& rhs) {
return EqualityConstraints{lhs, rhs};
}
@@ -196,10 +206,11 @@ EqualityConstraints operator==(const LHS& lhs, const RHS& rhs) {
* @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) {
requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
(ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
(!std::same_as<std::decay_t<LHS>, double> ||
!std::same_as<std::decay_t<RHS>, double>)
InequalityConstraints operator<(LHS&& lhs, RHS&& rhs) {
return rhs >= lhs;
}
@@ -211,10 +222,11 @@ InequalityConstraints operator<(const LHS& lhs, const RHS& rhs) {
* @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) {
requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
(ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
(!std::same_as<std::decay_t<LHS>, double> ||
!std::same_as<std::decay_t<RHS>, double>)
InequalityConstraints operator<=(LHS&& lhs, RHS&& rhs) {
return rhs >= lhs;
}
@@ -226,10 +238,11 @@ InequalityConstraints operator<=(const LHS& lhs, const RHS& rhs) {
* @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) {
requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
(ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
(!std::same_as<std::decay_t<LHS>, double> ||
!std::same_as<std::decay_t<RHS>, double>)
InequalityConstraints operator>(LHS&& lhs, RHS&& rhs) {
return lhs >= rhs;
}
@@ -241,10 +254,11 @@ InequalityConstraints operator>(const LHS& lhs, const RHS& rhs) {
* @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) {
requires(ScalarLike<std::decay_t<LHS>> || MatrixLike<std::decay_t<LHS>>) &&
(ScalarLike<std::decay_t<RHS>> || MatrixLike<std::decay_t<RHS>>) &&
(!std::same_as<std::decay_t<LHS>, double> ||
!std::same_as<std::decay_t<RHS>, double>)
InequalityConstraints operator>=(LHS&& lhs, RHS&& rhs) {
return InequalityConstraints{lhs, rhs};
}

7
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/Multistart.hpp vendored
View File

@@ -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/SmallVector.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) {

183
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp vendored
View File

@@ -12,6 +12,7 @@
#include <utility>
#include <Eigen/Core>
#include <wpi/SmallVector.h>
#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 <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.
* 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<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 =

5
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Assert.hpp vendored
View File

@@ -3,9 +3,8 @@
#pragma once
#ifdef JORMUNGANDR
#include <format>
#include <stdexcept>
#include <fmt/format.h>
/**
* 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);

7
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Pool.hpp vendored
View File

@@ -5,7 +5,8 @@
#include <cstddef>
#include <memory>
#include "sleipnir/util/SmallVector.hpp"
#include <wpi/SmallVector.h>
#include "sleipnir/util/SymbolExports.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;
/**

163
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/SmallVector.hpp vendored
View File

@@ -1,163 +0,0 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <algorithm>
#include <cstddef>
#include <memory>
#include <type_traits>
#include <utility>
#include <vector>
namespace sleipnir {
template <typename T, size_t MaxSize = 8, typename NonReboundT = T>
struct small_buffer_vector_allocator {
alignas(alignof(T)) std::byte m_smallBuffer[MaxSize * sizeof(T)];
std::allocator<T> 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 <class U>
constexpr small_buffer_vector_allocator( // NOLINT
const small_buffer_vector_allocator<U, MaxSize, NonReboundT>&) noexcept {}
template <class U>
struct rebind {
using other = small_buffer_vector_allocator<U, MaxSize, NonReboundT>;
};
// 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<T, NonReboundT>) {
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<T*>(&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<T*>(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 <typename T, size_t N = 8>
class small_vector
: public std::vector<T, small_buffer_vector_allocator<T, N>> {
public:
using vectorT = std::vector<T, small_buffer_vector_allocator<T, N>>;
// 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<T>) {
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<T>) {
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 <class InputIt>
small_vector(InputIt first, InputIt last) : small_vector() {
vectorT::insert(vectorT::begin(), first, last);
}
small_vector(std::initializer_list<T> init) : small_vector() { // NOLINT
vectorT::insert(vectorT::begin(), init);
}
friend void swap(small_vector& a, small_vector& b) noexcept {
std::swap(static_cast<vectorT&>(a), static_cast<vectorT&>(b));
}
};
template <typename T, size_t N, typename Pred>
constexpr typename small_vector<T, N>::size_type erase_if(small_vector<T, N>& 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

4
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/Spy.hpp vendored
View File

@@ -7,8 +7,8 @@
#include <string_view>
#include <Eigen/SparseCore>
#include <wpi/SmallVector.h>
#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<uint8_t> cells;
wpi::SmallVector<uint8_t> cells;
// Allocate space for matrix of characters plus trailing newlines
cells.reserve(cells_width * cells_height);