diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp index 9eb82ef880..2fa9944c98 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp @@ -44,9 +44,9 @@ DriveSubsystem::DriveSubsystem() void DriveSubsystem::Periodic() { // Implementation of subsystem periodic method goes here. - m_odometry.Update(m_gyro.GetRotation2d(), m_frontLeft.GetPosition(), - m_rearLeft.GetPosition(), m_frontRight.GetPosition(), - m_rearRight.GetPosition()); + m_odometry.Update(m_gyro.GetRotation2d(), + {m_frontLeft.GetPosition(), m_rearLeft.GetPosition(), + m_frontRight.GetPosition(), m_rearRight.GetPosition()}); } void DriveSubsystem::Drive(units::meters_per_second_t xSpeed, diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java index 03929d5269..b3facf7442 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java @@ -8,6 +8,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.wpilibj.AnalogGyro; /** Represents a swerve drive style drivetrain. */ @@ -32,7 +33,15 @@ public class Drivetrain { m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation); private final SwerveDriveOdometry m_odometry = - new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d()); + new SwerveDriveOdometry( + m_kinematics, + m_gyro.getRotation2d(), + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }); public Drivetrain() { m_gyro.reset(); @@ -63,9 +72,11 @@ public class Drivetrain { public void updateOdometry() { m_odometry.update( m_gyro.getRotation2d(), - m_frontLeft.getPosition(), - m_frontRight.getPosition(), - m_backLeft.getPosition(), - m_backRight.getPosition()); + new SwerveModulePosition[] { + m_frontLeft.getPosition(), + m_frontRight.getPosition(), + m_backLeft.getPosition(), + m_backRight.getPosition() + }); } } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java index dc0a17d755..93a1cc4ebe 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java @@ -8,6 +8,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.ADXRS450_Gyro; import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants; @@ -57,7 +58,15 @@ public class DriveSubsystem extends SubsystemBase { // Odometry class for tracking robot pose SwerveDriveOdometry m_odometry = - new SwerveDriveOdometry(DriveConstants.kDriveKinematics, m_gyro.getRotation2d()); + new SwerveDriveOdometry( + DriveConstants.kDriveKinematics, + m_gyro.getRotation2d(), + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }); /** Creates a new DriveSubsystem. */ public DriveSubsystem() {} @@ -67,10 +76,12 @@ public class DriveSubsystem extends SubsystemBase { // Update the odometry in the periodic block m_odometry.update( m_gyro.getRotation2d(), - m_frontLeft.getPosition(), - m_frontRight.getPosition(), - m_rearLeft.getPosition(), - m_rearRight.getPosition()); + new SwerveModulePosition[] { + m_frontLeft.getPosition(), + m_frontRight.getPosition(), + m_rearLeft.getPosition(), + m_rearRight.getPosition() + }); } /** @@ -88,7 +99,15 @@ public class DriveSubsystem extends SubsystemBase { * @param pose The pose to which to set the odometry. */ public void resetOdometry(Pose2d pose) { - m_odometry.resetPosition(pose, m_gyro.getRotation2d()); + m_odometry.resetPosition( + pose, + m_gyro.getRotation2d(), + new SwerveModulePosition[] { + m_frontLeft.getPosition(), + m_frontRight.getPosition(), + m_rearLeft.getPosition(), + m_rearRight.getPosition() + }); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java index 6e2b1350ba..5cddda0715 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java @@ -252,7 +252,7 @@ public class SwerveDrivePoseEstimator modulePositions, const Pose2d& initialPose = Pose2d{}); - /** - * Resets the robot's position on the field. - * - * The gyroscope angle does not need to be reset here on the user's robot - * code. The library automatically takes care of offsetting the gyro angle. - * - * @param pose The position on the field that your robot is at. - * @param gyroAngle The angle reported by the gyroscope. - * @param wheelPositions The wheel positions reported by each module. - */ - template - void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle, - ModulePositions&&... wheelPositions); - /** * Resets the robot's position on the field. * @@ -77,25 +63,6 @@ class SwerveDriveOdometry { */ const Pose2d& GetPose() const { return m_pose; } - /** - * Updates the robot's position on the field using forward kinematics and - * integration of the pose over time. This method takes in the current time as - * a parameter to calculate period (difference between two timestamps). The - * period is used to calculate the change in distance from a velocity. This - * also takes in an angle parameter which is used instead of the - * angular rate that is calculated from forward kinematics. - * - * @param gyroAngle The angle reported by the gyroscope. - * @param wheelPositions The current position of all swerve modules. Please - * provide the positions in the same order in which you instantiated your - * SwerveDriveKinematics. - * - * @return The new pose of the robot. - */ - template - const Pose2d& Update(const Rotation2d& gyroAngle, - ModulePositions&&... wheelPositions); - /** * Updates the robot's position on the field using forward kinematics and * integration of the pose over time. This method takes in the current time as diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc index bae7cf31ea..86af47f323 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc @@ -22,20 +22,6 @@ SwerveDriveOdometry::SwerveDriveOdometry( wpi::math::MathUsageId::kOdometry_SwerveDrive, 1); } -template -template -void SwerveDriveOdometry::ResetPosition( - const Pose2d& pose, const Rotation2d& gyroAngle, - ModulePositions&&... wheelPositions) { - static_assert(sizeof...(wheelPositions) == NumModules, - "Number of modules is not consistent with number of wheel " - "locations provided in constructor."); - - wpi::array modulePositions{ - wheelPositions...}; - this->ResetPosition(pose, gyroAngle, modulePositions); -} - template void SwerveDriveOdometry::ResetPosition( const Pose2d& pose, const Rotation2d& gyroAngle, @@ -46,20 +32,6 @@ void SwerveDriveOdometry::ResetPosition( m_previousModulePositions = modulePositions; } -template -template -const Pose2d& frc::SwerveDriveOdometry::Update( - const Rotation2d& gyroAngle, ModulePositions&&... wheelPositions) { - static_assert(sizeof...(wheelPositions) == NumModules, - "Number of modules is not consistent with number of wheel " - "locations provided in constructor."); - - wpi::array modulePositions{ - wheelPositions...}; - - return this->Update(gyroAngle, modulePositions); -} - template const Pose2d& frc::SwerveDriveOdometry::Update( const Rotation2d& gyroAngle, diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java index 419942efd0..c5ffe9de53 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java @@ -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()); diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp index ef8f0f5ae5..7c8ff0a97c 100644 --- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp @@ -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();