Package frc.robot.subsystems.swerve.util

Class SwerveArcOdometry

java.lang.Object

edu.wpi.first.math.kinematics.Odometry<SwerveModulePosition[]>

edu.wpi.first.math.kinematics.SwerveDriveOdometry

frc.robot.subsystems.swerve.util.SwerveArcOdometry

public final class SwerveArcOdometry
extends SwerveDriveOdometry

Swerve drive odometry implementation that integrates wheel motion using circular arc geometry rather than straight-line approximations.

Inspired by Team 1690, this odometry model treats each swerve module's motion between updates as an arc defined by the change in wheel angle and distance traveled. This more accurately represents real swerve motion, especially during simultaneous translation and rotation, where straight-line assumptions can introduce measurable integration error.

Compared to SwerveDriveOdometry, which assumes each module travels in a straight line over a timestep, this class:

• Computes per-module displacement along a circular arc
• Transforms those displacements into the field frame
• Averages the resulting module displacements to update the robot pose

The robot heading is still sourced directly from the gyro, while translation is derived purely from module motion.

This class is intended as a drop-in replacement for SwerveDriveOdometry when improved accuracy is desired at the cost of slightly increased computation.

Constructor Summary

Constructors

Constructor	Description
SwerveArcOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngle, SwerveModulePosition[] modulePositions)	Constructs a new arc-based swerve odometry instance.

Method Summary

Modifier and Type	Method	Description
Pose2d	getPoseMeters()
void	resetPose(Pose2d poseMeters)
void	resetPosition(Rotation2d gyroAngle, SwerveModulePosition[] wheelPositions, Pose2d poseMeters)
void	resetRotation(Rotation2d rotation)
void	resetTranslation(Translation2d translation)
Pose2d	update(Rotation2d currentGyroAngle, SwerveModulePosition[] currentWheelPositions)

Methods inherited from class java.lang.Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Constructor Details

SwerveArcOdometry

public SwerveArcOdometry(SwerveDriveKinematics kinematics,
 Rotation2d gyroAngle,
 SwerveModulePosition[] modulePositions)

Constructs a new arc-based swerve odometry instance.

Parameters:

kinematics - the swerve drive kinematics describing module locations

gyroAngle - the initial robot heading from the gyro

modulePositions - the initial positions of each swerve module

Method Details

update

public Pose2d update(Rotation2d currentGyroAngle,
 SwerveModulePosition[] currentWheelPositions)

Overrides:

update in class SwerveDriveOdometry

resetPosition

public void resetPosition(Rotation2d gyroAngle,
 SwerveModulePosition[] wheelPositions,
 Pose2d poseMeters)

Overrides:

resetPosition in class SwerveDriveOdometry

resetPose

public void resetPose(Pose2d poseMeters)

Overrides:

resetPose in class Odometry<SwerveModulePosition[]>

resetTranslation

public void resetTranslation(Translation2d translation)

Overrides:

resetTranslation in class Odometry<SwerveModulePosition[]>

resetRotation

public void resetRotation(Rotation2d rotation)

Overrides:

resetRotation in class Odometry<SwerveModulePosition[]>

getPoseMeters

public Pose2d getPoseMeters()

Overrides:

getPoseMeters in class Odometry<SwerveModulePosition[]>