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] Make swerve and differential kinematics functions immutable (#8274)

Browse Source 
Originally started with just swerve, but expanded to diff and mecanum
(docs only) for parity across the drivetrains. Return value checks are
applied when possible to make migration easier and to error loudly if
people forget.

---------

Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com>
This commit is contained in:
Gold856
2026-04-17 00:23:32 -04:00
committed by GitHub
parent 8c80cdcf28
commit 056d7bcbbe
34 changed files with 396 additions and 394 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
wpimath/src/test/java/org/wpilib/math/kinematics/SwerveDriveKinematicsTest.java
View File

@@ -350,15 +350,15 @@ class SwerveDriveKinematicsTest {
SwerveModuleVelocity br = new SwerveModuleVelocity(7, Rotation2d.kZero);
SwerveModuleVelocity[] arr = {fl, fr, bl, br};
SwerveDriveKinematics.desaturateWheelVelocities(arr, 5.5);
var desaturatedArr = SwerveDriveKinematics.desaturateWheelVelocities(arr, 5.5);
double factor = 5.5 / 7.0;
assertAll(
() -> assertEquals(5.0 * factor, arr[0].velocity, kEpsilon),
() -> assertEquals(6.0 * factor, arr[1].velocity, kEpsilon),
() -> assertEquals(4.0 * factor, arr[2].velocity, kEpsilon),
() -> assertEquals(7.0 * factor, arr[3].velocity, kEpsilon));
() -> assertEquals(5.0 * factor, desaturatedArr[0].velocity, kEpsilon),
() -> assertEquals(6.0 * factor, desaturatedArr[1].velocity, kEpsilon),
() -> assertEquals(4.0 * factor, desaturatedArr[2].velocity, kEpsilon),
() -> assertEquals(7.0 * factor, desaturatedArr[3].velocity, kEpsilon));
}
@Test
@@ -369,16 +369,17 @@ class SwerveDriveKinematicsTest {
SwerveModuleVelocity br = new SwerveModuleVelocity(7, Rotation2d.kZero);
SwerveModuleVelocity[] arr = {fl, fr, bl, br};
SwerveDriveKinematics.desaturateWheelVelocities(
arr, m_kinematics.toChassisVelocities(arr), 5.5, 5.5, 3.5);
var desaturatedArr =
SwerveDriveKinematics.desaturateWheelVelocities(
arr, m_kinematics.toChassisVelocities(arr), 5.5, 5.5, 3.5);
double factor = 5.5 / 7.0;
assertAll(
() -> assertEquals(5.0 * factor, arr[0].velocity, kEpsilon),
() -> assertEquals(6.0 * factor, arr[1].velocity, kEpsilon),
() -> assertEquals(4.0 * factor, arr[2].velocity, kEpsilon),
() -> assertEquals(7.0 * factor, arr[3].velocity, kEpsilon));
() -> assertEquals(5.0 * factor, desaturatedArr[0].velocity, kEpsilon),
() -> assertEquals(6.0 * factor, desaturatedArr[1].velocity, kEpsilon),
() -> assertEquals(4.0 * factor, desaturatedArr[2].velocity, kEpsilon),
() -> assertEquals(7.0 * factor, desaturatedArr[3].velocity, kEpsilon));
}
@Test
@@ -389,13 +390,13 @@ class SwerveDriveKinematicsTest {
SwerveModuleVelocity br = new SwerveModuleVelocity(-2, Rotation2d.kZero);
SwerveModuleVelocity[] arr = {fl, fr, bl, br};
SwerveDriveKinematics.desaturateWheelVelocities(arr, 1);
var desaturatedArr = SwerveDriveKinematics.desaturateWheelVelocities(arr, 1);
assertAll(
() -> assertEquals(0.5, arr[0].velocity, kEpsilon),
() -> assertEquals(0.5, arr[1].velocity, kEpsilon),
() -> assertEquals(-1.0, arr[2].velocity, kEpsilon),
() -> assertEquals(-1.0, arr[3].velocity, kEpsilon));
() -> assertEquals(0.5, desaturatedArr[0].velocity, kEpsilon),
() -> assertEquals(0.5, desaturatedArr[1].velocity, kEpsilon),
() -> assertEquals(-1.0, desaturatedArr[2].velocity, kEpsilon),
() -> assertEquals(-1.0, desaturatedArr[3].velocity, kEpsilon));
}
@Test

