Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-30 02:31:44 +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

3
wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java
View File

@@ -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));
}
}

1
wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
View File

@@ -62,5 +62,4 @@ void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl,
1, 1, (rl.X() - rl.Y()).template to<double>(),
1, -1, (-(rr.X() + rr.Y())).template to<double>();
// clang-format on
m_inverseKinematics /= std::sqrt(2);
}

136
wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.java
View File

@@ -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

14
wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java
View File

@@ -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));
}
}

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) {

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

@@ -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<double>(), 0.5, 0.01);
EXPECT_NEAR(pose.X().to<double>(), 0.3536, 0.01);
EXPECT_NEAR(pose.Y().to<double>(), 0.0, 0.01);
EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 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<double>(), 12, 0.01);
EXPECT_NEAR(pose.Y().to<double>(), 12, 0.01);
EXPECT_NEAR(pose.X().to<double>(), 8.4855, 0.01);
EXPECT_NEAR(pose.Y().to<double>(), 8.4855, 0.01);
EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 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<double>(), 0.5, 0.01);
EXPECT_NEAR(pose.X().to<double>(), 0.3536, 0.01);
EXPECT_NEAR(pose.Y().to<double>(), 0.0, 0.01);
EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, 0.01);
}