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

2
wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
View File

@@ -18,6 +18,8 @@
TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
frc::DifferentialDrivePoseEstimator estimator{frc::Rotation2d{},
0_m,
0_m,
frc::Pose2d{},
{0.02, 0.02, 0.01, 0.02, 0.02},
{0.01, 0.01, 0.001},

4
wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
View File

@@ -19,8 +19,8 @@ TEST(MecanumDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
frc::MecanumDriveWheelPositions wheelPositions;
frc::MecanumDrivePoseEstimator estimator{frc::Rotation2d{},
frc::Pose2d{},
wheelPositions,
frc::Pose2d{},
kinematics,
{0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
{0.05, 0.05, 0.05, 0.05, 0.05},
@@ -103,8 +103,8 @@ TEST(MecanumDrivePoseEstimatorTest, AccuracyFacingXAxis) {
frc::MecanumDriveWheelPositions wheelPositions;
frc::MecanumDrivePoseEstimator estimator{frc::Rotation2d{},
frc::Pose2d{},
wheelPositions,
frc::Pose2d{},
kinematics,
{0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
{0.05, 0.05, 0.05, 0.05, 0.05},

4
wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
View File

@@ -23,8 +23,8 @@ TEST(SwerveDrivePoseEstimatorTest, AccuracyFacingTrajectory) {
frc::SwerveDrivePoseEstimator<4> estimator{
frc::Rotation2d{},
frc::Pose2d{},
{fl, fr, bl, br},
frc::Pose2d{},
kinematics,
{0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
{0.05, 0.05, 0.05, 0.05, 0.05},
@@ -115,8 +115,8 @@ TEST(SwerveDrivePoseEstimatorTest, AccuracyFacingXAxis) {
frc::SwerveDrivePoseEstimator<4> estimator{
frc::Rotation2d{},
frc::Pose2d{},
{fl, fr, bl, br},
frc::Pose2d{},
kinematics,
{0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
{0.05, 0.05, 0.05, 0.05, 0.05},

2
wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
View File

@@ -13,7 +13,7 @@ static constexpr double kEpsilon = 1E-9;
using namespace frc;
TEST(DifferentialDriveOdometryTest, EncoderDistances) {
DifferentialDriveOdometry odometry{45_deg};
DifferentialDriveOdometry odometry{45_deg, 0_m, 0_m};
const auto& pose =
odometry.Update(135_deg, 0_m, units::meter_t{5 * std::numbers::pi});

8
wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
View File

@@ -27,7 +27,7 @@ class MecanumDriveOdometryTest : public ::testing::Test {
TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) {
MecanumDriveWheelPositions wheelDeltas{3.536_m, 3.536_m, 3.536_m, 3.536_m};
odometry.ResetPosition(Pose2d{}, 0_rad, wheelDeltas);
odometry.ResetPosition(0_rad, wheelDeltas, Pose2d{});
odometry.Update(0_deg, wheelDeltas);
auto secondPose = odometry.Update(0_deg, wheelDeltas);
@@ -38,7 +38,7 @@ TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) {
}
TEST_F(MecanumDriveOdometryTest, TwoIterations) {
odometry.ResetPosition(Pose2d{}, 0_rad, zero);
odometry.ResetPosition(0_rad, zero, Pose2d{});
MecanumDriveWheelPositions wheelDeltas{0.3536_m, 0.3536_m, 0.3536_m,
0.3536_m};
@@ -51,7 +51,7 @@ TEST_F(MecanumDriveOdometryTest, TwoIterations) {
}
TEST_F(MecanumDriveOdometryTest, 90DegreeTurn) {
odometry.ResetPosition(Pose2d{}, 0_rad, zero);
odometry.ResetPosition(0_rad, zero, Pose2d{});
MecanumDriveWheelPositions wheelDeltas{-13.328_m, 39.986_m, -13.329_m,
39.986_m};
odometry.Update(0_deg, MecanumDriveWheelPositions{});
@@ -63,7 +63,7 @@ TEST_F(MecanumDriveOdometryTest, 90DegreeTurn) {
}
TEST_F(MecanumDriveOdometryTest, GyroAngleReset) {
odometry.ResetPosition(Pose2d{}, 90_deg, zero);
odometry.ResetPosition(90_deg, zero, Pose2d{});
MecanumDriveWheelPositions wheelDeltas{0.3536_m, 0.3536_m, 0.3536_m,
0.3536_m};

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

@@ -32,7 +32,7 @@ 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(0_rad, {zero, zero, zero, zero}, Pose2d{});
m_odometry.Update(0_deg, {zero, zero, zero, zero});
@@ -50,7 +50,7 @@ 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});
m_odometry.ResetPosition(0_rad, {zero, zero, zero, zero}, Pose2d{});
auto pose = m_odometry.Update(90_deg, {fl, fr, bl, br});
EXPECT_NEAR(12.0, pose.X().value(), kEpsilon);
@@ -59,7 +59,7 @@ TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) {
}
TEST_F(SwerveDriveOdometryTest, GyroAngleReset) {
m_odometry.ResetPosition(Pose2d{}, 90_deg, {zero, zero, zero, zero});
m_odometry.ResetPosition(90_deg, {zero, zero, zero, zero}, Pose2d{});
SwerveModulePosition position{0.5_m, 0_deg};