Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-28 02:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'main' into 2027

Browse Source
This commit is contained in:
Peter Johnson
2025-09-25 22:03:55 -07:00
parent cf4f6ce4b6 6b8be313c7
commit ef676aca70
93 changed files with 941 additions and 387 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
wpimath/src/main/native/thirdparty/eigen/include/Eigen/Core vendored
View File

@@ -11,6 +11,9 @@
#ifndef EIGEN_CORE_MODULE_H
#define EIGEN_CORE_MODULE_H
// Eigen version information.
#include "Version"
// first thing Eigen does: stop the compiler from reporting useless warnings.
#include "src/Core/util/DisableStupidWarnings.h"

14
wpimath/src/main/native/thirdparty/eigen/include/Eigen/Version vendored Normal file
View File

@@ -0,0 +1,14 @@
#ifndef EIGEN_VERSION_H
#define EIGEN_VERSION_H
// The "WORLD" version will forever remain "3" for the "Eigen3" library.
#define EIGEN_WORLD_VERSION 3
// As of Eigen3 5.0.0, we have moved to Semantic Versioning (semver.org).
#define EIGEN_MAJOR_VERSION 5
#define EIGEN_MINOR_VERSION 0
#define EIGEN_PATCH_VERSION 0
#define EIGEN_PRERELEASE_VERSION
#define EIGEN_BUILD_VERSION
#define EIGEN_VERSION_STRING "5.0.0"
#endif // EIGEN_VERSION_H

6
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CwiseNullaryOp.h vendored
View File

