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

[wpimath] Fix MecanumDriveKinematics (#3266)

Browse Source
This commit is contained in:
Tyler Veness
2021-04-30 15:50:16 -07:00
committed by GitHub
parent ff52f207cc
commit 365f5449ca
6 changed files with 84 additions and 223 deletions
Download Patch File Download Diff File Expand all files Collapse all files

145
wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
View File

@@ -25,29 +25,16 @@ TEST_F(MecanumDriveKinematicsTest, StraightLineInverseKinematics) {
ChassisSpeeds speeds{5_mps, 0_mps, 0_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
/*
By equation (13.12) of the state-space-guide, the wheel speeds should
be as follows:
velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534
*/
EXPECT_NEAR(3.536, moduleStates.frontLeft.to<double>(), 0.1);
EXPECT_NEAR(3.536, moduleStates.frontRight.to<double>(), 0.1);
EXPECT_NEAR(3.536, moduleStates.rearLeft.to<double>(), 0.1);
EXPECT_NEAR(3.536, moduleStates.rearRight.to<double>(), 0.1);
EXPECT_NEAR(5.0, moduleStates.frontLeft.to<double>(), 0.1);
EXPECT_NEAR(5.0, moduleStates.frontRight.to<double>(), 0.1);
EXPECT_NEAR(5.0, moduleStates.rearLeft.to<double>(), 0.1);
EXPECT_NEAR(5.0, moduleStates.rearRight.to<double>(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, StraightLineForwardKinematics) {
MecanumDriveWheelSpeeds wheelSpeeds{3.536_mps, 3.536_mps, 3.536_mps,
3.536_mps};
MecanumDriveWheelSpeeds wheelSpeeds{5_mps, 5_mps, 5_mps, 5_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
/*
By equation (13.13) of the state-space-guide, the chassis motion from wheel
velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534 will be
[[5][0][0]]
*/
EXPECT_NEAR(5.0, chassisSpeeds.vx.to<double>(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.vy.to<double>(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.omega.to<double>(), 0.1);
@@ -57,31 +44,18 @@ TEST_F(MecanumDriveKinematicsTest, StrafeInverseKinematics) {
ChassisSpeeds speeds{0_mps, 4_mps, 0_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
/*
By equation (13.12) of the state-space-guide, the wheel speeds should
be as follows:
velocities: fl -2.828427 fr 2.828427 rl 2.828427 rr -2.828427
*/
EXPECT_NEAR(-2.828427, moduleStates.frontLeft.to<double>(), 0.1);
EXPECT_NEAR(2.828427, moduleStates.frontRight.to<double>(), 0.1);
EXPECT_NEAR(2.828427, moduleStates.rearLeft.to<double>(), 0.1);
EXPECT_NEAR(-2.828427, moduleStates.rearRight.to<double>(), 0.1);
EXPECT_NEAR(-4.0, moduleStates.frontLeft.to<double>(), 0.1);
EXPECT_NEAR(4.0, moduleStates.frontRight.to<double>(), 0.1);
EXPECT_NEAR(4.0, moduleStates.rearLeft.to<double>(), 0.1);
EXPECT_NEAR(-4.0, moduleStates.rearRight.to<double>(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, StrafeForwardKinematics) {
MecanumDriveWheelSpeeds wheelSpeeds{-2.828427_mps, 2.828427_mps, 2.828427_mps,
-2.828427_mps};
MecanumDriveWheelSpeeds wheelSpeeds{-5_mps, 5_mps, 5_mps, -5_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
/*
By equation (13.13) of the state-space-guide, the chassis motion from wheel
velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534 will be
[[5][0][0]]
*/
EXPECT_NEAR(0.0, chassisSpeeds.vx.to<double>(), 0.1);
EXPECT_NEAR(4.0, chassisSpeeds.vy.to<double>(), 0.1);
EXPECT_NEAR(5.0, chassisSpeeds.vy.to<double>(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.omega.to<double>(), 0.1);
}
@@ -90,29 +64,17 @@ TEST_F(MecanumDriveKinematicsTest, RotationInverseKinematics) {
units::radians_per_second_t(2 * wpi::math::pi)};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
/*
By equation (13.12) of the state-space-guide, the wheel speeds should
be as follows:
velocities: fl -106.629191 fr 106.629191 rl -106.629191 rr 106.629191
*/
EXPECT_NEAR(-106.62919, moduleStates.frontLeft.to<double>(), 0.1);
EXPECT_NEAR(106.62919, moduleStates.frontRight.to<double>(), 0.1);
EXPECT_NEAR(-106.62919, moduleStates.rearLeft.to<double>(), 0.1);
EXPECT_NEAR(106.62919, moduleStates.rearRight.to<double>(), 0.1);
EXPECT_NEAR(-150.79644737, moduleStates.frontLeft.to<double>(), 0.1);
EXPECT_NEAR(150.79644737, moduleStates.frontRight.to<double>(), 0.1);
EXPECT_NEAR(-150.79644737, moduleStates.rearLeft.to<double>(), 0.1);
EXPECT_NEAR(150.79644737, moduleStates.rearRight.to<double>(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, RotationForwardKinematics) {
MecanumDriveWheelSpeeds wheelSpeeds{-106.62919_mps, 106.62919_mps,
-106.62919_mps, 106.62919_mps};
MecanumDriveWheelSpeeds wheelSpeeds{-150.79644737_mps, 150.79644737_mps,
-150.79644737_mps, 150.79644737_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
/*
By equation (13.13) of the state-space-guide, the chassis motion from wheel
velocities: fl -106.629191 fr 106.629191 rl -106.629191 rr 106.629191 should
be [[0][0][2pi]]
*/
EXPECT_NEAR(0.0, chassisSpeeds.vx.to<double>(), 0.1);
EXPECT_NEAR(0.0, chassisSpeeds.vy.to<double>(), 0.1);
EXPECT_NEAR(2 * wpi::math::pi, chassisSpeeds.omega.to<double>(), 0.1);
@@ -122,16 +84,10 @@ TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationInverseKinematics) {
ChassisSpeeds speeds{2_mps, 3_mps, 1_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds);
/*
By equation (13.12) of the state-space-guide, the wheel speeds should
be as follows:
velocities: fl -17.677670 fr 20.506097 rl -13.435029 rr 16.263456
*/
EXPECT_NEAR(-17.677670, moduleStates.frontLeft.to<double>(), 0.1);
EXPECT_NEAR(20.506097, moduleStates.frontRight.to<double>(), 0.1);
EXPECT_NEAR(-13.435, moduleStates.rearLeft.to<double>(), 0.1);
EXPECT_NEAR(16.26, moduleStates.rearRight.to<double>(), 0.1);
EXPECT_NEAR(-25.0, moduleStates.frontLeft.to<double>(), 0.1);
EXPECT_NEAR(29.0, moduleStates.frontRight.to<double>(), 0.1);
EXPECT_NEAR(-19.0, moduleStates.rearLeft.to<double>(), 0.1);
EXPECT_NEAR(23.0, moduleStates.rearRight.to<double>(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationForwardKinematics) {
@@ -140,31 +96,19 @@ TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationForwardKinematics) {
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
/*
By equation (13.13) of the state-space-guide, the chassis motion from wheel
velocities: fl -17.677670 fr 20.506097 rl -13.435029 rr 16.263456 should be
[[2][3][1]]
*/
EXPECT_NEAR(2.0, chassisSpeeds.vx.to<double>(), 0.1);
EXPECT_NEAR(3.0, chassisSpeeds.vy.to<double>(), 0.1);
EXPECT_NEAR(1.0, chassisSpeeds.omega.to<double>(), 0.1);
EXPECT_NEAR(1.41335, chassisSpeeds.vx.to<double>(), 0.1);
EXPECT_NEAR(2.1221, chassisSpeeds.vy.to<double>(), 0.1);
EXPECT_NEAR(0.707, chassisSpeeds.omega.to<double>(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, OffCenterRotationInverseKinematics) {
ChassisSpeeds speeds{0_mps, 0_mps, 1_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl);
/*
By equation (13.12) of the state-space-guide, the wheel speeds should
be as follows:
velocities: fl 0.000000 fr 16.970563 rl -16.970563 rr 33.941125
*/
EXPECT_NEAR(0, moduleStates.frontLeft.to<double>(), 0.1);
EXPECT_NEAR(16.971, moduleStates.frontRight.to<double>(), 0.1);
EXPECT_NEAR(-16.971, moduleStates.rearLeft.to<double>(), 0.1);
EXPECT_NEAR(33.941, moduleStates.rearRight.to<double>(), 0.1);
EXPECT_NEAR(24.0, moduleStates.frontRight.to<double>(), 0.1);
EXPECT_NEAR(-24.0, moduleStates.rearLeft.to<double>(), 0.1);
EXPECT_NEAR(48.0, moduleStates.rearRight.to<double>(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, OffCenterRotationForwardKinematics) {
@@ -172,14 +116,9 @@ TEST_F(MecanumDriveKinematicsTest, OffCenterRotationForwardKinematics) {
33.941_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
/*
By equation (13.13) of the state-space-guide, the chassis motion from the
wheel velocities should be [[12][-12][1]]
*/
EXPECT_NEAR(12.0, chassisSpeeds.vx.to<double>(), 0.1);
EXPECT_NEAR(-12, chassisSpeeds.vy.to<double>(), 0.1);
EXPECT_NEAR(1.0, chassisSpeeds.omega.to<double>(), 0.1);
EXPECT_NEAR(8.48525, chassisSpeeds.vx.to<double>(), 0.1);
EXPECT_NEAR(-8.48525, chassisSpeeds.vy.to<double>(), 0.1);
EXPECT_NEAR(0.707, chassisSpeeds.omega.to<double>(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest,
@@ -187,15 +126,10 @@ TEST_F(MecanumDriveKinematicsTest,
ChassisSpeeds speeds{5_mps, 2_mps, 1_rad_per_s};
auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl);
/*
By equation (13.12) of the state-space-guide, the wheel speeds should
be as follows:
velocities: fl 2.121320 fr 21.920310 rl -12.020815 rr 36.062446
*/
EXPECT_NEAR(2.12, moduleStates.frontLeft.to<double>(), 0.1);
EXPECT_NEAR(21.92, moduleStates.frontRight.to<double>(), 0.1);
EXPECT_NEAR(-12.02, moduleStates.rearLeft.to<double>(), 0.1);
EXPECT_NEAR(36.06, moduleStates.rearRight.to<double>(), 0.1);
EXPECT_NEAR(3.0, moduleStates.frontLeft.to<double>(), 0.1);
EXPECT_NEAR(31.0, moduleStates.frontRight.to<double>(), 0.1);
EXPECT_NEAR(-17.0, moduleStates.rearLeft.to<double>(), 0.1);
EXPECT_NEAR(51.0, moduleStates.rearRight.to<double>(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest,
@@ -204,14 +138,9 @@ TEST_F(MecanumDriveKinematicsTest,
36.06_mps};
auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
/*
By equation (13.13) of the state-space-guide, the chassis motion from the
wheel velocities should be [[17][-10][1]]
*/
EXPECT_NEAR(17.0, chassisSpeeds.vx.to<double>(), 0.1);
EXPECT_NEAR(-10, chassisSpeeds.vy.to<double>(), 0.1);
EXPECT_NEAR(1.0, chassisSpeeds.omega.to<double>(), 0.1);
EXPECT_NEAR(12.02, chassisSpeeds.vx.to<double>(), 0.1);
EXPECT_NEAR(-7.07, chassisSpeeds.vy.to<double>(), 0.1);
EXPECT_NEAR(0.707, chassisSpeeds.omega.to<double>(), 0.1);
}
TEST_F(MecanumDriveKinematicsTest, NormalizeTest) {