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.geometry.Pose2d;
009import edu.wpi.first.math.geometry.Pose3d;
010import edu.wpi.first.math.geometry.Rotation2d;
011import edu.wpi.first.math.geometry.Rotation3d;
012import edu.wpi.first.math.geometry.Translation2d;
013import edu.wpi.first.math.geometry.Translation3d;
014
015/**
016 * Class for swerve drive odometry. Odometry allows you to track the robot's position on the field
017 * over a course of a match using readings from your swerve drive encoders and swerve azimuth
018 * encoders.
019 *
020 * <p>This class is meant to be an easy replacement for {@link SwerveDriveOdometry}, only requiring
021 * the addition of appropriate conversions between 2D and 3D versions of geometry classes. (See
022 * {@link Pose3d#Pose3d(Pose2d)}, {@link Rotation3d#Rotation3d(Rotation2d)}, {@link
023 * Translation3d#Translation3d(Translation2d)}, and {@link Pose3d#toPose2d()}.)
024 *
025 * <p>Teams can use odometry during the autonomous period for complex tasks like path following.
026 * Furthermore, odometry can be used for latency compensation when using computer-vision systems.
027 */
028public class SwerveDriveOdometry3d extends Odometry3d<SwerveModulePosition[]> {
029  private final int m_numModules;
030
031  /**
032   * Constructs a SwerveDriveOdometry3d object.
033   *
034   * @param kinematics The swerve drive kinematics for your drivetrain.
035   * @param gyroAngle The angle reported by the gyroscope.
036   * @param modulePositions The wheel positions reported by each module.
037   * @param initialPose The starting position of the robot on the field.
038   */
039  public SwerveDriveOdometry3d(
040      SwerveDriveKinematics kinematics,
041      Rotation3d gyroAngle,
042      SwerveModulePosition[] modulePositions,
043      Pose3d initialPose) {
044    super(kinematics, gyroAngle, modulePositions, initialPose);
045
046    m_numModules = modulePositions.length;
047
048    MathSharedStore.reportUsage("SwerveDriveOdometry3d", "");
049  }
050
051  /**
052   * Constructs a SwerveDriveOdometry3d object with the default pose at the origin.
053   *
054   * @param kinematics The swerve drive kinematics for your drivetrain.
055   * @param gyroAngle The angle reported by the gyroscope.
056   * @param modulePositions The wheel positions reported by each module.
057   */
058  public SwerveDriveOdometry3d(
059      SwerveDriveKinematics kinematics,
060      Rotation3d gyroAngle,
061      SwerveModulePosition[] modulePositions) {
062    this(kinematics, gyroAngle, modulePositions, Pose3d.kZero);
063  }
064
065  @Override
066  public void resetPosition(
067      Rotation3d gyroAngle, SwerveModulePosition[] modulePositions, Pose3d pose) {
068    if (modulePositions.length != m_numModules) {
069      throw new IllegalArgumentException(
070          "Number of modules is not consistent with number of wheel locations provided in "
071              + "constructor");
072    }
073    super.resetPosition(gyroAngle, modulePositions, pose);
074  }
075
076  @Override
077  public Pose3d update(Rotation3d gyroAngle, SwerveModulePosition[] modulePositions) {
078    if (modulePositions.length != m_numModules) {
079      throw new IllegalArgumentException(
080          "Number of modules is not consistent with number of wheel locations provided in "
081              + "constructor");
082    }
083    return super.update(gyroAngle, modulePositions);
084  }
085}