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

[wpimath] Rename 1D copySignPow to match 2D copyDirectionPow (#8286)

Browse Source
This commit is contained in:
Michael Lesirge
2025-10-11 09:24:10 -07:00
committed by GitHub
parent 2b43541b94
commit 9e85f3cf55
6 changed files with 73 additions and 71 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 = CopySignPow(xSpeed, 2);
zRotation = CopySignPow(zRotation, 2);
xSpeed = CopyDirectionPow(xSpeed, 2);
zRotation = CopyDirectionPow(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 = CopySignPow(leftSpeed, 2);
rightSpeed = CopySignPow(rightSpeed, 2);
leftSpeed = CopyDirectionPow(leftSpeed, 2);
rightSpeed = CopyDirectionPow(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 = MathUtil.copySignPow(xSpeed, 2);
zRotation = MathUtil.copySignPow(zRotation, 2);
xSpeed = MathUtil.copyDirectionPow(xSpeed, 2);
zRotation = MathUtil.copyDirectionPow(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 = MathUtil.copySignPow(leftSpeed, 2);
rightSpeed = MathUtil.copySignPow(rightSpeed, 2);
leftSpeed = MathUtil.copyDirectionPow(leftSpeed, 2);
rightSpeed = MathUtil.copyDirectionPow(rightSpeed, 2);
}
return new WheelSpeeds(leftSpeed, rightSpeed);

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

@@ -157,7 +157,7 @@ public final class MathUtil {
* @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) {
public static double copyDirectionPow(double value, double exponent, double maxMagnitude) {
return Math.copySign(Math.pow(Math.abs(value) / maxMagnitude, exponent), value) * maxMagnitude;
}
@@ -172,8 +172,8 @@ public final class MathUtil {
* positive.
* @return The transformed value with the same sign.
*/
public static double copySignPow(double value, double exponent) {
return copySignPow(value, exponent, 1);
public static double copyDirectionPow(double value, double exponent) {
return copyDirectionPow(value, exponent, 1);
}
/**
@@ -196,7 +196,7 @@ public final class MathUtil {
if (value.norm() < 1e-9) {
return value.times(0);
}
return value.unit().times(copySignPow(value.norm(), exponent, maxMagnitude));
return value.unit().times(copyDirectionPow(value.norm(), exponent, maxMagnitude));
}
/**

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

@@ -143,7 +143,8 @@ Eigen::Vector<T, N> ApplyDeadband(const Eigen::Vector<T, N>& value, T deadband,
*/
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,
@@ -183,7 +184,7 @@ Eigen::Vector<T, N> CopyDirectionPow(const Eigen::Vector<T, N>& value,
return Eigen::Vector<T, N>::Zero();
}
return value.normalized() *
CopySignPow(value.norm(), exponent, maxMagnitude);
CopyDirectionPow(value.norm(), exponent, maxMagnitude);
} else {
const Eigen::Vector<double, N> asDouble = value.template cast<double>();
const Eigen::Vector<double, N> processed =

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

@@ -127,41 +127,41 @@ class MathUtilTest extends UtilityClassTest<MathUtil> {
}
@Test
void testCopySignPow() {
assertEquals(0.5, MathUtil.copySignPow(0.5, 1.0));
assertEquals(-0.5, MathUtil.copySignPow(-0.5, 1.0));
void testCopyDirectionPow() {
assertEquals(0.5, MathUtil.copyDirectionPow(0.5, 1.0));
assertEquals(-0.5, MathUtil.copyDirectionPow(-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(0.5 * 0.5, MathUtil.copyDirectionPow(0.5, 2.0));
assertEquals(-(0.5 * 0.5), MathUtil.copyDirectionPow(-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(Math.sqrt(0.5), MathUtil.copyDirectionPow(0.5, 0.5));
assertEquals(-Math.sqrt(0.5), MathUtil.copyDirectionPow(-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(0.0, MathUtil.copyDirectionPow(0.0, 2.0));
assertEquals(1.0, MathUtil.copyDirectionPow(1.0, 2.0));
assertEquals(-1.0, MathUtil.copyDirectionPow(-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));
assertEquals(Math.pow(0.8, 0.3), MathUtil.copyDirectionPow(0.8, 0.3));
assertEquals(-Math.pow(0.8, 0.3), MathUtil.copyDirectionPow(-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));
void testCopyDirectionPowMaxMagnitude() {
assertEquals(5, MathUtil.copyDirectionPow(5.0, 1.0, 10.0));
assertEquals(-5, MathUtil.copyDirectionPow(-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(0.5 * 0.5 * 10, MathUtil.copyDirectionPow(5.0, 2.0, 10.0));
assertEquals(-0.5 * 0.5 * 10, MathUtil.copyDirectionPow(-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(Math.sqrt(0.5) * 10, MathUtil.copyDirectionPow(5.0, 0.5, 10.0));
assertEquals(-Math.sqrt(0.5) * 10, MathUtil.copyDirectionPow(-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(0.0, MathUtil.copyDirectionPow(0.0, 2.0, 5.0));
assertEquals(5.0, MathUtil.copyDirectionPow(5.0, 2.0, 5.0));
assertEquals(-5.0, MathUtil.copyDirectionPow(-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));
assertEquals(Math.pow(0.8, 0.3) * 100, MathUtil.copyDirectionPow(80, 0.3, 100.0));
assertEquals(-Math.pow(0.8, 0.3) * 100, MathUtil.copyDirectionPow(-80, 0.3, 100.0));
}
@Test

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

@@ -131,58 +131,59 @@ TEST(MathUtilTest, ApplyDeadband2dUnits) {
0.02_mps, 2.5_mps));
}
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));
TEST(MathUtilTest, CopyDirectionPow) {
EXPECT_DOUBLE_EQ(0.5, frc::CopyDirectionPow(0.5, 1.0));
EXPECT_DOUBLE_EQ(-0.5, frc::CopyDirectionPow(-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(0.5 * 0.5, frc::CopyDirectionPow(0.5, 2.0));
EXPECT_DOUBLE_EQ(-(0.5 * 0.5), frc::CopyDirectionPow(-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(std::sqrt(0.5), frc::CopyDirectionPow(0.5, 0.5));
EXPECT_DOUBLE_EQ(-std::sqrt(0.5), frc::CopyDirectionPow(-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(0.0, frc::CopyDirectionPow(0.0, 2.0));
EXPECT_DOUBLE_EQ(1.0, frc::CopyDirectionPow(1.0, 2.0));
EXPECT_DOUBLE_EQ(-1.0, frc::CopyDirectionPow(-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));
EXPECT_DOUBLE_EQ(std::pow(0.8, 0.3), frc::CopyDirectionPow(0.8, 0.3));
EXPECT_DOUBLE_EQ(-std::pow(0.8, 0.3), frc::CopyDirectionPow(-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));
TEST(MathUtilTest, CopyDirectionPowWithMaxMagnitude) {
EXPECT_DOUBLE_EQ(5.0, frc::CopyDirectionPow(5.0, 1.0, 10.0));
EXPECT_DOUBLE_EQ(-5.0, frc::CopyDirectionPow(-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(0.5 * 0.5 * 10, frc::CopyDirectionPow(5.0, 2.0, 10.0));
EXPECT_DOUBLE_EQ(-0.5 * 0.5 * 10, frc::CopyDirectionPow(-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(std::sqrt(0.5) * 10, frc::CopyDirectionPow(5.0, 0.5, 10.0));
EXPECT_DOUBLE_EQ(-std::sqrt(0.5) * 10,
frc::CopyDirectionPow(-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(0.0, frc::CopyDirectionPow(0.0, 2.0, 5.0));
EXPECT_DOUBLE_EQ(5.0, frc::CopyDirectionPow(5.0, 2.0, 5.0));
EXPECT_DOUBLE_EQ(-5.0, frc::CopyDirectionPow(-5.0, 2.0, 5.0));
EXPECT_DOUBLE_EQ(std::pow(0.8, 0.3) * 100,
frc::CopySignPow(80.0, 0.3, 100.0));
frc::CopyDirectionPow(80.0, 0.3, 100.0));
EXPECT_DOUBLE_EQ(-std::pow(0.8, 0.3) * 100,
frc::CopySignPow(-80.0, 0.3, 100.0));
frc::CopyDirectionPow(-80.0, 0.3, 100.0));
}
TEST(MathUtilTest, CopySignPowWithUnits) {
EXPECT_UNITS_EQ(0_mps,
frc::CopySignPow<units::meters_per_second_t>(0_mps, 2.0));
EXPECT_UNITS_EQ(1_mps,
frc::CopySignPow<units::meters_per_second_t>(1_mps, 2.0));
EXPECT_UNITS_EQ(-1_mps,
frc::CopySignPow<units::meters_per_second_t>(-1_mps, 2.0));
TEST(MathUtilTest, CopyDirectionPowWithUnits) {
EXPECT_UNITS_EQ(
0_mps, frc::CopyDirectionPow<units::meters_per_second_t>(0_mps, 2.0));
EXPECT_UNITS_EQ(
1_mps, frc::CopyDirectionPow<units::meters_per_second_t>(1_mps, 2.0));
EXPECT_UNITS_EQ(
-1_mps, frc::CopyDirectionPow<units::meters_per_second_t>(-1_mps, 2.0));
EXPECT_UNITS_EQ(
units::meters_per_second_t{0.5 * 0.5 * 10},
frc::CopySignPow<units::meters_per_second_t>(5_mps, 2.0, 10_mps));
frc::CopyDirectionPow<units::meters_per_second_t>(5_mps, 2.0, 10_mps));
EXPECT_UNITS_EQ(
units::meters_per_second_t{-0.5 * 0.5 * 10},
frc::CopySignPow<units::meters_per_second_t>(-5_mps, 2.0, 10_mps));
frc::CopyDirectionPow<units::meters_per_second_t>(-5_mps, 2.0, 10_mps));
}
TEST(MathUtilTest, CopyDirectionPow2d) {