diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java index d8208a3a85..eea3f2e280 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java @@ -30,7 +30,7 @@ import org.ejml.simple.SimpleMatrix; * field using encoders and a gyro. */ public class MecanumDriveKinematics { - private SimpleMatrix m_inverseKinematics; + private final SimpleMatrix m_inverseKinematics; private final SimpleMatrix m_forwardKinematics; private final Translation2d m_frontLeftWheelMeters; @@ -167,6 +167,5 @@ public class MecanumDriveKinematics { m_inverseKinematics.setRow(1, 0, 1, 1, fr.getX() - fr.getY()); m_inverseKinematics.setRow(2, 0, 1, 1, rl.getX() - rl.getY()); m_inverseKinematics.setRow(3, 0, 1, -1, -(rr.getX() + rr.getY())); - m_inverseKinematics = m_inverseKinematics.scale(1.0 / Math.sqrt(2)); } } diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp index 054799e950..a89e4f9fed 100644 --- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp +++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp @@ -62,5 +62,4 @@ void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl, 1, 1, (rl.X() - rl.Y()).template to(), 1, -1, (-(rr.X() + rr.Y())).template to(); // clang-format on - m_inverseKinematics /= std::sqrt(2); } diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.java index 0cf9aa1934..64667f0204 100644 --- a/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.java @@ -26,17 +26,11 @@ class MecanumDriveKinematicsTest { ChassisSpeeds speeds = new ChassisSpeeds(5, 0, 0); var moduleStates = m_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 - */ - assertAll( - () -> assertEquals(3.536, moduleStates.frontLeftMetersPerSecond, 0.1), - () -> assertEquals(3.536, moduleStates.frontRightMetersPerSecond, 0.1), - () -> assertEquals(3.536, moduleStates.rearLeftMetersPerSecond, 0.1), - () -> assertEquals(3.536, moduleStates.rearRightMetersPerSecond, 0.1)); + () -> assertEquals(5.0, moduleStates.frontLeftMetersPerSecond, 0.1), + () -> assertEquals(5.0, moduleStates.frontRightMetersPerSecond, 0.1), + () -> assertEquals(5.0, moduleStates.rearLeftMetersPerSecond, 0.1), + () -> assertEquals(5.0, moduleStates.rearRightMetersPerSecond, 0.1)); } @Test @@ -45,13 +39,8 @@ class MecanumDriveKinematicsTest { var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536); var moduleStates = m_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]] - */ - assertAll( - () -> assertEquals(5, moduleStates.vxMetersPerSecond, 0.1), + () -> assertEquals(3.536, moduleStates.vxMetersPerSecond, 0.1), () -> assertEquals(0, moduleStates.vyMetersPerSecond, 0.1), () -> assertEquals(0, moduleStates.omegaRadiansPerSecond, 0.1)); } @@ -61,17 +50,11 @@ class MecanumDriveKinematicsTest { ChassisSpeeds speeds = new ChassisSpeeds(0, 4, 0); var moduleStates = m_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 - */ - assertAll( - () -> assertEquals(-2.828427, moduleStates.frontLeftMetersPerSecond, 0.1), - () -> assertEquals(2.828427, moduleStates.frontRightMetersPerSecond, 0.1), - () -> assertEquals(2.828427, moduleStates.rearLeftMetersPerSecond, 0.1), - () -> assertEquals(-2.828427, moduleStates.rearRightMetersPerSecond, 0.1)); + () -> assertEquals(-4.0, moduleStates.frontLeftMetersPerSecond, 0.1), + () -> assertEquals(4.0, moduleStates.frontRightMetersPerSecond, 0.1), + () -> assertEquals(4.0, moduleStates.rearLeftMetersPerSecond, 0.1), + () -> assertEquals(-4.0, moduleStates.rearRightMetersPerSecond, 0.1)); } @Test @@ -80,14 +63,9 @@ class MecanumDriveKinematicsTest { var wheelSpeeds = new MecanumDriveWheelSpeeds(-2.828427, 2.828427, 2.828427, -2.828427); var moduleStates = m_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]] - */ - assertAll( () -> assertEquals(0, moduleStates.vxMetersPerSecond, 0.1), - () -> assertEquals(4, moduleStates.vyMetersPerSecond, 0.1), + () -> assertEquals(2.8284, moduleStates.vyMetersPerSecond, 0.1), () -> assertEquals(0, moduleStates.omegaRadiansPerSecond, 0.1)); } @@ -96,29 +74,18 @@ class MecanumDriveKinematicsTest { ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI); var moduleStates = m_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 - */ - assertAll( - () -> assertEquals(-106.629191, moduleStates.frontLeftMetersPerSecond, 0.1), - () -> assertEquals(106.629191, moduleStates.frontRightMetersPerSecond, 0.1), - () -> assertEquals(-106.629191, moduleStates.rearLeftMetersPerSecond, 0.1), - () -> assertEquals(106.629191, moduleStates.rearRightMetersPerSecond, 0.1)); + () -> assertEquals(-150.79645, moduleStates.frontLeftMetersPerSecond, 0.1), + () -> assertEquals(150.79645, moduleStates.frontRightMetersPerSecond, 0.1), + () -> assertEquals(-150.79645, moduleStates.rearLeftMetersPerSecond, 0.1), + () -> assertEquals(150.79645, moduleStates.rearRightMetersPerSecond, 0.1)); } @Test void testRotationForwardKinematicsKinematics() { - var wheelSpeeds = new MecanumDriveWheelSpeeds(-106.629191, 106.629191, -106.629191, 106.629191); + var wheelSpeeds = new MecanumDriveWheelSpeeds(-150.79645, 150.79645, -150.79645, 150.79645); var moduleStates = m_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]] - */ - assertAll( () -> assertEquals(0, moduleStates.vxMetersPerSecond, 0.1), () -> assertEquals(0, moduleStates.vyMetersPerSecond, 0.1), @@ -130,17 +97,11 @@ class MecanumDriveKinematicsTest { ChassisSpeeds speeds = new ChassisSpeeds(2, 3, 1); var moduleStates = m_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 - */ - assertAll( - () -> assertEquals(-17.677670, moduleStates.frontLeftMetersPerSecond, 0.1), - () -> assertEquals(20.506097, moduleStates.frontRightMetersPerSecond, 0.1), - () -> assertEquals(-13.435, moduleStates.rearLeftMetersPerSecond, 0.1), - () -> assertEquals(16.26, moduleStates.rearRightMetersPerSecond, 0.1)); + () -> assertEquals(-25.0, moduleStates.frontLeftMetersPerSecond, 0.1), + () -> assertEquals(29.0, moduleStates.frontRightMetersPerSecond, 0.1), + () -> assertEquals(-19.0, moduleStates.rearLeftMetersPerSecond, 0.1), + () -> assertEquals(23.0, moduleStates.rearRightMetersPerSecond, 0.1)); } @Test @@ -148,15 +109,10 @@ class MecanumDriveKinematicsTest { var wheelSpeeds = new MecanumDriveWheelSpeeds(-17.677670, 20.51, -13.44, 16.26); var moduleStates = m_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]] - */ - assertAll( - () -> assertEquals(2, moduleStates.vxMetersPerSecond, 0.1), - () -> assertEquals(3, moduleStates.vyMetersPerSecond, 0.1), - () -> assertEquals(1, moduleStates.omegaRadiansPerSecond, 0.1)); + () -> assertEquals(1.413, moduleStates.vxMetersPerSecond, 0.1), + () -> assertEquals(2.122, moduleStates.vyMetersPerSecond, 0.1), + () -> assertEquals(0.707, moduleStates.omegaRadiansPerSecond, 0.1)); } @Test @@ -164,17 +120,11 @@ class MecanumDriveKinematicsTest { ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 1); var moduleStates = m_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 - */ - assertAll( () -> assertEquals(0, moduleStates.frontLeftMetersPerSecond, 0.1), - () -> assertEquals(16.971, moduleStates.frontRightMetersPerSecond, 0.1), - () -> assertEquals(-16.971, moduleStates.rearLeftMetersPerSecond, 0.1), - () -> assertEquals(33.941, moduleStates.rearRightMetersPerSecond, 0.1)); + () -> assertEquals(24.0, moduleStates.frontRightMetersPerSecond, 0.1), + () -> assertEquals(-24.0, moduleStates.rearLeftMetersPerSecond, 0.1), + () -> assertEquals(48.0, moduleStates.rearRightMetersPerSecond, 0.1)); } @Test @@ -182,15 +132,10 @@ class MecanumDriveKinematicsTest { var wheelSpeeds = new MecanumDriveWheelSpeeds(0, 16.971, -16.971, 33.941); var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds); - /* - By equation (13.13) of the state-space-guide, the chassis motion from the wheel - velocities should be [[12][-12][1]] - */ - assertAll( - () -> assertEquals(12, moduleStates.vxMetersPerSecond, 0.1), - () -> assertEquals(-12, moduleStates.vyMetersPerSecond, 0.1), - () -> assertEquals(1, moduleStates.omegaRadiansPerSecond, 0.1)); + () -> assertEquals(8.48525, moduleStates.vxMetersPerSecond, 0.1), + () -> assertEquals(-8.48525, moduleStates.vyMetersPerSecond, 0.1), + () -> assertEquals(0.707, moduleStates.omegaRadiansPerSecond, 0.1)); } @Test @@ -198,17 +143,11 @@ class MecanumDriveKinematicsTest { ChassisSpeeds speeds = new ChassisSpeeds(5, 2, 1); var moduleStates = m_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 - */ - assertAll( - () -> assertEquals(2.12, moduleStates.frontLeftMetersPerSecond, 0.1), - () -> assertEquals(21.92, moduleStates.frontRightMetersPerSecond, 0.1), - () -> assertEquals(-12.02, moduleStates.rearLeftMetersPerSecond, 0.1), - () -> assertEquals(36.06, moduleStates.rearRightMetersPerSecond, 0.1)); + () -> assertEquals(3.0, moduleStates.frontLeftMetersPerSecond, 0.1), + () -> assertEquals(31.0, moduleStates.frontRightMetersPerSecond, 0.1), + () -> assertEquals(-17.0, moduleStates.rearLeftMetersPerSecond, 0.1), + () -> assertEquals(51.0, moduleStates.rearRightMetersPerSecond, 0.1)); } @Test @@ -217,15 +156,10 @@ class MecanumDriveKinematicsTest { var wheelSpeeds = new MecanumDriveWheelSpeeds(2.12, 21.92, -12.02, 36.06); var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds); - /* - By equation (13.13) of the state-space-guide, the chassis motion from the wheel - velocities should be [[17][-10][1]] - */ - assertAll( - () -> assertEquals(17, moduleStates.vxMetersPerSecond, 0.1), - () -> assertEquals(-10, moduleStates.vyMetersPerSecond, 0.1), - () -> assertEquals(1, moduleStates.omegaRadiansPerSecond, 0.1)); + () -> assertEquals(12.02, moduleStates.vxMetersPerSecond, 0.1), + () -> assertEquals(-7.07, moduleStates.vyMetersPerSecond, 0.1), + () -> assertEquals(0.707, moduleStates.omegaRadiansPerSecond, 0.1)); } @Test diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java index 5d26d0def7..483223fce1 100644 --- a/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java @@ -46,8 +46,8 @@ class MecanumDriveOdometryTest { var pose = m_odometry.updateWithTime(0.10, new Rotation2d(), wheelSpeeds); assertAll( - () -> assertEquals(5.0 / 10.0, pose.getX(), 0.01), - () -> assertEquals(0, pose.getY(), 0.01), + () -> assertEquals(0.3536, pose.getX(), 0.01), + () -> assertEquals(0.0, pose.getY(), 0.01), () -> assertEquals(0.0, pose.getRotation().getDegrees(), 0.01)); } @@ -61,8 +61,8 @@ class MecanumDriveOdometryTest { final var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), wheelSpeeds); assertAll( - () -> assertEquals(12.0, pose.getX(), 0.01), - () -> assertEquals(12.0, pose.getY(), 0.01), + () -> assertEquals(8.4855, pose.getX(), 0.01), + () -> assertEquals(8.4855, pose.getY(), 0.01), () -> assertEquals(90.0, pose.getRotation().getDegrees(), 0.01)); } @@ -76,8 +76,8 @@ class MecanumDriveOdometryTest { var pose = m_odometry.updateWithTime(1.0, gyro, speeds); assertAll( - () -> assertEquals(5.0, pose.getX(), 0.1), - () -> assertEquals(0.00, pose.getY(), 0.1), - () -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1)); + () -> assertEquals(3.536, pose.getX(), 0.1), + () -> assertEquals(0.0, pose.getY(), 0.1), + () -> assertEquals(0.0, pose.getRotation().getRadians(), 0.1)); } } diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp index a8134a631e..f9eaa8bfef 100644 --- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp @@ -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(), 0.1); - EXPECT_NEAR(3.536, moduleStates.frontRight.to(), 0.1); - EXPECT_NEAR(3.536, moduleStates.rearLeft.to(), 0.1); - EXPECT_NEAR(3.536, moduleStates.rearRight.to(), 0.1); + EXPECT_NEAR(5.0, moduleStates.frontLeft.to(), 0.1); + EXPECT_NEAR(5.0, moduleStates.frontRight.to(), 0.1); + EXPECT_NEAR(5.0, moduleStates.rearLeft.to(), 0.1); + EXPECT_NEAR(5.0, moduleStates.rearRight.to(), 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(), 0.1); EXPECT_NEAR(0.0, chassisSpeeds.vy.to(), 0.1); EXPECT_NEAR(0.0, chassisSpeeds.omega.to(), 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(), 0.1); - EXPECT_NEAR(2.828427, moduleStates.frontRight.to(), 0.1); - EXPECT_NEAR(2.828427, moduleStates.rearLeft.to(), 0.1); - EXPECT_NEAR(-2.828427, moduleStates.rearRight.to(), 0.1); + EXPECT_NEAR(-4.0, moduleStates.frontLeft.to(), 0.1); + EXPECT_NEAR(4.0, moduleStates.frontRight.to(), 0.1); + EXPECT_NEAR(4.0, moduleStates.rearLeft.to(), 0.1); + EXPECT_NEAR(-4.0, moduleStates.rearRight.to(), 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(), 0.1); - EXPECT_NEAR(4.0, chassisSpeeds.vy.to(), 0.1); + EXPECT_NEAR(5.0, chassisSpeeds.vy.to(), 0.1); EXPECT_NEAR(0.0, chassisSpeeds.omega.to(), 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(), 0.1); - EXPECT_NEAR(106.62919, moduleStates.frontRight.to(), 0.1); - EXPECT_NEAR(-106.62919, moduleStates.rearLeft.to(), 0.1); - EXPECT_NEAR(106.62919, moduleStates.rearRight.to(), 0.1); + EXPECT_NEAR(-150.79644737, moduleStates.frontLeft.to(), 0.1); + EXPECT_NEAR(150.79644737, moduleStates.frontRight.to(), 0.1); + EXPECT_NEAR(-150.79644737, moduleStates.rearLeft.to(), 0.1); + EXPECT_NEAR(150.79644737, moduleStates.rearRight.to(), 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(), 0.1); EXPECT_NEAR(0.0, chassisSpeeds.vy.to(), 0.1); EXPECT_NEAR(2 * wpi::math::pi, chassisSpeeds.omega.to(), 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(), 0.1); - EXPECT_NEAR(20.506097, moduleStates.frontRight.to(), 0.1); - EXPECT_NEAR(-13.435, moduleStates.rearLeft.to(), 0.1); - EXPECT_NEAR(16.26, moduleStates.rearRight.to(), 0.1); + EXPECT_NEAR(-25.0, moduleStates.frontLeft.to(), 0.1); + EXPECT_NEAR(29.0, moduleStates.frontRight.to(), 0.1); + EXPECT_NEAR(-19.0, moduleStates.rearLeft.to(), 0.1); + EXPECT_NEAR(23.0, moduleStates.rearRight.to(), 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(), 0.1); - EXPECT_NEAR(3.0, chassisSpeeds.vy.to(), 0.1); - EXPECT_NEAR(1.0, chassisSpeeds.omega.to(), 0.1); + EXPECT_NEAR(1.41335, chassisSpeeds.vx.to(), 0.1); + EXPECT_NEAR(2.1221, chassisSpeeds.vy.to(), 0.1); + EXPECT_NEAR(0.707, chassisSpeeds.omega.to(), 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(), 0.1); - EXPECT_NEAR(16.971, moduleStates.frontRight.to(), 0.1); - EXPECT_NEAR(-16.971, moduleStates.rearLeft.to(), 0.1); - EXPECT_NEAR(33.941, moduleStates.rearRight.to(), 0.1); + EXPECT_NEAR(24.0, moduleStates.frontRight.to(), 0.1); + EXPECT_NEAR(-24.0, moduleStates.rearLeft.to(), 0.1); + EXPECT_NEAR(48.0, moduleStates.rearRight.to(), 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(), 0.1); - EXPECT_NEAR(-12, chassisSpeeds.vy.to(), 0.1); - EXPECT_NEAR(1.0, chassisSpeeds.omega.to(), 0.1); + EXPECT_NEAR(8.48525, chassisSpeeds.vx.to(), 0.1); + EXPECT_NEAR(-8.48525, chassisSpeeds.vy.to(), 0.1); + EXPECT_NEAR(0.707, chassisSpeeds.omega.to(), 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(), 0.1); - EXPECT_NEAR(21.92, moduleStates.frontRight.to(), 0.1); - EXPECT_NEAR(-12.02, moduleStates.rearLeft.to(), 0.1); - EXPECT_NEAR(36.06, moduleStates.rearRight.to(), 0.1); + EXPECT_NEAR(3.0, moduleStates.frontLeft.to(), 0.1); + EXPECT_NEAR(31.0, moduleStates.frontRight.to(), 0.1); + EXPECT_NEAR(-17.0, moduleStates.rearLeft.to(), 0.1); + EXPECT_NEAR(51.0, moduleStates.rearRight.to(), 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(), 0.1); - EXPECT_NEAR(-10, chassisSpeeds.vy.to(), 0.1); - EXPECT_NEAR(1.0, chassisSpeeds.omega.to(), 0.1); + EXPECT_NEAR(12.02, chassisSpeeds.vx.to(), 0.1); + EXPECT_NEAR(-7.07, chassisSpeeds.vy.to(), 0.1); + EXPECT_NEAR(0.707, chassisSpeeds.omega.to(), 0.1); } TEST_F(MecanumDriveKinematicsTest, NormalizeTest) { diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp index aada85c0dc..235930c436 100644 --- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp @@ -38,7 +38,7 @@ TEST_F(MecanumDriveOdometryTest, TwoIterations) { odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{}); auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(), speeds); - EXPECT_NEAR(pose.X().to(), 0.5, 0.01); + EXPECT_NEAR(pose.X().to(), 0.3536, 0.01); EXPECT_NEAR(pose.Y().to(), 0.0, 0.01); EXPECT_NEAR(pose.Rotation().Radians().to(), 0.0, 0.01); } @@ -50,8 +50,8 @@ TEST_F(MecanumDriveOdometryTest, Test90DegreeTurn) { odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{}); auto pose = odometry.UpdateWithTime(1_s, Rotation2d(90_deg), speeds); - EXPECT_NEAR(pose.X().to(), 12, 0.01); - EXPECT_NEAR(pose.Y().to(), 12, 0.01); + EXPECT_NEAR(pose.X().to(), 8.4855, 0.01); + EXPECT_NEAR(pose.Y().to(), 8.4855, 0.01); EXPECT_NEAR(pose.Rotation().Degrees().to(), 90.0, 0.01); } @@ -63,7 +63,7 @@ TEST_F(MecanumDriveOdometryTest, GyroAngleReset) { odometry.UpdateWithTime(0_s, Rotation2d(90_deg), MecanumDriveWheelSpeeds{}); auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(90_deg), speeds); - EXPECT_NEAR(pose.X().to(), 0.5, 0.01); + EXPECT_NEAR(pose.X().to(), 0.3536, 0.01); EXPECT_NEAR(pose.Y().to(), 0.0, 0.01); EXPECT_NEAR(pose.Rotation().Radians().to(), 0.0, 0.01); }