@@ -235,8 +235,7 @@ DenseBase<Derived>::Constant(const Scalar& value) {
* \sa LinSpaced(Index,const Scalar&, const Scalar&), setLinSpaced(Index,const Scalar&,const Scalar&)
*/
template <typename Derived>
EIGEN_DEPRECATED EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<
Derived>::RandomAccessLinSpacedReturnType
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::RandomAccessLinSpacedReturnType
DenseBase<Derived>::LinSpaced(Sequential_t, Index size, const Scalar& low, const Scalar& high) {
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
return DenseBase<Derived>::NullaryExpr(size, internal::linspaced_op<Scalar>(low, high, size));
@@ -247,8 +246,7 @@ DenseBase<Derived>::LinSpaced(Sequential_t, Index size, const Scalar& low, const
* \sa LinSpaced(const Scalar&, const Scalar&)
*/
template <typename Derived>
EIGEN_DEPRECATED EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<
Derived>::RandomAccessLinSpacedReturnType
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::RandomAccessLinSpacedReturnType
DenseBase<Derived>::LinSpaced(Sequential_t, const Scalar& low, const Scalar& high) {
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived)

12
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/DenseBase.h vendored
View File

@@ -306,12 +306,12 @@ class DenseBase
EIGEN_DEVICE_FUNC static const ConstantReturnType Constant(Index size, const Scalar& value);
EIGEN_DEVICE_FUNC static const ConstantReturnType Constant(const Scalar& value);
EIGEN_DEPRECATED EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType LinSpaced(Sequential_t, Index size,
const Scalar& low,
const Scalar& high);
EIGEN_DEPRECATED EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType LinSpaced(Sequential_t,
const Scalar& low,
const Scalar& high);
EIGEN_DEPRECATED_WITH_REASON("The method may result in accuracy loss. Use .EqualSpaced() instead.")
EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType LinSpaced(Sequential_t, Index size, const Scalar& low,
const Scalar& high);
EIGEN_DEPRECATED_WITH_REASON("The method may result in accuracy loss. Use .EqualSpaced() instead.")
EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType LinSpaced(Sequential_t, const Scalar& low,
const Scalar& high);
EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType LinSpaced(Index size, const Scalar& low,
const Scalar& high);

50
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/GenericPacketMath.h vendored
View File

@@ -65,7 +65,7 @@ struct default_packet_traits {
HasAbsDiff = 0,
HasBlend = 0,
// This flag is used to indicate whether packet comparison is supported.
// pcmp_eq, pcmp_lt and pcmp_le should be defined for it to be true.
// pcmp_eq and pcmp_lt should be defined for it to be true.
HasCmp = 0,
HasDiv = 0,
@@ -432,30 +432,6 @@ EIGEN_DEVICE_FUNC inline Packet pzero(const Packet& a) {
return pzero_impl<Packet>::run(a);
}
/** \internal \returns a <= b as a bit mask */
template <typename Packet>
EIGEN_DEVICE_FUNC inline Packet pcmp_le(const Packet& a, const Packet& b) {
return a <= b ? ptrue(a) : pzero(a);
}
/** \internal \returns a < b as a bit mask */
template <typename Packet>
EIGEN_DEVICE_FUNC inline Packet pcmp_lt(const Packet& a, const Packet& b) {
return a < b ? ptrue(a) : pzero(a);
}
/** \internal \returns a == b as a bit mask */
template <typename Packet>
EIGEN_DEVICE_FUNC inline Packet pcmp_eq(const Packet& a, const Packet& b) {
return a == b ? ptrue(a) : pzero(a);
}
/** \internal \returns a < b or a==NaN or b==NaN as a bit mask */
template <typename Packet>
EIGEN_DEVICE_FUNC inline Packet pcmp_lt_or_nan(const Packet& a, const Packet& b) {
return a >= b ? pzero(a) : ptrue(a);
}
template <typename T>
struct bit_and {
EIGEN_DEVICE_FUNC constexpr EIGEN_ALWAYS_INLINE T operator()(const T& a, const T& b) const { return a & b; }
@@ -582,6 +558,30 @@ EIGEN_DEVICE_FUNC inline Packet pandnot(const Packet& a, const Packet& b) {
return pand(a, pnot(b));
}
/** \internal \returns a < b as a bit mask */
template <typename Packet>
EIGEN_DEVICE_FUNC inline Packet pcmp_lt(const Packet& a, const Packet& b) {
return a < b ? ptrue(a) : pzero(a);
}
/** \internal \returns a == b as a bit mask */
template <typename Packet>
EIGEN_DEVICE_FUNC inline Packet pcmp_eq(const Packet& a, const Packet& b) {
return a == b ? ptrue(a) : pzero(a);
}
/** \internal \returns a <= b as a bit mask */
template <typename Packet>
EIGEN_DEVICE_FUNC inline Packet pcmp_le(const Packet& a, const Packet& b) {
return por(pcmp_eq(a, b), pcmp_lt(a, b));
}
/** \internal \returns a < b or a==NaN or b==NaN as a bit mask */
template <typename Packet>
EIGEN_DEVICE_FUNC inline Packet pcmp_lt_or_nan(const Packet& a, const Packet& b) {
return a >= b ? pzero(a) : ptrue(a);
}
// In the general case, use bitwise select.
template <typename Packet, bool is_scalar = is_scalar<Packet>::value>
struct pselect_impl {

9
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/MatrixBase.h vendored
View File

@@ -373,12 +373,14 @@ class MatrixBase : public DenseBase<Derived> {
template <int Options = 0>
inline JacobiSVD<PlainObject, Options> jacobiSvd() const;
template <int Options = 0>
EIGEN_DEPRECATED inline JacobiSVD<PlainObject, Options> jacobiSvd(unsigned int computationOptions) const;
EIGEN_DEPRECATED_WITH_REASON("Options should be specified using method's template parameter.")
inline JacobiSVD<PlainObject, Options> jacobiSvd(unsigned int computationOptions) const;
template <int Options = 0>
inline BDCSVD<PlainObject, Options> bdcSvd() const;
template <int Options = 0>
EIGEN_DEPRECATED inline BDCSVD<PlainObject, Options> bdcSvd(unsigned int computationOptions) const;
EIGEN_DEPRECATED_WITH_REASON("Options should be specified using method's template parameter.")
inline BDCSVD<PlainObject, Options> bdcSvd(unsigned int computationOptions) const;
/////////// Geometry module ///////////
@@ -391,7 +393,8 @@ class MatrixBase : public DenseBase<Derived> {
EIGEN_DEVICE_FUNC inline PlainObject unitOrthogonal(void) const;
EIGEN_DEPRECATED EIGEN_DEVICE_FUNC inline Matrix<Scalar, 3, 1> eulerAngles(Index a0, Index a1, Index a2) const;
EIGEN_DEPRECATED_WITH_REASON("Use .canonicalEulerAngles() instead.")
EIGEN_DEVICE_FUNC inline Matrix<Scalar, 3, 1> eulerAngles(Index a0, Index a1, Index a2) const;
EIGEN_DEVICE_FUNC inline Matrix<Scalar, 3, 1> canonicalEulerAngles(Index a0, Index a1, Index a2) const;

8
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/ProductEvaluators.h vendored
View File

@@ -1264,6 +1264,14 @@ struct generic_product_impl<Lhs, Rhs, SkewSymmetricShape, SkewSymmetricShape, Pr
}
};
template <typename Lhs, typename Rhs, int ProductTag, typename MatrixShape>
struct generic_product_impl<Lhs, Rhs, MatrixShape, HomogeneousShape, ProductTag>
: generic_product_impl<Lhs, typename Rhs::PlainObject, MatrixShape, DenseShape, ProductTag> {};
template <typename Lhs, typename Rhs, int ProductTag, typename MatrixShape>
struct generic_product_impl<Lhs, Rhs, HomogeneousShape, MatrixShape, ProductTag>
: generic_product_impl<typename Lhs::PlainObject, Rhs, DenseShape, MatrixShape, ProductTag> {};
} // end namespace internal
} // end namespace Eigen

15
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/arch/SSE/PacketMath.h vendored
View File

@@ -287,7 +287,7 @@ struct packet_traits<bool> : default_packet_traits {
AlignedOnScalar = 1,
size = 16,
HasCmp = 1, // note -- only pcmp_eq is defined
HasCmp = 1,
HasShift = 0,
HasAbs = 0,
HasAbs2 = 0,
@@ -883,7 +883,14 @@ template <>
EIGEN_STRONG_INLINE Packet4ui pandnot<Packet4ui>(const Packet4ui& a, const Packet4ui& b) {
return _mm_andnot_si128(b, a);
}
template <>
EIGEN_STRONG_INLINE Packet16b pandnot<Packet16b>(const Packet16b& a, const Packet16b& b) {
return _mm_andnot_si128(b, a);
}
template <>
EIGEN_STRONG_INLINE Packet16b pcmp_lt(const Packet16b& a, const Packet16b& b) {
return _mm_andnot_si128(a, b);
}
template <>
EIGEN_STRONG_INLINE Packet4f pcmp_le(const Packet4f& a, const Packet4f& b) {
return _mm_cmple_ps(a, b);
@@ -927,7 +934,11 @@ EIGEN_STRONG_INLINE Packet4i pcmp_eq(const Packet4i& a, const Packet4i& b) {
}
template <>
EIGEN_STRONG_INLINE Packet4i pcmp_le(const Packet4i& a, const Packet4i& b) {
#ifdef EIGEN_VECTORIZE_SSE4_1
return _mm_cmpeq_epi32(a, _mm_min_epi32(a, b));
#else
return por(pcmp_lt(a, b), pcmp_eq(a, b));
#endif
}
template <>
EIGEN_STRONG_INLINE Packet2l pcmp_lt(const Packet2l& a, const Packet2l& b) {

2
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/products/Parallelizer.h vendored
View File

@@ -47,7 +47,7 @@ inline void manage_multi_threading(Action action, int* v);
// Public APIs.
/** Must be call first when calling Eigen from multiple threads */
EIGEN_DEPRECATED inline void initParallel() {}
EIGEN_DEPRECATED_WITH_REASON("Initialization is no longer needed.") inline void initParallel() {}
/** \returns the max number of threads reserved for Eigen
* \sa setNbThreads */

20
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Macros.h vendored
View File

@@ -17,13 +17,9 @@
// Eigen version and basic defaults
//------------------------------------------------------------------------------------------
#define EIGEN_WORLD_VERSION 3
#define EIGEN_MAJOR_VERSION 4
#define EIGEN_MINOR_VERSION 90
#define EIGEN_VERSION_AT_LEAST(x, y, z) \
(EIGEN_WORLD_VERSION > x || \
(EIGEN_WORLD_VERSION >= x && (EIGEN_MAJOR_VERSION > y || (EIGEN_MAJOR_VERSION >= y && EIGEN_MINOR_VERSION >= z))))
(EIGEN_MAJOR_VERSION > x || \
(EIGEN_MAJOR_VERSION >= x && (EIGEN_MINOR_VERSION > y || (EIGEN_MINOR_VERSION >= y && EIGEN_PATCH_VERSION >= z))))
#ifdef EIGEN_DEFAULT_TO_ROW_MAJOR
#define EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION Eigen::RowMajor
@@ -944,6 +940,18 @@
#define EIGEN_DEPRECATED
#endif
#ifndef EIGEN_NO_DEPRECATED_WARNING
#if EIGEN_COMP_GNUC
#define EIGEN_DEPRECATED_WITH_REASON(message) __attribute__((deprecated(message)))
#elif EIGEN_COMP_MSVC
#define EIGEN_DEPRECATED_WITH_REASON(message) __declspec(deprecated(message))
#else
#define EIGEN_DEPRECATED_WITH_REASON(message)
#endif
#else
#define EIGEN_DEPRECATED_WITH_REASON(message)
#endif
#if EIGEN_COMP_GNUC
#define EIGEN_UNUSED __attribute__((unused))
#else

3
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/util/Serializer.h vendored
View File

@@ -28,7 +28,8 @@ class Serializer;
// Specialization for POD types.
template <typename T>
class Serializer<T, typename std::enable_if_t<std::is_trivial<T>::value && std::is_standard_layout<T>::value>> {
class Serializer<T,
typename std::enable_if_t<std::is_trivially_copyable<T>::value && std::is_standard_layout<T>::value>> {
public:
/**
* Determines the required size of the serialization buffer for a value.

2
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Eigenvalues/RealSchur.h vendored
View File

@@ -409,7 +409,7 @@ inline void RealSchur<MatrixType>::computeShift(Index iu, Index iter, Scalar& ex
shiftInfo.coeffRef(2) = m_matT.coeff(iu, iu - 1) * m_matT.coeff(iu - 1, iu);
// Alternate exceptional shifting strategy every 16 iterations.
if (iter % 16 == 0) {
if (iter > 0 && iter % 16 == 0) {
// Wilkinson's original ad hoc shift
if (iter % 32 != 0) {
exshift += shiftInfo.coeff(0);

4
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/EulerAngles.h vendored
View File

@@ -133,8 +133,8 @@ EIGEN_DEVICE_FUNC inline Matrix<typename MatrixBase<Derived>::Scalar, 3, 1> Matr
* \sa class AngleAxis
*/
template <typename Derived>
EIGEN_DEPRECATED EIGEN_DEVICE_FUNC inline Matrix<typename MatrixBase<Derived>::Scalar, 3, 1>
MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2) const {
EIGEN_DEVICE_FUNC inline Matrix<typename MatrixBase<Derived>::Scalar, 3, 1> MatrixBase<Derived>::eulerAngles(
Index a0, Index a1, Index a2) const {
/* Implemented from Graphics Gems IV */
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3)

2
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Geometry/Homogeneous.h vendored
View File

@@ -80,14 +80,12 @@ class Homogeneous : public MatrixBase<Homogeneous<MatrixType, Direction_> >, int
template <typename Rhs>
EIGEN_DEVICE_FUNC inline const Product<Homogeneous, Rhs> operator*(const MatrixBase<Rhs>& rhs) const {
eigen_assert(int(Direction) == Horizontal);
return Product<Homogeneous, Rhs>(*this, rhs.derived());
}
template <typename Lhs>
friend EIGEN_DEVICE_FUNC inline const Product<Lhs, Homogeneous> operator*(const MatrixBase<Lhs>& lhs,
const Homogeneous& rhs) {
eigen_assert(int(Direction) == Vertical);
return Product<Lhs, Homogeneous>(lhs.derived(), rhs);
}

10
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/BDCSVD.h vendored
View File

@@ -155,7 +155,8 @@ class BDCSVD : public SVDBase<BDCSVD<MatrixType_, Options_> > {
* \deprecated Will be removed in the next major Eigen version. Options should
* be specified in the \a Options template parameter.
*/
EIGEN_DEPRECATED BDCSVD(Index rows, Index cols, unsigned int computationOptions) : m_algoswap(16), m_numIters(0) {
EIGEN_DEPRECATED_WITH_REASON("Options should be specified using the class template parameter.")
BDCSVD(Index rows, Index cols, unsigned int computationOptions) : m_algoswap(16), m_numIters(0) {
internal::check_svd_options_assertions<MatrixType, Options>(computationOptions, rows, cols);
allocate(rows, cols, computationOptions);
}
@@ -183,8 +184,8 @@ class BDCSVD : public SVDBase<BDCSVD<MatrixType_, Options_> > {
* be specified in the \a Options template parameter.
*/
template <typename Derived>
EIGEN_DEPRECATED BDCSVD(const MatrixBase<Derived>& matrix, unsigned int computationOptions)
: m_algoswap(16), m_numIters(0) {
EIGEN_DEPRECATED_WITH_REASON("Options should be specified using the class template parameter.")
BDCSVD(const MatrixBase<Derived>& matrix, unsigned int computationOptions) : m_algoswap(16), m_numIters(0) {
internal::check_svd_options_assertions<MatrixType, Options>(computationOptions, matrix.rows(), matrix.cols());
compute_impl(matrix, computationOptions);
}
@@ -211,7 +212,8 @@ class BDCSVD : public SVDBase<BDCSVD<MatrixType_, Options_> > {
* be specified in the \a Options template parameter.
*/
template <typename Derived>
EIGEN_DEPRECATED BDCSVD& compute(const MatrixBase<Derived>& matrix, unsigned int computationOptions) {
EIGEN_DEPRECATED_WITH_REASON("Options should be specified using the class template parameter.")
BDCSVD& compute(const MatrixBase<Derived>& matrix, unsigned int computationOptions) {
internal::check_svd_options_assertions<MatrixType, Options>(computationOptions, matrix.rows(), matrix.cols());
return compute_impl(matrix, computationOptions);
}

6
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SVD/JacobiSVD.h vendored
View File

@@ -555,7 +555,8 @@ class JacobiSVD : public SVDBase<JacobiSVD<MatrixType_, Options_> > {
* \deprecated Will be removed in the next major Eigen version. Options should
* be specified in the \a Options template parameter.
*/
EIGEN_DEPRECATED JacobiSVD(Index rows, Index cols, unsigned int computationOptions) {
EIGEN_DEPRECATED_WITH_REASON("Options should be specified using the class template parameter.")
JacobiSVD(Index rows, Index cols, unsigned int computationOptions) {
internal::check_svd_options_assertions<MatrixType, Options>(computationOptions, rows, cols);
allocate(rows, cols, computationOptions);
}
@@ -610,7 +611,8 @@ class JacobiSVD : public SVDBase<JacobiSVD<MatrixType_, Options_> > {
* be specified in the \a Options template parameter.
*/
template <typename Derived>
EIGEN_DEPRECATED JacobiSVD& compute(const MatrixBase<Derived>& matrix, unsigned int computationOptions) {
EIGEN_DEPRECATED_WITH_REASON("Options should be specified using the class template parameter.")
JacobiSVD& compute(const MatrixBase<Derived>& matrix, unsigned int computationOptions) {
internal::check_svd_options_assertions<MatrixBase<Derived>, Options>(m_computationOptions, matrix.rows(),
matrix.cols());
return compute_impl(matrix, computationOptions);

16
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseVector.h vendored
View File

@@ -354,40 +354,40 @@ class SparseVector : public SparseCompressedBase<SparseVector<Scalar_, Options_,
public:
/** \internal \deprecated use setZero() and reserve() */
EIGEN_DEPRECATED void startFill(Index reserve) {
EIGEN_DEPRECATED_WITH_REASON("Use .setZero() and .reserve() instead.") void startFill(Index reserve) {
setZero();
m_data.reserve(reserve);
}
/** \internal \deprecated use insertBack(Index,Index) */
EIGEN_DEPRECATED Scalar& fill(Index r, Index c) {
EIGEN_DEPRECATED_WITH_REASON("Use .insertBack() instead.") Scalar& fill(Index r, Index c) {
eigen_assert(r == 0 || c == 0);
return fill(IsColVector ? r : c);
}
/** \internal \deprecated use insertBack(Index) */
EIGEN_DEPRECATED Scalar& fill(Index i) {
EIGEN_DEPRECATED_WITH_REASON("Use .insertBack() instead.") Scalar& fill(Index i) {
m_data.append(0, i);
return m_data.value(m_data.size() - 1);
}
/** \internal \deprecated use insert(Index,Index) */
EIGEN_DEPRECATED Scalar& fillrand(Index r, Index c) {
EIGEN_DEPRECATED_WITH_REASON("Use .insert() instead.") Scalar& fillrand(Index r, Index c) {
eigen_assert(r == 0 || c == 0);
return fillrand(IsColVector ? r : c);
}
/** \internal \deprecated use insert(Index) */
EIGEN_DEPRECATED Scalar& fillrand(Index i) { return insert(i); }
EIGEN_DEPRECATED_WITH_REASON("Use .insert() instead.") Scalar& fillrand(Index i) { return insert(i); }
/** \internal \deprecated use finalize() */
EIGEN_DEPRECATED void endFill() {}
EIGEN_DEPRECATED_WITH_REASON("Use .finalize() instead.") void endFill() {}
// These two functions were here in the 3.1 release, so let's keep them in case some code rely on them.
/** \internal \deprecated use data() */
EIGEN_DEPRECATED Storage& _data() { return m_data; }
EIGEN_DEPRECATED_WITH_REASON("Use .data() instead.") Storage& _data() { return m_data; }
/** \internal \deprecated use data() */
EIGEN_DEPRECATED const Storage& _data() const { return m_data; }
EIGEN_DEPRECATED_WITH_REASON("Use .data() instead.") const Storage& _data() const { return m_data; }
#ifdef EIGEN_SPARSEVECTOR_PLUGIN
#include EIGEN_SPARSEVECTOR_PLUGIN

3
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/adjoint_expression_graph.hpp vendored
View File

@@ -11,6 +11,7 @@
#include "sleipnir/autodiff/expression_graph.hpp"
#include "sleipnir/autodiff/variable.hpp"
#include "sleipnir/autodiff/variable_matrix.hpp"
#include "sleipnir/util/assert.hpp"
namespace slp::detail {
@@ -50,6 +51,8 @@ class AdjointExpressionGraph {
* @return The variable's gradient tree.
*/
VariableMatrix generate_gradient_tree(const VariableMatrix& wrt) const {
slp_assert(wrt.cols() == 1);
// Read docs/algorithms.md#Reverse_accumulation_automatic_differentiation
// for background on reverse accumulation automatic differentiation.

63
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression.hpp vendored
View File

@@ -422,6 +422,12 @@ struct Expression {
}
};
inline ExpressionPtr cbrt(const ExpressionPtr& x);
inline ExpressionPtr exp(const ExpressionPtr& x);
inline ExpressionPtr sin(const ExpressionPtr& x);
inline ExpressionPtr sinh(const ExpressionPtr& x);
inline ExpressionPtr sqrt(const ExpressionPtr& x);
/**
* Derived expression type for binary minus operator.
*
@@ -504,6 +510,58 @@ struct BinaryPlusExpression final : Expression {
}
};
/**
* Derived expression type for std::cbrt().
*/
struct CbrtExpression final : Expression {
/**
* Constructs an unary expression (an operator with one argument).
*
* @param lhs Unary operator's operand.
*/
explicit constexpr CbrtExpression(ExpressionPtr lhs)
: Expression{std::move(lhs)} {}
double value(double x, double) const override { return std::cbrt(x); }
ExpressionType type() const override { return ExpressionType::NONLINEAR; }
double grad_l(double x, double, double parent_adjoint) const override {
double c = std::cbrt(x);
return parent_adjoint / (3.0 * c * c);
}
ExpressionPtr grad_expr_l(
const ExpressionPtr& x, const ExpressionPtr&,
const ExpressionPtr& parent_adjoint) const override {
auto c = slp::detail::cbrt(x);
return parent_adjoint / (make_expression_ptr<ConstExpression>(3.0) * c * c);
}
};
/**
* std::cbrt() for Expressions.
*
* @param x The argument.
*/
inline ExpressionPtr cbrt(const ExpressionPtr& x) {
using enum ExpressionType;
// Evaluate constant
if (x->type() == CONSTANT) {
if (x->val == 0.0) {
// Return zero
return x;
} else if (x->val == -1.0 || x->val == 1.0) {
return x;
} else {
return make_expression_ptr<ConstExpression>(std::cbrt(x->val));
}
}
return make_expression_ptr<CbrtExpression>(x);
}
/**
* Derived expression type for constant.
*/
@@ -661,11 +719,6 @@ struct UnaryMinusExpression final : Expression {
}
};
inline ExpressionPtr exp(const ExpressionPtr& x);
inline ExpressionPtr sin(const ExpressionPtr& x);
inline ExpressionPtr sinh(const ExpressionPtr& x);
inline ExpressionPtr sqrt(const ExpressionPtr& x);
/**
* Refcount increment for intrusive shared pointer.
*

3
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_graph.hpp vendored
View File

@@ -21,8 +21,7 @@ inline gch::small_vector<Expression*> topological_sort(
const ExpressionPtr& root) {
gch::small_vector<Expression*> list;
// If the root type is a constant, Update() is a no-op, so there's no work
// to do
// If the root type is constant, updates are a no-op, so return an empty list
if (root == nullptr || root->type() == ExpressionType::CONSTANT) {
return list;
}

10
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient.hpp vendored
View File

@@ -15,8 +15,8 @@
namespace slp {
/**
* This class calculates the gradient of a a variable with respect to a vector
* of variables.
* This class calculates the gradient of a variable with respect to a vector of
* variables.
*
* The gradient is only recomputed if the variable expression is quadratic or
* higher order.
@@ -29,7 +29,7 @@ class SLEIPNIR_DLLEXPORT Gradient {
* @param variable Variable of which to compute the gradient.
* @param wrt Variable with respect to which to compute the gradient.
*/
Gradient(Variable variable, Variable wrt) noexcept
Gradient(Variable variable, Variable wrt)
: m_jacobian{std::move(variable), std::move(wrt)} {}
/**
@@ -39,7 +39,7 @@ class SLEIPNIR_DLLEXPORT Gradient {
* @param wrt Vector of variables with respect to which to compute the
* gradient.
*/
Gradient(Variable variable, SleipnirMatrixLike auto wrt) noexcept
Gradient(Variable variable, SleipnirMatrixLike auto wrt)
: m_jacobian{VariableMatrix{std::move(variable)}, std::move(wrt)} {}
/**
@@ -58,7 +58,7 @@ class SLEIPNIR_DLLEXPORT Gradient {
* @return The gradient at wrt's value.
*/
const Eigen::SparseVector<double>& value() {
m_g = m_jacobian.value();
m_g = m_jacobian.value().transpose();
return m_g;
}

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

@@ -10,6 +10,7 @@
#include "sleipnir/autodiff/adjoint_expression_graph.hpp"
#include "sleipnir/autodiff/variable.hpp"
#include "sleipnir/autodiff/variable_matrix.hpp"
#include "sleipnir/util/assert.hpp"
#include "sleipnir/util/concepts.hpp"
#include "sleipnir/util/symbol_exports.hpp"
@@ -34,7 +35,7 @@ class SLEIPNIR_DLLEXPORT Hessian {
* @param variable Variable of which to compute the Hessian.
* @param wrt Variable with respect to which to compute the Hessian.
*/
Hessian(Variable variable, Variable wrt) noexcept
Hessian(Variable variable, Variable wrt)
: Hessian{std::move(variable), VariableMatrix{std::move(wrt)}} {}
/**
@@ -44,10 +45,12 @@ class SLEIPNIR_DLLEXPORT Hessian {
* @param wrt Vector of variables with respect to which to compute the
* Hessian.
*/
Hessian(Variable variable, SleipnirMatrixLike auto wrt) noexcept
Hessian(Variable variable, SleipnirMatrixLike auto wrt)
: m_variables{detail::AdjointExpressionGraph{variable}
.generate_gradient_tree(wrt)},
m_wrt{wrt} {
slp_assert(m_wrt.cols() == 1);
// Initialize column each expression's adjoint occupies in the Jacobian
for (size_t col = 0; col < m_wrt.size(); ++col) {
m_wrt[col].expr->col = col;
@@ -136,15 +139,9 @@ class SLEIPNIR_DLLEXPORT Hessian {
m_graphs[row].append_adjoint_triplets(triplets, row, m_wrt);
}
if (!triplets.empty()) {
m_H.setFromTriplets(triplets.begin(), triplets.end());
if constexpr (UpLo == Eigen::Lower) {
m_H = m_H.triangularView<Eigen::Lower>();
}
} else {
// setFromTriplets() is a no-op on empty triplets, so explicitly zero out
// the storage
m_H.setZero();
m_H.setFromTriplets(triplets.begin(), triplets.end());
if constexpr (UpLo == Eigen::Lower) {
m_H = m_H.triangularView<Eigen::Lower>();
}
return m_H;

16
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/jacobian.hpp vendored
View File

@@ -10,6 +10,7 @@
#include "sleipnir/autodiff/adjoint_expression_graph.hpp"
#include "sleipnir/autodiff/variable.hpp"
#include "sleipnir/autodiff/variable_matrix.hpp"
#include "sleipnir/util/assert.hpp"
#include "sleipnir/util/concepts.hpp"
#include "sleipnir/util/symbol_exports.hpp"
@@ -30,7 +31,7 @@ class SLEIPNIR_DLLEXPORT Jacobian {
* @param variable Variable of which to compute the Jacobian.
* @param wrt Variable with respect to which to compute the Jacobian.
*/
Jacobian(Variable variable, Variable wrt) noexcept
Jacobian(Variable variable, Variable wrt)
: Jacobian{VariableMatrix{std::move(variable)},
VariableMatrix{std::move(wrt)}} {}
@@ -41,8 +42,11 @@ class SLEIPNIR_DLLEXPORT Jacobian {
* @param wrt Vector of variables with respect to which to compute the
* Jacobian.
*/
Jacobian(VariableMatrix variables, SleipnirMatrixLike auto wrt) noexcept
Jacobian(VariableMatrix variables, SleipnirMatrixLike auto wrt)
: m_variables{std::move(variables)}, m_wrt{std::move(wrt)} {
slp_assert(m_variables.cols() == 1);
slp_assert(m_wrt.cols() == 1);
// Initialize column each expression's adjoint occupies in the Jacobian
for (size_t col = 0; col < m_wrt.size(); ++col) {
m_wrt[col].expr->col = col;
@@ -128,13 +132,7 @@ class SLEIPNIR_DLLEXPORT Jacobian {
m_graphs[row].append_adjoint_triplets(triplets, row, m_wrt);
}
if (!triplets.empty()) {
m_J.setFromTriplets(triplets.begin(), triplets.end());
} else {
// setFromTriplets() is a no-op on empty triplets, so explicitly zero out
// the storage
m_J.setZero();
}
m_J.setFromTriplets(triplets.begin(), triplets.end());
return m_J;
}

35
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable.hpp vendored
View File

@@ -87,6 +87,7 @@ class SLEIPNIR_DLLEXPORT Variable {
*/
Variable& operator=(double value) {
expr = detail::make_expression_ptr<detail::ConstExpression>(value);
m_graph_initialized = false;
return *this;
}
@@ -97,22 +98,18 @@ class SLEIPNIR_DLLEXPORT Variable {
* @param value The value of the Variable.
*/
void set_value(double value) {
if (expr->is_constant(0.0)) {
expr = detail::make_expression_ptr<detail::ConstExpression>(value);
} else {
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
// We only need to check the first argument since unary and binary
// operators both use it
if (expr->args[0] != nullptr) {
auto location = std::source_location::current();
slp::println(
stderr,
"WARNING: {}:{}: {}: Modified the value of a dependent variable",
location.file_name(), location.line(), location.function_name());
}
#endif
expr->val = value;
// We only need to check the first argument since unary and binary operators
// both use it
if (expr->args[0] != nullptr) {
auto location = std::source_location::current();
slp::println(
stderr,
"WARNING: {}:{}: {}: Modified the value of a dependent variable",
location.file_name(), location.line(), location.function_name());
}
#endif
expr->val = value;
}
/**
@@ -266,6 +263,7 @@ class SLEIPNIR_DLLEXPORT Variable {
friend SLEIPNIR_DLLEXPORT Variable atan(const Variable& x);
friend SLEIPNIR_DLLEXPORT Variable atan2(const Variable& y,
const Variable& x);
friend SLEIPNIR_DLLEXPORT Variable cbrt(const Variable& x);
friend SLEIPNIR_DLLEXPORT Variable cos(const Variable& x);
friend SLEIPNIR_DLLEXPORT Variable cosh(const Variable& x);
friend SLEIPNIR_DLLEXPORT Variable erf(const Variable& x);
@@ -338,6 +336,15 @@ SLEIPNIR_DLLEXPORT inline Variable atan2(const Variable& y, const Variable& x) {
return Variable{detail::atan2(y.expr, x.expr)};
}
/**
* std::cbrt() for Variables.
*
* @param x The argument.
*/
SLEIPNIR_DLLEXPORT inline Variable cbrt(const Variable& x) {
return Variable{detail::cbrt(x.expr)};
}
/**
* std::cos() for Variables.
*

2
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_matrix.hpp vendored
View File

@@ -1149,7 +1149,7 @@ class SLEIPNIR_DLLEXPORT VariableMatrix {
SLEIPNIR_DLLEXPORT inline VariableMatrix cwise_reduce(
const VariableMatrix& lhs, const VariableMatrix& rhs,
function_ref<Variable(const Variable& x, const Variable& y)> binary_op) {
slp_assert(lhs.rows() == rhs.rows() && lhs.rows() == rhs.rows());
slp_assert(lhs.rows() == rhs.rows() && lhs.cols() == rhs.cols());
VariableMatrix result{VariableMatrix::empty, lhs.rows(), lhs.cols()};

189
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/control/ocp.hpp → wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp.hpp vendored
View File

@@ -8,6 +8,9 @@
#include <utility>
#include "sleipnir/autodiff/variable_matrix.hpp"
#include "sleipnir/optimization/ocp/dynamics_type.hpp"
#include "sleipnir/optimization/ocp/timestep_method.hpp"
#include "sleipnir/optimization/ocp/transcription_method.hpp"
#include "sleipnir/optimization/problem.hpp"
#include "sleipnir/util/assert.hpp"
#include "sleipnir/util/concepts.hpp"
@@ -16,64 +19,6 @@
namespace slp {
/**
* Performs 4th order Runge-Kutta integration of dx/dt = f(t, x, u) for dt.
*
* @param f The function to integrate. It must take two arguments x and u.
* @param x The initial value of x.
* @param u The value u held constant over the integration period.
* @param t0 The initial time.
* @param dt The time over which to integrate.
*/
template <typename F, typename State, typename Input, typename Time>
State rk4(F&& f, State x, Input u, Time t0, Time dt) {
auto halfdt = dt * 0.5;
State k1 = f(t0, x, u, dt);
State k2 = f(t0 + halfdt, x + k1 * halfdt, u, dt);
State k3 = f(t0 + halfdt, x + k2 * halfdt, u, dt);
State k4 = f(t0 + dt, x + k3 * dt, u, dt);
return x + (k1 + k2 * 2.0 + k3 * 2.0 + k4) * (dt / 6.0);
}
/**
* Enum describing an OCP transcription method.
*/
enum class TranscriptionMethod : uint8_t {
/// Each state is a decision variable constrained to the integrated dynamics
/// of the previous state.
DIRECT_TRANSCRIPTION,
/// The trajectory is modeled as a series of cubic polynomials where the
/// centerpoint slope is constrained.
DIRECT_COLLOCATION,
/// States depend explicitly as a function of all previous states and all
/// previous inputs.
SINGLE_SHOOTING
};
/**
* Enum describing a type of system dynamics constraints.
*/
enum class DynamicsType : uint8_t {
/// The dynamics are a function in the form dx/dt = f(t, x, u).
EXPLICIT_ODE,
/// The dynamics are a function in the form xₖ₊₁ = f(t, xₖ, uₖ).
DISCRETE
};
/**
* Enum describing the type of system timestep.
*/
enum class TimestepMethod : uint8_t {
/// The timestep is a fixed constant.
FIXED,
/// The timesteps are allowed to vary as independent decision variables.
VARIABLE,
/// The timesteps are equal length but allowed to vary as a single decision
/// variable.
VARIABLE_SINGLE
};
/**
* This class allows the user to pose and solve a constrained optimal control
* problem (OCP) in a variety of ways.
@@ -117,7 +62,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
* - State transition: xₖ₊₁ = f(xₖ, uₖ)
* @param dynamics_type The type of system evolution function.
* @param timestep_method The timestep method.
* @param method The transcription method.
* @param transcription_method The transcription method.
*/
OCP(int num_states, int num_inputs, std::chrono::duration<double> dt,
int num_steps,
@@ -126,7 +71,8 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
dynamics,
DynamicsType dynamics_type = DynamicsType::EXPLICIT_ODE,
TimestepMethod timestep_method = TimestepMethod::FIXED,
TranscriptionMethod method = TranscriptionMethod::DIRECT_TRANSCRIPTION)
TranscriptionMethod transcription_method =
TranscriptionMethod::DIRECT_TRANSCRIPTION)
: OCP{num_states,
num_inputs,
dt,
@@ -139,7 +85,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
},
dynamics_type,
timestep_method,
method} {}
transcription_method} {}
/**
* Build an optimization problem using a system evolution function (explicit
@@ -156,7 +102,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
* - State transition: xₖ₊₁ = f(t, xₖ, uₖ, dt)
* @param dynamics_type The type of system evolution function.
* @param timestep_method The timestep method.
* @param method The transcription method.
* @param transcription_method The transcription method.
*/
OCP(int num_states, int num_inputs, std::chrono::duration<double> dt,
int num_steps,
@@ -165,50 +111,46 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
dynamics,
DynamicsType dynamics_type = DynamicsType::EXPLICIT_ODE,
TimestepMethod timestep_method = TimestepMethod::FIXED,
TranscriptionMethod method = TranscriptionMethod::DIRECT_TRANSCRIPTION)
: m_num_states{num_states},
m_num_inputs{num_inputs},
m_dt{dt},
m_num_steps{num_steps},
m_transcription_method{method},
m_dynamics_type{dynamics_type},
m_dynamics_function{std::move(dynamics)},
m_timestep_method{timestep_method} {
TranscriptionMethod transcription_method =
TranscriptionMethod::DIRECT_TRANSCRIPTION)
: m_num_steps{num_steps},
m_dynamics{std::move(dynamics)},
m_dynamics_type{dynamics_type} {
// u is num_steps + 1 so that the final constraint function evaluation works
m_U = decision_variable(m_num_inputs, m_num_steps + 1);
m_U = decision_variable(num_inputs, m_num_steps + 1);
if (m_timestep_method == TimestepMethod::FIXED) {
if (timestep_method == TimestepMethod::FIXED) {
m_DT = VariableMatrix{1, m_num_steps + 1};
for (int i = 0; i < num_steps + 1; ++i) {
m_DT(0, i) = m_dt.count();
m_DT(0, i) = dt.count();
}
} else if (m_timestep_method == TimestepMethod::VARIABLE_SINGLE) {
Variable dt = decision_variable();
dt.set_value(m_dt.count());
} else if (timestep_method == TimestepMethod::VARIABLE_SINGLE) {
Variable single_dt = decision_variable();
single_dt.set_value(dt.count());
// Set the member variable matrix to track the decision variable
m_DT = VariableMatrix{1, m_num_steps + 1};
for (int i = 0; i < num_steps + 1; ++i) {
m_DT(0, i) = dt;
m_DT(0, i) = single_dt;
}
} else if (m_timestep_method == TimestepMethod::VARIABLE) {
} else if (timestep_method == TimestepMethod::VARIABLE) {
m_DT = decision_variable(1, m_num_steps + 1);
for (int i = 0; i < num_steps + 1; ++i) {
m_DT(0, i).set_value(m_dt.count());
m_DT(0, i).set_value(dt.count());
}
}
if (m_transcription_method == TranscriptionMethod::DIRECT_TRANSCRIPTION) {
m_X = decision_variable(m_num_states, m_num_steps + 1);
if (transcription_method == TranscriptionMethod::DIRECT_TRANSCRIPTION) {
m_X = decision_variable(num_states, m_num_steps + 1);
constrain_direct_transcription();
} else if (m_transcription_method ==
} else if (transcription_method ==
TranscriptionMethod::DIRECT_COLLOCATION) {
m_X = decision_variable(m_num_states, m_num_steps + 1);
m_X = decision_variable(num_states, m_num_steps + 1);
constrain_direct_collocation();
} else if (m_transcription_method == TranscriptionMethod::SINGLE_SHOOTING) {
} else if (transcription_method == TranscriptionMethod::SINGLE_SHOOTING) {
// In single-shooting the states aren't decision variables, but instead
// depend on the input and previous states
m_X = VariableMatrix{m_num_states, m_num_steps + 1};
m_X = VariableMatrix{num_states, m_num_steps + 1};
constrain_single_shooting();
}
}
@@ -370,6 +312,40 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
VariableMatrix final_state() { return m_X.col(m_num_steps); }
private:
int m_num_steps;
function_ref<VariableMatrix(const Variable& t, const VariableMatrix& x,
const VariableMatrix& u, const Variable& dt)>
m_dynamics;
DynamicsType m_dynamics_type;
VariableMatrix m_X;
VariableMatrix m_U;
VariableMatrix m_DT;
/**
* Performs 4th order Runge-Kutta integration of dx/dt = f(t, x, u) for dt.
*
* @param f The function to integrate. It must take two arguments x and u.
* @param x The initial value of x.
* @param u The value u held constant over the integration period.
* @param t0 The initial time.
* @param dt The time over which to integrate.
*/
template <typename F, typename State, typename Input, typename Time>
State rk4(F&& f, State x, Input u, Time t0, Time dt) {
auto halfdt = dt * 0.5;
State k1 = f(t0, x, u, dt);
State k2 = f(t0 + halfdt, x + k1 * halfdt, u, dt);
State k3 = f(t0 + halfdt, x + k2 * halfdt, u, dt);
State k4 = f(t0 + dt, x + k3 * dt, u, dt);
return x + (k1 + k2 * 2.0 + k3 * 2.0 + k4) * (dt / 6.0);
}
/**
* Apply direct collocation dynamics constraints.
*/
void constrain_direct_collocation() {
slp_assert(m_dynamics_type == DynamicsType::EXPLICIT_ODE);
@@ -379,7 +355,7 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
for (int i = 0; i < m_num_steps; ++i) {
Variable h = dt()(0, i);
auto& f = m_dynamics_function;
auto& f = m_dynamics;
auto t_begin = time;
auto t_end = t_begin + h;
@@ -405,6 +381,9 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
}
}
/**
* Apply direct transcription dynamics constraints.
*/
void constrain_direct_transcription() {
Variable time = 0.0;
@@ -415,17 +394,20 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
Variable dt = this->dt()(0, i);
if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) {
subject_to(x_end == rk4<const decltype(m_dynamics_function)&,
VariableMatrix, VariableMatrix, Variable>(
m_dynamics_function, x_begin, u, time, dt));
subject_to(x_end == rk4<const decltype(m_dynamics)&, VariableMatrix,
VariableMatrix, Variable>(m_dynamics, x_begin,
u, time, dt));
} else if (m_dynamics_type == DynamicsType::DISCRETE) {
subject_to(x_end == m_dynamics_function(time, x_begin, u, dt));
subject_to(x_end == m_dynamics(time, x_begin, u, dt));
}
time += dt;
}
}
/**
* Apply single shooting dynamics constraints.
*/
void constrain_single_shooting() {
Variable time = 0.0;
@@ -436,34 +418,15 @@ class SLEIPNIR_DLLEXPORT OCP : public Problem {
Variable dt = this->dt()(0, i);
if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) {
x_end = rk4<const decltype(m_dynamics_function)&, VariableMatrix,
VariableMatrix, Variable>(m_dynamics_function, x_begin, u,
time, dt);
x_end = rk4<const decltype(m_dynamics)&, VariableMatrix, VariableMatrix,
Variable>(m_dynamics, x_begin, u, time, dt);
} else if (m_dynamics_type == DynamicsType::DISCRETE) {
x_end = m_dynamics_function(time, x_begin, u, dt);
x_end = m_dynamics(time, x_begin, u, dt);
}
time += dt;
}
}
int m_num_states;
int m_num_inputs;
std::chrono::duration<double> m_dt;
int m_num_steps;
TranscriptionMethod m_transcription_method;
DynamicsType m_dynamics_type;
function_ref<VariableMatrix(const Variable& t, const VariableMatrix& x,
const VariableMatrix& u, const Variable& dt)>
m_dynamics_function;
TimestepMethod m_timestep_method;
VariableMatrix m_X;
VariableMatrix m_U;
VariableMatrix m_DT;
};
} // namespace slp

19
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/dynamics_type.hpp vendored Normal file
View File

@@ -0,0 +1,19 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <stdint.h>
namespace slp {
/**
* Enum describing a type of system dynamics constraints.
*/
enum class DynamicsType : uint8_t {
/// The dynamics are a function in the form dx/dt = f(t, x, u).
EXPLICIT_ODE,
/// The dynamics are a function in the form xₖ₊₁ = f(t, xₖ, uₖ).
DISCRETE
};
} // namespace slp

22
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/timestep_method.hpp vendored Normal file
View File

@@ -0,0 +1,22 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <stdint.h>
namespace slp {
/**
* Enum describing the type of system timestep.
*/
enum class TimestepMethod : uint8_t {
/// The timestep is a fixed constant.
FIXED,
/// The timesteps are allowed to vary as independent decision variables.
VARIABLE,
/// The timesteps are equal length but allowed to vary as a single decision
/// variable.
VARIABLE_SINGLE
};
} // namespace slp

24
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp/transcription_method.hpp vendored Normal file
View File

@@ -0,0 +1,24 @@
// Copyright (c) Sleipnir contributors
#pragma once
#include <stdint.h>
namespace slp {
/**
* Enum describing an OCP transcription method.
*/
enum class TranscriptionMethod : uint8_t {
/// Each state is a decision variable constrained to the integrated dynamics
/// of the previous state.
DIRECT_TRANSCRIPTION,
/// The trajectory is modeled as a series of cubic polynomials where the
/// centerpoint slope is constrained.
DIRECT_COLLOCATION,
/// States depend explicitly as a function of all previous states and all
/// previous inputs.
SINGLE_SHOOTING
};
} // namespace slp

24
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp vendored
View File

@@ -73,7 +73,7 @@ class SLEIPNIR_DLLEXPORT Problem {
VariableMatrix decision_variable(int rows, int cols = 1) {
m_decision_variables.reserve(m_decision_variables.size() + rows * cols);
VariableMatrix vars{rows, cols};
VariableMatrix vars{VariableMatrix::empty, rows, cols};
for (int row = 0; row < rows; ++row) {
for (int col = 0; col < cols; ++col) {
@@ -108,7 +108,7 @@ class SLEIPNIR_DLLEXPORT Problem {
m_decision_variables.reserve(m_decision_variables.size() +
(rows * rows + rows) / 2);
VariableMatrix vars{rows, rows};
VariableMatrix vars{VariableMatrix::empty, rows, rows};
for (int row = 0; row < rows; ++row) {
for (int col = 0; col <= row; ++col) {
@@ -317,6 +317,24 @@ class SLEIPNIR_DLLEXPORT Problem {
*/
void clear_callbacks() { m_iteration_callbacks.clear(); }
/**
* Adds a callback to be called at the beginning of each solver iteration.
*
* Language bindings should call this in the Problem constructor to register
* callbacks that shouldn't be removed by clear_callbacks(). Persistent
* callbacks run after non-persistent callbacks.
*
* @param callback The callback. Returning true from the callback causes the
* solver to exit early with the solution it has so far.
*/
template <typename F>
requires requires(F callback, const IterationInfo& info) {
{ callback(info) } -> std::same_as<bool>;
}
void add_persistent_callback(F&& callback) {
m_persistent_iteration_callbacks.emplace_back(std::forward<F>(callback));
}
private:
// The list of decision variables, which are the root of the problem's
// expression tree
@@ -334,6 +352,8 @@ class SLEIPNIR_DLLEXPORT Problem {
// The iteration callbacks
gch::small_vector<std::function<bool(const IterationInfo& info)>>
m_iteration_callbacks;
gch::small_vector<std::function<bool(const IterationInfo& info)>>
m_persistent_iteration_callbacks;
void print_exit_conditions([[maybe_unused]] const Options& options);
void print_problem_analysis();

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

@@ -3,9 +3,10 @@
#pragma once
#ifdef JORMUNGANDR
#include <format>
#include <source_location>
#include <stdexcept>
#include <fmt/format.h>
/**
* Throw an exception in Python.
*/
@@ -13,7 +14,7 @@
do { \
if (!(condition)) { \
auto location = std::source_location::current(); \
throw std::invalid_argument(std::format( \
throw std::invalid_argument(fmt::format( \
"{}:{}: {}: Assertion `{}' failed.", location.file_name(), \
location.line(), location.function_name(), #condition)); \
} \

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

@@ -8,5 +8,6 @@ cppSrcFileInclude {
includeOtherLibs {
^Eigen/
^fmt/
^gch/
}

18
wpimath/src/main/native/thirdparty/sleipnir/src/autodiff/variable_matrix.cpp vendored
View File

@@ -24,7 +24,7 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
const auto& c = A(1, 0);
const auto& d = A(1, 1);
slp::VariableMatrix adj_A{{d, -b}, {-c, a}};
VariableMatrix adj_A{{d, -b}, {-c, a}};
auto det_A = a * d - b * c;
return adj_A / det_A * B;
} else if (A.rows() == 3 && A.cols() == 3) {
@@ -72,9 +72,9 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
auto adj_A10 = fg - di;
auto adj_A20 = dh - eg;
slp::VariableMatrix adj_A{{adj_A00, ch - bi, bf - ce},
{adj_A10, ai - cg, cd - af},
{adj_A20, bg - ah, ae - bd}};
VariableMatrix adj_A{{adj_A00, ch - bi, bf - ce},
{adj_A10, ai - cg, cd - af},
{adj_A20, bg - ah, ae - bd}};
auto det_A = a * adj_A00 + b * adj_A10 + c * adj_A20;
return adj_A / det_A * B;
} else if (A.rows() == 4 && A.cols() == 4) {
@@ -220,10 +220,10 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
auto adj_A32 = -afo + agn + beo - bgm - cen + cfm;
auto adj_A33 = afk - agj - bek + bgi + cej - cfi;
slp::VariableMatrix adj_A{{adj_A00, adj_A01, adj_A02, adj_A03},
{adj_A10, adj_A11, adj_A12, adj_A13},
{adj_A20, adj_A21, adj_A22, adj_A23},
{adj_A30, adj_A31, adj_A32, adj_A33}};
VariableMatrix adj_A{{adj_A00, adj_A01, adj_A02, adj_A03},
{adj_A10, adj_A11, adj_A12, adj_A13},
{adj_A20, adj_A21, adj_A22, adj_A23},
{adj_A30, adj_A31, adj_A32, adj_A33}};
auto det_A = a * adj_A00 + b * adj_A10 + c * adj_A20 + d * adj_A30;
return adj_A / det_A * B;
} else {
@@ -245,7 +245,7 @@ VariableMatrix solve(const VariableMatrix& A, const VariableMatrix& B) {
MatrixXv eigen_X = eigen_A.householderQr().solve(eigen_B);
VariableMatrix X{A.cols(), B.cols()};
VariableMatrix X{VariableMatrix::empty, A.cols(), B.cols()};
for (int row = 0; row < X.rows(); ++row) {
for (int col = 0; col < X.cols(); ++col) {
X(row, col) = eigen_X(row, col);

32
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/problem.cpp vendored
View File

@@ -9,6 +9,7 @@
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <fmt/chrono.h>
#include <gch/small_vector.hpp>
#include "optimization/bounds.hpp"
@@ -79,6 +80,14 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) {
[[maybe_unused]]
int num_inequality_constraints = m_inequality_constraints.size();
gch::small_vector<std::function<bool(const IterationInfo& info)>> callbacks;
for (const auto& callback : m_iteration_callbacks) {
callbacks.emplace_back(callback);
}
for (const auto& callback : m_persistent_iteration_callbacks) {
callbacks.emplace_back(callback);
}
// Solve the optimization problem
ExitStatus status;
if (m_equality_constraints.empty() && m_inequality_constraints.empty()) {
@@ -101,7 +110,7 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) {
H_spy = std::make_unique<Spy>(
"H.spy", "Hessian", "Decision variables", "Decision variables",
num_decision_variables, num_decision_variables);
m_iteration_callbacks.push_back([&](const IterationInfo& info) -> bool {
callbacks.push_back([&](const IterationInfo& info) -> bool {
H_spy->add(info.H);
return false;
});
@@ -123,7 +132,7 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) {
x_ad.set_value(x);
return H.value();
}},
m_iteration_callbacks, options, x);
callbacks, options, x);
} else if (m_inequality_constraints.empty()) {
if (options.diagnostics) {
slp::println("\nInvoking SQP solver\n");
@@ -160,7 +169,7 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) {
"Constraints", "Decision variables",
num_equality_constraints,
num_decision_variables);
m_iteration_callbacks.push_back([&](const IterationInfo& info) -> bool {
callbacks.push_back([&](const IterationInfo& info) -> bool {
H_spy->add(info.H);
A_e_spy->add(info.A_e);
return false;
@@ -193,7 +202,7 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) {
x_ad.set_value(x);
return A_e.value();
}},
m_iteration_callbacks, options, x);
callbacks, options, x);
} else {
if (options.diagnostics) {
slp::println("\nInvoking IPM solver...\n");
@@ -242,7 +251,7 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) {
"A_i.spy", "Inequality constraint Jacobian", "Constraints",
"Decision variables", num_inequality_constraints,
num_decision_variables);
m_iteration_callbacks.push_back([&](const IterationInfo& info) -> bool {
callbacks.push_back([&](const IterationInfo& info) -> bool {
H_spy->add(info.H);
A_e_spy->add(info.A_e);
A_i_spy->add(info.A_i);
@@ -298,19 +307,13 @@ ExitStatus Problem::solve(const Options& options, [[maybe_unused]] bool spy) {
x_ad.set_value(x);
return A_i.value();
}},
m_iteration_callbacks, options,
callbacks, options,
#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
bound_constraint_mask,
#endif
x);
}
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
if (spy) {
m_iteration_callbacks.pop_back();
}
#endif
if (options.diagnostics) {
print_autodiff_diagnostics(ad_setup_profilers);
slp::println("\nExit: {}", to_message(status));
@@ -326,14 +329,15 @@ void Problem::print_exit_conditions([[maybe_unused]] const Options& options) {
// Print possible exit conditions
slp::println("User-configured exit conditions:");
slp::println(" ↳ error below {}", options.tolerance);
if (!m_iteration_callbacks.empty()) {
if (!m_iteration_callbacks.empty() ||
!m_persistent_iteration_callbacks.empty()) {
slp::println(" ↳ iteration callback requested stop");
}
if (std::isfinite(options.max_iterations)) {
slp::println(" ↳ executed {} iterations", options.max_iterations);
}
if (std::isfinite(options.timeout.count())) {
slp::println(" ↳ {} elapsed", options.timeout.count());
slp::println(" ↳ {} elapsed", options.timeout);
}
}

3
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/interior_point.cpp vendored
View File

@@ -327,8 +327,7 @@ ExitStatus interior_point(
Eigen::SparseMatrix<double> lhs(
num_decision_variables + num_equality_constraints,
num_decision_variables + num_equality_constraints);
lhs.setFromSortedTriplets(triplets.begin(), triplets.end(),
[](const auto&, const auto& b) { return b; });
lhs.setFromSortedTriplets(triplets.begin(), triplets.end());
// rhs = −[∇f − Aₑᵀy − Aᵢᵀ(−Σcᵢ + μS⁻¹e + z)]
// [ cₑ ]

1
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/newton.cpp vendored
View File

@@ -2,7 +2,6 @@
#include "sleipnir/optimization/solver/newton.hpp"
#include <algorithm>
#include <chrono>
#include <cmath>
#include <functional>

5
wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/sqp.cpp vendored
View File

@@ -2,7 +2,6 @@
#include "sleipnir/optimization/solver/sqp.hpp"
#include <algorithm>
#include <chrono>
#include <cmath>
#include <functional>
@@ -232,8 +231,7 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks,
Eigen::SparseMatrix<double> lhs(
num_decision_variables + num_equality_constraints,
num_decision_variables + num_equality_constraints);
lhs.setFromSortedTriplets(triplets.begin(), triplets.end(),
[](const auto&, const auto& b) { return b; });
lhs.setFromSortedTriplets(triplets.begin(), triplets.end());
// rhs = −[∇f − Aₑᵀy]
// [ cₑ ]
@@ -407,7 +405,6 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks,
trial_x = x + α_max * step.p_x;
trial_y = y + α_max * step.p_y;
trial_f = matrices.f(trial_x);
trial_c_e = matrices.c_e(trial_x);
double next_kkt_error = kkt_error(