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;
009import edu.wpi.first.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  private Rotation2d m_previousAngle;
028  private final T m_previousWheelPositions;
029
030  /**
031   * Constructs an Odometry object.
032   *
033   * @param kinematics The kinematics of the drivebase.
034   * @param gyroAngle The angle reported by the gyroscope.
035   * @param wheelPositions The current encoder readings.
036   * @param initialPose The starting position of the robot on the field.
037   */
038  public Odometry(
039      Kinematics<?, T> kinematics, Rotation2d gyroAngle, T wheelPositions, Pose2d initialPose) {
040    m_kinematics = kinematics;
041    m_pose = initialPose;
042    m_gyroOffset = m_pose.getRotation().minus(gyroAngle);
043    m_previousAngle = m_pose.getRotation();
044    m_previousWheelPositions = m_kinematics.copy(wheelPositions);
045  }
046
047  /**
048   * Resets the robot's position on the field.
049   *
050   * <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
051   * automatically takes care of offsetting the gyro angle.
052   *
053   * @param gyroAngle The angle reported by the gyroscope.
054   * @param wheelPositions The current encoder readings.
055   * @param pose The position on the field that your robot is at.
056   */
057  public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d pose) {
058    m_pose = pose;
059    m_previousAngle = m_pose.getRotation();
060    m_gyroOffset = m_pose.getRotation().minus(gyroAngle);
061    m_kinematics.copyInto(wheelPositions, m_previousWheelPositions);
062  }
063
064  /**
065   * Resets the pose.
066   *
067   * @param pose The pose to reset to.
068   */
069  public void resetPose(Pose2d pose) {
070    m_gyroOffset = m_gyroOffset.plus(pose.getRotation().minus(m_pose.getRotation()));
071    m_pose = pose;
072    m_previousAngle = m_pose.getRotation();
073  }
074
075  /**
076   * Resets the translation of the pose.
077   *
078   * @param translation The translation to reset to.
079   */
080  public void resetTranslation(Translation2d translation) {
081    m_pose = new Pose2d(translation, m_pose.getRotation());
082  }
083
084  /**
085   * Resets the rotation of the pose.
086   *
087   * @param rotation The rotation to reset to.
088   */
089  public void resetRotation(Rotation2d rotation) {
090    m_gyroOffset = m_gyroOffset.plus(rotation.minus(m_pose.getRotation()));
091    m_pose = new Pose2d(m_pose.getTranslation(), rotation);
092    m_previousAngle = m_pose.getRotation();
093  }
094
095  /**
096   * Returns the position of the robot on the field.
097   *
098   * @return The pose of the robot (x and y are in meters).
099   */
100  public Pose2d getPose() {
101    return m_pose;
102  }
103
104  /**
105   * Updates the robot's position on the field using forward kinematics and integration of the pose
106   * over time. This method takes in an angle parameter which is used instead of the angular rate
107   * that is calculated from forward kinematics, in addition to the current distance measurement at
108   * each wheel.
109   *
110   * @param gyroAngle The angle reported by the gyroscope.
111   * @param wheelPositions The current encoder readings.
112   * @return The new pose of the robot.
113   */
114  public Pose2d update(Rotation2d gyroAngle, T wheelPositions) {
115    var angle = gyroAngle.plus(m_gyroOffset);
116
117    var twist = m_kinematics.toTwist2d(m_previousWheelPositions, wheelPositions);
118    twist.dtheta = angle.minus(m_previousAngle).getRadians();
119
120    var newPose = m_pose.exp(twist);
121
122    m_kinematics.copyInto(wheelPositions, m_previousWheelPositions);
123    m_previousAngle = angle;
124    m_pose = new Pose2d(newPose.getTranslation(), angle);
125
126    return m_pose;
127  }
128}