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.Pose3d; 009import edu.wpi.first.math.geometry.Rotation2d; 010import edu.wpi.first.math.geometry.Rotation3d; 011import edu.wpi.first.math.geometry.Translation2d; 012import edu.wpi.first.math.geometry.Translation3d; 013import edu.wpi.first.math.geometry.Twist3d; 014 015/** 016 * Class for odometry. Robot code should not use this directly- Instead, use the particular type for 017 * your drivetrain (e.g., {@link DifferentialDriveOdometry3d}). Odometry allows you to track the 018 * robot's position on the field over the course of a match using readings from encoders and a 019 * gyroscope. 020 * 021 * <p>This class is meant to be an easy replacement for {@link Odometry}, only requiring the 022 * addition of appropriate conversions between 2D and 3D versions of geometry classes. (See {@link 023 * 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 * 029 * @param <T> Wheel positions type. 030 */ 031public class Odometry3d<T> { 032 private final Kinematics<?, T> m_kinematics; 033 private Pose3d m_pose; 034 035 private Rotation3d m_gyroOffset; 036 private Rotation3d m_previousAngle; 037 private final T m_previousWheelPositions; 038 039 /** 040 * Constructs an Odometry3d object. 041 * 042 * @param kinematics The kinematics of the drivebase. 043 * @param gyroAngle The angle reported by the gyroscope. 044 * @param wheelPositions The current encoder readings. 045 * @param initialPose The starting position of the robot on the field. 046 */ 047 public Odometry3d( 048 Kinematics<?, T> kinematics, Rotation3d gyroAngle, T wheelPositions, Pose3d initialPose) { 049 m_kinematics = kinematics; 050 m_pose = initialPose; 051 m_gyroOffset = m_pose.getRotation().minus(gyroAngle); 052 m_previousAngle = m_pose.getRotation(); 053 m_previousWheelPositions = m_kinematics.copy(wheelPositions); 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 gyroAngle The angle reported by the gyroscope. 063 * @param wheelPositions The current encoder readings. 064 * @param pose The position on the field that your robot is at. 065 */ 066 public void resetPosition(Rotation3d gyroAngle, T wheelPositions, Pose3d pose) { 067 m_pose = pose; 068 m_previousAngle = m_pose.getRotation(); 069 m_gyroOffset = m_pose.getRotation().minus(gyroAngle); 070 m_kinematics.copyInto(wheelPositions, m_previousWheelPositions); 071 } 072 073 /** 074 * Resets the pose. 075 * 076 * @param pose The pose to reset to. 077 */ 078 public void resetPose(Pose3d pose) { 079 m_gyroOffset = m_gyroOffset.plus(pose.getRotation().minus(m_pose.getRotation())); 080 m_pose = pose; 081 m_previousAngle = m_pose.getRotation(); 082 } 083 084 /** 085 * Resets the translation of the pose. 086 * 087 * @param translation The translation to reset to. 088 */ 089 public void resetTranslation(Translation3d translation) { 090 m_pose = new Pose3d(translation, m_pose.getRotation()); 091 } 092 093 /** 094 * Resets the rotation of the pose. 095 * 096 * @param rotation The rotation to reset to. 097 */ 098 public void resetRotation(Rotation3d rotation) { 099 m_gyroOffset = m_gyroOffset.plus(rotation.minus(m_pose.getRotation())); 100 m_pose = new Pose3d(m_pose.getTranslation(), rotation); 101 m_previousAngle = m_pose.getRotation(); 102 } 103 104 /** 105 * Returns the position of the robot on the field. 106 * 107 * @return The pose of the robot (x, y, and z are in meters). 108 */ 109 public Pose3d getPose() { 110 return m_pose; 111 } 112 113 /** 114 * Updates the robot's position on the field using forward kinematics and integration of the pose 115 * over time. This method takes in an angle parameter which is used instead of the angular rate 116 * that is calculated from forward kinematics, in addition to the current distance measurement at 117 * each wheel. 118 * 119 * @param gyroAngle The angle reported by the gyroscope. 120 * @param wheelPositions The current encoder readings. 121 * @return The new pose of the robot. 122 */ 123 public Pose3d update(Rotation3d gyroAngle, T wheelPositions) { 124 var angle = gyroAngle.plus(m_gyroOffset); 125 var angle_difference = angle.minus(m_previousAngle).toVector(); 126 127 var twist2d = m_kinematics.toTwist2d(m_previousWheelPositions, wheelPositions); 128 var twist = 129 new Twist3d( 130 twist2d.dx, 131 twist2d.dy, 132 0, 133 angle_difference.get(0), 134 angle_difference.get(1), 135 angle_difference.get(2)); 136 137 var newPose = m_pose.exp(twist); 138 139 m_kinematics.copyInto(wheelPositions, m_previousWheelPositions); 140 m_previousAngle = angle; 141 m_pose = new Pose3d(newPose.getTranslation(), angle); 142 143 return m_pose; 144 } 145}