001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.math.kinematics;
006
007import edu.wpi.first.math.geometry.Pose2d;
008import edu.wpi.first.math.geometry.Rotation2d;
009
010/**
011 * Class for odometry. Robot code should not use this directly- Instead, use the particular type for
012 * your drivetrain (e.g., {@link DifferentialDriveOdometry}). Odometry allows you to track the
013 * robot's position on the field over the course of a match using readings from encoders and a
014 * gyroscope.
015 *
016 * <p>Teams can use odometry during the autonomous period for complex tasks like path following.
017 * Furthermore, odometry can be used for latency compensation when using computer-vision systems.
018 *
019 * @param <T> Wheel positions type.
020 */
021public class Odometry<T extends WheelPositions<T>> {
022  private final Kinematics<?, T> m_kinematics;
023  private Pose2d m_poseMeters;
024
025  private Rotation2d m_gyroOffset;
026  private Rotation2d m_previousAngle;
027  private T m_previousWheelPositions;
028
029  /**
030   * Constructs an Odometry object.
031   *
032   * @param kinematics The kinematics of the drivebase.
033   * @param gyroAngle The angle reported by the gyroscope.
034   * @param wheelPositions The current encoder readings.
035   * @param initialPoseMeters The starting position of the robot on the field.
036   */
037  public Odometry(
038      Kinematics<?, T> kinematics,
039      Rotation2d gyroAngle,
040      T wheelPositions,
041      Pose2d initialPoseMeters) {
042    m_kinematics = kinematics;
043    m_poseMeters = initialPoseMeters;
044    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
045    m_previousAngle = m_poseMeters.getRotation();
046    m_previousWheelPositions = wheelPositions.copy();
047  }
048
049  /**
050   * Resets the robot's position on the field.
051   *
052   * <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
053   * automatically takes care of offsetting the gyro angle.
054   *
055   * @param gyroAngle The angle reported by the gyroscope.
056   * @param wheelPositions The current encoder readings.
057   * @param poseMeters The position on the field that your robot is at.
058   */
059  public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMeters) {
060    m_poseMeters = poseMeters;
061    m_previousAngle = m_poseMeters.getRotation();
062    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
063    m_previousWheelPositions = wheelPositions.copy();
064  }
065
066  /**
067   * Returns the position of the robot on the field.
068   *
069   * @return The pose of the robot (x and y are in meters).
070   */
071  public Pose2d getPoseMeters() {
072    return m_poseMeters;
073  }
074
075  /**
076   * Updates the robot's position on the field using forward kinematics and integration of the pose
077   * over time. This method takes in an angle parameter which is used instead of the angular rate
078   * that is calculated from forward kinematics, in addition to the current distance measurement at
079   * each wheel.
080   *
081   * @param gyroAngle The angle reported by the gyroscope.
082   * @param wheelPositions The current encoder readings.
083   * @return The new pose of the robot.
084   */
085  public Pose2d update(Rotation2d gyroAngle, T wheelPositions) {
086    var angle = gyroAngle.plus(m_gyroOffset);
087
088    var twist = m_kinematics.toTwist2d(m_previousWheelPositions, wheelPositions);
089    twist.dtheta = angle.minus(m_previousAngle).getRadians();
090
091    var newPose = m_poseMeters.exp(twist);
092
093    m_previousWheelPositions = wheelPositions.copy();
094    m_previousAngle = angle;
095    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
096
097    return m_poseMeters;
098  }
099}