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