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