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

Merge branch 'main' into 2027

Browse Source
This commit is contained in:
Peter Johnson
2025-10-11 23:54:41 -07:00
parent b37e2d9343 9e85f3cf55
commit 7a2a982e66
14 changed files with 546 additions and 125 deletions
Download Patch File Download Diff File Expand all files Collapse all files

79
wpimath/src/main/native/include/frc/MathUtil.h
View File

@@ -94,15 +94,41 @@ constexpr T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) {
}
}
/**
* Returns a zero vector if the given vector is within the specified
* distance from the origin. The remaining distance between the deadband and the
* maximum distance is scaled from the origin to the maximum distance.
*
* @param value Value to clip.
* @param deadband Distance from origin.
* @param maxMagnitude The maximum distance from the origin of the input
* (defaults to 1). Can be infinite.
* @return The value after the deadband is applied.
*/
template <typename T, int N>
requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
Eigen::Vector<T, N> ApplyDeadband(const Eigen::Vector<T, N>& value, T deadband,
T maxMagnitude = T{1.0}) {
if constexpr (std::is_arithmetic_v<T>) {
if (value.norm() < T{1e-9}) {
return Eigen::Vector<T, N>::Zero();
}
return value.normalized() *
ApplyDeadband(value.norm(), deadband, maxMagnitude);
} else {
const Eigen::Vector<double, N> asDouble = value.template cast<double>();
const Eigen::Vector<double, N> processed =
ApplyDeadband(asDouble, deadband.value(), maxMagnitude.value());
return processed.template cast<T>();
}
}
/**
* Raises the input to the power of the given exponent while preserving its
* sign.
*
* The function normalizes the input value to the range [0, 1] based on the
* maximum magnitude, raises it to the power of the exponent, then scales the
* result back to the original range and copying the sign. This keeps the value
* in the original range and gives consistent curve behavior regardless of the
* input value's scale.
* maximum magnitude so that the output stays in the range.
*
* This is useful for applying smoother or more aggressive control response
* curves (e.g. joystick input shaping).
@@ -110,14 +136,15 @@ constexpr T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) {
* @param value The input value to transform.
* @param exponent The exponent to apply (e.g. 1.0 = linear, 2.0 = squared
* curve). Must be positive.
* @param maxMagnitude The maximum expected absolute value of input. Must be
* positive.
* @param maxMagnitude The maximum expected absolute value of input (defaults to
* 1). Must be positive.
* @return The transformed value with the same sign and scaled to the input
* range.
*/
template <typename T>
requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
constexpr T CopySignPow(T value, double exponent, T maxMagnitude = T{1.0}) {
constexpr T CopyDirectionPow(T value, double exponent,
T maxMagnitude = T{1.0}) {
if constexpr (std::is_arithmetic_v<T>) {
return gcem::copysign(
gcem::pow(gcem::abs(value) / maxMagnitude, exponent) * maxMagnitude,
@@ -130,6 +157,42 @@ constexpr T CopySignPow(T value, double exponent, T maxMagnitude = T{1.0}) {
}
}
/**
* Raises the norm of the input to the power of the given exponent while
* preserving its direction.
*
* The function normalizes the input value to the range [0, 1] based on the
* maximum magnitude so that the output stays in the range.
*
* This is useful for applying smoother or more aggressive control response
* curves (e.g. joystick input shaping).
*
* @param value The input vector to transform.
* @param exponent The exponent to apply (e.g. 1.0 = linear, 2.0 = squared
* curve). Must be positive.
* @param maxMagnitude The maximum expected distance from origin of input
* (defaults to 1). Must be positive.
* @return The transformed value with the same direction and norm scaled to
* the input range.
*/
template <typename T, int N>
requires std::is_arithmetic_v<T> || units::traits::is_unit_t_v<T>
Eigen::Vector<T, N> CopyDirectionPow(const Eigen::Vector<T, N>& value,
double exponent, T maxMagnitude = T{1.0}) {
if constexpr (std::is_arithmetic_v<T>) {
if (value.norm() < T{1e-9}) {
return Eigen::Vector<T, N>::Zero();
}
return value.normalized() *
CopyDirectionPow(value.norm(), exponent, maxMagnitude);
} else {
const Eigen::Vector<double, N> asDouble = value.template cast<double>();
const Eigen::Vector<double, N> processed =
CopyDirectionPow(asDouble, exponent, maxMagnitude.value());
return processed.template cast<T>();
}
}
/**
* Returns modulus of input.
*
@@ -279,6 +342,7 @@ constexpr Translation2d SlewRateLimit(const Translation2d& current,
}
if (dist > maxVelocity * dt) {
// Move maximum allowed amount in direction of the difference
// NOLINTNEXTLINE(bugprone-integer-division)
return current + diff * (maxVelocity * dt / dist);
}
return next;
@@ -309,6 +373,7 @@ constexpr Translation3d SlewRateLimit(const Translation3d& current,
}
if (dist > maxVelocity * dt) {
// Move maximum allowed amount in direction of the difference
// NOLINTNEXTLINE(bugprone-integer-division)
return current + diff * (maxVelocity * dt / dist);
}
return next;

4
wpimath/src/main/native/thirdparty/eigen/include/Eigen/Version vendored
View File

@@ -7,8 +7,8 @@
#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_PRERELEASE_VERSION ""
#define EIGEN_BUILD_VERSION ""
#define EIGEN_VERSION_STRING "5.0.0"
#endif // EIGEN_VERSION_H

27
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/CoreEvaluators.h vendored
View File

@@ -1048,22 +1048,33 @@ struct ternary_evaluator<CwiseTernaryOp<TernaryOp, Arg1, Arg2, Arg3>, IndexBased
Data m_d;
};
// specialization for expressions like (a < b).select(c, d) to enable full vectorization
template <typename Arg1, typename Arg2, typename Scalar, typename CmpLhsType, typename CmpRhsType, ComparisonName cmp>
struct evaluator<CwiseTernaryOp<scalar_boolean_select_op<Scalar, Scalar, bool>, Arg1, Arg2,
CwiseBinaryOp<scalar_cmp_op<Scalar, Scalar, cmp, false>, CmpLhsType, CmpRhsType>>>
: public ternary_evaluator<
CwiseTernaryOp<scalar_boolean_select_op<Scalar, Scalar, Scalar>, Arg1, Arg2,
CwiseBinaryOp<scalar_cmp_op<Scalar, Scalar, cmp, true>, CmpLhsType, CmpRhsType>>> {
struct scalar_boolean_select_spec {
using DummyTernaryOp = scalar_boolean_select_op<Scalar, Scalar, bool>;
using DummyArg3 = CwiseBinaryOp<scalar_cmp_op<Scalar, Scalar, cmp, false>, CmpLhsType, CmpRhsType>;
using DummyXprType = CwiseTernaryOp<DummyTernaryOp, Arg1, Arg2, DummyArg3>;
using TernaryOp = scalar_boolean_select_op<Scalar, Scalar, Scalar>;
using Arg3 = CwiseBinaryOp<scalar_cmp_op<Scalar, Scalar, cmp, true>, CmpLhsType, CmpRhsType>;
// only use the typed comparison if it is vectorized
static constexpr bool UseTyped = functor_traits<scalar_cmp_op<Scalar, Scalar, cmp, true>>::PacketAccess;
using CondScalar = std::conditional_t<UseTyped, Scalar, bool>;
using TernaryOp = scalar_boolean_select_op<Scalar, Scalar, CondScalar>;
using Arg3 = CwiseBinaryOp<scalar_cmp_op<Scalar, Scalar, cmp, UseTyped>, CmpLhsType, CmpRhsType>;
using XprType = CwiseTernaryOp<TernaryOp, Arg1, Arg2, Arg3>;
using Base = ternary_evaluator<XprType>;
};
// specialization for expressions like (a < b).select(c, d) to enable full vectorization
template <typename Arg1, typename Arg2, typename Scalar, typename CmpLhsType, typename CmpRhsType, ComparisonName cmp>
struct evaluator<CwiseTernaryOp<scalar_boolean_select_op<Scalar, Scalar, bool>, Arg1, Arg2,
CwiseBinaryOp<scalar_cmp_op<Scalar, Scalar, cmp, false>, CmpLhsType, CmpRhsType>>>
: public scalar_boolean_select_spec<Arg1, Arg2, Scalar, CmpLhsType, CmpRhsType, cmp>::Base {
using Helper = scalar_boolean_select_spec<Arg1, Arg2, Scalar, CmpLhsType, CmpRhsType, cmp>;
using Base = typename Helper::Base;
using DummyXprType = typename Helper::DummyXprType;
using Arg3 = typename Helper::Arg3;
using XprType = typename Helper::XprType;
EIGEN_DEVICE_FUNC explicit evaluator(const DummyXprType& xpr)
: Base(XprType(xpr.arg1(), xpr.arg2(), Arg3(xpr.arg3().lhs(), xpr.arg3().rhs()))) {}

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

@@ -1272,6 +1272,14 @@ 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> {};
template <typename Lhs, typename Rhs, int ProductTag>
struct generic_product_impl<Lhs, Rhs, PermutationShape, HomogeneousShape, ProductTag>
: generic_product_impl<Lhs, Rhs, PermutationShape, DenseShape, ProductTag> {};
template <typename Lhs, typename Rhs, int ProductTag>
struct generic_product_impl<Lhs, Rhs, HomogeneousShape, PermutationShape, ProductTag>
: generic_product_impl<Lhs, Rhs, DenseShape, PermutationShape, ProductTag> {};
} // end namespace internal
} // end namespace Eigen

25
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/Core/functors/BinaryFunctors.h vendored
View File

@@ -207,20 +207,9 @@ struct functor_traits<scalar_cmp_op<LhsScalar, RhsScalar, cmp, UseTypedComparato
};
};
template <typename LhsScalar, typename RhsScalar, bool UseTypedComparators>
struct typed_cmp_helper {
static constexpr bool SameType = is_same<LhsScalar, RhsScalar>::value;
static constexpr bool IsNumeric = is_arithmetic<typename NumTraits<LhsScalar>::Real>::value;
static constexpr bool UseTyped = UseTypedComparators && SameType && IsNumeric;
using type = typename conditional<UseTyped, LhsScalar, bool>::type;
};
template <typename LhsScalar, typename RhsScalar, bool UseTypedComparators>
using cmp_return_t = typename typed_cmp_helper<LhsScalar, RhsScalar, UseTypedComparators>::type;
template <typename LhsScalar, typename RhsScalar, bool UseTypedComparators>
struct scalar_cmp_op<LhsScalar, RhsScalar, cmp_EQ, UseTypedComparators> : binary_op_base<LhsScalar, RhsScalar> {
using result_type = cmp_return_t<LhsScalar, RhsScalar, UseTypedComparators>;
using result_type = std::conditional_t<UseTypedComparators, LhsScalar, bool>;
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator()(const LhsScalar& a, const RhsScalar& b) const {
return a == b ? result_type(1) : result_type(0);
}
@@ -233,7 +222,7 @@ struct scalar_cmp_op<LhsScalar, RhsScalar, cmp_EQ, UseTypedComparators> : binary
template <typename LhsScalar, typename RhsScalar, bool UseTypedComparators>
struct scalar_cmp_op<LhsScalar, RhsScalar, cmp_LT, UseTypedComparators> : binary_op_base<LhsScalar, RhsScalar> {
using result_type = cmp_return_t<LhsScalar, RhsScalar, UseTypedComparators>;
using result_type = std::conditional_t<UseTypedComparators, LhsScalar, bool>;
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator()(const LhsScalar& a, const RhsScalar& b) const {
return a < b ? result_type(1) : result_type(0);
}
@@ -246,7 +235,7 @@ struct scalar_cmp_op<LhsScalar, RhsScalar, cmp_LT, UseTypedComparators> : binary
template <typename LhsScalar, typename RhsScalar, bool UseTypedComparators>
struct scalar_cmp_op<LhsScalar, RhsScalar, cmp_LE, UseTypedComparators> : binary_op_base<LhsScalar, RhsScalar> {
using result_type = cmp_return_t<LhsScalar, RhsScalar, UseTypedComparators>;
using result_type = std::conditional_t<UseTypedComparators, LhsScalar, bool>;
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator()(const LhsScalar& a, const RhsScalar& b) const {
return a <= b ? result_type(1) : result_type(0);
}
@@ -259,7 +248,7 @@ struct scalar_cmp_op<LhsScalar, RhsScalar, cmp_LE, UseTypedComparators> : binary
template <typename LhsScalar, typename RhsScalar, bool UseTypedComparators>
struct scalar_cmp_op<LhsScalar, RhsScalar, cmp_GT, UseTypedComparators> : binary_op_base<LhsScalar, RhsScalar> {
using result_type = cmp_return_t<LhsScalar, RhsScalar, UseTypedComparators>;
using result_type = std::conditional_t<UseTypedComparators, LhsScalar, bool>;
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator()(const LhsScalar& a, const RhsScalar& b) const {
return a > b ? result_type(1) : result_type(0);
}
@@ -272,7 +261,7 @@ struct scalar_cmp_op<LhsScalar, RhsScalar, cmp_GT, UseTypedComparators> : binary
template <typename LhsScalar, typename RhsScalar, bool UseTypedComparators>
struct scalar_cmp_op<LhsScalar, RhsScalar, cmp_GE, UseTypedComparators> : binary_op_base<LhsScalar, RhsScalar> {
using result_type = cmp_return_t<LhsScalar, RhsScalar, UseTypedComparators>;
using result_type = std::conditional_t<UseTypedComparators, LhsScalar, bool>;
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator()(const LhsScalar& a, const RhsScalar& b) const {
return a >= b ? result_type(1) : result_type(0);
}
@@ -285,7 +274,7 @@ struct scalar_cmp_op<LhsScalar, RhsScalar, cmp_GE, UseTypedComparators> : binary
template <typename LhsScalar, typename RhsScalar, bool UseTypedComparators>
struct scalar_cmp_op<LhsScalar, RhsScalar, cmp_UNORD, UseTypedComparators> : binary_op_base<LhsScalar, RhsScalar> {
using result_type = cmp_return_t<LhsScalar, RhsScalar, UseTypedComparators>;
using result_type = std::conditional_t<UseTypedComparators, LhsScalar, bool>;
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator()(const LhsScalar& a, const RhsScalar& b) const {
return !(a <= b || b <= a) ? result_type(1) : result_type(0);
}
@@ -298,7 +287,7 @@ struct scalar_cmp_op<LhsScalar, RhsScalar, cmp_UNORD, UseTypedComparators> : bin
template <typename LhsScalar, typename RhsScalar, bool UseTypedComparators>
struct scalar_cmp_op<LhsScalar, RhsScalar, cmp_NEQ, UseTypedComparators> : binary_op_base<LhsScalar, RhsScalar> {
using result_type = cmp_return_t<LhsScalar, RhsScalar, UseTypedComparators>;
using result_type = std::conditional_t<UseTypedComparators, LhsScalar, bool>;
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator()(const LhsScalar& a, const RhsScalar& b) const {
return a != b ? result_type(1) : result_type(0);
}

3
wpimath/src/main/native/thirdparty/eigen/include/Eigen/src/SparseCore/SparseMatrixBase.h vendored
View File

@@ -227,9 +227,6 @@ class SparseMatrixBase : public EigenBase<Derived> {
using Nested = typename Derived::Nested;
using NestedCleaned = typename internal::remove_all<Nested>::type;
/// For converting `0's` to the matrices numerical type
using Scalar = typename Derived::Scalar;
if (Flags & RowMajorBit) {
Nested nm(m.derived());
internal::evaluator<NestedCleaned> thisEval(nm);