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 org.wpilib.math.kinematics;
006
007import org.wpilib.math.geometry.Pose2d;
008import org.wpilib.math.geometry.Rotation2d;
009import org.wpilib.math.geometry.Translation2d;
010
011/**
012 * Class for odometry. Robot code should not use this directly- Instead, use the particular type for
013 * your drivetrain (e.g., {@link DifferentialDriveOdometry}). Odometry allows you to track the
014 * robot's position on the field over the course of a match using readings from encoders and a
015 * gyroscope.
016 *
017 * <p>Teams can use odometry during the autonomous period for complex tasks like path following.
018 * Furthermore, odometry can be used for latency compensation when using computer-vision systems.
019 *
020 * @param <T> Wheel positions type.
021 */
022public class Odometry<T> {
023  private final Kinematics<T, ?, ?> m_kinematics;
024  private Pose2d m_pose;
025
026  private Rotation2d m_gyroOffset;
027
028  // Always equal to m_poseMeters.getRotation()
029  private Rotation2d m_previousAngle;
030
031  private final T m_previousWheelPositions;
032
033  /**
034   * Constructs an Odometry object.
035   *
036   * @param kinematics The kinematics of the drivebase.
037   * @param gyroAngle The angle reported by the gyroscope.
038   * @param wheelPositions The current encoder readings.
039   * @param initialPose The starting position of the robot on the field.
040   */
041  public Odometry(
042      Kinematics<T, ?, ?> kinematics, Rotation2d gyroAngle, T wheelPositions, Pose2d initialPose) {
043    m_kinematics = kinematics;
044    m_pose = initialPose;
045    m_gyroOffset = gyroAngle.unaryMinus().rotateBy(m_pose.getRotation());
046    m_previousAngle = m_pose.getRotation();
047    m_previousWheelPositions = m_kinematics.copy(wheelPositions);
048  }
049
050  /**
051   * Resets the robot's position on the field.
052   *
053   * <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
054   * automatically takes care of offsetting the gyro angle.
055   *
056   * @param gyroAngle The angle reported by the gyroscope.
057   * @param wheelPositions The current encoder readings.
058   * @param pose The position on the field that your robot is at.
059   */
060  public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d pose) {
061    m_pose = pose;
062    m_gyroOffset = gyroAngle.unaryMinus().rotateBy(m_pose.getRotation());
063    m_previousAngle = m_pose.getRotation();
064    m_kinematics.copyInto(wheelPositions, m_previousWheelPositions);
065  }
066
067  /**
068   * Resets the pose.
069   *
070   * @param pose The pose to reset to.
071   */
072  public void resetPose(Pose2d pose) {
073    m_gyroOffset =
074        m_gyroOffset.rotateBy(m_pose.getRotation().unaryMinus()).rotateBy(pose.getRotation());
075    m_pose = pose;
076    m_previousAngle = m_pose.getRotation();
077  }
078
079  /**
080   * Resets the translation of the pose.
081   *
082   * @param translation The translation to reset to.
083   */
084  public void resetTranslation(Translation2d translation) {
085    m_pose = new Pose2d(translation, m_pose.getRotation());
086  }
087
088  /**
089   * Resets the rotation of the pose.
090   *
091   * @param rotation The rotation to reset to.
092   */
093  public void resetRotation(Rotation2d rotation) {
094    m_gyroOffset = m_gyroOffset.rotateBy(m_pose.getRotation().unaryMinus()).rotateBy(rotation);
095    m_pose = new Pose2d(m_pose.getTranslation(), rotation);
096    m_previousAngle = m_pose.getRotation();
097  }
098
099  /**
100   * Returns the position of the robot on the field.
101   *
102   * @return The pose of the robot (x and y are in meters).
103   */
104  public Pose2d getPose() {
105    return m_pose;
106  }
107
108  /**
109   * Updates the robot's position on the field using forward kinematics and integration of the pose
110   * over time. This method takes in an angle parameter which is used instead of the angular rate
111   * that is calculated from forward kinematics, in addition to the current distance measurement at
112   * each wheel.
113   *
114   * @param gyroAngle The angle reported by the gyroscope.
115   * @param wheelPositions The current encoder readings.
116   * @return The new pose of the robot.
117   */
118  public Pose2d update(Rotation2d gyroAngle, T wheelPositions) {
119    var angle = gyroAngle.rotateBy(m_gyroOffset);
120
121    var twist = m_kinematics.toTwist2d(m_previousWheelPositions, wheelPositions);
122    twist.dtheta = angle.minus(m_previousAngle).getRadians();
123
124    var newPose = m_pose.plus(twist.exp());
125
126    m_kinematics.copyInto(wheelPositions, m_previousWheelPositions);
127    m_previousAngle = angle;
128    m_pose = new Pose2d(newPose.getTranslation(), angle);
129
130    return m_pose;
131  }
132}