96
wpimath/src/test/java/org/wpilib/math/kinematics/SwerveModuleVelocityTest.java
View File

@@ -17,137 +17,137 @@ class SwerveModuleVelocityTest {
void testOptimize() {
var angleA = Rotation2d.fromDegrees(45);
var refA = new SwerveModuleVelocity(-2.0, Rotation2d.kPi);
refA.optimize(angleA);
var optimizedA = refA.optimize(angleA);
assertAll(
() -> assertEquals(2.0, refA.velocity, kEpsilon),
() -> assertEquals(0.0, refA.angle.getDegrees(), kEpsilon));
() -> assertEquals(2.0, optimizedA.velocity, kEpsilon),
() -> assertEquals(0.0, optimizedA.angle.getDegrees(), kEpsilon));
var angleB = Rotation2d.fromDegrees(-50);
var refB = new SwerveModuleVelocity(4.7, Rotation2d.fromDegrees(41));
refB.optimize(angleB);
var optimizedB = refB.optimize(angleB);
assertAll(
() -> assertEquals(-4.7, refB.velocity, kEpsilon),
() -> assertEquals(-139.0, refB.angle.getDegrees(), kEpsilon));
() -> assertEquals(-4.7, optimizedB.velocity, kEpsilon),
() -> assertEquals(-139.0, optimizedB.angle.getDegrees(), kEpsilon));
}
@Test
void testNoOptimize() {
var angleA = Rotation2d.kZero;
var refA = new SwerveModuleVelocity(2.0, Rotation2d.fromDegrees(89));
refA.optimize(angleA);
var optimizedA = refA.optimize(angleA);
assertAll(
() -> assertEquals(2.0, refA.velocity, kEpsilon),
() -> assertEquals(89.0, refA.angle.getDegrees(), kEpsilon));
() -> assertEquals(2.0, optimizedA.velocity, kEpsilon),
() -> assertEquals(89.0, optimizedA.angle.getDegrees(), kEpsilon));
var angleB = Rotation2d.kZero;
var refB = new SwerveModuleVelocity(-2.0, Rotation2d.fromDegrees(-2));
refB.optimize(angleB);
var optimizedB = refB.optimize(angleB);
assertAll(
() -> assertEquals(-2.0, refB.velocity, kEpsilon),
() -> assertEquals(-2.0, refB.angle.getDegrees(), kEpsilon));
() -> assertEquals(-2.0, optimizedB.velocity, kEpsilon),
() -> assertEquals(-2.0, optimizedB.angle.getDegrees(), kEpsilon));
}
@Test
void testCosineScale() {
var angleA = Rotation2d.fromDegrees(0.0);
var refA = new SwerveModuleVelocity(2.0, Rotation2d.fromDegrees(45.0));
refA.cosineScale(angleA);
var optimizedA = refA.cosineScale(angleA);
assertAll(
() -> assertEquals(Math.sqrt(2.0), refA.velocity, kEpsilon),
() -> assertEquals(45.0, refA.angle.getDegrees(), kEpsilon));
() -> assertEquals(Math.sqrt(2.0), optimizedA.velocity, kEpsilon),
() -> assertEquals(45.0, optimizedA.angle.getDegrees(), kEpsilon));
var angleB = Rotation2d.fromDegrees(45.0);
var refB = new SwerveModuleVelocity(2.0, Rotation2d.fromDegrees(45.0));
refB.cosineScale(angleB);
var optimizedB = refB.cosineScale(angleB);
assertAll(
() -> assertEquals(2.0, refB.velocity, kEpsilon),
() -> assertEquals(45.0, refB.angle.getDegrees(), kEpsilon));
() -> assertEquals(2.0, optimizedB.velocity, kEpsilon),
() -> assertEquals(45.0, optimizedB.angle.getDegrees(), kEpsilon));
var angleC = Rotation2d.fromDegrees(-45.0);
var refC = new SwerveModuleVelocity(2.0, Rotation2d.fromDegrees(45.0));
refC.cosineScale(angleC);
var optimizedC = refC.cosineScale(angleC);
assertAll(
() -> assertEquals(0.0, refC.velocity, kEpsilon),
() -> assertEquals(45.0, refC.angle.getDegrees(), kEpsilon));
() -> assertEquals(0.0, optimizedC.velocity, kEpsilon),
() -> assertEquals(45.0, optimizedC.angle.getDegrees(), kEpsilon));
var angleD = Rotation2d.fromDegrees(135.0);
var refD = new SwerveModuleVelocity(2.0, Rotation2d.fromDegrees(45.0));
refD.cosineScale(angleD);
var optimizedD = refD.cosineScale(angleD);
assertAll(
() -> assertEquals(0.0, refD.velocity, kEpsilon),
() -> assertEquals(45.0, refD.angle.getDegrees(), kEpsilon));
() -> assertEquals(0.0, optimizedD.velocity, kEpsilon),
() -> assertEquals(45.0, optimizedD.angle.getDegrees(), kEpsilon));
var angleE = Rotation2d.fromDegrees(-135.0);
var refE = new SwerveModuleVelocity(2.0, Rotation2d.fromDegrees(45.0));
refE.cosineScale(angleE);
var optimizedE = refE.cosineScale(angleE);
assertAll(
() -> assertEquals(-2.0, refE.velocity, kEpsilon),
() -> assertEquals(45.0, refE.angle.getDegrees(), kEpsilon));
() -> assertEquals(-2.0, optimizedE.velocity, kEpsilon),
() -> assertEquals(45.0, optimizedE.angle.getDegrees(), kEpsilon));
var angleF = Rotation2d.fromDegrees(180.0);
var refF = new SwerveModuleVelocity(2.0, Rotation2d.fromDegrees(45.0));
refF.cosineScale(angleF);
var optimizedF = refF.cosineScale(angleF);
assertAll(
() -> assertEquals(-Math.sqrt(2.0), refF.velocity, kEpsilon),
() -> assertEquals(45.0, refF.angle.getDegrees(), kEpsilon));
() -> assertEquals(-Math.sqrt(2.0), optimizedF.velocity, kEpsilon),
() -> assertEquals(45.0, optimizedF.angle.getDegrees(), kEpsilon));
var angleG = Rotation2d.fromDegrees(0.0);
var refG = new SwerveModuleVelocity(-2.0, Rotation2d.fromDegrees(45.0));
refG.cosineScale(angleG);
var optimizedG = refG.cosineScale(angleG);
assertAll(
() -> assertEquals(-Math.sqrt(2.0), refG.velocity, kEpsilon),
() -> assertEquals(45.0, refG.angle.getDegrees(), kEpsilon));
() -> assertEquals(-Math.sqrt(2.0), optimizedG.velocity, kEpsilon),
() -> assertEquals(45.0, optimizedG.angle.getDegrees(), kEpsilon));
var angleH = Rotation2d.fromDegrees(45.0);
var refH = new SwerveModuleVelocity(-2.0, Rotation2d.fromDegrees(45.0));
refH.cosineScale(angleH);
var optimizedH = refH.cosineScale(angleH);
assertAll(
() -> assertEquals(-2.0, refH.velocity, kEpsilon),
() -> assertEquals(45.0, refH.angle.getDegrees(), kEpsilon));
() -> assertEquals(-2.0, optimizedH.velocity, kEpsilon),
() -> assertEquals(45.0, optimizedH.angle.getDegrees(), kEpsilon));
var angleI = Rotation2d.fromDegrees(-45.0);
var refI = new SwerveModuleVelocity(-2.0, Rotation2d.fromDegrees(45.0));
refI.cosineScale(angleI);
var optimizedI = refI.cosineScale(angleI);
assertAll(
() -> assertEquals(0.0, refI.velocity, kEpsilon),
() -> assertEquals(45.0, refI.angle.getDegrees(), kEpsilon));
() -> assertEquals(0.0, optimizedI.velocity, kEpsilon),
() -> assertEquals(45.0, optimizedI.angle.getDegrees(), kEpsilon));
var angleJ = Rotation2d.fromDegrees(135.0);
var refJ = new SwerveModuleVelocity(-2.0, Rotation2d.fromDegrees(45.0));
refJ.cosineScale(angleJ);
var optimizedJ = refJ.cosineScale(angleJ);
assertAll(
() -> assertEquals(0.0, refJ.velocity, kEpsilon),
() -> assertEquals(45.0, refJ.angle.getDegrees(), kEpsilon));
() -> assertEquals(0.0, optimizedJ.velocity, kEpsilon),
() -> assertEquals(45.0, optimizedJ.angle.getDegrees(), kEpsilon));
var angleK = Rotation2d.fromDegrees(-135.0);
var refK = new SwerveModuleVelocity(-2.0, Rotation2d.fromDegrees(45.0));
refK.cosineScale(angleK);
var optimizedK = refK.cosineScale(angleK);
assertAll(
() -> assertEquals(2.0, refK.velocity, kEpsilon),
() -> assertEquals(45.0, refK.angle.getDegrees(), kEpsilon));
() -> assertEquals(2.0, optimizedK.velocity, kEpsilon),
() -> assertEquals(45.0, optimizedK.angle.getDegrees(), kEpsilon));
var angleL = Rotation2d.fromDegrees(180.0);
var refL = new SwerveModuleVelocity(-2.0, Rotation2d.fromDegrees(45.0));
refL.cosineScale(angleL);
var optimizedL = refL.cosineScale(angleL);
assertAll(
() -> assertEquals(Math.sqrt(2.0), refL.velocity, kEpsilon),
() -> assertEquals(45.0, refL.angle.getDegrees(), kEpsilon));
() -> assertEquals(Math.sqrt(2.0), optimizedL.velocity, kEpsilon),
() -> assertEquals(45.0, optimizedL.angle.getDegrees(), kEpsilon));
}
@Test

