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

[wpimath] Add copySignPow to MathUtil for joystick input shaping (#8013)

Browse Source
This commit is contained in:
Michael Lesirge
2025-06-15 14:08:41 -07:00
committed by GitHub
parent e2517b7a21
commit fb399eef3d
6 changed files with 173 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
View File

@@ -115,8 +115,8 @@ DifferentialDrive::WheelSpeeds DifferentialDrive::ArcadeDriveIK(
// Square the inputs (while preserving the sign) to increase fine control
// while permitting full power.
if (squareInputs) {
xSpeed = std::copysign(xSpeed * xSpeed, xSpeed);
zRotation = std::copysign(zRotation * zRotation, zRotation);
xSpeed = CopySignPow(xSpeed, 2);
zRotation = CopySignPow(zRotation, 2);
}
double leftSpeed = xSpeed - zRotation;
@@ -170,8 +170,8 @@ DifferentialDrive::WheelSpeeds DifferentialDrive::TankDriveIK(
// Square the inputs (while preserving the sign) to increase fine control
// while permitting full power.
if (squareInputs) {
leftSpeed = std::copysign(leftSpeed * leftSpeed, leftSpeed);
rightSpeed = std::copysign(rightSpeed * rightSpeed, rightSpeed);
leftSpeed = CopySignPow(leftSpeed, 2);
rightSpeed = CopySignPow(rightSpeed, 2);
}
return {leftSpeed, rightSpeed};

8
wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java
View File

@@ -265,8 +265,8 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
// Square the inputs (while preserving the sign) to increase fine control
// while permitting full power.
if (squareInputs) {
xSpeed = Math.copySign(xSpeed * xSpeed, xSpeed);
zRotation = Math.copySign(zRotation * zRotation, zRotation);
xSpeed = MathUtil.copySignPow(xSpeed, 2);
zRotation = MathUtil.copySignPow(zRotation, 2);
}
double leftSpeed = xSpeed - zRotation;
@@ -340,8 +340,8 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
// Square the inputs (while preserving the sign) to increase fine control
// while permitting full power.
if (squareInputs) {
leftSpeed = Math.copySign(leftSpeed * leftSpeed, leftSpeed);
rightSpeed = Math.copySign(rightSpeed * rightSpeed, rightSpeed);
leftSpeed = MathUtil.copySignPow(leftSpeed, 2);
rightSpeed = MathUtil.copySignPow(rightSpeed, 2);
}
return new WheelSpeeds(leftSpeed, rightSpeed);

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

@@ -107,6 +107,42 @@ public final class MathUtil {
return applyDeadband(value, deadband, 1);
}
/**
* Raises the input to the power of the given exponent while preserving its sign.
*
* <p>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.
*
* <p>This is useful for applying smoother or more aggressive control response curves (e.g.
* joystick input shaping).
*
* @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.
* @return The transformed value with the same sign and scaled to the input range.
*/
public static double copySignPow(double value, double exponent, double maxMagnitude) {
return Math.copySign(Math.pow(Math.abs(value) / maxMagnitude, exponent), value) * maxMagnitude;
}
/**
* Raises the input to the power of the given exponent while preserving its sign.
*
* <p>This is useful for applying smoother or more aggressive control response curves (e.g.
* joystick input shaping).
*
* @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.
* @return The transformed value with the same sign.
*/
public static double copySignPow(double value, double exponent) {
return copySignPow(value, exponent, 1);
}
/**
* Returns modulus of input.
*

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

@@ -94,6 +94,42 @@ constexpr T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) {
}
}
/**
* 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.
*
* This is useful for applying smoother or more aggressive control response
* curves (e.g. joystick input shaping).
*
* @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.
* @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}) {
if constexpr (std::is_arithmetic_v<T>) {
return gcem::copysign(
gcem::pow(gcem::abs(value) / maxMagnitude, exponent) * maxMagnitude,
value);
} else {
return units::math::copysign(
gcem::pow((units::math::abs(value) / maxMagnitude).value(), exponent) *
maxMagnitude,
value);
}
}
/**
* Returns modulus of input.
*

38
wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java
View File

@@ -72,6 +72,44 @@ class MathUtilTest extends UtilityClassTest<MathUtil> {
assertEquals(80.0, MathUtil.applyDeadband(100.0, 20, Double.POSITIVE_INFINITY));
}
@Test
void testCopySignPow() {
assertEquals(0.5, MathUtil.copySignPow(0.5, 1.0));
assertEquals(-0.5, MathUtil.copySignPow(-0.5, 1.0));
assertEquals(0.5 * 0.5, MathUtil.copySignPow(0.5, 2.0));
assertEquals(-(0.5 * 0.5), MathUtil.copySignPow(-0.5, 2.0));
assertEquals(Math.sqrt(0.5), MathUtil.copySignPow(0.5, 0.5));
assertEquals(-Math.sqrt(0.5), MathUtil.copySignPow(-0.5, 0.5));
assertEquals(0.0, MathUtil.copySignPow(0.0, 2.0));
assertEquals(1.0, MathUtil.copySignPow(1.0, 2.0));
assertEquals(-1.0, MathUtil.copySignPow(-1.0, 2.0));
assertEquals(Math.pow(0.8, 0.3), MathUtil.copySignPow(0.8, 0.3));
assertEquals(-Math.pow(0.8, 0.3), MathUtil.copySignPow(-0.8, 0.3));
}
@Test
void testCopySignPowMaxMagnitude() {
assertEquals(5, MathUtil.copySignPow(5.0, 1.0, 10.0));
assertEquals(-5, MathUtil.copySignPow(-5.0, 1.0, 10.0));
assertEquals(0.5 * 0.5 * 10, MathUtil.copySignPow(5.0, 2.0, 10.0));
assertEquals(-0.5 * 0.5 * 10, MathUtil.copySignPow(-5.0, 2.0, 10.0));
assertEquals(Math.sqrt(0.5) * 10, MathUtil.copySignPow(5.0, 0.5, 10.0));
assertEquals(-Math.sqrt(0.5) * 10, MathUtil.copySignPow(-5.0, 0.5, 10.0));
assertEquals(0.0, MathUtil.copySignPow(0.0, 2.0, 5.0));
assertEquals(5.0, MathUtil.copySignPow(5.0, 2.0, 5.0));
assertEquals(-5.0, MathUtil.copySignPow(-5.0, 2.0, 5.0));
assertEquals(Math.pow(0.8, 0.3) * 100, MathUtil.copySignPow(80, 0.3, 100.0));
assertEquals(-Math.pow(0.8, 0.3) * 100, MathUtil.copySignPow(-80, 0.3, 100.0));
}
@Test
void testInputModulus() {
// These tests check error wrapping. That is, the result of wrapping the

55
wpimath/src/test/native/cpp/MathUtilTest.cpp
View File

@@ -65,6 +65,61 @@ TEST(MathUtilTest, ApplyDeadbandLargeMaxMagnitude) {
frc::ApplyDeadband(100.0, 20.0, std::numeric_limits<double>::infinity()));
}
TEST(MathUtilTest, CopySignPow) {
EXPECT_DOUBLE_EQ(0.5, frc::CopySignPow(0.5, 1.0));
EXPECT_DOUBLE_EQ(-0.5, frc::CopySignPow(-0.5, 1.0));
EXPECT_DOUBLE_EQ(0.5 * 0.5, frc::CopySignPow(0.5, 2.0));
EXPECT_DOUBLE_EQ(-(0.5 * 0.5), frc::CopySignPow(-0.5, 2.0));
EXPECT_DOUBLE_EQ(std::sqrt(0.5), frc::CopySignPow(0.5, 0.5));
EXPECT_DOUBLE_EQ(-std::sqrt(0.5), frc::CopySignPow(-0.5, 0.5));
EXPECT_DOUBLE_EQ(0.0, frc::CopySignPow(0.0, 2.0));
EXPECT_DOUBLE_EQ(1.0, frc::CopySignPow(1.0, 2.0));
EXPECT_DOUBLE_EQ(-1.0, frc::CopySignPow(-1.0, 2.0));
EXPECT_DOUBLE_EQ(std::pow(0.8, 0.3), frc::CopySignPow(0.8, 0.3));
EXPECT_DOUBLE_EQ(-std::pow(0.8, 0.3), frc::CopySignPow(-0.8, 0.3));
}
TEST(MathUtilTest, CopySignPowWithMaxMagnitude) {
EXPECT_DOUBLE_EQ(5.0, frc::CopySignPow(5.0, 1.0, 10.0));
EXPECT_DOUBLE_EQ(-5.0, frc::CopySignPow(-5.0, 1.0, 10.0));
EXPECT_DOUBLE_EQ(0.5 * 0.5 * 10, frc::CopySignPow(5.0, 2.0, 10.0));
EXPECT_DOUBLE_EQ(-0.5 * 0.5 * 10, frc::CopySignPow(-5.0, 2.0, 10.0));
EXPECT_DOUBLE_EQ(std::sqrt(0.5) * 10, frc::CopySignPow(5.0, 0.5, 10.0));
EXPECT_DOUBLE_EQ(-std::sqrt(0.5) * 10, frc::CopySignPow(-5.0, 0.5, 10.0));
EXPECT_DOUBLE_EQ(0.0, frc::CopySignPow(0.0, 2.0, 5.0));
EXPECT_DOUBLE_EQ(5.0, frc::CopySignPow(5.0, 2.0, 5.0));
EXPECT_DOUBLE_EQ(-5.0, frc::CopySignPow(-5.0, 2.0, 5.0));
EXPECT_DOUBLE_EQ(std::pow(0.8, 0.3) * 100,
frc::CopySignPow(80.0, 0.3, 100.0));
EXPECT_DOUBLE_EQ(-std::pow(0.8, 0.3) * 100,
frc::CopySignPow(-80.0, 0.3, 100.0));
}
TEST(MathUtilTest, CopySignPowWithUnits) {
EXPECT_DOUBLE_EQ(
0, frc::CopySignPow<units::meters_per_second_t>(0_mps, 2.0).value());
EXPECT_DOUBLE_EQ(
1, frc::CopySignPow<units::meters_per_second_t>(1_mps, 2.0).value());
EXPECT_DOUBLE_EQ(
-1, frc::CopySignPow<units::meters_per_second_t>(-1_mps, 2.0).value());
EXPECT_DOUBLE_EQ(
0.5 * 0.5 * 10,
frc::CopySignPow<units::meters_per_second_t>(5_mps, 2.0, 10_mps).value());
EXPECT_DOUBLE_EQ(
-0.5 * 0.5 * 10,
frc::CopySignPow<units::meters_per_second_t>(-5_mps, 2.0, 10_mps)
.value());
}
TEST(MathUtilTest, InputModulus) {
// These tests check error wrapping. That is, the result of wrapping the
// result of an angle reference minus the measurement.