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