8
wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
View File

@@ -272,7 +272,7 @@ TEST_F(SwerveDriveKinematicsTest, Desaturate) {
SwerveModuleVelocity state4{7.0_mps, 0_deg};
wpi::util::array<SwerveModuleVelocity, 4> arr{state1, state2, state3, state4};
SwerveDriveKinematics<4>::DesaturateWheelVelocities(&arr, 5.5_mps);
arr = SwerveDriveKinematics<4>::DesaturateWheelVelocities(arr, 5.5_mps);
double kFactor = 5.5 / 7.0;
@@ -289,8 +289,8 @@ TEST_F(SwerveDriveKinematicsTest, DesaturateSmooth) {
SwerveModuleVelocity state4{7.0_mps, 0_deg};
wpi::util::array<SwerveModuleVelocity, 4> arr{state1, state2, state3, state4};
SwerveDriveKinematics<4>::DesaturateWheelVelocities(
&arr, m_kinematics.ToChassisVelocities(arr), 5.5_mps, 5.5_mps,
arr = SwerveDriveKinematics<4>::DesaturateWheelVelocities(
arr, m_kinematics.ToChassisVelocities(arr), 5.5_mps, 5.5_mps,
3.5_rad_per_s);
double kFactor = 5.5 / 7.0;
@@ -308,7 +308,7 @@ TEST_F(SwerveDriveKinematicsTest, DesaturateNegativeVelocity) {
SwerveModuleVelocity state4{-2.0_mps, 0_deg};
wpi::util::array<SwerveModuleVelocity, 4> arr{state1, state2, state3, state4};
SwerveDriveKinematics<4>::DesaturateWheelVelocities(&arr, 1.0_mps);
arr = SwerveDriveKinematics<4>::DesaturateWheelVelocities(arr, 1.0_mps);
EXPECT_NEAR(arr[0].velocity.value(), 0.5, kEpsilon);
EXPECT_NEAR(arr[1].velocity.value(), 0.5, kEpsilon);

96
wpimath/src/test/native/cpp/kinematics/SwerveModuleVelocityTest.cpp
View File

@@ -13,119 +13,119 @@ static constexpr double kEpsilon = 1E-9;
TEST(SwerveModuleVelocityTest, Optimize) {
wpi::math::Rotation2d angleA{45_deg};
wpi::math::SwerveModuleVelocity refA{-2_mps, 180_deg};
refA.Optimize(angleA);
auto optimizedA = refA.Optimize(angleA);
EXPECT_NEAR(refA.velocity.value(), 2.0, kEpsilon);
EXPECT_NEAR(refA.angle.Degrees().value(), 0.0, kEpsilon);
EXPECT_NEAR(optimizedA.velocity.value(), 2.0, kEpsilon);
EXPECT_NEAR(optimizedA.angle.Degrees().value(), 0.0, kEpsilon);
wpi::math::Rotation2d angleB{-50_deg};
wpi::math::SwerveModuleVelocity refB{4.7_mps, 41_deg};
refB.Optimize(angleB);
auto optimizedB = refB.Optimize(angleB);
EXPECT_NEAR(refB.velocity.value(), -4.7, kEpsilon);
EXPECT_NEAR(refB.angle.Degrees().value(), -139.0, kEpsilon);
EXPECT_NEAR(optimizedB.velocity.value(), -4.7, kEpsilon);
EXPECT_NEAR(optimizedB.angle.Degrees().value(), -139.0, kEpsilon);
}
TEST(SwerveModuleVelocityTest, NoOptimize) {
wpi::math::Rotation2d angleA{0_deg};
wpi::math::SwerveModuleVelocity refA{2_mps, 89_deg};
refA.Optimize(angleA);
auto optimizedA = refA.Optimize(angleA);
EXPECT_NEAR(refA.velocity.value(), 2.0, kEpsilon);
EXPECT_NEAR(refA.angle.Degrees().value(), 89.0, kEpsilon);
EXPECT_NEAR(optimizedA.velocity.value(), 2.0, kEpsilon);
EXPECT_NEAR(optimizedA.angle.Degrees().value(), 89.0, kEpsilon);
wpi::math::Rotation2d angleB{0_deg};
wpi::math::SwerveModuleVelocity refB{-2_mps, -2_deg};
refB.Optimize(angleB);
auto optimizedB = refB.Optimize(angleB);
EXPECT_NEAR(refB.velocity.value(), -2.0, kEpsilon);
EXPECT_NEAR(refB.angle.Degrees().value(), -2.0, kEpsilon);
EXPECT_NEAR(optimizedB.velocity.value(), -2.0, kEpsilon);
EXPECT_NEAR(optimizedB.angle.Degrees().value(), -2.0, kEpsilon);
}
TEST(SwerveModuleVelocityTest, CosineScaling) {
wpi::math::Rotation2d angleA{0_deg};
wpi::math::SwerveModuleVelocity refA{2_mps, 45_deg};
refA.CosineScale(angleA);
auto optimizedA = refA.CosineScale(angleA);
EXPECT_NEAR(refA.velocity.value(), std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(refA.angle.Degrees().value(), 45.0, kEpsilon);
EXPECT_NEAR(optimizedA.velocity.value(), std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(optimizedA.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleB{45_deg};
wpi::math::SwerveModuleVelocity refB{2_mps, 45_deg};
refB.CosineScale(angleB);
auto optimizedB = refB.CosineScale(angleB);
EXPECT_NEAR(refB.velocity.value(), 2.0, kEpsilon);
EXPECT_NEAR(refB.angle.Degrees().value(), 45.0, kEpsilon);
EXPECT_NEAR(optimizedB.velocity.value(), 2.0, kEpsilon);
EXPECT_NEAR(optimizedB.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleC{-45_deg};
wpi::math::SwerveModuleVelocity refC{2_mps, 45_deg};
refC.CosineScale(angleC);
auto optimizedC = refC.CosineScale(angleC);
EXPECT_NEAR(refC.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(refC.angle.Degrees().value(), 45.0, kEpsilon);
EXPECT_NEAR(optimizedC.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(optimizedC.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleD{135_deg};
wpi::math::SwerveModuleVelocity refD{2_mps, 45_deg};
refD.CosineScale(angleD);
auto optimizedD = refD.CosineScale(angleD);
EXPECT_NEAR(refD.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(refD.angle.Degrees().value(), 45.0, kEpsilon);
EXPECT_NEAR(optimizedD.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(optimizedD.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleE{-135_deg};
wpi::math::SwerveModuleVelocity refE{2_mps, 45_deg};
refE.CosineScale(angleE);
auto optimizedE = refE.CosineScale(angleE);
EXPECT_NEAR(refE.velocity.value(), -2.0, kEpsilon);
EXPECT_NEAR(refE.angle.Degrees().value(), 45.0, kEpsilon);
EXPECT_NEAR(optimizedE.velocity.value(), -2.0, kEpsilon);
EXPECT_NEAR(optimizedE.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleF{180_deg};
wpi::math::SwerveModuleVelocity refF{2_mps, 45_deg};
refF.CosineScale(angleF);
auto optimizedF = refF.CosineScale(angleF);
EXPECT_NEAR(refF.velocity.value(), -std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(refF.angle.Degrees().value(), 45.0, kEpsilon);
EXPECT_NEAR(optimizedF.velocity.value(), -std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(optimizedF.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleG{0_deg};
wpi::math::SwerveModuleVelocity refG{-2_mps, 45_deg};
refG.CosineScale(angleG);
auto optimizedG = refG.CosineScale(angleG);
EXPECT_NEAR(refG.velocity.value(), -std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(refG.angle.Degrees().value(), 45.0, kEpsilon);
EXPECT_NEAR(optimizedG.velocity.value(), -std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(optimizedG.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleH{45_deg};
wpi::math::SwerveModuleVelocity refH{-2_mps, 45_deg};
refH.CosineScale(angleH);
auto optimizedH = refH.CosineScale(angleH);
EXPECT_NEAR(refH.velocity.value(), -2.0, kEpsilon);
EXPECT_NEAR(refH.angle.Degrees().value(), 45.0, kEpsilon);
EXPECT_NEAR(optimizedH.velocity.value(), -2.0, kEpsilon);
EXPECT_NEAR(optimizedH.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleI{-45_deg};
wpi::math::SwerveModuleVelocity refI{-2_mps, 45_deg};
refI.CosineScale(angleI);
auto optimizedI = refI.CosineScale(angleI);
EXPECT_NEAR(refI.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(refI.angle.Degrees().value(), 45.0, kEpsilon);
EXPECT_NEAR(optimizedI.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(optimizedI.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleJ{135_deg};
wpi::math::SwerveModuleVelocity refJ{-2_mps, 45_deg};
refJ.CosineScale(angleJ);
auto optimizedJ = refJ.CosineScale(angleJ);
EXPECT_NEAR(refJ.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(refJ.angle.Degrees().value(), 45.0, kEpsilon);
EXPECT_NEAR(optimizedJ.velocity.value(), 0.0, kEpsilon);
EXPECT_NEAR(optimizedJ.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleK{-135_deg};
wpi::math::SwerveModuleVelocity refK{-2_mps, 45_deg};
refK.CosineScale(angleK);
auto optimizedK = refK.CosineScale(angleK);
EXPECT_NEAR(refK.velocity.value(), 2.0, kEpsilon);
EXPECT_NEAR(refK.angle.Degrees().value(), 45.0, kEpsilon);
EXPECT_NEAR(optimizedK.velocity.value(), 2.0, kEpsilon);
EXPECT_NEAR(optimizedK.angle.Degrees().value(), 45.0, kEpsilon);
wpi::math::Rotation2d angleL{180_deg};
wpi::math::SwerveModuleVelocity refL{-2_mps, 45_deg};
refL.CosineScale(angleL);
auto optimizedL = refL.CosineScale(angleL);
EXPECT_NEAR(refL.velocity.value(), std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(refL.angle.Degrees().value(), 45.0, kEpsilon);
EXPECT_NEAR(optimizedL.velocity.value(), std::sqrt(2.0), kEpsilon);
EXPECT_NEAR(optimizedL.angle.Degrees().value(), 45.0, kEpsilon);
}
TEST(SwerveModuleVelocityTest, Equality) {

96
wpimath/src/test/python/kinematics/test_swerve_module_velocity.py
View File

@@ -9,119 +9,119 @@ kEpsilon = 1e-9
def test_optimize():
angle_a = Rotation2d.fromDegrees(45)
ref_a = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(180))
ref_a.optimize(angle_a)
optimized_a = ref_a.optimize(angle_a)
assert ref_a.velocity == pytest.approx(2.0, abs=kEpsilon)
assert ref_a.angle.degrees() == pytest.approx(0.0, abs=kEpsilon)
assert optimized_a.velocity == pytest.approx(2.0, abs=kEpsilon)
assert optimized_a.angle.degrees() == pytest.approx(0.0, abs=kEpsilon)
angle_b = Rotation2d.fromDegrees(-50)
ref_b = SwerveModuleVelocity(4.7, Rotation2d.fromDegrees(41))
ref_b.optimize(angle_b)
optimized_b = ref_b.optimize(angle_b)
assert ref_b.velocity == pytest.approx(-4.7, abs=kEpsilon)
assert ref_b.angle.degrees() == pytest.approx(-139.0, abs=kEpsilon)
assert optimized_b.velocity == pytest.approx(-4.7, abs=kEpsilon)
assert optimized_b.angle.degrees() == pytest.approx(-139.0, abs=kEpsilon)
def test_no_optimize():
angle_a = Rotation2d.fromDegrees(0)
ref_a = SwerveModuleVelocity(2, Rotation2d.fromDegrees(89))
ref_a.optimize(angle_a)
optimized_a = ref_a.optimize(angle_a)
assert ref_a.velocity == pytest.approx(2.0, abs=kEpsilon)
assert ref_a.angle.degrees() == pytest.approx(89.0, abs=kEpsilon)
assert optimized_a.velocity == pytest.approx(2.0, abs=kEpsilon)
assert optimized_a.angle.degrees() == pytest.approx(89.0, abs=kEpsilon)
angle_b = Rotation2d.fromDegrees(0)
ref_b = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(-2))
ref_b.optimize(angle_b)
optimized_b = ref_b.optimize(angle_b)
assert ref_b.velocity == pytest.approx(-2.0, abs=kEpsilon)
assert ref_b.angle.degrees() == pytest.approx(-2.0, abs=kEpsilon)
assert optimized_b.velocity == pytest.approx(-2.0, abs=kEpsilon)
assert optimized_b.angle.degrees() == pytest.approx(-2.0, abs=kEpsilon)
def test_cosine_scaling():
angle_a = Rotation2d.fromDegrees(0)
ref_a = SwerveModuleVelocity(2, Rotation2d.fromDegrees(45))
ref_a.cosineScale(angle_a)
optimized_a = ref_a.cosineScale(angle_a)
assert ref_a.velocity == pytest.approx(math.sqrt(2.0), abs=kEpsilon)
assert ref_a.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
assert optimized_a.velocity == pytest.approx(math.sqrt(2.0), abs=kEpsilon)
assert optimized_a.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_b = Rotation2d.fromDegrees(45)
ref_b = SwerveModuleVelocity(2, Rotation2d.fromDegrees(45))
ref_b.cosineScale(angle_b)
optimized_b = ref_b.cosineScale(angle_b)
assert ref_b.velocity == pytest.approx(2.0, abs=kEpsilon)
assert ref_b.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
assert optimized_b.velocity == pytest.approx(2.0, abs=kEpsilon)
assert optimized_b.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_c = Rotation2d.fromDegrees(-45)
ref_c = SwerveModuleVelocity(2, Rotation2d.fromDegrees(45))
ref_c.cosineScale(angle_c)
optimized_c = ref_c.cosineScale(angle_c)
assert ref_c.velocity == pytest.approx(0.0, abs=kEpsilon)
assert ref_c.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
assert optimized_c.velocity == pytest.approx(0.0, abs=kEpsilon)
assert optimized_c.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_d = Rotation2d.fromDegrees(135)
ref_d = SwerveModuleVelocity(2, Rotation2d.fromDegrees(45))
ref_d.cosineScale(angle_d)
optimized_d = ref_d.cosineScale(angle_d)
assert ref_d.velocity == pytest.approx(0.0, abs=kEpsilon)
assert ref_d.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
assert optimized_d.velocity == pytest.approx(0.0, abs=kEpsilon)
assert optimized_d.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_e = Rotation2d.fromDegrees(-135)
ref_e = SwerveModuleVelocity(2, Rotation2d.fromDegrees(45))
ref_e.cosineScale(angle_e)
optimized_e = ref_e.cosineScale(angle_e)
assert ref_e.velocity == pytest.approx(-2.0, abs=kEpsilon)
assert ref_e.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
assert optimized_e.velocity == pytest.approx(-2.0, abs=kEpsilon)
assert optimized_e.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_f = Rotation2d.fromDegrees(180)
ref_f = SwerveModuleVelocity(2, Rotation2d.fromDegrees(45))
ref_f.cosineScale(angle_f)
optimized_f = ref_f.cosineScale(angle_f)
assert ref_f.velocity == pytest.approx(-math.sqrt(2.0), abs=kEpsilon)
assert ref_f.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
assert optimized_f.velocity == pytest.approx(-math.sqrt(2.0), abs=kEpsilon)
assert optimized_f.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_g = Rotation2d.fromDegrees(0)
ref_g = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(45))
ref_g.cosineScale(angle_g)
optimized_g = ref_g.cosineScale(angle_g)
assert ref_g.velocity == pytest.approx(-math.sqrt(2.0), abs=kEpsilon)
assert ref_g.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
assert optimized_g.velocity == pytest.approx(-math.sqrt(2.0), abs=kEpsilon)
assert optimized_g.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_h = Rotation2d.fromDegrees(45)
ref_h = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(45))
ref_h.cosineScale(angle_h)
optimized_h = ref_h.cosineScale(angle_h)
assert ref_h.velocity == pytest.approx(-2.0, abs=kEpsilon)
assert ref_h.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
assert optimized_h.velocity == pytest.approx(-2.0, abs=kEpsilon)
assert optimized_h.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_i = Rotation2d.fromDegrees(-45)
ref_i = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(45))
ref_i.cosineScale(angle_i)
optimized_i = ref_i.cosineScale(angle_i)
assert ref_i.velocity == pytest.approx(0.0, abs=kEpsilon)
assert ref_i.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
assert optimized_i.velocity == pytest.approx(0.0, abs=kEpsilon)
assert optimized_i.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_j = Rotation2d.fromDegrees(135)
ref_j = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(45))
ref_j.cosineScale(angle_j)
optimized_j = ref_j.cosineScale(angle_j)
assert ref_j.velocity == pytest.approx(0.0, abs=kEpsilon)
assert ref_j.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
assert optimized_j.velocity == pytest.approx(0.0, abs=kEpsilon)
assert optimized_j.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_k = Rotation2d.fromDegrees(-135)
ref_k = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(45))
ref_k.cosineScale(angle_k)
optimized_k = ref_k.cosineScale(angle_k)
assert ref_k.velocity == pytest.approx(2.0, abs=kEpsilon)
assert ref_k.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
assert optimized_k.velocity == pytest.approx(2.0, abs=kEpsilon)
assert optimized_k.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
angle_l = Rotation2d.fromDegrees(180)
ref_l = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(45))
ref_l.cosineScale(angle_l)
optimized_l = ref_l.cosineScale(angle_l)
assert ref_l.velocity == pytest.approx(math.sqrt(2.0), abs=kEpsilon)
assert ref_l.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
assert optimized_l.velocity == pytest.approx(math.sqrt(2.0), abs=kEpsilon)
assert optimized_l.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
def test_equality():