Package edu.wpi.first.math.kinematics

Class Odometry3d<T>

java.lang.Object
edu.wpi.first.math.kinematics.Odometry3d<T>
Type Parameters:
T - Wheel positions type.
Direct Known Subclasses:
DifferentialDriveOdometry3d, MecanumDriveOdometry3d, SwerveDriveOdometry3d
public class Odometry3d<T> extends Object
Class for odometry. Robot code should not use this directly- Instead, use the particular type for your drivetrain (e.g., DifferentialDriveOdometry3d). Odometry allows you to track the robot's position on the field over the course of a match using readings from encoders and a gyroscope.

This class is meant to be an easy replacement for Odometry, 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

    • Odometry3d

      public Odometry3d(Kinematics<?,T> kinematics, Rotation3d gyroAngle, T wheelPositions, Pose3d initialPoseMeters)
      Constructs an Odometry3d object.
      Parameters:
      kinematics - The kinematics of the drivebase.
      gyroAngle - The angle reported by the gyroscope.
      wheelPositions - The current encoder readings.
      initialPoseMeters - The starting position of the robot on the field.

  • Method Details

    • resetPosition

      public void resetPosition(Rotation3d gyroAngle, T wheelPositions, Pose3d poseMeters)
      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.

      Parameters:
      gyroAngle - The angle reported by the gyroscope.
      wheelPositions - The current encoder readings.
      poseMeters - The position on the field that your robot is at.

    • resetPose

      public void resetPose(Pose3d poseMeters)
      Resets the pose.
      Parameters:
      poseMeters - The pose to reset to.

    • resetTranslation

      public void resetTranslation(Translation3d translation)
      Resets the translation of the pose.
      Parameters:
      translation - The translation to reset to.

    • resetRotation

      public void resetRotation(Rotation3d rotation)
      Resets the rotation of the pose.
      Parameters:
      rotation - The rotation to reset to.

    • getPoseMeters

      public Pose3d getPoseMeters()
      Returns the position of the robot on the field.
      Returns:
      The pose of the robot (x, y, and z are in meters).

    • update

      public Pose3d update(Rotation3d gyroAngle, T wheelPositions)
      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.
      Parameters:
      gyroAngle - The angle reported by the gyroscope.
      wheelPositions - The current encoder readings.
      Returns:
      The new pose of the robot.