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 odometry APIs to improve feature parity (#4645)

Browse Source 
Co-authored-by: Ryan Blue <ryanzblue@gmail.com>
This commit is contained in:
Jordan McMichael
2022-11-18 23:42:00 -05:00
committed by GitHub
parent e2d49181da
commit 902e8686d3
53 changed files with 266 additions and 157 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -27,6 +27,8 @@ class DifferentialDrivePoseEstimatorTest {
var estimator =
new DifferentialDrivePoseEstimator(
new Rotation2d(),
0,
0,
new Pose2d(),
new MatBuilder<>(Nat.N5(), Nat.N1()).fill(0.02, 0.02, 0.01, 0.02, 0.02),
new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.01, 0.01, 0.001),

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

@@ -32,8 +32,8 @@ class MecanumDrivePoseEstimatorTest {
var estimator =
new MecanumDrivePoseEstimator(
new Rotation2d(),
new Pose2d(),
wheelPositions,
new Pose2d(),
kinematics,
VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1),
VecBuilder.fill(0.05, 0.05, 0.05, 0.05, 0.05),
@@ -136,8 +136,8 @@ class MecanumDrivePoseEstimatorTest {
var estimator =
new MecanumDrivePoseEstimator(
new Rotation2d(),
new Pose2d(),
wheelPositions,
new Pose2d(),
kinematics,
VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1),
VecBuilder.fill(0.05, 0.05, 0.05, 0.05, 0.05),

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

@@ -43,8 +43,8 @@ class SwerveDrivePoseEstimatorTest {
Nat.N7(),
Nat.N5(),
new Rotation2d(),
new Pose2d(),
new SwerveModulePosition[] {fl, fr, bl, br},
new Pose2d(),
kinematics,
VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1),
VecBuilder.fill(0.005, 0.005, 0.005, 0.005, 0.005),
@@ -158,8 +158,8 @@ class SwerveDrivePoseEstimatorTest {
Nat.N7(),
Nat.N5(),
new Rotation2d(),
new Pose2d(),
new SwerveModulePosition[] {fl, fr, bl, br},
new Pose2d(),
kinematics,
VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1),
VecBuilder.fill(0.005, 0.005, 0.005, 0.005, 0.005),

4
wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometryTest.java
View File

@@ -14,11 +14,11 @@ import org.junit.jupiter.api.Test;
class DifferentialDriveOdometryTest {
private static final double kEpsilon = 1E-9;
private final DifferentialDriveOdometry m_odometry =
new DifferentialDriveOdometry(new Rotation2d());
new DifferentialDriveOdometry(new Rotation2d(), 0, 0);
@Test
void testOdometryWithEncoderDistances() {
m_odometry.resetPosition(new Pose2d(), Rotation2d.fromDegrees(45));
m_odometry.resetPosition(Rotation2d.fromDegrees(45), 0, 0, new Pose2d());
var pose = m_odometry.update(Rotation2d.fromDegrees(135.0), 0.0, 5 * Math.PI);
assertAll(

8
wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java
View File

@@ -34,7 +34,7 @@ class MecanumDriveOdometryTest {
void testMultipleConsecutiveUpdates() {
var wheelPositions = new MecanumDriveWheelPositions(3.536, 3.536, 3.536, 3.536);
m_odometry.resetPosition(new Pose2d(), new Rotation2d(), wheelPositions);
m_odometry.resetPosition(new Rotation2d(), wheelPositions, new Pose2d());
m_odometry.update(new Rotation2d(), wheelPositions);
var secondPose = m_odometry.update(new Rotation2d(), wheelPositions);
@@ -49,7 +49,7 @@ class MecanumDriveOdometryTest {
void testTwoIterations() {
// 5 units/sec in the x axis (forward)
final var wheelPositions = new MecanumDriveWheelPositions(0.3536, 0.3536, 0.3536, 0.3536);
m_odometry.resetPosition(new Pose2d(), new Rotation2d(), new MecanumDriveWheelPositions());
m_odometry.resetPosition(new Rotation2d(), new MecanumDriveWheelPositions(), new Pose2d());
m_odometry.update(new Rotation2d(), new MecanumDriveWheelPositions());
var pose = m_odometry.update(new Rotation2d(), wheelPositions);
@@ -65,7 +65,7 @@ class MecanumDriveOdometryTest {
// This is a 90 degree turn about the point between front left and rear left wheels
// fl -13.328649 fr 39.985946 rl -13.328649 rr 39.985946
final var wheelPositions = new MecanumDriveWheelPositions(-13.328, 39.986, -13.329, 39.986);
m_odometry.resetPosition(new Pose2d(), new Rotation2d(), new MecanumDriveWheelPositions());
m_odometry.resetPosition(new Rotation2d(), new MecanumDriveWheelPositions(), new Pose2d());
m_odometry.update(new Rotation2d(), new MecanumDriveWheelPositions());
final var pose = m_odometry.update(Rotation2d.fromDegrees(90.0), wheelPositions);
@@ -81,7 +81,7 @@ class MecanumDriveOdometryTest {
var gyro = Rotation2d.fromDegrees(90.0);
var fieldAngle = Rotation2d.fromDegrees(0.0);
m_odometry.resetPosition(
new Pose2d(new Translation2d(), fieldAngle), gyro, new MecanumDriveWheelPositions());
gyro, new MecanumDriveWheelPositions(), new Pose2d(new Translation2d(), fieldAngle));
var speeds = new MecanumDriveWheelPositions(3.536, 3.536, 3.536, 3.536);
m_odometry.update(gyro, new MecanumDriveWheelPositions());
var pose = m_odometry.update(gyro, speeds);

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

@@ -87,9 +87,9 @@ class SwerveDriveOdometryTest {
var gyro = Rotation2d.fromDegrees(90.0);
var fieldAngle = Rotation2d.fromDegrees(0.0);
m_odometry.resetPosition(
new Pose2d(new Translation2d(), fieldAngle),
gyro,
new SwerveModulePosition[] {zero, zero, zero, zero});
new SwerveModulePosition[] {zero, zero, zero, zero},
new Pose2d(new Translation2d(), fieldAngle));
var delta = new SwerveModulePosition();
m_odometry.update(gyro, new SwerveModulePosition[] {delta, delta, delta, delta});
delta = new SwerveModulePosition(1.0, Rotation2d.fromDegrees(0));