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] Rework function signatures for Pose Estimation / Odometry (#4642)

Browse Source 
Forcing the use of SwerveModulePostition[] and wpi::array<SwerveModulePosition, len> in place of variadic function signatures.
This commit is contained in:
Jordan McMichael
2022-11-17 13:36:33 -05:00
committed by GitHub
parent 0401597d3b
commit ce4c45df13
9 changed files with 84 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java
View File

@@ -28,7 +28,8 @@ class SwerveDriveOdometryTest {
new SwerveDriveKinematics(m_fl, m_fr, m_bl, m_br);
private final SwerveDriveOdometry m_odometry =
new SwerveDriveOdometry(m_kinematics, new Rotation2d(), zero, zero, zero, zero);
new SwerveDriveOdometry(
m_kinematics, new Rotation2d(), new SwerveModulePosition[] {zero, zero, zero, zero});
@Test
void testTwoIterations() {
@@ -42,10 +43,12 @@ class SwerveDriveOdometryTest {
m_odometry.update(
new Rotation2d(),
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition());
new SwerveModulePosition[] {
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition()
});
var pose = m_odometry.update(new Rotation2d(), wheelDeltas);
assertAll(
@@ -70,7 +73,7 @@ class SwerveDriveOdometryTest {
};
final var zero = new SwerveModulePosition();
m_odometry.update(new Rotation2d(), zero, zero, zero, zero);
m_odometry.update(new Rotation2d(), new SwerveModulePosition[] {zero, zero, zero, zero});
final var pose = m_odometry.update(Rotation2d.fromDegrees(90.0), wheelDeltas);
assertAll(
@@ -84,11 +87,13 @@ class SwerveDriveOdometryTest {
var gyro = Rotation2d.fromDegrees(90.0);
var fieldAngle = Rotation2d.fromDegrees(0.0);
m_odometry.resetPosition(
new Pose2d(new Translation2d(), fieldAngle), gyro, zero, zero, zero, zero);
new Pose2d(new Translation2d(), fieldAngle),
gyro,
new SwerveModulePosition[] {zero, zero, zero, zero});
var delta = new SwerveModulePosition();
m_odometry.update(gyro, delta, delta, delta, delta);
m_odometry.update(gyro, new SwerveModulePosition[] {delta, delta, delta, delta});
delta = new SwerveModulePosition(1.0, Rotation2d.fromDegrees(0));
var pose = m_odometry.update(gyro, delta, delta, delta, delta);
var pose = m_odometry.update(gyro, new SwerveModulePosition[] {delta, delta, delta, delta});
assertAll(
() -> assertEquals(1.0, pose.getX(), 0.1),
@@ -104,7 +109,9 @@ class SwerveDriveOdometryTest {
new Translation2d(1, -1),
new Translation2d(-1, -1),
new Translation2d(-1, 1));
var odometry = new SwerveDriveOdometry(kinematics, new Rotation2d(), zero, zero, zero, zero);
var odometry =
new SwerveDriveOdometry(
kinematics, new Rotation2d(), new SwerveModulePosition[] {zero, zero, zero, zero});
SwerveModulePosition fl = new SwerveModulePosition();
SwerveModulePosition fr = new SwerveModulePosition();
@@ -159,10 +166,7 @@ class SwerveDriveOdometryTest {
.poseMeters
.getRotation()
.plus(new Rotation2d(rand.nextGaussian() * 0.05)),
fl,
fr,
bl,
br);
new SwerveModulePosition[] {fl, fr, bl, br});
double error =
groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
@@ -187,7 +191,9 @@ class SwerveDriveOdometryTest {
new Translation2d(1, -1),
new Translation2d(-1, -1),
new Translation2d(-1, 1));
var odometry = new SwerveDriveOdometry(kinematics, new Rotation2d(), zero, zero, zero, zero);
var odometry =
new SwerveDriveOdometry(
kinematics, new Rotation2d(), new SwerveModulePosition[] {zero, zero, zero, zero});
SwerveModulePosition fl = new SwerveModulePosition();
SwerveModulePosition fr = new SwerveModulePosition();
@@ -232,7 +238,10 @@ class SwerveDriveOdometryTest {
bl.angle = groundTruthState.poseMeters.getRotation();
br.angle = groundTruthState.poseMeters.getRotation();
var xHat = odometry.update(new Rotation2d(rand.nextGaussian() * 0.05), fl, fr, bl, br);
var xHat =
odometry.update(
new Rotation2d(rand.nextGaussian() * 0.05),
new SwerveModulePosition[] {fl, fr, bl, br});
double error =
groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());