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

[wpimath] Add cosineScale method to SwerveModuleState and instance optimize (#7114)

Browse Source
This commit is contained in:
Nicholas Armstrong
2024-09-30 15:23:30 -04:00
committed by GitHub
parent fde264b041
commit fe80d72fba
13 changed files with 311 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files

123
wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleStateTest.java
View File

@@ -17,37 +17,136 @@ class SwerveModuleStateTest {
void testOptimize() {
var angleA = Rotation2d.fromDegrees(45);
var refA = new SwerveModuleState(-2.0, Rotation2d.kPi);
var optimizedA = SwerveModuleState.optimize(refA, angleA);
refA.optimize(angleA);
assertAll(
() -> assertEquals(2.0, optimizedA.speedMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, optimizedA.angle.getDegrees(), kEpsilon));
() -> assertEquals(2.0, refA.speedMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, refA.angle.getDegrees(), kEpsilon));
var angleB = Rotation2d.fromDegrees(-50);
var refB = new SwerveModuleState(4.7, Rotation2d.fromDegrees(41));
var optimizedB = SwerveModuleState.optimize(refB, angleB);
refB.optimize(angleB);
assertAll(
() -> assertEquals(-4.7, optimizedB.speedMetersPerSecond, kEpsilon),
() -> assertEquals(-139.0, optimizedB.angle.getDegrees(), kEpsilon));
() -> assertEquals(-4.7, refB.speedMetersPerSecond, kEpsilon),
() -> assertEquals(-139.0, refB.angle.getDegrees(), kEpsilon));
}
@Test
void testNoOptimize() {
var angleA = Rotation2d.kZero;
var refA = new SwerveModuleState(2.0, Rotation2d.fromDegrees(89));
var optimizedA = SwerveModuleState.optimize(refA, angleA);
refA.optimize(angleA);
assertAll(
() -> assertEquals(2.0, optimizedA.speedMetersPerSecond, kEpsilon),
() -> assertEquals(89.0, optimizedA.angle.getDegrees(), kEpsilon));
() -> assertEquals(2.0, refA.speedMetersPerSecond, kEpsilon),
() -> assertEquals(89.0, refA.angle.getDegrees(), kEpsilon));
var angleB = Rotation2d.kZero;
var refB = new SwerveModuleState(-2.0, Rotation2d.fromDegrees(-2));
var optimizedB = SwerveModuleState.optimize(refB, angleB);
refB.optimize(angleB);
assertAll(
() -> assertEquals(-2.0, optimizedB.speedMetersPerSecond, kEpsilon),
() -> assertEquals(-2.0, optimizedB.angle.getDegrees(), kEpsilon));
() -> assertEquals(-2.0, refB.speedMetersPerSecond, kEpsilon),
() -> assertEquals(-2.0, refB.angle.getDegrees(), kEpsilon));
}
@Test
void testCosineScale() {
var angleA = Rotation2d.fromDegrees(0.0);
var refA = new SwerveModuleState(2.0, Rotation2d.fromDegrees(45.0));
refA.cosineScale(angleA);
assertAll(
() -> assertEquals(Math.sqrt(2.0), refA.speedMetersPerSecond, kEpsilon),
() -> assertEquals(45.0, refA.angle.getDegrees(), kEpsilon));
var angleB = Rotation2d.fromDegrees(45.0);
var refB = new SwerveModuleState(2.0, Rotation2d.fromDegrees(45.0));
refB.cosineScale(angleB);
assertAll(
() -> assertEquals(2.0, refB.speedMetersPerSecond, kEpsilon),
() -> assertEquals(45.0, refB.angle.getDegrees(), kEpsilon));
var angleC = Rotation2d.fromDegrees(-45.0);
var refC = new SwerveModuleState(2.0, Rotation2d.fromDegrees(45.0));
refC.cosineScale(angleC);
assertAll(
() -> assertEquals(0.0, refC.speedMetersPerSecond, kEpsilon),
() -> assertEquals(45.0, refC.angle.getDegrees(), kEpsilon));
var angleD = Rotation2d.fromDegrees(135.0);
var refD = new SwerveModuleState(2.0, Rotation2d.fromDegrees(45.0));
refD.cosineScale(angleD);
assertAll(
() -> assertEquals(0.0, refD.speedMetersPerSecond, kEpsilon),
() -> assertEquals(45.0, refD.angle.getDegrees(), kEpsilon));
var angleE = Rotation2d.fromDegrees(-135.0);
var refE = new SwerveModuleState(2.0, Rotation2d.fromDegrees(45.0));
refE.cosineScale(angleE);
assertAll(
() -> assertEquals(-2.0, refE.speedMetersPerSecond, kEpsilon),
() -> assertEquals(45.0, refE.angle.getDegrees(), kEpsilon));
var angleF = Rotation2d.fromDegrees(180.0);
var refF = new SwerveModuleState(2.0, Rotation2d.fromDegrees(45.0));
refF.cosineScale(angleF);
assertAll(
() -> assertEquals(-Math.sqrt(2.0), refF.speedMetersPerSecond, kEpsilon),
() -> assertEquals(45.0, refF.angle.getDegrees(), kEpsilon));
var angleG = Rotation2d.fromDegrees(0.0);
var refG = new SwerveModuleState(-2.0, Rotation2d.fromDegrees(45.0));
refG.cosineScale(angleG);
assertAll(
() -> assertEquals(-Math.sqrt(2.0), refG.speedMetersPerSecond, kEpsilon),
() -> assertEquals(45.0, refG.angle.getDegrees(), kEpsilon));
var angleH = Rotation2d.fromDegrees(45.0);
var refH = new SwerveModuleState(-2.0, Rotation2d.fromDegrees(45.0));
refH.cosineScale(angleH);
assertAll(
() -> assertEquals(-2.0, refH.speedMetersPerSecond, kEpsilon),
() -> assertEquals(45.0, refH.angle.getDegrees(), kEpsilon));
var angleI = Rotation2d.fromDegrees(-45.0);
var refI = new SwerveModuleState(-2.0, Rotation2d.fromDegrees(45.0));
refI.cosineScale(angleI);
assertAll(
() -> assertEquals(0.0, refI.speedMetersPerSecond, kEpsilon),
() -> assertEquals(45.0, refI.angle.getDegrees(), kEpsilon));
var angleJ = Rotation2d.fromDegrees(135.0);
var refJ = new SwerveModuleState(-2.0, Rotation2d.fromDegrees(45.0));
refJ.cosineScale(angleJ);
assertAll(
() -> assertEquals(0.0, refJ.speedMetersPerSecond, kEpsilon),
() -> assertEquals(45.0, refJ.angle.getDegrees(), kEpsilon));
var angleK = Rotation2d.fromDegrees(-135.0);
var refK = new SwerveModuleState(-2.0, Rotation2d.fromDegrees(45.0));
refK.cosineScale(angleK);
assertAll(
() -> assertEquals(2.0, refK.speedMetersPerSecond, kEpsilon),
() -> assertEquals(45.0, refK.angle.getDegrees(), kEpsilon));
var angleL = Rotation2d.fromDegrees(180.0);
var refL = new SwerveModuleState(-2.0, Rotation2d.fromDegrees(45.0));
refL.cosineScale(angleL);
assertAll(
() -> assertEquals(Math.sqrt(2.0), refL.speedMetersPerSecond, kEpsilon),
() -> assertEquals(45.0, refL.angle.getDegrees(), kEpsilon));
}
}

