Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-01 02:41:48 +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());

20
wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
View File

@@ -32,11 +32,12 @@ class SwerveDriveOdometryTest : public ::testing::Test {
TEST_F(SwerveDriveOdometryTest, TwoIterations) {
SwerveModulePosition position{0.5_m, 0_deg};
m_odometry.ResetPosition(Pose2d{}, 0_rad, zero, zero, zero, zero);
m_odometry.ResetPosition(Pose2d{}, 0_rad, {zero, zero, zero, zero});
m_odometry.Update(0_deg, zero, zero, zero, zero);
m_odometry.Update(0_deg, {zero, zero, zero, zero});
auto pose = m_odometry.Update(0_deg, position, position, position, position);
auto pose =
m_odometry.Update(0_deg, {position, position, position, position});
EXPECT_NEAR(0.5, pose.X().value(), kEpsilon);
EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon);
@@ -49,8 +50,8 @@ TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) {
SwerveModulePosition bl{18.85_m, -90_deg};
SwerveModulePosition br{42.15_m, -26.565_deg};
m_odometry.ResetPosition(Pose2d{}, 0_rad, zero, zero, zero, zero);
auto pose = m_odometry.Update(90_deg, fl, fr, bl, br);
m_odometry.ResetPosition(Pose2d{}, 0_rad, {zero, zero, zero, zero});
auto pose = m_odometry.Update(90_deg, {fl, fr, bl, br});
EXPECT_NEAR(12.0, pose.X().value(), kEpsilon);
EXPECT_NEAR(12.0, pose.Y().value(), kEpsilon);
@@ -58,11 +59,12 @@ TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) {
}
TEST_F(SwerveDriveOdometryTest, GyroAngleReset) {
m_odometry.ResetPosition(Pose2d{}, 90_deg, zero, zero, zero, zero);
m_odometry.ResetPosition(Pose2d{}, 90_deg, {zero, zero, zero, zero});
SwerveModulePosition position{0.5_m, 0_deg};
auto pose = m_odometry.Update(90_deg, position, position, position, position);
auto pose =
m_odometry.Update(90_deg, {position, position, position, position});
EXPECT_NEAR(0.5, pose.X().value(), kEpsilon);
EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon);
@@ -116,7 +118,7 @@ TEST_F(SwerveDriveOdometryTest, AccuracyFacingTrajectory) {
auto xhat =
odometry.Update(groundTruthState.pose.Rotation() +
frc::Rotation2d{distribution(generator) * 0.05_rad},
fl, fr, bl, br);
{fl, fr, bl, br});
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation())
.value();
@@ -178,7 +180,7 @@ TEST_F(SwerveDriveOdometryTest, AccuracyFacingXAxis) {
br.angle = groundTruthState.pose.Rotation();
auto xhat = odometry.Update(
frc::Rotation2d{distribution(generator) * 0.05_rad}, fl, fr, bl, br);
frc::Rotation2d{distribution(generator) * 0.05_rad}, {fl, fr, bl, br});
double error = groundTruthState.pose.Translation()
.Distance(xhat.Translation())
.value();