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

[wpimath] MathUtil: Add 2D variants of applyDeadband and copySignPow (#8057)

Browse Source
This commit is contained in:
Michael Lesirge
2025-10-10 13:44:12 -07:00
committed by GitHub
parent 4c4996e638
commit 2b43541b94
4 changed files with 432 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

69
wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
View File

@@ -107,6 +107,39 @@ public final class MathUtil {
return applyDeadband(value, deadband, 1);
}
/**
* 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. Can be infinite.
* @param <R> The number of rows in the vector.
* @return The value after the deadband is applied.
*/
public static <R extends Num> Vector<R> applyDeadband(
Vector<R> value, double deadband, double maxMagnitude) {
if (value.norm() < 1e-9) {
return value.times(0);
}
return value.unit().times(applyDeadband(value.norm(), deadband, maxMagnitude));
}
/**
* Returns a zero vector if the given vector is within the specified distance from the origin. The
* remaining distance between the deadband and a distance of 1.0 is scaled from the origin to a
* distance of 1.0.
*
* @param value Value to clip.
* @param deadband Distance from origin.
* @param <R> The number of rows in the vector.
* @return The value after the deadband is applied.
*/
public static <R extends Num> Vector<R> applyDeadband(Vector<R> value, double deadband) {
return applyDeadband(value, deadband, 1);
}
/**
* Raises the input to the power of the given exponent while preserving its sign.
*
@@ -143,6 +176,42 @@ public final class MathUtil {
return copySignPow(value, exponent, 1);
}
/**
* Raises the norm of the input to the power of the given exponent while preserving its direction.
*
* <p>The function normalizes the norm of the input to the range [0, 1] based on the maximum
* distance, raises it to the power of the exponent, then scales the result back to the original
* range. This keeps the value in the original max distance and gives consistent curve behavior
* regardless of the input norm's scale.
*
* @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. Must be positive.
* @param <R> The number of rows in the vector.
* @return The transformed value with the same direction and norm scaled to the input range.
*/
public static <R extends Num> Vector<R> copyDirectionPow(
Vector<R> value, double exponent, double maxMagnitude) {
if (value.norm() < 1e-9) {
return value.times(0);
}
return value.unit().times(copySignPow(value.norm(), exponent, maxMagnitude));
}
/**
* Raises the norm of the input to the power of the given exponent while preserving its direction.
*
* @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 <R> The number of rows in the vector.
* @return The transformed value with the same direction.
*/
public static <R extends Num> Vector<R> copyDirectionPow(Vector<R> value, double exponent) {
return copyDirectionPow(value, exponent, 1);
}
/**
* Returns modulus of input.
*

76
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,8 +136,8 @@ 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.
*/
@@ -130,6 +156,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() *
CopySignPow(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 +341,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 +372,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;