Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +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

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