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.MathSharedStore;
008import edu.wpi.first.math.MathUsageId;
009import edu.wpi.first.math.geometry.Pose2d;
010import edu.wpi.first.math.geometry.Rotation2d;
011import edu.wpi.first.math.geometry.Twist2d;
012import edu.wpi.first.util.WPIUtilJNI;
013
014/**
015 * Class for swerve drive odometry. Odometry allows you to track the robot's position on the field
016 * over a course of a match using readings from your swerve drive encoders and swerve azimuth
017 * encoders.
018 *
019 * <p>Teams can use odometry during the autonomous period for complex tasks like path following.
020 * Furthermore, odometry can be used for latency compensation when using computer-vision systems.
021 */
022public class SwerveDriveOdometry {
023  private final SwerveDriveKinematics m_kinematics;
024  private Pose2d m_poseMeters;
025  private double m_prevTimeSeconds = -1;
026
027  private Rotation2d m_gyroOffset;
028  private Rotation2d m_previousAngle;
029
030  /**
031   * Constructs a SwerveDriveOdometry object.
032   *
033   * @param kinematics The swerve drive kinematics for your drivetrain.
034   * @param gyroAngle The angle reported by the gyroscope.
035   * @param initialPose The starting position of the robot on the field.
036   */
037  public SwerveDriveOdometry(
038      SwerveDriveKinematics kinematics, Rotation2d gyroAngle, Pose2d initialPose) {
039    m_kinematics = kinematics;
040    m_poseMeters = initialPose;
041    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
042    m_previousAngle = initialPose.getRotation();
043    MathSharedStore.reportUsage(MathUsageId.kOdometry_SwerveDrive, 1);
044  }
045
046  /**
047   * Constructs a SwerveDriveOdometry object with the default pose at the origin.
048   *
049   * @param kinematics The swerve drive kinematics for your drivetrain.
050   * @param gyroAngle The angle reported by the gyroscope.
051   */
052  public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngle) {
053    this(kinematics, gyroAngle, new Pose2d());
054  }
055
056  /**
057   * Resets the robot's position on the field.
058   *
059   * <p>The gyroscope angle does not need to be reset here on the user's robot code. The library
060   * automatically takes care of offsetting the gyro angle.
061   *
062   * @param pose The position on the field that your robot is at.
063   * @param gyroAngle The angle reported by the gyroscope.
064   */
065  public void resetPosition(Pose2d pose, Rotation2d gyroAngle) {
066    m_poseMeters = pose;
067    m_previousAngle = pose.getRotation();
068    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
069  }
070
071  /**
072   * Returns the position of the robot on the field.
073   *
074   * @return The pose of the robot (x and y are in meters).
075   */
076  public Pose2d getPoseMeters() {
077    return m_poseMeters;
078  }
079
080  /**
081   * Updates the robot's position on the field using forward kinematics and integration of the pose
082   * over time. This method takes in the current time as a parameter to calculate period (difference
083   * between two timestamps). The period is used to calculate the change in distance from a
084   * velocity. This also takes in an angle parameter which is used instead of the angular rate that
085   * is calculated from forward kinematics.
086   *
087   * @param currentTimeSeconds The current time in seconds.
088   * @param gyroAngle The angle reported by the gyroscope.
089   * @param moduleStates The current state of all swerve modules. Please provide the states in the
090   *     same order in which you instantiated your SwerveDriveKinematics.
091   * @return The new pose of the robot.
092   */
093  public Pose2d updateWithTime(
094      double currentTimeSeconds, Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
095    double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
096    m_prevTimeSeconds = currentTimeSeconds;
097
098    var angle = gyroAngle.plus(m_gyroOffset);
099
100    var chassisState = m_kinematics.toChassisSpeeds(moduleStates);
101    var newPose =
102        m_poseMeters.exp(
103            new Twist2d(
104                chassisState.vxMetersPerSecond * period,
105                chassisState.vyMetersPerSecond * period,
106                angle.minus(m_previousAngle).getRadians()));
107
108    m_previousAngle = angle;
109    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
110
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 automatically calculates the current time to calculate period
117   * (difference between two timestamps). The period is used to calculate the change in distance
118   * from a velocity. This also takes in an angle parameter which is used instead of the angular
119   * rate that is calculated from forward kinematics.
120   *
121   * @param gyroAngle The angle reported by the gyroscope.
122   * @param moduleStates The current state of all swerve modules. Please provide the states in the
123   *     same order in which you instantiated your SwerveDriveKinematics.
124   * @return The new pose of the robot.
125   */
126  public Pose2d update(Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
127    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, moduleStates);
128  }
129}