Package edu.wpi.first.math.kinematics

Class SwerveDriveOdometry3d

java.lang.Object
edu.wpi.first.math.kinematics.Odometry3d<SwerveModulePosition[]>
edu.wpi.first.math.kinematics.SwerveDriveOdometry3d
public class SwerveDriveOdometry3d extends Odometry3d<SwerveModulePosition[]>
Class for swerve drive odometry. Odometry allows you to track the robot's position on the field over a course of a match using readings from your swerve drive encoders and swerve azimuth encoders.

This class is meant to be an easy replacement for SwerveDriveOdometry, only requiring the addition of appropriate conversions between 2D and 3D versions of geometry classes. (See Pose3d(Pose2d), Rotation3d(Rotation2d), Translation3d(Translation2d), and Pose3d.toPose2d().)

Teams can use odometry during the autonomous period for complex tasks like path following. Furthermore, odometry can be used for latency compensation when using computer-vision systems.

  • Constructor Details

    • SwerveDriveOdometry3d

      public SwerveDriveOdometry3d(SwerveDriveKinematics kinematics, Rotation3d gyroAngle, SwerveModulePosition[] modulePositions, Pose3d initialPose)
      Constructs a SwerveDriveOdometry3d object.
      Parameters:
      kinematics - The swerve drive kinematics for your drivetrain.
      gyroAngle - The angle reported by the gyroscope.
      modulePositions - The wheel positions reported by each module.
      initialPose - The starting position of the robot on the field.

    • SwerveDriveOdometry3d

      public SwerveDriveOdometry3d(SwerveDriveKinematics kinematics, Rotation3d gyroAngle, SwerveModulePosition[] modulePositions)
      Constructs a SwerveDriveOdometry3d object with the default pose at the origin.
      Parameters:
      kinematics - The swerve drive kinematics for your drivetrain.
      gyroAngle - The angle reported by the gyroscope.
      modulePositions - The wheel positions reported by each module.

  • Method Details

    • resetPosition

      public void resetPosition(Rotation3d gyroAngle, SwerveModulePosition[] modulePositions, Pose3d pose)
      Description copied from class: Odometry3d
      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.

      Overrides:
      resetPosition in class Odometry3d<SwerveModulePosition[]>
      Parameters:
      gyroAngle - The angle reported by the gyroscope.
      modulePositions - The current encoder readings.
      pose - The position on the field that your robot is at.

    • update

      public Pose3d update(Rotation3d gyroAngle, SwerveModulePosition[] modulePositions)
      Description copied from class: Odometry3d
      Updates the robot's position on the field using forward kinematics and integration of the pose over time. This method takes in an angle parameter which is used instead of the angular rate that is calculated from forward kinematics, in addition to the current distance measurement at each wheel.
      Overrides:
      update in class Odometry3d<SwerveModulePosition[]>
      Parameters:
      gyroAngle - The angle reported by the gyroscope.
      modulePositions - The current encoder readings.
      Returns:
      The new pose of the robot.