Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-03 03:01:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Fix pose estimator reset methods (#7225)

Browse Source
This commit is contained in:
Joseph Eng
2024-10-18 16:06:32 -07:00
committed by GitHub
parent ee22482f4a
commit 2054d0f57e
8 changed files with 474 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

80
wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
View File

@@ -4,6 +4,7 @@
package edu.wpi.first.math.estimator;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
@@ -25,6 +26,8 @@ import java.util.function.Function;
import org.junit.jupiter.api.Test;
class DifferentialDrivePoseEstimatorTest {
private static final double kEpsilon = 1e-9;
@Test
void testAccuracy() {
var kinematics = new DifferentialDriveKinematics(1);
@@ -355,4 +358,81 @@ class DifferentialDrivePoseEstimatorTest {
assertEquals(Optional.of(new Pose2d(1, 0.1, Rotation2d.kZero)), estimator.sampleAt(0.5));
assertEquals(Optional.of(new Pose2d(2, 0.1, Rotation2d.kZero)), estimator.sampleAt(2.5));
}
@Test
void testReset() {
var kinematics = new DifferentialDriveKinematics(1);
var estimator =
new DifferentialDrivePoseEstimator(
kinematics,
Rotation2d.kZero,
0,
0,
Pose2d.kZero,
VecBuilder.fill(1, 1, 1),
VecBuilder.fill(1, 1, 1));
// Test reset position
estimator.resetPosition(Rotation2d.kZero, 1, 1, new Pose2d(1, 0, Rotation2d.kZero));
assertAll(
() -> assertEquals(1, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
() ->
assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon));
// Test orientation and wheel positions
estimator.update(Rotation2d.kZero, 2, 2);
assertAll(
() -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
() ->
assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon));
// Test reset rotation
estimator.resetRotation(Rotation2d.kCCW_Pi_2);
assertAll(
() -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
() ->
assertEquals(
Math.PI / 2,
estimator.getEstimatedPosition().getRotation().getRadians(),
kEpsilon));
// Test orientation
estimator.update(Rotation2d.kZero, 3, 3);
assertAll(
() -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon),
() ->
assertEquals(
Math.PI / 2,
estimator.getEstimatedPosition().getRotation().getRadians(),
kEpsilon));
// Test reset translation
estimator.resetTranslation(new Translation2d(-1, -1));
assertAll(
() -> assertEquals(-1, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(-1, estimator.getEstimatedPosition().getY(), kEpsilon),
() ->
assertEquals(
Math.PI / 2,
estimator.getEstimatedPosition().getRotation().getRadians(),
kEpsilon));
// Test reset pose
estimator.resetPose(Pose2d.kZero);
assertAll(
() -> assertEquals(0, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
() ->
assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon));
}
}

87
wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
View File

@@ -4,6 +4,7 @@
package edu.wpi.first.math.estimator;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
@@ -26,6 +27,8 @@ import java.util.function.Function;
import org.junit.jupiter.api.Test;
class MecanumDrivePoseEstimatorTest {
private static final double kEpsilon = 1e-9;
@Test
void testAccuracyFacingTrajectory() {
var kinematics =
@@ -377,4 +380,88 @@ class MecanumDrivePoseEstimatorTest {
assertEquals(Optional.of(new Pose2d(1, 0.1, Rotation2d.kZero)), estimator.sampleAt(0.5));
assertEquals(Optional.of(new Pose2d(2, 0.1, Rotation2d.kZero)), estimator.sampleAt(2.5));
}
@Test
void testReset() {
var kinematics =
new MecanumDriveKinematics(
new Translation2d(1, 1),
new Translation2d(-1, 1),
new Translation2d(1, -1),
new Translation2d(-1, -1));
var estimator =
new MecanumDrivePoseEstimator(
kinematics,
Rotation2d.kZero,
new MecanumDriveWheelPositions(),
Pose2d.kZero,
VecBuilder.fill(1, 1, 1),
VecBuilder.fill(1, 1, 1));
// Test reset position
estimator.resetPosition(
Rotation2d.kZero,
new MecanumDriveWheelPositions(1, 1, 1, 1),
new Pose2d(1, 0, Rotation2d.kZero));
assertAll(
() -> assertEquals(1, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
() ->
assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon));
// Test orientation and wheel positions
estimator.update(Rotation2d.kZero, new MecanumDriveWheelPositions(2, 2, 2, 2));
assertAll(
() -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
() ->
assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon));
// Test reset rotation
estimator.resetRotation(Rotation2d.kCCW_Pi_2);
assertAll(
() -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
() ->
assertEquals(
Math.PI / 2,
estimator.getEstimatedPosition().getRotation().getRadians(),
kEpsilon));
// Test orientation
estimator.update(Rotation2d.kZero, new MecanumDriveWheelPositions(3, 3, 3, 3));
assertAll(
() -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon),
() ->
assertEquals(
Math.PI / 2,
estimator.getEstimatedPosition().getRotation().getRadians(),
kEpsilon));
// Test reset translation
estimator.resetTranslation(new Translation2d(-1, -1));
assertAll(
() -> assertEquals(-1, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(-1, estimator.getEstimatedPosition().getY(), kEpsilon),
() ->
assertEquals(
Math.PI / 2,
estimator.getEstimatedPosition().getRotation().getRadians(),
kEpsilon));
// Test reset pose
estimator.resetPose(Pose2d.kZero);
assertAll(
() -> assertEquals(0, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
() ->
assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon));
}
}

111
wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
View File

@@ -4,6 +4,7 @@
package edu.wpi.first.math.estimator;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
@@ -26,6 +27,8 @@ import java.util.function.Function;
import org.junit.jupiter.api.Test;
class SwerveDrivePoseEstimatorTest {
private static final double kEpsilon = 1e-9;
@Test
void testAccuracyFacingTrajectory() {
var kinematics =
@@ -422,4 +425,112 @@ class SwerveDrivePoseEstimatorTest {
assertEquals(Optional.of(new Pose2d(1, 0.1, Rotation2d.kZero)), estimator.sampleAt(0.5));
assertEquals(Optional.of(new Pose2d(2, 0.1, Rotation2d.kZero)), estimator.sampleAt(2.5));
}
@Test
void testReset() {
var kinematics =
new SwerveDriveKinematics(
new Translation2d(1, 1),
new Translation2d(-1, 1),
new Translation2d(1, -1),
new Translation2d(-1, -1));
var estimator =
new SwerveDrivePoseEstimator(
kinematics,
Rotation2d.kZero,
new SwerveModulePosition[] {
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition()
},
Pose2d.kZero,
VecBuilder.fill(1, 1, 1),
VecBuilder.fill(1, 1, 1));
// Test reset position
{
var modulePosition = new SwerveModulePosition(1, Rotation2d.kZero);
estimator.resetPosition(
Rotation2d.kZero,
new SwerveModulePosition[] {
modulePosition, modulePosition, modulePosition, modulePosition
},
new Pose2d(1, 0, Rotation2d.kZero));
}
assertAll(
() -> assertEquals(1, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
() ->
assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon));
// Test orientation and wheel positions
{
var modulePosition = new SwerveModulePosition(2, Rotation2d.kZero);
estimator.update(
Rotation2d.kZero,
new SwerveModulePosition[] {
modulePosition, modulePosition, modulePosition, modulePosition
});
}
assertAll(
() -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
() ->
assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon));
// Test reset rotation
estimator.resetRotation(Rotation2d.kCCW_Pi_2);
assertAll(
() -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
() ->
assertEquals(
Math.PI / 2,
estimator.getEstimatedPosition().getRotation().getRadians(),
kEpsilon));
// Test orientation
{
var modulePosition = new SwerveModulePosition(3, Rotation2d.kZero);
estimator.update(
Rotation2d.kZero,
new SwerveModulePosition[] {
modulePosition, modulePosition, modulePosition, modulePosition
});
}
assertAll(
() -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon),
() ->
assertEquals(
Math.PI / 2,
estimator.getEstimatedPosition().getRotation().getRadians(),
kEpsilon));
// Test reset translation
estimator.resetTranslation(new Translation2d(-1, -1));
assertAll(
() -> assertEquals(-1, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(-1, estimator.getEstimatedPosition().getY(), kEpsilon),
() ->
assertEquals(
Math.PI / 2,
estimator.getEstimatedPosition().getRotation().getRadians(),
kEpsilon));
// Test reset pose
estimator.resetPose(Pose2d.kZero);
assertAll(
() -> assertEquals(0, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
() ->
assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon));
}
}