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}