110
wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp
View File

@@ -12,33 +12,119 @@ static constexpr double kEpsilon = 1E-9;
TEST(SwerveModuleStateTest, Optimize) {
frc::Rotation2d angleA{45_deg};
frc::SwerveModuleState refA{-2_mps, 180_deg};
auto optimizedA = frc::SwerveModuleState::Optimize(refA, angleA);
refA.Optimize(angleA);
EXPECT_NEAR(optimizedA.speed.value(), 2.0, kEpsilon);
EXPECT_NEAR(optimizedA.angle.Degrees().value(), 0.0, kEpsilon);
EXPECT_NEAR(refA.speed.value(), 2.0, kEpsilon);
EXPECT_NEAR(refA.angle.Degrees().value(), 0.0, kEpsilon);
frc::Rotation2d angleB{-50_deg};
frc::SwerveModuleState refB{4.7_mps, 41_deg};
auto optimizedB = frc::SwerveModuleState::Optimize(refB, angleB);
refB.Optimize(angleB);
EXPECT_NEAR(optimizedB.speed.value(), -4.7, kEpsilon);
EXPECT_NEAR(optimizedB.angle.Degrees().value(), -139.0, kEpsilon);
EXPECT_NEAR(refB.speed.value(), -4.7, kEpsilon);
EXPECT_NEAR(refB.angle.Degrees().value(), -139.0, kEpsilon);
}
TEST(SwerveModuleStateTest, NoOptimize) {
frc::Rotation2d angleA{0_deg};
frc::SwerveModuleState refA{2_mps, 89_deg};
auto optimizedA = frc::SwerveModuleState::Optimize(refA, angleA);
refA.Optimize(angleA);
EXPECT_NEAR(optimizedA.speed.value(), 2.0, kEpsilon);
EXPECT_NEAR(optimizedA.angle.Degrees().value(), 89.0, kEpsilon);
EXPECT_NEAR(refA.speed.value(), 2.0, kEpsilon);
EXPECT_NEAR(refA.angle.Degrees().value(), 89.0, kEpsilon);
frc::Rotation2d angleB{0_deg};
frc::SwerveModuleState refB{-2_mps, -2_deg};
auto optimizedB = frc::SwerveModuleState::Optimize(refB, angleB);
refB.Optimize(angleB);
EXPECT_NEAR(optimizedB.speed.value(), -2.0, kEpsilon);
EXPECT_NEAR(optimizedB.angle.Degrees().value(), -2.0, kEpsilon);
EXPECT_NEAR(refB.speed.value(), -2.0, kEpsilon);
EXPECT_NEAR(refB.angle.Degrees().value(), -2.0, kEpsilon);
}
TEST(SwerveModuleStateTest, CosineScaling) {
frc::Rotation2d angleA{0_deg};
frc::SwerveModuleState refA{2_mps, 45_deg};
refA.CosineScale(angleA);
EXPECT_NEAR(refA.speed.value(), std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(refA.angle.Degrees().value(), 45.0, kEpsilon);
frc::Rotation2d angleB{45_deg};
frc::SwerveModuleState refB{2_mps, 45_deg};
refB.CosineScale(angleB);
EXPECT_NEAR(refB.speed.value(), 2.0, kEpsilon);
EXPECT_NEAR(refB.angle.Degrees().value(), 45.0, kEpsilon);
frc::Rotation2d angleC{-45_deg};
frc::SwerveModuleState refC{2_mps, 45_deg};
refC.CosineScale(angleC);
EXPECT_NEAR(refC.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(refC.angle.Degrees().value(), 45.0, kEpsilon);
frc::Rotation2d angleD{135_deg};
frc::SwerveModuleState refD{2_mps, 45_deg};
refD.CosineScale(angleD);
EXPECT_NEAR(refD.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(refD.angle.Degrees().value(), 45.0, kEpsilon);
frc::Rotation2d angleE{-135_deg};
frc::SwerveModuleState refE{2_mps, 45_deg};
refE.CosineScale(angleE);
EXPECT_NEAR(refE.speed.value(), -2.0, kEpsilon);
EXPECT_NEAR(refE.angle.Degrees().value(), 45.0, kEpsilon);
frc::Rotation2d angleF{180_deg};
frc::SwerveModuleState refF{2_mps, 45_deg};
refF.CosineScale(angleF);
EXPECT_NEAR(refF.speed.value(), -std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(refF.angle.Degrees().value(), 45.0, kEpsilon);
frc::Rotation2d angleG{0_deg};
frc::SwerveModuleState refG{-2_mps, 45_deg};
refG.CosineScale(angleG);
EXPECT_NEAR(refG.speed.value(), -std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(refG.angle.Degrees().value(), 45.0, kEpsilon);
frc::Rotation2d angleH{45_deg};
frc::SwerveModuleState refH{-2_mps, 45_deg};
refH.CosineScale(angleH);
EXPECT_NEAR(refH.speed.value(), -2.0, kEpsilon);
EXPECT_NEAR(refH.angle.Degrees().value(), 45.0, kEpsilon);
frc::Rotation2d angleI{-45_deg};
frc::SwerveModuleState refI{-2_mps, 45_deg};
refI.CosineScale(angleI);
EXPECT_NEAR(refI.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(refI.angle.Degrees().value(), 45.0, kEpsilon);
frc::Rotation2d angleJ{135_deg};
frc::SwerveModuleState refJ{-2_mps, 45_deg};
refJ.CosineScale(angleJ);
EXPECT_NEAR(refJ.speed.value(), 0.0, kEpsilon);
EXPECT_NEAR(refJ.angle.Degrees().value(), 45.0, kEpsilon);
frc::Rotation2d angleK{-135_deg};
frc::SwerveModuleState refK{-2_mps, 45_deg};
refK.CosineScale(angleK);
EXPECT_NEAR(refK.speed.value(), 2.0, kEpsilon);
EXPECT_NEAR(refK.angle.Degrees().value(), 45.0, kEpsilon);
frc::Rotation2d angleL{180_deg};
frc::SwerveModuleState refL{-2_mps, 45_deg};
refL.CosineScale(angleL);
EXPECT_NEAR(refL.speed.value(), std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(refL.angle.Degrees().value(), 45.0, kEpsilon);
}
TEST(SwerveModuleStateTest, Equality) {