Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-29 02:21:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Position Delta Odometry for Swerve (#4493)

Browse Source
This commit is contained in:
Jordan McMichael
2022-10-25 15:28:36 -04:00
committed by GitHub
parent fe400f68c5
commit 4170ec6107
35 changed files with 1508 additions and 277 deletions
Download Patch File Download Diff File Expand all files Collapse all files

145
wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
View File

@@ -38,11 +38,13 @@ namespace frc {
* The state-space system used internally has the following states (x), inputs
* (u), and outputs (y):
*
* <strong> x = [x, y, theta]ᵀ </strong> in the field coordinate system
* containing x position, y position, and heading.
* <strong> x = [x, y, theta, s_0, ... s_n]ᵀ </strong> in the field coordinate
* system containing x position, y position, and heading, followed by the
* distance travelled by each wheel.
*
* <strong> u = [v_x, v_y, omega]ᵀ </strong> containing x velocity, y velocity,
* and angular velocity in the field coordinate system.
* <strong> u = [v_x, v_y, omega, v_0, ... v_n]ᵀ </strong> containing x
* velocity, y velocity, and angular velocity in the field coordinate system,
* followed by the velocity measured at each wheel.
*
* <strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y
* position, and heading; or <strong> y = [theta]ᵀ </strong> containing gyro
@@ -51,22 +53,29 @@ namespace frc {
template <size_t NumModules>
class SwerveDrivePoseEstimator {
public:
static constexpr size_t States = 3 + NumModules;
static constexpr size_t Inputs = 3 + NumModules;
static constexpr size_t Outputs = 1 + NumModules;
/**
* Constructs a SwerveDrivePoseEstimator.
*
* @param gyroAngle The current gyro angle.
* @param initialPose The starting pose estimate.
* @param modulePositions The current distance and rotation
* measurements of the swerve modules.
* @param kinematics A correctly-configured kinematics object
* for your drivetrain.
* @param stateStdDevs Standard deviations of model states.
* Increase these numbers to trust your
* model's state estimates less. This matrix
* is in the form [x, y, theta]ᵀ, with units
* in meters and radians.
* is in the form [x, y, theta, s_0, ...
* s_n]ᵀ, with units in meters and radians, then meters.
* @param localMeasurementStdDevs Standard deviation of the gyro measurement.
* Increase this number to trust sensor
* readings from the gyro less. This matrix is
* in the form [theta], with units in radians.
* in the form [theta, s_0, ... s_n], with
* units in radians followed by meters.
* @param visionMeasurementStdDevs Standard deviations of the vision
* measurements. Increase these numbers to
* trust global measurements from vision
@@ -78,33 +87,48 @@ class SwerveDrivePoseEstimator {
*/
SwerveDrivePoseEstimator(
const Rotation2d& gyroAngle, const Pose2d& initialPose,
const wpi::array<SwerveModulePosition, NumModules> modulePositions,
SwerveDriveKinematics<NumModules>& kinematics,
const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 1>& localMeasurementStdDevs,
const wpi::array<double, States>& stateStdDevs,
const wpi::array<double, Outputs>& localMeasurementStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs,
units::second_t nominalDt = 20_ms)
: m_observer([](const Vectord<3>& x, const Vectord<3>& u) { return u; },
[](const Vectord<3>& x, const Vectord<3>& u) {
return x.block<1, 1>(2, 0);
: m_observer([](const Vectord<States>& x,
const Vectord<Inputs>& u) { return u; },
[](const Vectord<States>& x, const Vectord<Inputs>& u) {
return x.template block<States - 2, 1>(2, 0);
},
stateStdDevs, localMeasurementStdDevs,
frc::AngleMean<3, 3>(2), frc::AngleMean<1, 3>(0),
frc::AngleResidual<3>(2), frc::AngleResidual<1>(0),
frc::AngleAdd<3>(2), nominalDt),
frc::AngleMean<States, States>(2),
frc::AngleMean<Outputs, States>(0),
frc::AngleResidual<States>(2),
frc::AngleResidual<Outputs>(0), frc::AngleAdd<States>(2),
nominalDt),
m_kinematics(kinematics),
m_nominalDt(nominalDt) {
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
// Create correction mechanism for vision measurements.
m_visionCorrect = [&](const Vectord<3>& u, const Vectord<3>& y) {
m_observer.Correct<3>(
u, y, [](const Vectord<3>& x, const Vectord<3>& u) { return x; },
m_visionContR, frc::AngleMean<3, 3>(2), frc::AngleResidual<3>(2),
frc::AngleResidual<3>(2), frc::AngleAdd<3>(2));
m_visionCorrect = [&](const Vectord<Inputs>& u, const Vectord<3>& y) {
m_observer.template Correct<3>(
u, y,
[](const Vectord<States>& x, const Vectord<Inputs>& u) {
return x.template block<3, 1>(0, 0);
},
m_visionContR, frc::AngleMean<3, States>(2), frc::AngleResidual<3>(2),
frc::AngleResidual<States>(2), frc::AngleAdd<States>(2));
};
// Set initial state.
m_observer.SetXhat(PoseTo3dVector(initialPose));
Vectord<States> xhat;
auto poseVec = PoseTo3dVector(initialPose);
xhat(0) = poseVec(0);
xhat(1) = poseVec(1);
xhat(2) = poseVec(2);
for (size_t i = 0; i < NumModules; i++) {
xhat(3 + i) = modulePositions[i].distance.value();
}
m_observer.SetXhat(xhat);
// Calculate offsets.
m_gyroOffset = initialPose.Rotation() - gyroAngle;
@@ -117,15 +141,27 @@ class SwerveDrivePoseEstimator {
* The gyroscope angle does not need to be reset in the user's robot code.
* The library automatically takes care of offsetting the gyro angle.
*
* @param pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param modulePositions The current distance and rotation measurements of
* the swerve modules.
*/
void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
void ResetPosition(
const Pose2d& pose, const Rotation2d& gyroAngle,
wpi::array<SwerveModulePosition, NumModules> modulePositions) {
// Reset state estimate and error covariance
m_observer.Reset();
m_poseBuffer.Clear();
m_observer.SetXhat(PoseTo3dVector(pose));
Vectord<States> xhat;
auto poseVec = PoseTo3dVector(pose);
xhat(0) = poseVec(0);
xhat(1) = poseVec(1);
xhat(2) = poseVec(2);
for (size_t i = 0; i < NumModules; i++) {
xhat(3 + i) = modulePositions[i].distance.value();
}
m_observer.SetXhat(xhat);
m_prevTime = -1_s;
@@ -187,7 +223,7 @@ class SwerveDrivePoseEstimator {
void AddVisionMeasurement(const Pose2d& visionRobotPose,
units::second_t timestamp) {
if (auto sample = m_poseBuffer.Sample(timestamp)) {
m_visionCorrect(Vectord<3>::Zero(),
m_visionCorrect(Vectord<States>::Zero(),
PoseTo3dVector(GetEstimatedPosition().TransformBy(
visionRobotPose - sample.value())));
}
@@ -240,15 +276,19 @@ class SwerveDrivePoseEstimator {
* information. This should be called every loop, and the correct loop period
* must be passed into the constructor of this class.
*
* @param gyroAngle The current gyro angle.
* @param moduleStates The current velocities and rotations of the swerve
* modules.
* @param gyroAngle The current gyro angle.
* @param moduleStates The current velocities and rotations of the swerve
* modules.
* @param modulePositions The current distance and rotation measurements of
* the swerve modules.
* @return The estimated pose of the robot in meters.
*/
template <typename... ModuleState>
Pose2d Update(const Rotation2d& gyroAngle, ModuleState&&... moduleStates) {
Pose2d Update(
const Rotation2d& gyroAngle,
const wpi::array<SwerveModuleState, NumModules> moduleStates,
const wpi::array<SwerveModulePosition, NumModules> modulePositions) {
return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
moduleStates...);
moduleStates, modulePositions);
}
/**
@@ -256,31 +296,43 @@ class SwerveDrivePoseEstimator {
* information. This should be called every loop, and the correct loop period
* must be passed into the constructor of this class.
*
* @param currentTime Time at which this method was called, in seconds.
* @param gyroAngle The current gyroscope angle.
* @param moduleStates The current velocities and rotations of the swerve
* modules.
* @param currentTime Time at which this method was called, in seconds.
* @param gyroAngle The current gyro angle.
* @param moduleStates The current velocities and rotations of the swerve
* modules.
* @param modulePositions The current distance travelled and rotations of
* the swerve modules.
* @return The estimated pose of the robot in meters.
*/
template <typename... ModuleState>
Pose2d UpdateWithTime(units::second_t currentTime,
const Rotation2d& gyroAngle,
ModuleState&&... moduleStates) {
Pose2d UpdateWithTime(
units::second_t currentTime, const Rotation2d& gyroAngle,
const wpi::array<SwerveModuleState, NumModules> moduleStates,
const wpi::array<SwerveModulePosition, NumModules> modulePositions) {
auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt;
m_prevTime = currentTime;
auto angle = gyroAngle + m_gyroOffset;
auto omega = (angle - m_previousAngle).Radians() / dt;
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(moduleStates...);
auto chassisSpeeds = m_kinematics.ToChassisSpeeds(moduleStates);
auto fieldRelativeSpeeds =
Translation2d{chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s}.RotateBy(
angle);
Vectord<3> u{fieldRelativeSpeeds.X().value(),
fieldRelativeSpeeds.Y().value(), omega.value()};
Vectord<Inputs> u;
u(0) = fieldRelativeSpeeds.X().value();
u(1) = fieldRelativeSpeeds.Y().value();
u(2) = omega.value();
for (size_t i = 0; i < NumModules; i++) {
u(3 + i) = moduleStates[i].speed.value();
}
Vectord<Outputs> localY;
localY(0) = angle.Radians().value();
for (size_t i = 0; i < NumModules; i++) {
localY(1 + i) = modulePositions[i].distance.value();
}
Vectord<1> localY{angle.Radians().value()};
m_previousAngle = angle;
m_poseBuffer.AddSample(currentTime, GetEstimatedPosition());
@@ -292,10 +344,11 @@ class SwerveDrivePoseEstimator {
}
private:
UnscentedKalmanFilter<3, 3, 1> m_observer;
UnscentedKalmanFilter<States, Inputs, Outputs> m_observer;
SwerveDriveKinematics<NumModules>& m_kinematics;
TimeInterpolatableBuffer<Pose2d> m_poseBuffer{1.5_s};
std::function<void(const Vectord<3>& u, const Vectord<3>& y)> m_visionCorrect;
std::function<void(const Vectord<Inputs>& u, const Vectord<3>& y)>
m_visionCorrect;
Eigen::Matrix3d m_visionContR;

34
wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
View File

@@ -13,7 +13,9 @@
#include "frc/EigenCore.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/geometry/Translation2d.h"
#include "frc/geometry/Twist2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
#include "frc/kinematics/SwerveModulePosition.h"
#include "frc/kinematics/SwerveModuleState.h"
#include "units/velocity.h"
#include "wpimath/MathShared.h"
@@ -162,6 +164,38 @@ class SwerveDriveKinematics {
ChassisSpeeds ToChassisSpeeds(
wpi::array<SwerveModuleState, NumModules> moduleStates) const;
/**
* Performs forward kinematics to return the resulting Twist2d from the
* given module position deltas. This method is often used for odometry --
* determining the robot's position on the field using data from the
* real-world position delta and angle of each module on the robot.
*
* @param wheelDeltas The latest change in position of the modules (as a
* SwerveModulePosition type) as measured from respective encoders and gyros.
* The order of the swerve module states should be same as passed into the
* constructor of this class.
*
* @return The resulting Twist2d.
*/
template <typename... ModuleDeltas>
Twist2d ToTwist2d(ModuleDeltas&&... wheelDeltas) const;
/**
* Performs forward kinematics to return the resulting Twist2d from the
* given module position deltas. This method is often used for odometry --
* determining the robot's position on the field using data from the
* real-world position delta and angle of each module on the robot.
*
* @param wheelDeltas The latest change in position of the modules (as a
* SwerveModulePosition type) as measured from respective encoders and gyros.
* The order of the swerve module states should be same as passed into the
* constructor of this class.
*
* @return The resulting Twist2d.
*/
Twist2d ToTwist2d(
wpi::array<SwerveModulePosition, NumModules> wheelDeltas) const;
/**
* Renormalizes the wheel speeds if any individual speed is above the
* specified maximum.

33
wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
View File

@@ -95,6 +95,39 @@ ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
units::radians_per_second_t{chassisSpeedsVector(2)}};
}
template <size_t NumModules>
template <typename... ModuleDeltas>
Twist2d SwerveDriveKinematics<NumModules>::ToTwist2d(
ModuleDeltas&&... wheelDeltas) const {
static_assert(sizeof...(wheelDeltas) == NumModules,
"Number of modules is not consistent with number of wheel "
"locations provided in constructor.");
wpi::array<SwerveModulePosition, NumModules> moduleDeltas{wheelDeltas...};
return this->ToTwist2d(moduleDeltas);
}
template <size_t NumModules>
Twist2d SwerveDriveKinematics<NumModules>::ToTwist2d(
wpi::array<SwerveModulePosition, NumModules> moduleDeltas) const {
Matrixd<NumModules * 2, 1> moduleDeltaMatrix;
for (size_t i = 0; i < NumModules; ++i) {
SwerveModulePosition module = moduleDeltas[i];
moduleDeltaMatrix(i * 2, 0) = module.distance.value() * module.angle.Cos();
moduleDeltaMatrix(i * 2 + 1, 0) =
module.distance.value() * module.angle.Sin();
}
Eigen::Vector3d chassisDeltaVector =
m_forwardKinematics.solve(moduleDeltaMatrix);
return {units::meter_t{chassisDeltaVector(0)},
units::meter_t{chassisDeltaVector(1)},
units::radian_t{chassisDeltaVector(2)}};
}
template <size_t NumModules>
void SwerveDriveKinematics<NumModules>::DesaturateWheelSpeeds(
wpi::array<SwerveModuleState, NumModules>* moduleStates,

72
wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
View File

@@ -12,7 +12,7 @@
#include <wpi/timestamp.h>
#include "SwerveDriveKinematics.h"
#include "SwerveModuleState.h"
#include "SwerveModulePosition.h"
#include "frc/geometry/Pose2d.h"
#include "units/time.h"
@@ -35,11 +35,13 @@ class SwerveDriveOdometry {
*
* @param kinematics The swerve drive kinematics for your drivetrain.
* @param gyroAngle The angle reported by the gyroscope.
* @param modulePositions The wheel positions reported by each module.
* @param initialPose The starting position of the robot on the field.
*/
SwerveDriveOdometry(SwerveDriveKinematics<NumModules> kinematics,
const Rotation2d& gyroAngle,
const Pose2d& initialPose = Pose2d{});
SwerveDriveOdometry(
SwerveDriveKinematics<NumModules> kinematics, const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules> modulePositions,
const Pose2d& initialPose = Pose2d{});
/**
* Resets the robot's position on the field.
@@ -49,12 +51,25 @@ class SwerveDriveOdometry {
*
* @param pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The wheel positions reported by each module.
*/
void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
}
template <typename... ModulePositions>
void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle,
ModulePositions&&... wheelPositions);
/**
* Resets the robot's position on the field.
*
* The gyroscope angle does not need to be reset here on the user's robot
* code. The library automatically takes care of offsetting the gyro angle.
*
* @param pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param modulePositions The wheel positions reported by each module.
*/
void ResetPosition(
const Pose2d& pose, const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules> modulePositions);
/**
* Returns the position of the robot on the field.
@@ -70,47 +85,44 @@ class SwerveDriveOdometry {
* also takes in an angle parameter which is used instead of the
* angular rate that is calculated from forward kinematics.
*
* @param currentTime The current time.
* @param gyroAngle The angle reported by the gyroscope.
* @param moduleStates The current state of all swerve modules. Please provide
* the states in the same order in which you instantiated
* your SwerveDriveKinematics.
* @param wheelPositions The current position of all swerve modules. Please
* provide the positions in the same order in which you instantiated your
* SwerveDriveKinematics.
*
* @return The new pose of the robot.
*/
template <typename... ModuleStates>
const Pose2d& UpdateWithTime(units::second_t currentTime,
const Rotation2d& gyroAngle,
ModuleStates&&... moduleStates);
template <typename... ModulePositions>
const Pose2d& Update(const Rotation2d& gyroAngle,
ModulePositions&&... wheelPositions);
/**
* Updates the robot's position on the field using forward kinematics and
* integration of the pose over time. This method automatically calculates
* the current time to calculate period (difference between two timestamps).
* The period is used to calculate the change in distance from a velocity.
* This also takes in an angle parameter which is used instead of the
* integration of the pose over time. This method takes in the current time as
* a parameter to calculate period (difference between two timestamps). The
* period is used to calculate the change in distance from a velocity. This
* also takes in an angle parameter which is used instead of the
* angular rate that is calculated from forward kinematics.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param moduleStates The current state of all swerve modules. Please provide
* the states in the same order in which you instantiated
* your SwerveDriveKinematics.
* @param modulePositions The current position of all swerve modules. Please
* provide the positions in the same order in which you instantiated your
* SwerveDriveKinematics.
*
* @return The new pose of the robot.
*/
template <typename... ModuleStates>
const Pose2d& Update(const Rotation2d& gyroAngle,
ModuleStates&&... moduleStates) {
return UpdateWithTime(wpi::Now() * 1.0e-6_s, gyroAngle, moduleStates...);
}
const Pose2d& Update(
const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules> modulePositions);
private:
SwerveDriveKinematics<NumModules> m_kinematics;
Pose2d m_pose;
units::second_t m_previousTime = -1_s;
Rotation2d m_previousAngle;
Rotation2d m_gyroOffset;
wpi::array<SwerveModulePosition, NumModules> m_previousModulePositions;
};
extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)

69
wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
View File

@@ -11,8 +11,11 @@ namespace frc {
template <size_t NumModules>
SwerveDriveOdometry<NumModules>::SwerveDriveOdometry(
SwerveDriveKinematics<NumModules> kinematics, const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules> modulePositions,
const Pose2d& initialPose)
: m_kinematics(kinematics), m_pose(initialPose) {
: m_kinematics(kinematics),
m_pose(initialPose),
m_previousModulePositions(modulePositions) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
wpi::math::MathSharedStore::ReportUsage(
@@ -20,24 +23,66 @@ SwerveDriveOdometry<NumModules>::SwerveDriveOdometry(
}
template <size_t NumModules>
template <typename... ModuleStates>
const Pose2d& frc::SwerveDriveOdometry<NumModules>::UpdateWithTime(
units::second_t currentTime, const Rotation2d& gyroAngle,
ModuleStates&&... moduleStates) {
units::second_t deltaTime =
(m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
m_previousTime = currentTime;
template <typename... ModulePositions>
void SwerveDriveOdometry<NumModules>::ResetPosition(
const Pose2d& pose, const Rotation2d& gyroAngle,
ModulePositions&&... wheelPositions) {
static_assert(sizeof...(wheelPositions) == NumModules,
"Number of modules is not consistent with number of wheel "
"locations provided in constructor.");
wpi::array<SwerveModulePosition, NumModules> modulePositions{
wheelPositions...};
this->ResetPosition(pose, gyroAngle, modulePositions);
}
template <size_t NumModules>
void SwerveDriveOdometry<NumModules>::ResetPosition(
const Pose2d& pose, const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules> modulePositions) {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
m_previousModulePositions = modulePositions;
}
template <size_t NumModules>
template <typename... ModulePositions>
const Pose2d& frc::SwerveDriveOdometry<NumModules>::Update(
const Rotation2d& gyroAngle, ModulePositions&&... wheelPositions) {
static_assert(sizeof...(wheelPositions) == NumModules,
"Number of modules is not consistent with number of wheel "
"locations provided in constructor.");
wpi::array<SwerveModulePosition, NumModules> modulePositions{
wheelPositions...};
return this->Update(gyroAngle, modulePositions);
}
template <size_t NumModules>
const Pose2d& frc::SwerveDriveOdometry<NumModules>::Update(
const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules> modulePositions) {
auto moduleDeltas =
wpi::array<SwerveModulePosition, NumModules>(wpi::empty_array);
for (size_t index = 0; index < modulePositions.size(); index++) {
auto lastPosition = m_previousModulePositions[index];
auto currentPosition = modulePositions[index];
moduleDeltas[index] = {currentPosition.distance - lastPosition.distance,
currentPosition.angle};
}
auto angle = gyroAngle + m_gyroOffset;
auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(moduleStates...);
static_cast<void>(dtheta);
auto twist = m_kinematics.ToTwist2d(moduleDeltas);
twist.dtheta = (angle - m_previousAngle).Radians();
auto newPose = m_pose.Exp(
{dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()});
auto newPose = m_pose.Exp(twist);
m_previousAngle = angle;
m_pose = {newPose.Translation(), angle};
m_previousModulePositions = modulePositions;
return m_pose;
}

29
wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h Normal file
View File

@@ -0,0 +1,29 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <wpi/SymbolExports.h>
#include "frc/geometry/Rotation2d.h"
#include "units/angle.h"
#include "units/length.h"
#include "units/math.h"
namespace frc {
/**
* Represents the position of one swerve module.
*/
struct WPILIB_DLLEXPORT SwerveModulePosition {
/**
* Distance the wheel of a module has traveled
*/
units::meter_t distance = 0_m;
/**
* Angle of the module.
*/
Rotation2d angle;
};
} // namespace frc